forked from xtdrone/XTDrone
add 3 contributer demos
This commit is contained in:
parent
481bd95ca6
commit
a3729a866d
|
|
@ -89,13 +89,13 @@ USV
|
|||
|
||||
- Founders: Kun Xiao, Shaochang Tan
|
||||
- Adviser: Xiangke Wang
|
||||
- Developers: Kun Xiao, Shaochang Tan, Guanzheng Wang, Lan Ma, Qipeng Wang, Xinyu Hu, Wenxin Hu, Yi Bao, Ruoqiao Guan, Xinyu Hu, Keyan Chen, Gao Chen
|
||||
- Developers: Kun Xiao, Shaochang Tan, Guanzheng Wang, Lan Ma, Qipeng Wang, Xinyu Hu, Ruoqiao Guan, Wenxin Hu, Feng Yi, Jiarun Yan, Yi Bao, Keyan Chen, Gao Chen
|
||||
|
||||
### Contributers
|
||||
|
||||
Sincerely thank you for your contribution to XTDrone.
|
||||
|
||||
Changhao Sun, Zihan Lin, Yao He
|
||||
Changhao Sun, Ying Nie, Fanjie Kong, Chaoran Li, Xudong Li, Zihan Lin, Yao He
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -93,7 +93,7 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
|
|||
|
||||
- 创立者：肖昆，谭劭昌
|
||||
- 指导老师：王祥科
|
||||
- 开发团队：肖昆，谭劭昌，王冠政，马澜，王齐鹏，胡新雨，胡文信，鲍毅，管若乔，陈科研，陈皋
|
||||
- 开发团队：肖昆，谭劭昌，王冠政，马澜，王齐鹏，胡新雨，管若乔，胡文信，易丰，颜佳润，鲍毅，陈科研，陈皋
|
||||
|
||||
### 加入我们
|
||||
|
||||
|
|
@ -103,7 +103,7 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
|
|||
|
||||
非常感谢你们为XTDrone的贡献
|
||||
|
||||
孙长浩 林梓涵 何瑶
|
||||
孙长浩，聂莹，孔凡杰，李超然，李旭东，林梓涵，何瑶
|
||||
|
||||
### 捐赠
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,22 @@
|
|||
|
||||
# UAV Collaborative Simulation
|
||||
|
||||
To simulate UAVs Collaborative for target searching , recognition and localization.
|
||||
|
||||
## Introduction
|
||||
To run the project, do as follows:
|
||||
### 1.Source Download
|
||||
```bash
|
||||
git clone https://codechina.csdn.net/qq_44715174/uav_collaborative_simulation.git
|
||||
```
|
||||
### 2.Simulation Environment
|
||||
1.copy .launch and .world to px4
|
||||
```bash
|
||||
cd uav_collaborative_simulation
|
||||
cp worlds/* ~/PX4_Firmware/Tools/sitl_gazebo/worlds/
|
||||
cp launch/* ~/PX4_Firmware/launch/
|
||||
```
|
||||
2.run the simulation environment
|
||||
```bash
|
||||
roslaunch px4 cic2021.launch
|
||||
```
|
||||
|
|
@ -0,0 +1,400 @@
|
|||
/clock
|
||||
/diagnostics
|
||||
/gazebo/link_states
|
||||
/gazebo/model_states
|
||||
/gazebo/parameter_descriptions
|
||||
/gazebo/parameter_updates
|
||||
/gazebo/set_link_state
|
||||
/gazebo/set_model_state
|
||||
/rosout
|
||||
/rosout_agg
|
||||
/tf
|
||||
/tf_static
|
||||
/typhoon_h480_0/cgo3_camera/camera_info
|
||||
/typhoon_h480_0/cgo3_camera/image_raw
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/compressed
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/compressed/parameter_descriptions
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/compressed/parameter_updates
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/compressedDepth
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/compressedDepth/parameter_descriptions
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/compressedDepth/parameter_updates
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/theora
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/theora/parameter_descriptions
|
||||
/typhoon_h480_0/cgo3_camera/image_raw/theora/parameter_updates
|
||||
/typhoon_h480_0/cgo3_camera/parameter_descriptions
|
||||
/typhoon_h480_0/cgo3_camera/parameter_updates
|
||||
/typhoon_h480_0/mavlink/from
|
||||
/typhoon_h480_0/mavlink/gcs_ip
|
||||
/typhoon_h480_0/mavlink/to
|
||||
/typhoon_h480_0/mavros/actuator_control
|
||||
/typhoon_h480_0/mavros/adsb/send
|
||||
/typhoon_h480_0/mavros/adsb/vehicle
|
||||
/typhoon_h480_0/mavros/altitude
|
||||
/typhoon_h480_0/mavros/battery
|
||||
/typhoon_h480_0/mavros/cam_imu_sync/cam_imu_stamp
|
||||
/typhoon_h480_0/mavros/companion_process/status
|
||||
/typhoon_h480_0/mavros/debug_value/debug
|
||||
/typhoon_h480_0/mavros/debug_value/debug_vector
|
||||
/typhoon_h480_0/mavros/debug_value/named_value_float
|
||||
/typhoon_h480_0/mavros/debug_value/named_value_int
|
||||
/typhoon_h480_0/mavros/debug_value/send
|
||||
/typhoon_h480_0/mavros/esc_info
|
||||
/typhoon_h480_0/mavros/esc_status
|
||||
/typhoon_h480_0/mavros/estimator_status
|
||||
/typhoon_h480_0/mavros/extended_state
|
||||
/typhoon_h480_0/mavros/fake_gps/mocap/tf
|
||||
/typhoon_h480_0/mavros/global_position/compass_hdg
|
||||
/typhoon_h480_0/mavros/global_position/global
|
||||
/typhoon_h480_0/mavros/global_position/gp_lp_offset
|
||||
/typhoon_h480_0/mavros/global_position/gp_origin
|
||||
/typhoon_h480_0/mavros/global_position/home
|
||||
/typhoon_h480_0/mavros/global_position/local
|
||||
/typhoon_h480_0/mavros/global_position/raw/fix
|
||||
/typhoon_h480_0/mavros/global_position/raw/gps_vel
|
||||
/typhoon_h480_0/mavros/global_position/raw/satellites
|
||||
/typhoon_h480_0/mavros/global_position/rel_alt
|
||||
/typhoon_h480_0/mavros/global_position/set_gp_origin
|
||||
/typhoon_h480_0/mavros/gps_rtk/rtk_baseline
|
||||
/typhoon_h480_0/mavros/gps_rtk/send_rtcm
|
||||
/typhoon_h480_0/mavros/gpsstatus/gps1/raw
|
||||
/typhoon_h480_0/mavros/gpsstatus/gps1/rtk
|
||||
/typhoon_h480_0/mavros/gpsstatus/gps2/raw
|
||||
/typhoon_h480_0/mavros/gpsstatus/gps2/rtk
|
||||
/typhoon_h480_0/mavros/hil/actuator_controls
|
||||
/typhoon_h480_0/mavros/hil/controls
|
||||
/typhoon_h480_0/mavros/hil/gps
|
||||
/typhoon_h480_0/mavros/hil/imu_ned
|
||||
/typhoon_h480_0/mavros/hil/optical_flow
|
||||
/typhoon_h480_0/mavros/hil/rc_inputs
|
||||
/typhoon_h480_0/mavros/hil/state
|
||||
/typhoon_h480_0/mavros/home_position/home
|
||||
/typhoon_h480_0/mavros/home_position/set
|
||||
/typhoon_h480_0/mavros/imu/data
|
||||
/typhoon_h480_0/mavros/imu/data_raw
|
||||
/typhoon_h480_0/mavros/imu/diff_pressure
|
||||
/typhoon_h480_0/mavros/imu/mag
|
||||
/typhoon_h480_0/mavros/imu/static_pressure
|
||||
/typhoon_h480_0/mavros/imu/temperature_baro
|
||||
/typhoon_h480_0/mavros/imu/temperature_imu
|
||||
/typhoon_h480_0/mavros/landing_target/lt_marker
|
||||
/typhoon_h480_0/mavros/landing_target/pose
|
||||
/typhoon_h480_0/mavros/landing_target/pose_in
|
||||
/typhoon_h480_0/mavros/local_position/accel
|
||||
/typhoon_h480_0/mavros/local_position/odom
|
||||
/typhoon_h480_0/mavros/local_position/pose
|
||||
/typhoon_h480_0/mavros/local_position/pose_cov
|
||||
/typhoon_h480_0/mavros/local_position/velocity_body
|
||||
/typhoon_h480_0/mavros/local_position/velocity_body_cov
|
||||
/typhoon_h480_0/mavros/local_position/velocity_local
|
||||
/typhoon_h480_0/mavros/log_transfer/raw/log_data
|
||||
/typhoon_h480_0/mavros/log_transfer/raw/log_entry
|
||||
/typhoon_h480_0/mavros/manual_control/control
|
||||
/typhoon_h480_0/mavros/manual_control/send
|
||||
/typhoon_h480_0/mavros/mission/reached
|
||||
/typhoon_h480_0/mavros/mission/waypoints
|
||||
/typhoon_h480_0/mavros/mocap/pose
|
||||
/typhoon_h480_0/mavros/mount_control/command
|
||||
/typhoon_h480_0/mavros/mount_control/orientation
|
||||
/typhoon_h480_0/mavros/obstacle/send
|
||||
/typhoon_h480_0/mavros/odometry/in
|
||||
/typhoon_h480_0/mavros/odometry/out
|
||||
/typhoon_h480_0/mavros/onboard_computer/status
|
||||
/typhoon_h480_0/mavros/param/param_value
|
||||
/typhoon_h480_0/mavros/play_tune
|
||||
/typhoon_h480_0/mavros/px4flow/ground_distance
|
||||
/typhoon_h480_0/mavros/px4flow/raw/optical_flow_rad
|
||||
/typhoon_h480_0/mavros/px4flow/raw/send
|
||||
/typhoon_h480_0/mavros/px4flow/temperature
|
||||
/typhoon_h480_0/mavros/radio_status
|
||||
/typhoon_h480_0/mavros/rc/in
|
||||
/typhoon_h480_0/mavros/rc/out
|
||||
/typhoon_h480_0/mavros/rc/override
|
||||
/typhoon_h480_0/mavros/setpoint_accel/accel
|
||||
/typhoon_h480_0/mavros/setpoint_attitude/cmd_vel
|
||||
/typhoon_h480_0/mavros/setpoint_attitude/thrust
|
||||
/typhoon_h480_0/mavros/setpoint_position/global
|
||||
/typhoon_h480_0/mavros/setpoint_position/global_to_local
|
||||
/typhoon_h480_0/mavros/setpoint_position/local
|
||||
/typhoon_h480_0/mavros/setpoint_raw/attitude
|
||||
/typhoon_h480_0/mavros/setpoint_raw/global
|
||||
/typhoon_h480_0/mavros/setpoint_raw/local
|
||||
/typhoon_h480_0/mavros/setpoint_raw/target_attitude
|
||||
/typhoon_h480_0/mavros/setpoint_raw/target_global
|
||||
/typhoon_h480_0/mavros/setpoint_raw/target_local
|
||||
/typhoon_h480_0/mavros/setpoint_trajectory/desired
|
||||
/typhoon_h480_0/mavros/setpoint_trajectory/local
|
||||
/typhoon_h480_0/mavros/setpoint_velocity/cmd_vel
|
||||
/typhoon_h480_0/mavros/setpoint_velocity/cmd_vel_unstamped
|
||||
/typhoon_h480_0/mavros/state
|
||||
/typhoon_h480_0/mavros/statustext/recv
|
||||
/typhoon_h480_0/mavros/statustext/send
|
||||
/typhoon_h480_0/mavros/target_actuator_control
|
||||
/typhoon_h480_0/mavros/time_reference
|
||||
/typhoon_h480_0/mavros/timesync_status
|
||||
/typhoon_h480_0/mavros/trajectory/desired
|
||||
/typhoon_h480_0/mavros/trajectory/generated
|
||||
/typhoon_h480_0/mavros/trajectory/path
|
||||
/typhoon_h480_0/mavros/vfr_hud
|
||||
/typhoon_h480_0/mavros/vision_pose/pose
|
||||
/typhoon_h480_0/mavros/vision_pose/pose_cov
|
||||
/typhoon_h480_0/mavros/vision_speed/speed_twist_cov
|
||||
/typhoon_h480_0/mavros/wind_estimation
|
||||
/typhoon_h480_1/cgo3_camera/camera_info
|
||||
/typhoon_h480_1/cgo3_camera/image_raw
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/compressed
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/compressed/parameter_descriptions
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/compressed/parameter_updates
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/compressedDepth
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/compressedDepth/parameter_descriptions
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/compressedDepth/parameter_updates
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/theora
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/theora/parameter_descriptions
|
||||
/typhoon_h480_1/cgo3_camera/image_raw/theora/parameter_updates
|
||||
/typhoon_h480_1/cgo3_camera/parameter_descriptions
|
||||
/typhoon_h480_1/cgo3_camera/parameter_updates
|
||||
/typhoon_h480_1/mavlink/from
|
||||
/typhoon_h480_1/mavlink/gcs_ip
|
||||
/typhoon_h480_1/mavlink/to
|
||||
/typhoon_h480_1/mavros/actuator_control
|
||||
/typhoon_h480_1/mavros/adsb/send
|
||||
/typhoon_h480_1/mavros/adsb/vehicle
|
||||
/typhoon_h480_1/mavros/altitude
|
||||
/typhoon_h480_1/mavros/battery
|
||||
/typhoon_h480_1/mavros/cam_imu_sync/cam_imu_stamp
|
||||
/typhoon_h480_1/mavros/companion_process/status
|
||||
/typhoon_h480_1/mavros/debug_value/debug
|
||||
/typhoon_h480_1/mavros/debug_value/debug_vector
|
||||
/typhoon_h480_1/mavros/debug_value/named_value_float
|
||||
/typhoon_h480_1/mavros/debug_value/named_value_int
|
||||
/typhoon_h480_1/mavros/debug_value/send
|
||||
/typhoon_h480_1/mavros/esc_info
|
||||
/typhoon_h480_1/mavros/esc_status
|
||||
/typhoon_h480_1/mavros/estimator_status
|
||||
/typhoon_h480_1/mavros/extended_state
|
||||
/typhoon_h480_1/mavros/fake_gps/mocap/tf
|
||||
/typhoon_h480_1/mavros/global_position/compass_hdg
|
||||
/typhoon_h480_1/mavros/global_position/global
|
||||
/typhoon_h480_1/mavros/global_position/gp_lp_offset
|
||||
/typhoon_h480_1/mavros/global_position/gp_origin
|
||||
/typhoon_h480_1/mavros/global_position/home
|
||||
/typhoon_h480_1/mavros/global_position/local
|
||||
/typhoon_h480_1/mavros/global_position/raw/fix
|
||||
/typhoon_h480_1/mavros/global_position/raw/gps_vel
|
||||
/typhoon_h480_1/mavros/global_position/raw/satellites
|
||||
/typhoon_h480_1/mavros/global_position/rel_alt
|
||||
/typhoon_h480_1/mavros/global_position/set_gp_origin
|
||||
/typhoon_h480_1/mavros/gps_rtk/rtk_baseline
|
||||
/typhoon_h480_1/mavros/gps_rtk/send_rtcm
|
||||
/typhoon_h480_1/mavros/gpsstatus/gps1/raw
|
||||
/typhoon_h480_1/mavros/gpsstatus/gps1/rtk
|
||||
/typhoon_h480_1/mavros/gpsstatus/gps2/raw
|
||||
/typhoon_h480_1/mavros/gpsstatus/gps2/rtk
|
||||
/typhoon_h480_1/mavros/hil/actuator_controls
|
||||
/typhoon_h480_1/mavros/hil/controls
|
||||
/typhoon_h480_1/mavros/hil/gps
|
||||
/typhoon_h480_1/mavros/hil/imu_ned
|
||||
/typhoon_h480_1/mavros/hil/optical_flow
|
||||
/typhoon_h480_1/mavros/hil/rc_inputs
|
||||
/typhoon_h480_1/mavros/hil/state
|
||||
/typhoon_h480_1/mavros/home_position/home
|
||||
/typhoon_h480_1/mavros/home_position/set
|
||||
/typhoon_h480_1/mavros/imu/data
|
||||
/typhoon_h480_1/mavros/imu/data_raw
|
||||
/typhoon_h480_1/mavros/imu/diff_pressure
|
||||
/typhoon_h480_1/mavros/imu/mag
|
||||
/typhoon_h480_1/mavros/imu/static_pressure
|
||||
/typhoon_h480_1/mavros/imu/temperature_baro
|
||||
/typhoon_h480_1/mavros/imu/temperature_imu
|
||||
/typhoon_h480_1/mavros/landing_target/lt_marker
|
||||
/typhoon_h480_1/mavros/landing_target/pose
|
||||
/typhoon_h480_1/mavros/landing_target/pose_in
|
||||
/typhoon_h480_1/mavros/local_position/accel
|
||||
/typhoon_h480_1/mavros/local_position/odom
|
||||
/typhoon_h480_1/mavros/local_position/pose
|
||||
/typhoon_h480_1/mavros/local_position/pose_cov
|
||||
/typhoon_h480_1/mavros/local_position/velocity_body
|
||||
/typhoon_h480_1/mavros/local_position/velocity_body_cov
|
||||
/typhoon_h480_1/mavros/local_position/velocity_local
|
||||
/typhoon_h480_1/mavros/log_transfer/raw/log_data
|
||||
/typhoon_h480_1/mavros/log_transfer/raw/log_entry
|
||||
/typhoon_h480_1/mavros/manual_control/control
|
||||
/typhoon_h480_1/mavros/manual_control/send
|
||||
/typhoon_h480_1/mavros/mission/reached
|
||||
/typhoon_h480_1/mavros/mission/waypoints
|
||||
/typhoon_h480_1/mavros/mocap/pose
|
||||
/typhoon_h480_1/mavros/mount_control/command
|
||||
/typhoon_h480_1/mavros/mount_control/orientation
|
||||
/typhoon_h480_1/mavros/obstacle/send
|
||||
/typhoon_h480_1/mavros/odometry/in
|
||||
/typhoon_h480_1/mavros/odometry/out
|
||||
/typhoon_h480_1/mavros/onboard_computer/status
|
||||
/typhoon_h480_1/mavros/param/param_value
|
||||
/typhoon_h480_1/mavros/play_tune
|
||||
/typhoon_h480_1/mavros/px4flow/ground_distance
|
||||
/typhoon_h480_1/mavros/px4flow/raw/optical_flow_rad
|
||||
/typhoon_h480_1/mavros/px4flow/raw/send
|
||||
/typhoon_h480_1/mavros/px4flow/temperature
|
||||
/typhoon_h480_1/mavros/radio_status
|
||||
/typhoon_h480_1/mavros/rc/in
|
||||
/typhoon_h480_1/mavros/rc/out
|
||||
/typhoon_h480_1/mavros/rc/override
|
||||
/typhoon_h480_1/mavros/setpoint_accel/accel
|
||||
/typhoon_h480_1/mavros/setpoint_attitude/cmd_vel
|
||||
/typhoon_h480_1/mavros/setpoint_attitude/thrust
|
||||
/typhoon_h480_1/mavros/setpoint_position/global
|
||||
/typhoon_h480_1/mavros/setpoint_position/global_to_local
|
||||
/typhoon_h480_1/mavros/setpoint_position/local
|
||||
/typhoon_h480_1/mavros/setpoint_raw/attitude
|
||||
/typhoon_h480_1/mavros/setpoint_raw/global
|
||||
/typhoon_h480_1/mavros/setpoint_raw/local
|
||||
/typhoon_h480_1/mavros/setpoint_raw/target_attitude
|
||||
/typhoon_h480_1/mavros/setpoint_raw/target_global
|
||||
/typhoon_h480_1/mavros/setpoint_raw/target_local
|
||||
/typhoon_h480_1/mavros/setpoint_trajectory/desired
|
||||
/typhoon_h480_1/mavros/setpoint_trajectory/local
|
||||
/typhoon_h480_1/mavros/setpoint_velocity/cmd_vel
|
||||
/typhoon_h480_1/mavros/setpoint_velocity/cmd_vel_unstamped
|
||||
/typhoon_h480_1/mavros/state
|
||||
/typhoon_h480_1/mavros/statustext/recv
|
||||
/typhoon_h480_1/mavros/statustext/send
|
||||
/typhoon_h480_1/mavros/target_actuator_control
|
||||
/typhoon_h480_1/mavros/time_reference
|
||||
/typhoon_h480_1/mavros/timesync_status
|
||||
/typhoon_h480_1/mavros/trajectory/desired
|
||||
/typhoon_h480_1/mavros/trajectory/generated
|
||||
/typhoon_h480_1/mavros/trajectory/path
|
||||
/typhoon_h480_1/mavros/vfr_hud
|
||||
/typhoon_h480_1/mavros/vision_pose/pose
|
||||
/typhoon_h480_1/mavros/vision_pose/pose_cov
|
||||
/typhoon_h480_1/mavros/vision_speed/speed_twist_cov
|
||||
/typhoon_h480_1/mavros/wind_estimation
|
||||
/typhoon_h480_2/cgo3_camera/camera_info
|
||||
/typhoon_h480_2/cgo3_camera/image_raw
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/compressed
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/compressed/parameter_descriptions
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/compressed/parameter_updates
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/compressedDepth
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/compressedDepth/parameter_descriptions
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/compressedDepth/parameter_updates
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/theora
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/theora/parameter_descriptions
|
||||
/typhoon_h480_2/cgo3_camera/image_raw/theora/parameter_updates
|
||||
/typhoon_h480_2/cgo3_camera/parameter_descriptions
|
||||
/typhoon_h480_2/cgo3_camera/parameter_updates
|
||||
/typhoon_h480_2/mavlink/from
|
||||
/typhoon_h480_2/mavlink/gcs_ip
|
||||
/typhoon_h480_2/mavlink/to
|
||||
/typhoon_h480_2/mavros/actuator_control
|
||||
/typhoon_h480_2/mavros/adsb/send
|
||||
/typhoon_h480_2/mavros/adsb/vehicle
|
||||
/typhoon_h480_2/mavros/altitude
|
||||
/typhoon_h480_2/mavros/battery
|
||||
/typhoon_h480_2/mavros/cam_imu_sync/cam_imu_stamp
|
||||
/typhoon_h480_2/mavros/companion_process/status
|
||||
/typhoon_h480_2/mavros/debug_value/debug
|
||||
/typhoon_h480_2/mavros/debug_value/debug_vector
|
||||
/typhoon_h480_2/mavros/debug_value/named_value_float
|
||||
/typhoon_h480_2/mavros/debug_value/named_value_int
|
||||
/typhoon_h480_2/mavros/debug_value/send
|
||||
/typhoon_h480_2/mavros/esc_info
|
||||
/typhoon_h480_2/mavros/esc_status
|
||||
/typhoon_h480_2/mavros/estimator_status
|
||||
/typhoon_h480_2/mavros/extended_state
|
||||
/typhoon_h480_2/mavros/fake_gps/mocap/tf
|
||||
/typhoon_h480_2/mavros/global_position/compass_hdg
|
||||
/typhoon_h480_2/mavros/global_position/global
|
||||
/typhoon_h480_2/mavros/global_position/gp_lp_offset
|
||||
/typhoon_h480_2/mavros/global_position/gp_origin
|
||||
/typhoon_h480_2/mavros/global_position/home
|
||||
/typhoon_h480_2/mavros/global_position/local
|
||||
/typhoon_h480_2/mavros/global_position/raw/fix
|
||||
/typhoon_h480_2/mavros/global_position/raw/gps_vel
|
||||
/typhoon_h480_2/mavros/global_position/raw/satellites
|
||||
/typhoon_h480_2/mavros/global_position/rel_alt
|
||||
/typhoon_h480_2/mavros/global_position/set_gp_origin
|
||||
/typhoon_h480_2/mavros/gps_rtk/rtk_baseline
|
||||
/typhoon_h480_2/mavros/gps_rtk/send_rtcm
|
||||
/typhoon_h480_2/mavros/gpsstatus/gps1/raw
|
||||
/typhoon_h480_2/mavros/gpsstatus/gps1/rtk
|
||||
/typhoon_h480_2/mavros/gpsstatus/gps2/raw
|
||||
/typhoon_h480_2/mavros/gpsstatus/gps2/rtk
|
||||
/typhoon_h480_2/mavros/hil/actuator_controls
|
||||
/typhoon_h480_2/mavros/hil/controls
|
||||
/typhoon_h480_2/mavros/hil/gps
|
||||
/typhoon_h480_2/mavros/hil/imu_ned
|
||||
/typhoon_h480_2/mavros/hil/optical_flow
|
||||
/typhoon_h480_2/mavros/hil/rc_inputs
|
||||
/typhoon_h480_2/mavros/hil/state
|
||||
/typhoon_h480_2/mavros/home_position/home
|
||||
/typhoon_h480_2/mavros/home_position/set
|
||||
/typhoon_h480_2/mavros/imu/data
|
||||
/typhoon_h480_2/mavros/imu/data_raw
|
||||
/typhoon_h480_2/mavros/imu/diff_pressure
|
||||
/typhoon_h480_2/mavros/imu/mag
|
||||
/typhoon_h480_2/mavros/imu/static_pressure
|
||||
/typhoon_h480_2/mavros/imu/temperature_baro
|
||||
/typhoon_h480_2/mavros/imu/temperature_imu
|
||||
/typhoon_h480_2/mavros/landing_target/lt_marker
|
||||
/typhoon_h480_2/mavros/landing_target/pose
|
||||
/typhoon_h480_2/mavros/landing_target/pose_in
|
||||
/typhoon_h480_2/mavros/local_position/accel
|
||||
/typhoon_h480_2/mavros/local_position/odom
|
||||
/typhoon_h480_2/mavros/local_position/pose
|
||||
/typhoon_h480_2/mavros/local_position/pose_cov
|
||||
/typhoon_h480_2/mavros/local_position/velocity_body
|
||||
/typhoon_h480_2/mavros/local_position/velocity_body_cov
|
||||
/typhoon_h480_2/mavros/local_position/velocity_local
|
||||
/typhoon_h480_2/mavros/log_transfer/raw/log_data
|
||||
/typhoon_h480_2/mavros/log_transfer/raw/log_entry
|
||||
/typhoon_h480_2/mavros/manual_control/control
|
||||
/typhoon_h480_2/mavros/manual_control/send
|
||||
/typhoon_h480_2/mavros/mission/reached
|
||||
/typhoon_h480_2/mavros/mission/waypoints
|
||||
/typhoon_h480_2/mavros/mocap/pose
|
||||
/typhoon_h480_2/mavros/mount_control/command
|
||||
/typhoon_h480_2/mavros/mount_control/orientation
|
||||
/typhoon_h480_2/mavros/obstacle/send
|
||||
/typhoon_h480_2/mavros/odometry/in
|
||||
/typhoon_h480_2/mavros/odometry/out
|
||||
/typhoon_h480_2/mavros/onboard_computer/status
|
||||
/typhoon_h480_2/mavros/param/param_value
|
||||
/typhoon_h480_2/mavros/play_tune
|
||||
/typhoon_h480_2/mavros/px4flow/ground_distance
|
||||
/typhoon_h480_2/mavros/px4flow/raw/optical_flow_rad
|
||||
/typhoon_h480_2/mavros/px4flow/raw/send
|
||||
/typhoon_h480_2/mavros/px4flow/temperature
|
||||
/typhoon_h480_2/mavros/radio_status
|
||||
/typhoon_h480_2/mavros/rc/in
|
||||
/typhoon_h480_2/mavros/rc/out
|
||||
/typhoon_h480_2/mavros/rc/override
|
||||
/typhoon_h480_2/mavros/setpoint_accel/accel
|
||||
/typhoon_h480_2/mavros/setpoint_attitude/cmd_vel
|
||||
/typhoon_h480_2/mavros/setpoint_attitude/thrust
|
||||
/typhoon_h480_2/mavros/setpoint_position/global
|
||||
/typhoon_h480_2/mavros/setpoint_position/global_to_local
|
||||
/typhoon_h480_2/mavros/setpoint_position/local
|
||||
/typhoon_h480_2/mavros/setpoint_raw/attitude
|
||||
/typhoon_h480_2/mavros/setpoint_raw/global
|
||||
/typhoon_h480_2/mavros/setpoint_raw/local
|
||||
/typhoon_h480_2/mavros/setpoint_raw/target_attitude
|
||||
/typhoon_h480_2/mavros/setpoint_raw/target_global
|
||||
/typhoon_h480_2/mavros/setpoint_raw/target_local
|
||||
/typhoon_h480_2/mavros/setpoint_trajectory/desired
|
||||
/typhoon_h480_2/mavros/setpoint_trajectory/local
|
||||
/typhoon_h480_2/mavros/setpoint_velocity/cmd_vel
|
||||
/typhoon_h480_2/mavros/setpoint_velocity/cmd_vel_unstamped
|
||||
/typhoon_h480_2/mavros/state
|
||||
/typhoon_h480_2/mavros/statustext/recv
|
||||
/typhoon_h480_2/mavros/statustext/send
|
||||
/typhoon_h480_2/mavros/target_actuator_control
|
||||
/typhoon_h480_2/mavros/time_reference
|
||||
/typhoon_h480_2/mavros/timesync_status
|
||||
/typhoon_h480_2/mavros/trajectory/desired
|
||||
/typhoon_h480_2/mavros/trajectory/generated
|
||||
/typhoon_h480_2/mavros/trajectory/path
|
||||
/typhoon_h480_2/mavros/vfr_hud
|
||||
/typhoon_h480_2/mavros/vision_pose/pose
|
||||
/typhoon_h480_2/mavros/vision_pose/pose_cov
|
||||
/typhoon_h480_2/mavros/vision_speed/speed_twist_cov
|
||||
/typhoon_h480_2/mavros/wind_estimation
|
||||
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/cic2021.world"/>
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<!-- Gazebo sim -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="world_name" value="$(arg world)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!-- typhoon_h480_0 -->
|
||||
<group ns="typhoon_h480_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="0"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_cic.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480"/>
|
||||
<arg name="mavlink_udp_port" value="18570"/>
|
||||
<arg name="mavlink_tcp_port" value="4560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h480_1 -->
|
||||
<group ns="typhoon_h480_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_cic.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480"/>
|
||||
<arg name="mavlink_udp_port" value="18571"/>
|
||||
<arg name="mavlink_tcp_port" value="4561"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h480_2 -->
|
||||
<group ns="typhoon_h480_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_cic.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480"/>
|
||||
<arg name="mavlink_udp_port" value="18572"/>
|
||||
<arg name="mavlink_tcp_port" value="4562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1 @@
|
|||
/opt/ros/melodic/share/catkin/cmake/toplevel.cmake
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(formation)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
geometry_msgs
|
||||
rosmsg
|
||||
rospy
|
||||
std_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>formation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The PX4 Formation Control package</description>
|
||||
<maintainer email="yan@todo.todo">malan</maintainer>
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>rosmsg</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>rosmsg</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>rosmsg</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<export>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import random
|
||||
|
||||
class Env():
|
||||
def __init__(self, vehicle_num, target_num, map_size, visualized):
|
||||
self.visualized = visualized
|
||||
self.vehicles_position = np.zeros(vehicle_num, dtype=np.int32)
|
||||
self.vehicles_speed = np.zeros(vehicle_num, dtype=np.int32)
|
||||
self.targets = np.zeros(shape=(target_num + 1, 4), dtype=np.int32)
|
||||
self.distant_mat = np.zeros((target_num + 1, target_num + 1), dtype=np.float32)
|
||||
self.map_size_x = map_size[0]
|
||||
self.map_size_y = map_size[1]
|
||||
self.speed_range = [1, 1, 1]
|
||||
self.time_lim = max(self.map_size_x, self.map_size_y )/ self.speed_range[0]
|
||||
self.vehicles_lefttime = np.ones(vehicle_num, dtype=np.float32) * self.time_lim
|
||||
self.assignment = [[] for i in range(vehicle_num)]
|
||||
self.total_reward = 0
|
||||
for i in range(self.vehicles_speed.shape[0]):
|
||||
speed_type = random.randint(0,2)
|
||||
self.vehicles_speed[i] = self.speed_range[speed_type]
|
||||
self.time_limit = max(self.map_size_x, self.map_size_y) / self.speed_range[0]
|
||||
self.end = False
|
||||
self.task_generator()
|
||||
|
||||
def task_generator(self):
|
||||
for i in range(self.targets.shape[0]-1):
|
||||
self.targets[i+1,0] = random.randint(1,self.map_size_x) - 0.5*self.map_size_x # x position
|
||||
self.targets[i+1,1] = random.randint(1,self.map_size_y) - 0.5*self.map_size_y # y position
|
||||
self.targets[i + 1, 2] = random.randint(1, 10) # reward
|
||||
self.targets[i + 1, 3] = random.randint(5, 30) # time consumption to finish the mission
|
||||
self.targets[self.targets.shape[0]-2, 0] = 20
|
||||
self.targets[self.targets.shape[0] - 2, 1] = -27
|
||||
for i in range(self.targets.shape[0]):
|
||||
for j in range(self.targets.shape[0]-i):
|
||||
self.distant_mat[i, j] = np.linalg.norm(self.targets[i, :2] - self.targets[j, :2])
|
||||
|
||||
def visualize(self):
|
||||
if self.assignment == None:
|
||||
plt.scatter(x=0,y=0,s=200,c='k')
|
||||
plt.scatter(x=self.targets[1:,0],y=self.targets[1:,1],s=self.targets[1:,2]*10,c='r')
|
||||
plt.title('Target distribution')
|
||||
plt.savefig('task_pic/'+'/'+self.algorithm+ "-%d-%d.png" % (self.play,self.rond))
|
||||
plt.cla()
|
||||
else:
|
||||
plt.title('Task assignment by '+self.algorithm +', total reward : '+str(self.total_reward))
|
||||
plt.scatter(x=0,y=0,s=200,c='k')
|
||||
plt.scatter(x=self.targets[1:,0],y=self.targets[1:,1],s=self.targets[1:,2]*10,c='r')
|
||||
for i in range(len(self.assignment)):
|
||||
trajectory = np.array([[0,0,20]])
|
||||
for j in range(len(self.assignment[i])):
|
||||
position = self.targets[self.assignment[i][j],:3]
|
||||
trajectory = np.insert(trajectory,j+1,values=position,axis=0)
|
||||
plt.scatter(x=trajectory[1:,0],y=trajectory[1:,1],s=trajectory[1:,2]*10,c='b')
|
||||
plt.plot(trajectory[:,0], trajectory[:,1])
|
||||
plt.savefig('task_picture/'+self.algorithm+ "-%d-%d.png" % (self.play,self.rond))
|
||||
plt.cla()
|
||||
|
||||
def get_total_reward(self):
|
||||
for i in range(len(self.assignment)):
|
||||
speed = self.vehicles_speed[i]
|
||||
for j in range(len(self.assignment[i])):
|
||||
position = self.targets[self.assignment[i][j],:4]
|
||||
self.total_reward = self.total_reward + position[2]
|
||||
if j == 0:
|
||||
self.vehicles_lefttime[i] = self.vehicles_lefttime[i] - np.linalg.norm(position[:2]) / speed - position[3]
|
||||
else:
|
||||
self.vehicles_lefttime[i] = self.vehicles_lefttime[i] - np.linalg.norm(position[:2]-position_last[:2]) / speed - position[3]
|
||||
position_last = position
|
||||
if self.vehicles_lefttime[i] > self.time_lim:
|
||||
self.end = True
|
||||
break
|
||||
if self.end:
|
||||
self.total_reward = 0
|
||||
break
|
||||
|
||||
def run(self, assignment, algorithm, play, rond):
|
||||
self.assignment = assignment
|
||||
self.algorithm = algorithm
|
||||
self.play = play
|
||||
self.rond = rond
|
||||
self.get_total_reward()
|
||||
if self.visualized:
|
||||
self.visualize()
|
||||
|
||||
def reset(self):
|
||||
self.vehicles_position = np.zeros(self.vehicles_position.shape[0],dtype=np.int32)
|
||||
self.vehicles_lefttime = np.ones(self.vehicles_position.shape[0],dtype=np.float32) * self.time_lim
|
||||
self.targets[:,2] = self.targets_value
|
||||
self.total_reward = 0
|
||||
self.reward = 0
|
||||
self.end = False
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
# -*- coding: UTF-8 -*-
|
||||
"""
|
||||
Framework for Formation GCS
|
||||
|
||||
* send cmd by ros topic
|
||||
|
||||
Before running this code, you need to make sure ros master started:
|
||||
if not:
|
||||
$ roscore # start ros master
|
||||
|
||||
And then, you can run this code via:
|
||||
$ python formation_gcs.py # start gcs
|
||||
"""
|
||||
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
import copy
|
||||
from enviroment import Env
|
||||
from ga2 import GA2
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
import math
|
||||
|
||||
msg2ui = """
|
||||
Welcome use gcs!
|
||||
Let's go!
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
t
|
||||
h l
|
||||
|
||||
t : takeoff
|
||||
l : land
|
||||
h : hover
|
||||
0 : search target
|
||||
1 : formation 1
|
||||
2 : fortion 2
|
||||
3~9 : extendable mission(eg.different formation configuration)
|
||||
q/Q : quit
|
||||
"""
|
||||
|
||||
uav_type = 'typhoon_h480'
|
||||
|
||||
uav_bias = [[0,0,0],[0,3,0],[0,-3,0]]
|
||||
|
||||
class GroundControler:
|
||||
def __init__(self):
|
||||
self.settings = termios.tcgetattr(sys.stdin)
|
||||
self.gcs_cmd = String()
|
||||
self.gcs_cmd = 'start'
|
||||
self.formation_mode = {'FORM_0':'search', 'FORM_1':'straight', 'FORM_2':'triangle'}
|
||||
# ros publishers
|
||||
self.gcs_cmd_pub = rospy.Publisher("/formation_gcs/cmd", String, queue_size=10)
|
||||
|
||||
#ga
|
||||
self.map_size_x = 80
|
||||
self.map_size_y = 80
|
||||
self.map_size = [self.map_size_x, self.map_size_y]
|
||||
self.vehicle_num = 3
|
||||
self.target_num = 10
|
||||
self.target = [PoseStamped() for i in range(self.vehicle_num)]
|
||||
self.env = Env(self.vehicle_num, self.target_num, self.map_size, False)
|
||||
self.ga2_result = None
|
||||
self.ga2 = GA2(self.vehicle_num, self.env.vehicles_speed, self.target_num, self.env.targets, self.env.time_limit)
|
||||
self.ga_flag = False
|
||||
self.pos_i = [1 for i in range(self.vehicle_num)]
|
||||
|
||||
# ros subscribers
|
||||
for i in range(self.vehicle_num):
|
||||
rospy.Subscriber(uav_type+ '_' + str(i) + "/mavros/local_position/pose", PoseStamped, self.local_pose_callback, i, queue_size = 2)
|
||||
|
||||
# ros publishers
|
||||
self.local_target_pub = [rospy.Publisher(uav_type + '_' + str(i) + '/mavros/setpoint_position/local', PoseStamped, queue_size=10) for i in range(self.vehicle_num)]
|
||||
|
||||
self.global_pose = [PoseStamped() for i in range(self.vehicle_num)]
|
||||
|
||||
print("Ground Controller Start!")
|
||||
|
||||
def working(self):
|
||||
rospy.init_node("gcs_control_node")
|
||||
rate = rospy.Rate(60)
|
||||
self.print_ui()
|
||||
while rospy.is_shutdown() is False:
|
||||
key = self.getKey()
|
||||
if key == 't' :
|
||||
self.gcs_cmd = 'TAKEOFF'
|
||||
self.print_ui()
|
||||
print(self.gcs_cmd)
|
||||
self.gcs_cmd_pub.publish(self.gcs_cmd)
|
||||
|
||||
elif key == 'l':
|
||||
self.gcs_cmd = 'AUTO.LAND'
|
||||
self.print_ui()
|
||||
print(self.gcs_cmd)
|
||||
self.gcs_cmd_pub.publish(self.gcs_cmd)
|
||||
|
||||
elif key == 'h':
|
||||
self.gcs_cmd = 'HOVOR'
|
||||
self.print_ui()
|
||||
print(self.gcs_cmd)
|
||||
self.gcs_cmd_pub.publish(self.gcs_cmd)
|
||||
|
||||
else:
|
||||
if key == '0':
|
||||
if not self.ga_flag:
|
||||
self.ga2_result = self.ga2.run()[0]
|
||||
# print(self.ga2_result[0])
|
||||
# print(self.env.targets[self.ga2_result[0]])
|
||||
# for uav_i in range(self.vehicle_num):
|
||||
# for i in range(len(self.ga2_result[uav_i])):
|
||||
# print(self.env.targets[self.ga2_result[uav_i]][i][0],self.env.targets[self.ga2_result[uav_i]][i][1])
|
||||
self.ga_flag = True
|
||||
self.gcs_cmd = 'FORM_' + key
|
||||
self.print_ui()
|
||||
print(self.formation_mode[self.gcs_cmd])
|
||||
self.gcs_cmd_pub.publish(self.gcs_cmd)
|
||||
|
||||
for i in range(2):
|
||||
if key == str(i+1):
|
||||
self.gcs_cmd = 'FORM_'+key
|
||||
self.print_ui()
|
||||
print(self.formation_mode[self.gcs_cmd])
|
||||
self.gcs_cmd_pub.publish(self.gcs_cmd)
|
||||
|
||||
if key == 'q' or key =='Q':
|
||||
break
|
||||
if self.ga_flag:
|
||||
for uav_i in range(self.vehicle_num):
|
||||
if uav_i == 1:
|
||||
print(self.pos_i[uav_i],self.env.targets[self.ga2_result[uav_i]][self.pos_i[uav_i]])
|
||||
if self.is_get_target(uav_i,self.env.targets[self.ga2_result[uav_i]][self.pos_i[uav_i]][0],self.env.targets[self.ga2_result[uav_i]][self.pos_i[uav_i]][1]):
|
||||
self.pos_i[uav_i] += 1
|
||||
if self.pos_i[uav_i] < len(self.ga2_result[uav_i]):
|
||||
self.target[uav_i] = self.construct_target(self.env.targets[self.ga2_result[uav_i]][self.pos_i[uav_i]][0],self.env.targets[self.ga2_result[uav_i]][self.pos_i[uav_i]][1])
|
||||
else:
|
||||
self.pos_i[uav_i] -= 1
|
||||
# print(self.pos_i[uav_i])
|
||||
self.local_target_pub[uav_i].publish(self.target[uav_i])
|
||||
print(self.target[uav_i])
|
||||
|
||||
rate.sleep()
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
|
||||
|
||||
def construct_target(self,x,y):
|
||||
target_pos = PoseStamped()
|
||||
target_pos.pose.position.x = x
|
||||
target_pos.pose.position.y = y
|
||||
target_pos.pose.position.z = 6
|
||||
print(target_pos)
|
||||
return target_pos
|
||||
|
||||
def is_get_target(self,i,x,y):
|
||||
if math.sqrt((self.global_pose[i].pose.position.x-x)*(self.global_pose[i].pose.position.x-x)+\
|
||||
(self.global_pose[i].pose.position.y-y)*(self.global_pose[i].pose.position.y-y))< 0.5:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
def local_pose_callback(self, msg, i):
|
||||
self.global_pose[i] = copy.deepcopy(msg)
|
||||
self.global_pose[i].pose.position.x += uav_bias[i][0]
|
||||
self.global_pose[i].pose.position.y += uav_bias[i][1]
|
||||
self.global_pose[i].pose.position.z += uav_bias[i][2]
|
||||
|
||||
def getKey(self):
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
|
||||
return key
|
||||
|
||||
def print_ui(self):
|
||||
print(msg2ui)
|
||||
|
||||
if __name__ == '__main__':
|
||||
groundcontroler = GroundControler()
|
||||
groundcontroler.working()
|
||||
|
|
@ -0,0 +1,314 @@
|
|||
# -*- coding: UTF-8 -*-
|
||||
"""
|
||||
Framework for Formation Control
|
||||
|
||||
* To control all UAV in Gazebo(simulation environment) using MPI
|
||||
|
||||
Before running this code, you need to launch your simulation Environment and px4 via:
|
||||
$ roslaunch px4 cic2021.launch # 3 UAVs in simulation
|
||||
|
||||
And then, you can run this code via:
|
||||
$ mpiexec -n 3 python formation_sim.py # 3 UAVs in simulation
|
||||
"""
|
||||
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
from mpi4py import MPI
|
||||
import rospy
|
||||
from mavros_msgs.msg import State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, SetMode, SetMavFrame
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import numpy as np
|
||||
import copy
|
||||
from fuzzy_pid import FuzzyPID
|
||||
|
||||
|
||||
uav_type = 'typhoon_h480'
|
||||
comm = MPI.COMM_WORLD
|
||||
uav_id = comm.Get_rank()
|
||||
uav_num = comm.Get_size()
|
||||
|
||||
uav_bias = [[0,0,0],[0,3,0],[0,-3,0]]
|
||||
|
||||
class Px4Controller:
|
||||
def __init__(self):
|
||||
self.uav_id = uav_id
|
||||
self.namespace = uav_type + '_' + "{proc}".format(proc=self.uav_id)
|
||||
|
||||
self.imu = Imu()
|
||||
self.gps = NavSatFix()
|
||||
self.local_pose = PoseStamped()
|
||||
|
||||
self.neighbor_id = []
|
||||
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 6
|
||||
|
||||
self.cur_target_pose = None
|
||||
self.target_yaw = 0
|
||||
|
||||
self.takeoff_target_pose = PoseStamped()
|
||||
self.hover_target_pose = PoseStamped()
|
||||
self.target_pose = PoseStamped()
|
||||
self.target_vel = TwistStamped()
|
||||
self.global_pose = [PoseStamped() for i in range(uav_num)]
|
||||
|
||||
self.received_new_task = False
|
||||
self.arm_state = False
|
||||
self.mavros_state = State()
|
||||
|
||||
self.gcs_cmd = String()
|
||||
self.last_gcs_cmd = String()
|
||||
self.form_flag = 0
|
||||
|
||||
# control parameters
|
||||
self.fuzzy_pid = FuzzyPID()
|
||||
self.Kpx = 1
|
||||
self.Kpy = 1
|
||||
self.Kpz = 1
|
||||
self.Kpw = 1
|
||||
self.velxy_max = 1
|
||||
self.velz_max = 1
|
||||
self.w_max = 0.5
|
||||
|
||||
# ros subscribers
|
||||
for i in range(uav_num):
|
||||
rospy.Subscriber(uav_type+ '_' + str(i) + "/mavros/local_position/pose", PoseStamped, self.local_pose_callback, i, queue_size = 2)
|
||||
# self.local_vel_sub = rospy.Subscriber(uav_type + '_' + str(i) + "/mavros/local_position/velocity_local", TwistStamped, self.local_vel_callback)
|
||||
self.mavros_sub = rospy.Subscriber(self.namespace + "/mavros/state", State, self.mavros_state_callback)
|
||||
self.gps_sub = rospy.Subscriber(self.namespace + "/mavros/global_position/global", NavSatFix, self.gps_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.namespace + "/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.gcs_cmd_sub = rospy.Subscriber("/formation_gcs/cmd", String, self.gcs_cmd_callback)
|
||||
|
||||
# ros publishers
|
||||
self.local_target_pub = rospy.Publisher(self.namespace + '/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
|
||||
self.twist_target_pub = rospy.Publisher(self.namespace + '/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
|
||||
|
||||
# ros services
|
||||
self.armService = rospy.ServiceProxy(self.namespace + '/mavros/cmd/arming', CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.namespace + '/mavros/set_mode', SetMode)
|
||||
self.frameService = rospy.ServiceProxy(self.namespace + '/mavros/setpoint_velocity/mav_frame', SetMavFrame)
|
||||
print(self.namespace, ": Px4 Controller Start!")
|
||||
|
||||
def working(self):
|
||||
rospy.init_node(self.namespace + "_control_node")
|
||||
time.sleep(1) #waiting or node init 1s
|
||||
rate = rospy.Rate(60)
|
||||
|
||||
for i in range(10):
|
||||
if self.current_heading is not None:
|
||||
print(self.namespace, ": initialization finished !")
|
||||
break
|
||||
else:
|
||||
print(self.namespace, ": Waiting for initialization...")
|
||||
time.sleep(0.5)
|
||||
|
||||
while rospy.is_shutdown() is False:
|
||||
|
||||
if self.gcs_cmd == 'TAKEOFF':
|
||||
self.last_gcs_cmd = 'TAKEOFF'
|
||||
self.form_flag = 0
|
||||
if self.mavros_state != 'OFFBOARD':
|
||||
self.flight_mode_set(mode='OFFBOARD')
|
||||
if not self.arm_state:
|
||||
self.arm()
|
||||
|
||||
self.target_pose = self.construct_target(self.takeoff_target_pose.pose.position.x,
|
||||
self.takeoff_target_pose.pose.position.y , self.takeoff_height,
|
||||
self.current_heading)
|
||||
self.local_target_pub.publish(self.target_pose)
|
||||
|
||||
if self.takeoff_detection():
|
||||
print(self.namespace, ": Takeoff Success !")
|
||||
else:
|
||||
print(self.namespace, ": Takeoff Failed !!!")
|
||||
# self.frameService(1)
|
||||
|
||||
elif self.gcs_cmd == 'FORM_0':
|
||||
self.last_gcs_cmd = 'FORM_0'
|
||||
if self.mavros_state != 'OFFBOARD':
|
||||
self.flight_mode_set(mode='OFFBOARD')
|
||||
|
||||
elif self.gcs_cmd == 'FORM_1':
|
||||
self.last_gcs_cmd = 'FORM_1'
|
||||
if self.form_flag != 1:
|
||||
self.read_set_file('FORM_1_id','FORM_1_pos')
|
||||
self.form_flag = 1
|
||||
if self.mavros_state != 'OFFBOARD':
|
||||
self.flight_mode_set(mode='OFFBOARD')
|
||||
self.formation_control()
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
|
||||
elif self.gcs_cmd == 'FORM_2':
|
||||
self.last_gcs_cmd = 'FORM_2'
|
||||
if self.form_flag != 2:
|
||||
self.read_set_file('FORM_2_id','FORM_2_pos')
|
||||
self.form_flag = 2
|
||||
if self.mavros_state != 'OFFBOARD':
|
||||
self.flight_mode_set(mode='OFFBOARD')
|
||||
self.formation_control()
|
||||
self.twist_target_pub.publish(self.target_vel)
|
||||
|
||||
elif self.gcs_cmd == 'AUTO.LAND':
|
||||
self.last_gcs_cmd = 'AUTO.LAND'
|
||||
self.form_flag = 0
|
||||
if self.mavros_state != "AUTO.LAND":
|
||||
self.flight_mode_set(mode='AUTO.LAND')
|
||||
if (self.mavros_state == 'AUTO.LAND') and (self.local_pose.pose.position.z < 0.05):
|
||||
if self.arm_state:
|
||||
self.disarm()
|
||||
print(self.namespace, ": Land Success!")
|
||||
elif self.gcs_cmd == 'HOVER':
|
||||
self.last_gcs_cmd = 'HOVER'
|
||||
self.form_flag = 0
|
||||
if self.mavros_state != 'OFFBOARD':
|
||||
self.flight_mode_set(mode='OFFBOARD')
|
||||
if not self.arm_state:
|
||||
self.arm()
|
||||
|
||||
self.target_pose = self.construct_target(self.hover_target_pose.pose.position.x,
|
||||
self.hover_target_pose.pose.position.y, self.hover_target_pose.pose.position.z,
|
||||
self.current_heading)
|
||||
self.local_target_pub.publish(self.target_pose)
|
||||
else:
|
||||
self.gcs_cmd = self.last_gcs_cmd
|
||||
self.form_flag = 0
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def formation_control(self):
|
||||
print('formation control here')
|
||||
neighbor_num = len(self.neighbor_id)
|
||||
self.target_vel.twist.linear.x = 0
|
||||
self.target_vel.twist.linear.y = 0
|
||||
self.target_vel.twist.linear.z = 0
|
||||
self.target_vel.twist.angular.x = 0
|
||||
self.target_vel.twist.angular.y = 0
|
||||
self.target_vel.twist.angular.z = self.Kpw*(self.target_yaw-self.current_heading)
|
||||
# print("neighbor_num",neighbor_num)
|
||||
for i in range(neighbor_num):
|
||||
self.target_vel.twist.linear.x += self.global_pose[self.neighbor_id[i]].pose.position.x - self.global_pose[self.uav_id].pose.position.x - \
|
||||
self.all_desired_position[self.neighbor_id[i]][0] + self.all_desired_position[self.uav_id][0]
|
||||
self.target_vel.twist.linear.y += self.global_pose[self.neighbor_id[i]].pose.position.y - self.global_pose[self.uav_id].pose.position.y - \
|
||||
self.all_desired_position[self.neighbor_id[i]][1] + self.all_desired_position[self.uav_id][1]
|
||||
self.target_vel.twist.linear.z += self.global_pose[self.neighbor_id[i]].pose.position.z - self.global_pose[self.uav_id].pose.position.z - \
|
||||
self.all_desired_position[self.neighbor_id[i]][2] + self.all_desired_position[self.uav_id][2]
|
||||
|
||||
self.target_vel.twist.linear.x = self.limit(self.target_vel.twist.linear.x * self.Kpx, -self.velxy_max, self.velxy_max)
|
||||
self.target_vel.twist.linear.y = self.limit(self.target_vel.twist.linear.y * self.Kpy, -self.velxy_max, self.velxy_max)
|
||||
self.target_vel.twist.linear.z = self.limit(self.target_vel.twist.linear.z * self.Kpz, -self.velz_max, self.velz_max)
|
||||
self.target_vel.twist.angular.z = self.limit(self.target_vel.twist.angular.z * self.Kpw, -self.w_max, self.w_max)
|
||||
|
||||
def limit(self, data, min, max):
|
||||
if data <= min:
|
||||
data = min
|
||||
elif data >= max:
|
||||
data = max
|
||||
return data
|
||||
|
||||
def read_set_file(self,txt_id,txt_pos):
|
||||
self.neighbor_id =[]
|
||||
id_path='txt/'+txt_id+'.txt'
|
||||
pos_path='txt/'+txt_pos+'.txt'
|
||||
txt_uav_neighbor_num = np.loadtxt(id_path,dtype=int)
|
||||
self.all_desired_position = np.loadtxt(pos_path)
|
||||
for i in range(0, len(txt_uav_neighbor_num[:, 0])):
|
||||
if txt_uav_neighbor_num[i, 0] == self.uav_id:
|
||||
self.neighbor_id.append(txt_uav_neighbor_num[i, 1])
|
||||
print(self.neighbor_id)
|
||||
|
||||
def construct_target(self, x, y, z, yaw):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.header.stamp = rospy.Time.now()
|
||||
|
||||
target_raw_pose.coordinate_frame = 7
|
||||
|
||||
target_raw_pose.position.x = x
|
||||
target_raw_pose.position.y = y
|
||||
target_raw_pose.position.z = z
|
||||
target_raw_pose.yaw = yaw
|
||||
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||
+ PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE \
|
||||
+ PositionTarget.FORCE
|
||||
return target_raw_pose
|
||||
|
||||
'''
|
||||
Callback Function
|
||||
'''
|
||||
def gcs_cmd_callback(self, msg):
|
||||
self.gcs_cmd = msg.data
|
||||
|
||||
def local_pose_callback(self, msg, i):
|
||||
if i == uav_id:
|
||||
self.local_pose = copy.deepcopy(msg)
|
||||
if self.gcs_cmd != 'TAKEOFF':
|
||||
self.takeoff_target_pose = copy.deepcopy(self.local_pose)
|
||||
if self.gcs_cmd !='HOVER':
|
||||
self.hover_target_pose = copy.deepcopy(self.local_pose)
|
||||
self.global_pose[i] = copy.deepcopy(msg)
|
||||
self.global_pose[i].pose.position.x += uav_bias[i][0]
|
||||
self.global_pose[i].pose.position.y += uav_bias[i][1]
|
||||
self.global_pose[i].pose.position.z += uav_bias[i][2]
|
||||
#
|
||||
# def local_vel_callback(self):
|
||||
#
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
self.arm_state = msg.armed
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.imu = msg
|
||||
self.current_heading = self.q2yaw(self.imu.orientation)
|
||||
|
||||
def gps_callback(self, msg):
|
||||
self.gps = msg
|
||||
|
||||
'''
|
||||
return yaw from current IMU
|
||||
'''
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("Vehicle arming failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("Vehicle disarming failed!")
|
||||
return False
|
||||
|
||||
def flight_mode_set(self, mode):
|
||||
""" mode selectable
|
||||
MANUAL, ACRO, ALTCTL, POSCTL, OFFBOARD, STABILIZED, RATTITUDE
|
||||
AUTO.MISSION, AUTO.LOITER, AUTO.RTL, AUTO.LAND, AUTO.RTGS, AUTO.READY, AUTO.TAKEOFF
|
||||
"""
|
||||
if self.flightModeService(custom_mode=mode):
|
||||
return True
|
||||
else:
|
||||
print(self.namespace + mode + "Failed")
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.2 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
px4controller = Px4Controller()
|
||||
px4controller.working()
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
# -*- coding: UTF-8 -*-
|
||||
"""
|
||||
Fuzzy PID Controller
|
||||
|
||||
* To control all UAV in Gazebo(simulation environment) using MPI
|
||||
|
||||
Before running this code, you need to launch your simulation Environment and px4 via:
|
||||
$ from fuzzy_pid import FuzzyPID() # 3 UAVs in simulation
|
||||
|
||||
And then, you can run this code via:
|
||||
$ mpiexec -n 3 python formation_sim.py # 3 UAVs in simulation
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
#fuzzy table
|
||||
CIC_FUZZY_TABLE = [[0,1,2,3],
|
||||
[1,2,3,4],
|
||||
[3,4,4,5],
|
||||
[5,5,6,6]]
|
||||
# kp table
|
||||
CIC_KP_TABLE = [0.5,0.6,0.8,0.9,1.0,1.1,1.2]
|
||||
|
||||
class FuzzyPID():
|
||||
def __init__(self):
|
||||
|
||||
self.fuzzy_table = CIC_FUZZY_TABLE
|
||||
self.kp_table = CIC_KP_TABLE
|
||||
self.ex_range = 5.0
|
||||
self.ey_range = 1.0
|
||||
|
||||
def get_kp(self, ex, ey):
|
||||
vx = self.get_x_approximation(ex)
|
||||
vy = self.get_x_approximation(ey)
|
||||
vx1 = math.floor(vx)
|
||||
vy1 = math.floor(vy)
|
||||
vx2 = vx1 + 1
|
||||
vy2 = vy1 + 1
|
||||
if vx1 > 3:
|
||||
vx1 = 3
|
||||
if vy1 > 3:
|
||||
vy1 = 3
|
||||
if vx2 > 3:
|
||||
vx2 = 3
|
||||
if vy2 > 3:
|
||||
vy2 = 3
|
||||
X2Y = (self.fuzzy_table[vx1][vy2] - self.fuzzy_table[vx1][vy1]) * (vy - vy1) + self.fuzzy_table[vx1][vy1]
|
||||
X1Y = (self.fuzzy_table[vx2][vy2] - self.fuzzy_table[vx2][vy1]) * (vy - vy1) + self.fuzzy_table[vx2][vy1]
|
||||
Y2X = (self.fuzzy_table[vx2][vy1] - self.fuzzy_table[vx1][vy1]) * (vx - vx1) + self.fuzzy_table[vx1][vy1]
|
||||
Y1X = (self.fuzzy_table[vx2][vy2] - self.fuzzy_table[vx1][vy2]) * (vx - vx1) + self.fuzzy_table[vx1][vy2]
|
||||
kp_approximation = (X2Y + X1Y + Y2X + Y1X)/4.0
|
||||
kp1 = math.floor(kp_approximation)
|
||||
kp2 = kp1 + 1
|
||||
if kp1 > 6:
|
||||
kp1 = 6
|
||||
if kp2 > 6:
|
||||
kp2 = 6
|
||||
return (self.kp_table[kp2]-self.kp_table[kp1])*(kp_approximation-kp1)+self.kp_table[kp1]
|
||||
|
||||
def get_x_approximation(self, ex):
|
||||
if ex < 0:
|
||||
ex = -ex
|
||||
return 3*ex/self.ex_range #divide into 3 parts
|
||||
|
||||
def get_y_approximation(self, ey):
|
||||
if ey < 0:
|
||||
ey = -ey
|
||||
return 3*ey/self.ey_range #divide into 3 parts
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
fuzzy_pid = FuzzyPID()
|
||||
kp = fuzzy_pid.get_kp(10,50)
|
||||
print(kp)
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
import numpy as np
|
||||
import random
|
||||
import time
|
||||
import os
|
||||
|
||||
|
||||
class GA1():
|
||||
def __init__(self, vehicle_num, vehicles_speed, target_num, targets, time_lim):
|
||||
# vehicles_speed,targets in the type of narray
|
||||
self.vehicle_num = vehicle_num
|
||||
self.vehicles_speed = vehicles_speed
|
||||
self.target_num = target_num
|
||||
self.targets = targets
|
||||
self.time_lim = time_lim
|
||||
self.map = np.zeros(shape=(target_num+1, target_num+1), dtype=float)
|
||||
self.pop_size = 50
|
||||
self.p_cross = 0.6
|
||||
self.p_mutate = 0.005
|
||||
for i in range(target_num+1):
|
||||
self.map[i, i] = 0
|
||||
for j in range(i):
|
||||
self.map[j, i] = self.map[i, j] = np.linalg.norm(
|
||||
targets[i, :2]-targets[j, :2])
|
||||
self.pop = np.zeros(
|
||||
shape=(self.pop_size, vehicle_num-1+target_num-1), dtype=np.int32)
|
||||
self.ff = np.zeros(self.pop_size, dtype=float)
|
||||
#use vihecle_num-1 numbers to cut the targets into pieces
|
||||
#use target_num-1 numbers to code the queue of targets
|
||||
#encoder()
|
||||
for i in range(self.pop_size):
|
||||
for j in range(vehicle_num-1):
|
||||
self.pop[i, j] = random.randint(0, target_num)
|
||||
for j in range(target_num-1):
|
||||
self.pop[i, vehicle_num+j-1] \
|
||||
= random.randint(0, target_num-j-1)
|
||||
self.ff[i] = self.fitness(self.pop[i, :])
|
||||
self.tmp_pop = np.array([])
|
||||
self.tmp_ff = np.array([])
|
||||
self.tmp_size = 0
|
||||
|
||||
def fitness(self, gene):
|
||||
#to record the break point
|
||||
ins = np.zeros(self.target_num+1, dtype=np.int32)
|
||||
#to decode the queue of targets
|
||||
seq = np.zeros(self.target_num, dtype=np.int32)
|
||||
#ins[self.target_num] = 1
|
||||
for i in range(self.vehicle_num-1):
|
||||
ins[gene[i]] += 1
|
||||
rest = np.array(range(1, self.target_num+1))
|
||||
for i in range(self.target_num-1):
|
||||
seq[i] = rest[gene[i+self.vehicle_num-1]]
|
||||
rest = np.delete(rest, gene[i+self.vehicle_num-1])
|
||||
seq[self.target_num-1] = rest[0]
|
||||
vehicle_num_i = 0 # index of vehicle
|
||||
pre = 0 # index of the last target
|
||||
post = 0 # index of instant point
|
||||
cost = 0
|
||||
reward = 0
|
||||
while vehicle_num_i < self.vehicle_num:
|
||||
if ins[post] > 0:
|
||||
vehicle_num_i += 1
|
||||
ins[post] -= 1
|
||||
pre = 0
|
||||
cost = 0
|
||||
else:
|
||||
cost += self.targets[pre, 3]
|
||||
time_cost = self.map[pre, seq[post]]/self.vehicles_speed[vehicle_num_i]
|
||||
cost += time_cost
|
||||
if cost < self.time_lim:
|
||||
reward += self.targets[seq[post], 2]
|
||||
pre = seq[post]
|
||||
#print("seq[post]",pre)
|
||||
post += 1
|
||||
if post > self.target_num-1:
|
||||
break
|
||||
return reward
|
||||
|
||||
def selection(self):
|
||||
roll = np.zeros(self.tmp_size, dtype=float)
|
||||
roll[0] = self.tmp_ff[0]
|
||||
for i in range(1, self.tmp_size):
|
||||
roll[i] = roll[i-1]+self.tmp_ff[i]
|
||||
for i in range(self.pop_size):
|
||||
xx = random.uniform(0, roll[self.tmp_size-1])
|
||||
j = 0
|
||||
while xx > roll[j]:
|
||||
j += 1
|
||||
self.pop[i, :] = self.tmp_pop[j, :]
|
||||
self.ff[i] = self.tmp_ff[j]
|
||||
|
||||
def crossover(self):
|
||||
new_pop = []
|
||||
new_ff = []
|
||||
new_size = 0
|
||||
for i in range(0, self.pop_size, 2):
|
||||
if random.random() < self.p_cross:
|
||||
x1 = random.randint(0, self.vehicle_num-2)
|
||||
x2 = random.randint(0, self.target_num-2)+self.vehicle_num
|
||||
g1 = self.pop[i, :]
|
||||
g2 = self.pop[i+1, :]
|
||||
g1[x1:x2] = self.pop[i+1, x1:x2]
|
||||
g2[x1:x2] = self.pop[i, x1:x2]
|
||||
new_pop.append(g1)
|
||||
new_pop.append(g2)
|
||||
new_ff.append(self.fitness(g1))
|
||||
new_ff.append(self.fitness(g2))
|
||||
new_size += 2
|
||||
self.tmp_size = self.pop_size+new_size
|
||||
self.tmp_pop = np.zeros(
|
||||
shape=(self.tmp_size, self.vehicle_num-1+self.target_num-1), dtype=np.int32)
|
||||
self.tmp_pop[0:self.pop_size, :] = self.pop
|
||||
self.tmp_pop[self.pop_size:self.tmp_size, :] = np.array(new_pop)
|
||||
self.tmp_ff = np.zeros(self.tmp_size, dtype=float)
|
||||
self.tmp_ff[0:self.pop_size] = self.ff
|
||||
self.tmp_ff[self.pop_size:self.tmp_size] = np.array(new_ff)
|
||||
|
||||
def mutation(self):
|
||||
for i in range(self.tmp_size):
|
||||
flag = False
|
||||
for j in range(self.vehicle_num-1):
|
||||
if random.random() < self.p_mutate:
|
||||
self.tmp_pop[i, j] = random.randint(0, self.target_num)
|
||||
flag = True
|
||||
for j in range(self.target_num-1):
|
||||
if random.random() < self.p_mutate:
|
||||
self.tmp_pop[i, self.vehicle_num+j-1
|
||||
] = random.randint(0, self.target_num-j-1)
|
||||
flag = True
|
||||
if flag:
|
||||
self.tmp_ff[i] = self.fitness(self.tmp_pop[i, :])
|
||||
|
||||
|
||||
|
||||
def run(self):
|
||||
#print("GA start, pid: %s" % os.getpid())
|
||||
start_time = time.time()
|
||||
gene = np.zeros(
|
||||
shape=(1, self.vehicle_num+self.target_num-1), dtype=np.int32)
|
||||
cut = 0
|
||||
count = 0
|
||||
|
||||
while count < 300:
|
||||
self.crossover()
|
||||
self.mutation()
|
||||
self.selection()
|
||||
new_cut = self.tmp_ff.max()
|
||||
if cut < new_cut:
|
||||
cut = new_cut
|
||||
count = 0
|
||||
gene = self.tmp_pop[np.argmax(self.tmp_ff)]
|
||||
else:
|
||||
count += 1
|
||||
|
||||
ins = np.zeros(self.target_num + 1, dtype=np.int32)
|
||||
seq = np.zeros(self.target_num, dtype=np.int32)
|
||||
#ins[self.target_num] = 1
|
||||
for i in range(self.vehicle_num-1):
|
||||
ins[gene[i]] += 1
|
||||
rest = np.array(range(1, self.target_num + 1))
|
||||
for i in range(self.target_num - 1):
|
||||
seq[i] = rest[gene[i + self.vehicle_num-1]]
|
||||
rest = np.delete(rest, gene[i + self.vehicle_num-1])
|
||||
seq[self.target_num - 1] = rest[0]
|
||||
#print('sequence : ', seq)
|
||||
#print('ins[]: ', ins)
|
||||
task_assignment = [[] for i in range(self.vehicle_num)]
|
||||
i = 0 # index of vehicle
|
||||
pre = 0 # index of last target
|
||||
post = 0 # index of instant point
|
||||
t = 0
|
||||
reward = 0
|
||||
while i < self.vehicle_num:
|
||||
if ins[post] > 0:
|
||||
i += 1
|
||||
ins[post] -= 1
|
||||
pre = 0
|
||||
t = 0
|
||||
else:
|
||||
t += self.targets[pre, 3]
|
||||
past = self.map[pre, seq[post]] / self.vehicles_speed[i]
|
||||
t += past
|
||||
task_assignment[i].append(seq[post])
|
||||
if t < self.time_lim:
|
||||
reward += self.targets[seq[post], 2]
|
||||
pre = seq[post]
|
||||
post += 1
|
||||
if post > self.target_num - 1:
|
||||
break
|
||||
self.reward = reward
|
||||
print("GA result1:", task_assignment)
|
||||
end_time = time.time()
|
||||
print("GA time:", end_time - start_time)
|
||||
print("total reward1:", reward)
|
||||
return task_assignment, end_time - start_time
|
||||
|
||||
|
|
@ -0,0 +1,219 @@
|
|||
import numpy as np
|
||||
import random
|
||||
import time
|
||||
import os
|
||||
|
||||
|
||||
class GA2():
|
||||
def __init__(self, vehicle_num, vehicles_speed, target_num, targets, time_lim):
|
||||
# vehicles_speed,targets in the type of narray
|
||||
self.vehicle_num = vehicle_num
|
||||
self.vehicles_speed = vehicles_speed
|
||||
self.target_num = target_num
|
||||
self.targets = targets
|
||||
self.time_lim = time_lim
|
||||
self.map = np.zeros(shape=(target_num+1, target_num+1), dtype=float)
|
||||
self.pop_size = 50
|
||||
self.p_cross = 0
|
||||
self.p_cross1 = 0.9
|
||||
self.p_cross2 = 0.6
|
||||
self.p_mutate = 0
|
||||
self.p_mutate1 = 0.1
|
||||
self.p_mutate2 = 0.005
|
||||
self.average_ff = 0
|
||||
self.max_ff = 1
|
||||
for i in range(target_num+1):
|
||||
self.map[i, i] = 0
|
||||
for j in range(i):
|
||||
self.map[j, i] = self.map[i, j] = np.linalg.norm(
|
||||
targets[i, :2]-targets[j, :2])
|
||||
self.pop = np.zeros(
|
||||
shape=(self.pop_size, vehicle_num-1+target_num-1), dtype=np.int32)
|
||||
self.ff = np.zeros(self.pop_size, dtype=float)
|
||||
#use vihecle_num-1 numbers to cut the targets into pieces
|
||||
#use target_num-1 numbers to code the queue of targets
|
||||
#encoder()
|
||||
for i in range(self.pop_size):
|
||||
for j in range(vehicle_num-1):
|
||||
self.pop[i, j] = random.randint(0, target_num)
|
||||
for j in range(target_num-1):
|
||||
self.pop[i, vehicle_num+j-1] \
|
||||
= random.randint(0, target_num-j-1)
|
||||
self.ff[i] = self.fitness(self.pop[i, :])
|
||||
self.tmp_pop = np.array([])
|
||||
self.tmp_ff = np.array([])
|
||||
self.tmp_size = 0
|
||||
|
||||
def fitness(self, gene):
|
||||
#to record the break point
|
||||
ins = np.zeros(self.target_num+1, dtype=np.int32)
|
||||
#to decode the queue of targets
|
||||
seq = np.zeros(self.target_num, dtype=np.int32)
|
||||
#ins[self.target_num] = 1
|
||||
for i in range(self.vehicle_num-1):
|
||||
ins[gene[i]] += 1
|
||||
rest = np.array(range(1, self.target_num+1))
|
||||
for i in range(self.target_num-1):
|
||||
seq[i] = rest[gene[i+self.vehicle_num-1]]
|
||||
rest = np.delete(rest, gene[i+self.vehicle_num-1])
|
||||
seq[self.target_num-1] = rest[0]
|
||||
vehicle_num_i = 0 # index of vehicle
|
||||
pre = 0 # index of the last target
|
||||
post = 0 # index of instant point
|
||||
cost = 0
|
||||
reward = 0
|
||||
while vehicle_num_i < self.vehicle_num:
|
||||
if ins[post] > 0:
|
||||
vehicle_num_i += 1
|
||||
ins[post] -= 1
|
||||
pre = 0
|
||||
cost = 0
|
||||
else:
|
||||
cost += self.targets[pre, 3]
|
||||
time_cost = self.map[pre, seq[post]]/self.vehicles_speed[vehicle_num_i]
|
||||
cost += time_cost
|
||||
if cost < self.time_lim:
|
||||
reward += self.targets[seq[post], 2]
|
||||
pre = seq[post]
|
||||
#print("seq[post]",pre)
|
||||
post += 1
|
||||
if post > self.target_num-1:
|
||||
break
|
||||
return reward
|
||||
|
||||
def selection(self):
|
||||
roll = np.zeros(self.tmp_size, dtype=float)
|
||||
roll[0] = self.tmp_ff[0]
|
||||
for i in range(1, self.tmp_size):
|
||||
roll[i] = roll[i-1]+self.tmp_ff[i]
|
||||
for i in range(self.pop_size):
|
||||
xx = random.uniform(0, roll[self.tmp_size-1])
|
||||
j = 0
|
||||
while xx > roll[j]:
|
||||
j += 1
|
||||
self.pop[i, :] = self.tmp_pop[j, :]
|
||||
self.ff[i] = self.tmp_ff[j]
|
||||
# adaptive ag
|
||||
self.average_ff = np.mean(self.ff)
|
||||
self.max_ff = max(self.ff)
|
||||
if self.ff[i] >= self.average_ff:
|
||||
self.p_mutate = self.p_mutate1 - \
|
||||
(self.p_mutate1 - self.p_mutate2) * (self.max_ff - self.ff[i]) / (
|
||||
self.max_ff - self.average_ff)
|
||||
else:
|
||||
self.p_mutate = self.p_mutate1
|
||||
|
||||
def crossover(self):
|
||||
new_pop = []
|
||||
new_ff = []
|
||||
new_size = 0
|
||||
ff_bigger = 0
|
||||
for i in range(0, self.pop_size, 2):
|
||||
#adaptive ag
|
||||
ff_bigger = max(self.ff[i], self.ff[i+1])
|
||||
if ff_bigger >= self.average_ff:
|
||||
self.p_cross = self.p_cross1 -\
|
||||
(self.p_cross1-self.p_cross2)/(self.max_ff-self.average_ff)
|
||||
else:
|
||||
self.p_cross = self.p_cross1
|
||||
if random.random() < self.p_cross:
|
||||
x1 = random.randint(0, self.vehicle_num-2)
|
||||
x2 = random.randint(0, self.target_num-2)+self.vehicle_num
|
||||
g1 = self.pop[i, :]
|
||||
g2 = self.pop[i+1, :]
|
||||
g1[x1:x2] = self.pop[i+1, x1:x2]
|
||||
g2[x1:x2] = self.pop[i, x1:x2]
|
||||
new_pop.append(g1)
|
||||
new_pop.append(g2)
|
||||
new_ff.append(self.fitness(g1))
|
||||
new_ff.append(self.fitness(g2))
|
||||
new_size += 2
|
||||
self.tmp_size = self.pop_size+new_size
|
||||
self.tmp_pop = np.zeros(
|
||||
shape=(self.tmp_size, self.vehicle_num-1+self.target_num-1), dtype=np.int32)
|
||||
self.tmp_pop[0:self.pop_size, :] = self.pop
|
||||
self.tmp_pop[self.pop_size:self.tmp_size, :] = np.array(new_pop)
|
||||
self.tmp_ff = np.zeros(self.tmp_size, dtype=float)
|
||||
self.tmp_ff[0:self.pop_size] = self.ff
|
||||
self.tmp_ff[self.pop_size:self.tmp_size] = np.array(new_ff)
|
||||
|
||||
|
||||
def mutation(self):
|
||||
for i in range(self.tmp_size):
|
||||
flag = False
|
||||
for j in range(self.vehicle_num-1):
|
||||
if random.random() < self.p_mutate:
|
||||
self.tmp_pop[i, j] = random.randint(0, self.target_num)
|
||||
flag = True
|
||||
for j in range(self.target_num-1):
|
||||
if random.random() < self.p_mutate:
|
||||
self.tmp_pop[i, self.vehicle_num+j-1
|
||||
] = random.randint(0, self.target_num-j-1)
|
||||
flag = True
|
||||
if flag:
|
||||
self.tmp_ff[i] = self.fitness(self.tmp_pop[i, :])
|
||||
|
||||
|
||||
|
||||
def run(self):
|
||||
#print("GA start, pid: %s" % os.getpid())
|
||||
start_time = time.time()
|
||||
gene = np.zeros(
|
||||
shape=(1, self.vehicle_num+self.target_num-1), dtype=np.int32)
|
||||
cut = 0
|
||||
count = 0
|
||||
|
||||
while count < 300:
|
||||
self.crossover()
|
||||
self.mutation()
|
||||
self.selection()
|
||||
new_cut = self.tmp_ff.max()
|
||||
if cut < new_cut:
|
||||
cut = new_cut
|
||||
count = 0
|
||||
gene = self.tmp_pop[np.argmax(self.tmp_ff)]
|
||||
else:
|
||||
count += 1
|
||||
|
||||
ins = np.zeros(self.target_num + 1, dtype=np.int32)
|
||||
seq = np.zeros(self.target_num, dtype=np.int32)
|
||||
#ins[self.target_num] = 1
|
||||
for i in range(self.vehicle_num-1):
|
||||
ins[gene[i]] += 1
|
||||
rest = np.array(range(1, self.target_num + 1))
|
||||
for i in range(self.target_num - 1):
|
||||
seq[i] = rest[gene[i + self.vehicle_num-1]]
|
||||
rest = np.delete(rest, gene[i + self.vehicle_num-1])
|
||||
seq[self.target_num - 1] = rest[0]
|
||||
#print('sequence : ', seq)
|
||||
#print('ins[]: ', ins)
|
||||
task_assignment = [[] for i in range(self.vehicle_num)]
|
||||
i = 0 # index of vehicle
|
||||
pre = 0 # index of last target
|
||||
post = 0 # index of ins/seq
|
||||
t = 0
|
||||
reward = 0
|
||||
while i < self.vehicle_num:
|
||||
if ins[post] > 0:
|
||||
i += 1
|
||||
ins[post] -= 1
|
||||
pre = 0
|
||||
t = 0
|
||||
else:
|
||||
t += self.targets[pre, 3]
|
||||
past = self.map[pre, seq[post]] / self.vehicles_speed[i]
|
||||
t += past
|
||||
task_assignment[i].append(seq[post])
|
||||
if t < self.time_lim:
|
||||
reward += self.targets[seq[post], 2]
|
||||
pre = seq[post]
|
||||
post += 1
|
||||
if post > self.target_num - 1:
|
||||
break
|
||||
self.reward = reward
|
||||
# print("GA result2:", task_assignment)
|
||||
end_time = time.time()
|
||||
# print("GA time:", end_time - start_time)
|
||||
# print("total reward2:", reward)
|
||||
return task_assignment, end_time - start_time
|
||||
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
from enviroment import Env
|
||||
from ga1 import GA1
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from ga2 import GA2
|
||||
if __name__ == '__main__':
|
||||
map_size_x = 80
|
||||
map_size_y = 80
|
||||
map_size = [map_size_x, map_size_y]
|
||||
vehicle_num = 3
|
||||
target_num = 10
|
||||
for i in range(5):
|
||||
env = Env(vehicle_num, target_num, map_size, True)
|
||||
for j in range(10):
|
||||
ga1 = GA1(vehicle_num, env.vehicles_speed, target_num, env.targets, env.time_limit)
|
||||
ga2 = GA2(vehicle_num, env.vehicles_speed, target_num, env.targets, env.time_limit)
|
||||
ga1_result = ga1.run()
|
||||
#print("result:", ga1_result[0])
|
||||
ga2_result = ga2.run()
|
||||
|
||||
env.run(ga1_result[0], 'GA', i, j)
|
||||
env.run(ga2_result[0], 'adaptGA', i, j)
|
||||
|
||||
|
||||
|
||||
|
||||
#f_cost = np.zeros(21, dtype=np.int32)
|
||||
#for i in range(10, 210, 10):
|
||||
# ga1.run(i)
|
||||
# f_cost[i//10]=ga1.reward
|
||||
#plt.plot(np.linspace(10, 200, 21),f_cost,'s-',color = 'r',label = 'iteration_influence')
|
||||
#plt.savefig("iteration_influence")
|
||||
#plt.show()
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
1 0
|
||||
2 0
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
0 0 0
|
||||
-1 0 0
|
||||
1 0 0
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
0 0
|
||||
1 0
|
||||
1 1
|
||||
2 0
|
||||
2 2
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
0 0 0
|
||||
-1 1 0
|
||||
1 1 0
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,280 @@
|
|||
# encoding: utf-8
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LINEAR = 20
|
||||
MAX_ANG_VEL = 3
|
||||
LINEAR_STEP_SIZE = 0.1
|
||||
ANG_VEL_STEP_SIZE = 0.1
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False #初始 控制所有无人机
|
||||
|
||||
msg2all = """
|
||||
Control Your XTDrone!
|
||||
To all drones (press g to control the leader)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity
|
||||
a/d : increase/decrease leftward velocity
|
||||
i/, : increase/decrease upward velocity
|
||||
j/l : increase/decrease orientation
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff/land
|
||||
b : offboard
|
||||
s/k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
msg2leader = """
|
||||
Control Your XTDrone!
|
||||
To the leader (press g to control all drones)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward velocity
|
||||
a/d : increase/decrease leftward velocity
|
||||
i/, : increase/decrease upward velocity
|
||||
j/l : increase/decrease orientation
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s/k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control all drones
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey(): # 获得键盘按键
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg(): # 打印提示信息
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
multirotor_type = sys.argv[1]
|
||||
multirotor_num = int(sys.argv[2])
|
||||
control_type = sys.argv[3] #vel 速度控制
|
||||
'''
|
||||
if multirotor_num == 18: #多无人机编队
|
||||
formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond'] # 0-9的参数
|
||||
elif multirotor_num == 9:
|
||||
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
elif multirotor_num == 6:
|
||||
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
''' # 0 1 2 3
|
||||
if multirotor_num == 21:
|
||||
formation_configs = ['waiting', 'cube21', 'circle21', 'pyramid21', 'diamond21']
|
||||
elif multirotor_num == 34:
|
||||
formation_configs = ['waiting', 'CUBE', 'HEART', '520', 'NUDT', '八一']
|
||||
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
rospy.init_node('multirotor_keyboard_multi_control')
|
||||
|
||||
if control_type == 'vel': # 速度控制
|
||||
multi_cmd_vel_flu_pub = [None]*multirotor_num
|
||||
multi_cmd_pub = [None]*multirotor_num
|
||||
for i in range(multirotor_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
|
||||
|
||||
else: # 加速度控制
|
||||
multi_cmd_accel_flu_pub = [None]*multirotor_num
|
||||
multi_cmd_pub = [None]*multirotor_num
|
||||
for i in range(multirotor_num):
|
||||
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
|
||||
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
upward = 0.0
|
||||
angular = 0.0
|
||||
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
forward = forward + LINEAR_STEP_SIZE
|
||||
print_msg()
|
||||
if control_type == 'vel':
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'x' :
|
||||
forward = forward - LINEAR_STEP_SIZE
|
||||
print_msg()
|
||||
if control_type == 'vel':
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'a' :
|
||||
|
||||
leftward = leftward + LINEAR_STEP_SIZE
|
||||
print_msg()
|
||||
if control_type == 'vel':
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'd' :
|
||||
leftward = leftward - LINEAR_STEP_SIZE
|
||||
print_msg()
|
||||
if control_type == 'vel':
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'i' :
|
||||
upward = upward + LINEAR_STEP_SIZE
|
||||
print_msg()
|
||||
if control_type == 'vel':
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == ',' :
|
||||
upward = upward - LINEAR_STEP_SIZE
|
||||
print_msg()
|
||||
if control_type == 'vel':
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
else:
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'j':
|
||||
angular = angular + ANG_VEL_STEP_SIZE
|
||||
print_msg()
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'l':
|
||||
angular = angular - ANG_VEL_STEP_SIZE
|
||||
print_msg()
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
|
||||
elif key == 'r': # 自动返回起飞点
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't': #飞机解锁
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y': # 飞机上锁
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print_msg()
|
||||
#print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n': # 自动着陆
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('Landing')
|
||||
elif key == 'g': # 开启或结束编队控制
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key in ['k', 's']: # 悬停，全部参数归零
|
||||
cmd_vel_mask = False
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
upward = 0.0
|
||||
angular = 0.0
|
||||
cmd = 'HOVER'
|
||||
print_msg()
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
print('Hover')
|
||||
else:
|
||||
for i in range(10): # 按键数字
|
||||
if key == str(i):
|
||||
cmd = formation_configs[i] # 编队的队形
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True # 有掩模，其他按键无效
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
if forward > MAX_LINEAR:
|
||||
forward = MAX_LINEAR
|
||||
elif forward < -MAX_LINEAR:
|
||||
forward = -MAX_LINEAR
|
||||
if leftward > MAX_LINEAR:
|
||||
leftward = MAX_LINEAR
|
||||
elif leftward < -MAX_LINEAR:
|
||||
leftward = -MAX_LINEAR
|
||||
if upward > MAX_LINEAR:
|
||||
upward = MAX_LINEAR
|
||||
elif upward < -MAX_LINEAR:
|
||||
upward = -MAX_LINEAR
|
||||
if angular > MAX_ANG_VEL:
|
||||
angular = MAX_ANG_VEL
|
||||
elif angular < -MAX_ANG_VEL:
|
||||
angular = - MAX_ANG_VEL
|
||||
|
||||
twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular
|
||||
|
||||
for i in range(multirotor_num): # 无人机数量
|
||||
if ctrl_leader:
|
||||
if control_type == 'vel':
|
||||
leader_cmd_vel_flu_pub.publish(twist)
|
||||
else:
|
||||
leader_cmd_aceel_flu_pub.publish(twist) #无定义
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask: #只有在输入0-9时，mask为True，其余情况为F 。这里是非数字时
|
||||
if control_type == 'vel':
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
else:
|
||||
multi_cmd_accel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3, PoseStamped
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
|
||||
vehicle_type = sys.argv[1]
|
||||
vehicle_num = int(sys.argv[2])
|
||||
pose = [None]*vehicle_num
|
||||
pose_sub = [None]*vehicle_num
|
||||
avoid_vel_pub = [None]*vehicle_num
|
||||
avoid_kp = 0.5
|
||||
avoid_radius = 1.5
|
||||
aid_vec1 = [1, 0, 0]
|
||||
aid_vec2 = [0, 1, 0]
|
||||
vehicles_avoid_vel = [Vector3()] * vehicle_num
|
||||
|
||||
def pose_callback(msg, id):
|
||||
pose[id] = msg
|
||||
|
||||
rospy.init_node('avoid')
|
||||
for i in range(vehicle_num):
|
||||
pose_sub[i] = rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/local_position/pose',PoseStamped,pose_callback,i)
|
||||
avoid_vel_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_vel", Vector3,queue_size=10)
|
||||
|
||||
time.sleep(1)
|
||||
rate = rospy.Rate(50)
|
||||
while not rospy.is_shutdown():
|
||||
for i in range(vehicle_num):
|
||||
position1 = numpy.array([pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z])
|
||||
for j in range(1, vehicle_num-i):
|
||||
position2 = numpy.array([pose[i+j].pose.position.x, pose[i+j].pose.position.y, pose[i+j].pose.position.z])
|
||||
dir_vec = position1-position2
|
||||
if numpy.linalg.norm(dir_vec) < avoid_radius:
|
||||
cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1))
|
||||
cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2))
|
||||
if abs(cos1) < abs(cos2):
|
||||
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
|
||||
else:
|
||||
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
|
||||
if dir_vec[2] * avoid_vel[2] > 0:
|
||||
vehicles_avoid_vel[i] = Vector3(vehicles_avoid_vel[i].x+avoid_vel[0],vehicles_avoid_vel[i].y+avoid_vel[1],vehicles_avoid_vel[i].z+avoid_vel[2])
|
||||
vehicles_avoid_vel[i+j] = Vector3(vehicles_avoid_vel[i+j].x-avoid_vel[0],vehicles_avoid_vel[i+j].y-avoid_vel[1],vehicles_avoid_vel[i+j].z-avoid_vel[2])
|
||||
else:
|
||||
vehicles_avoid_vel[i] = Vector3(vehicles_avoid_vel[i].x-avoid_vel[0],vehicles_avoid_vel[i].y-avoid_vel[1],vehicles_avoid_vel[i].z-avoid_vel[2])
|
||||
vehicles_avoid_vel[i+j] = Vector3(vehicles_avoid_vel[i+j].x+avoid_vel[0],vehicles_avoid_vel[i+j].y+avoid_vel[1],vehicles_avoid_vel[i+j].z+avoid_vel[2])
|
||||
for i in range(vehicle_num):
|
||||
avoid_vel_pub[i].publish(vehicles_avoid_vel[i])
|
||||
vehicles_avoid_vel = [Vector3()] * vehicle_num
|
||||
rate.sleep()
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,260 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
### This code is about the distributed formation control of consensus protocol with a certain
|
||||
### Laplacian matrix and the formation transformation based on a task allocation algorithm——
|
||||
### KM for the shorest distances of all the UAVs to achieve the new pattern.
|
||||
### For more information of these two algorithms, please see the latest paper on https://arxiv.org/abs/2005.01125
|
||||
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3, PoseStamped, TwistStamped
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
|
||||
# formation patterns
|
||||
#if sys.argv[3] == '6':
|
||||
# from formation_dict import formation_dict_6 as formation_dict
|
||||
#elif sys.argv[3] == '9':
|
||||
# from formation_dict import formation_dict_9 as formation_dict
|
||||
#elif sys.argv[3] == '18':
|
||||
# from formation_dict import formation_dict_18 as formation_dict
|
||||
if sys.argv[3] == '21':
|
||||
from my_formation_dict import formation_dict_my as formation_dict
|
||||
elif sys.argv[3] == '34':
|
||||
from my_formation_dict import formation_dict_my as formation_dict
|
||||
|
||||
import numpy
|
||||
import Queue
|
||||
|
||||
|
||||
class Follower:
|
||||
|
||||
def __init__(self, uav_type, uav_id, uav_num):
|
||||
self.hover = "HOVER"
|
||||
self.offboard = "OFFBOARD"
|
||||
self.uav_type = uav_type
|
||||
self.id = uav_id
|
||||
self.uav_num = uav_num
|
||||
self.f = 100 # control/communication rate
|
||||
self.local_pose = PoseStamped()
|
||||
self.local_pose_queue = Queue.Queue(self.f/10)
|
||||
for i in range(self.f/10):
|
||||
self.local_pose_queue.put(PoseStamped())
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.avoid_vel = Vector3()
|
||||
self.following_switch = False # determine whether the formation pattern is required to be changed
|
||||
self.arrive_print = True # determine whether the target position has been reached
|
||||
self.following_ids = [] # followers of this uav
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0 # the number of followers of this uav
|
||||
self.Kp = 1
|
||||
self.velxy_max = 1 #0.8
|
||||
self.velz_max = 1
|
||||
self.following_local_pose = [PoseStamped() for i in range(self.uav_num)] # local position of other uavs, and only the position of followers of this uav is not zero
|
||||
self.following_local_pose_sub = [None]*self.uav_num
|
||||
self.arrive_count = 0
|
||||
###############起飞位置偏置
|
||||
self.bias = [ [3, 3, 0],[6, 3, 0],[9, 3, 0],[12, 3, 0],[15, 3, 0],[18, 3, 0], [0, 6, 0],[3, 6, 0],[6, 6, 0],[9, 6, 0],[12, 6, 0],[15, 6, 0],[18, 6, 0], [0, 9, 0],[3, 9, 0],[6, 9, 0],[9, 9, 0],[12, 9, 0],[15, 9, 0],[18, 9, 0] ]
|
||||
self.local_pose_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
|
||||
self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback)
|
||||
self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.info_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/info', String, queue_size=10)
|
||||
self.cmd_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd',String,queue_size=10)
|
||||
self.first_formation = True
|
||||
self.orig_formation = None
|
||||
self.new_formation = None
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
self.local_pose.pose.position.x += self.bias[self.id-1][0]###########起飞位置偏置
|
||||
self.local_pose.pose.position.y += self.bias[self.id-1][1]
|
||||
|
||||
pose_comparison = self.local_pose_queue.get()
|
||||
self.local_pose_queue.put(self.local_pose)
|
||||
comparison = (self.local_pose.pose.position.x - pose_comparison.pose.position.x)**2+(self.local_pose.pose.position.y - pose_comparison.pose.position.y)**2+(self.local_pose.pose.position.z - pose_comparison.pose.position.z)**2
|
||||
if comparison < float(self.velxy_max**2+self.velxy_max**2+self.velz_max**2)/1e5: # if the target position is reached, arrive_count ++1
|
||||
self.arrive_count += 1
|
||||
else:
|
||||
self.arrive_count = 0
|
||||
|
||||
def following_local_pose_callback(self, msg, id):
|
||||
self.following_local_pose[id] = msg
|
||||
self.following_local_pose[id].pose.position.x += self.bias[id][0]#############起飞位置偏置
|
||||
self.following_local_pose[id].pose.position.y += self.bias[id][1]
|
||||
|
||||
# the order of changing the formation pattern
|
||||
def formation_switch_callback(self, msg):
|
||||
if not self.formation_config == msg.data:
|
||||
self.following_switch = True
|
||||
self.formation_config = msg.data
|
||||
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
rate = rospy.Rate(self.f)
|
||||
while not rospy.is_shutdown():
|
||||
if self.arrive_count > 2000 and self.arrive_print:
|
||||
print("Follower"+str(self.id-1)+":Arrived")
|
||||
self.arrive_print = False
|
||||
if self.following_switch:
|
||||
self.following_switch = False
|
||||
self.arrive_print = True
|
||||
self.arrive_count = 0
|
||||
for i in range(self.f/10):
|
||||
self.cmd_pub.publish(self.offboard)
|
||||
self.info_pub.publish("Received")
|
||||
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
|
||||
# The Laplacian matrix is invarible in this code, and you can change it if necessary.
|
||||
if self.formation_config=='waiting':
|
||||
self.L_matrix = self.get_L_central_matrix()
|
||||
else:
|
||||
# Change from the original pattern to the first pattern without KM.
|
||||
if self.first_formation:
|
||||
self.first_formation=False
|
||||
self.orig_formation=formation_dict[self.formation_config]
|
||||
self.L_matrix = self.get_L_central_matrix()
|
||||
else:
|
||||
self.adj_matrix = self.build_graph(self.orig_formation,formation_dict[self.formation_config])
|
||||
# These variables are determined for KM algorithm, see examples of KM algorithm on Github.
|
||||
self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left
|
||||
self.label_right = numpy.array([0]*(self.uav_num-1)) # init label for the right set
|
||||
self.match_right = numpy.array([-1] *(self.uav_num-1))
|
||||
self.visit_left = numpy.array([0]*(self.uav_num-1))
|
||||
self.visit_right = numpy.array([0]*(self.uav_num-1))
|
||||
self.slack_right = numpy.array([100]*(self.uav_num-1))
|
||||
|
||||
self.change_id = self.KM()
|
||||
# Get a new formation pattern of UAVs based on KM.
|
||||
self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config])
|
||||
self.L_matrix = self.get_L_central_matrix()
|
||||
self.orig_formation=self.new_formation
|
||||
if self.id == 3:
|
||||
print(self.L_matrix)
|
||||
# Get the followers of this uav based on the Laplacian matrix, and update the position of the followers.
|
||||
self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1)
|
||||
self.following_count = 0
|
||||
for i in range(self.uav_num):
|
||||
if not self.following_local_pose_sub[i] == None:
|
||||
self.following_local_pose_sub[i].unregister()
|
||||
for following_id in self.following_ids:
|
||||
self.following_local_pose_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
|
||||
self.following_count += 1
|
||||
|
||||
self.cmd_vel_enu.linear = Vector3(0, 0, 0)
|
||||
# Code of the consensus protocol, see details on the paper.
|
||||
for following_id in self.following_ids:
|
||||
#print(self.id,"following: ", self.following_ids)
|
||||
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + self.new_formation[0, self.id-1]
|
||||
self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + self.new_formation[1, self.id-1]
|
||||
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + self.new_formation[2, self.id-1]
|
||||
if not following_id[0] == 0:
|
||||
self.cmd_vel_enu.linear.x -= self.new_formation[0, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.y -= self.new_formation[1, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.z -= self.new_formation[2, following_id[0]-1]
|
||||
|
||||
self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x
|
||||
self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y
|
||||
self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z
|
||||
|
||||
if self.cmd_vel_enu.linear.x > self.velxy_max: #越界限制
|
||||
self.cmd_vel_enu.linear.x = self.velxy_max
|
||||
elif self.cmd_vel_enu.linear.x < - self.velxy_max:
|
||||
self.cmd_vel_enu.linear.x = - self.velxy_max
|
||||
if self.cmd_vel_enu.linear.y > self.velxy_max:
|
||||
self.cmd_vel_enu.linear.y = self.velxy_max
|
||||
elif self.cmd_vel_enu.linear.y < - self.velxy_max:
|
||||
self.cmd_vel_enu.linear.y = - self.velxy_max
|
||||
if self.cmd_vel_enu.linear.z > self.velz_max:
|
||||
self.cmd_vel_enu.linear.z = self.velz_max
|
||||
elif self.cmd_vel_enu.linear.z < - self.velz_max:
|
||||
self.cmd_vel_enu.linear.z = - self.velz_max
|
||||
|
||||
if not self.formation_config == 'waiting':
|
||||
self.vel_enu_pub.publish(self.cmd_vel_enu)
|
||||
if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2: #到达目标位置
|
||||
self.arrive_count += 1
|
||||
else:
|
||||
self.arrive_count = 0
|
||||
rate.sleep()
|
||||
|
||||
# 'build_graph', 'find_path' and 'KM' functions are all determined for KM algorithm.
|
||||
# A graph of UAVs is established based on distances between them in 'build_graph' function.
|
||||
def build_graph(self,orig_formation,change_formation):
|
||||
distance=[[0 for i in range(self.uav_num-1)]for j in range(self.uav_num-1)] #二维0矩阵，行列都为跟随者个数
|
||||
for i in range(self.uav_num-1):
|
||||
for j in range(self.uav_num-1):
|
||||
distance[i][j]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,j])
|
||||
distance[i][j]=int(50-distance[i][j])
|
||||
return distance
|
||||
|
||||
# Determine whether a path has been found.
|
||||
def find_path(self,i):
|
||||
self.visit_left[i] = True
|
||||
for j, match_weight in enumerate(self.adj_matrix[i],start=0):
|
||||
if self.visit_right[j]:
|
||||
continue
|
||||
gap = self.label_left[i] + self.label_right[j] - match_weight
|
||||
if gap == 0 :
|
||||
self.visit_right[j] = True
|
||||
if self.match_right[j]==-1 or self.find_path(self.match_right[j]):
|
||||
self.match_right[j] = i
|
||||
return True
|
||||
else:
|
||||
self.slack_right[j]=min(gap,self.slack_right[j])
|
||||
return False
|
||||
|
||||
# Main body of KM algorithm.
|
||||
def KM(self):
|
||||
for i in range(self.uav_num-1):
|
||||
self.slack_right = numpy.array([100]*(self.uav_num-1))
|
||||
while True:
|
||||
self.visit_left = numpy.array([0]*(self.uav_num-1))
|
||||
self.visit_right = numpy.array([0]*(self.uav_num-1))
|
||||
if self.find_path(i):
|
||||
break
|
||||
d = numpy.inf
|
||||
for j, slack in enumerate(self.slack_right):
|
||||
if not self.visit_right[j] :
|
||||
d = min(d,slack)
|
||||
for k in range(self.uav_num-1):
|
||||
if self.visit_left[k]:
|
||||
self.label_left[k] -= d
|
||||
if self.visit_right[k]:
|
||||
self.label_right[k] += d
|
||||
else:
|
||||
self.slack_right[k] -=d
|
||||
return self.match_right
|
||||
|
||||
# The formation patterns designed in the formation dictionaries are random (the old ones),
|
||||
# and a new formation pattern based on the distances of UAVs of the current pattern is designed as follows.
|
||||
# Note that only the desired position of each UAV has changed, while the form of the new pattern is the same as the one in the dictionary.
|
||||
def get_new_formation(self,change_id,change_formation):
|
||||
new_formation=numpy.zeros((3,self.uav_num-1))
|
||||
position=numpy.zeros((3,self.uav_num-1))
|
||||
change_id=[i + 1 for i in change_id]
|
||||
for i in range(0,self.uav_num-1):
|
||||
position[:,i]=change_formation[:,i]
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
for j in range(0,self.uav_num-1):
|
||||
if (j+1)==change_id[i]:
|
||||
new_formation[:,i]=position[:,j]
|
||||
return new_formation
|
||||
|
||||
# Laplacian matrix
|
||||
def get_L_central_matrix(self):
|
||||
|
||||
L=numpy.zeros((self.uav_num,self.uav_num))
|
||||
for i in range(1,self.uav_num):
|
||||
L[i][0]=1
|
||||
L[i][i]=-1
|
||||
|
||||
#L=numpy.array([[-0. ,0. , 0., 0., 0., 0.],[ 1. ,-1. , 0. , 0. , 0. , 0.],[ 0., 1. ,-1. , 0. , 0. , 0.],[ 0., 0., 1. ,-1. , 0. , 0.],[ 0. , 0. , 0. , 1. ,-1. , 0.],[ 0. , 0. , 0. , 0. , 1. ,-1.]])
|
||||
return L
|
||||
|
||||
if __name__ == '__main__':
|
||||
follower = Follower(sys.argv[1],int(sys.argv[2]),int(sys.argv[3]))
|
||||
follower.loop()
|
||||
|
|
@ -0,0 +1,380 @@
|
|||
# encoding: utf-8
|
||||
import numpy as np
|
||||
import random
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
import os
|
||||
|
||||
UAV_NUM = 33 #无人机数量-1，0 0 0位置不动
|
||||
POP_SIZE = 200 #种群数量
|
||||
N_GENERATIONS = 200 #迭代次数，迭代50轮
|
||||
CROSSOVER_RATE = 0.8
|
||||
# MUTATION_RATE = 0.2
|
||||
|
||||
formation_dict_my = {}
|
||||
# formation_dict_my["origin34"] = [ # XTDrone中简化仿真的初始位置
|
||||
# # [0, 0, 0],
|
||||
# [0, 1, 0], [0, 2, 0], [1, 0, 0], [1, 1, 0], [1, 2, 0],
|
||||
# [2, 0, 0], [2, 1, 0], [2, 2, 0], [3, 0, 0], [3, 1, 0], [3, 2, 0],
|
||||
# [4, 0, 0], [4, 1, 0], [4, 2, 0], [5, 0, 0], [5, 1, 0], [5, 2, 0],
|
||||
# [6, 0, 0], [6, 1, 0], [6, 2, 0], [7, 0, 0], [7, 1, 0], [7, 2, 0],
|
||||
# [8, 0, 0], [8, 1, 0], [8, 2, 0], [9, 0, 0], [9, 1, 0], [9, 2, 0],
|
||||
# [10, 0, 0], [10, 1, 0], [10, 2, 0], [11, 0, 0] ]
|
||||
formation_dict_my["origin34"] = [ # XTDrone中gazebo仿真的初始位置
|
||||
# [0, 3, 0],
|
||||
[3, 3, 0], [6, 3, 0], [9, 3, 0], [12, 3, 0],
|
||||
[0, 6, 0], [3, 6, 0], [6, 6, 0], [9, 6, 0], [12, 6, 0],
|
||||
[0, 9, 0], [3, 9, 0], [6, 9, 0], [9, 9, 0], [12, 9, 0],
|
||||
[0, 12, 0], [3, 12, 0], [6, 12, 0], [9, 12, 0], [12, 12, 0],
|
||||
[0, 15, 0], [3, 15, 0], [6, 15, 0], [9, 15, 0], [12, 15, 0],
|
||||
[0, 18, 0], [3, 18, 0], [6, 18, 0], [9, 18, 0], [12, 18, 0],
|
||||
[0, 21, 0], [3, 21, 0], [6, 21, 0], [9, 21, 0] ]
|
||||
formation_dict_my["CUBE"] = [ #自定义位置1，立方体
|
||||
# [0, 0, 0],
|
||||
[0, 0, 0],
|
||||
[-15, -15, 15], [-15, 15, 15], [15, 15, 15], [15, -15, 15], #上顶点
|
||||
[-15, -15, -15], [-15, 15, -15], [15, 15, -15], [15, -15, -15], #下顶点
|
||||
[-15, -5, 15], [-15, 5, 15], [-5, 15, 15], [5, 15, 15], #上棱
|
||||
[15, 5, 15], [15, -5, 15], [5, -15, 15], [-5, -15, 15], #上棱
|
||||
[-15, -5, -15], [-15, 5, -15], [-5, 15, -15], [5, 15, -15], #下棱
|
||||
[15, 5, -15], [15, -5, -15], [5, -15, -15], [-5, -15, -15], #下棱
|
||||
[-15, -15, -5], [-15, -15, 5], [15, -15, -5], [15, -15, 5], #中棱
|
||||
[-15, 15, -5], [-15, 15, 5], [15, 15, -5], [15, 15, 5] ] #中棱
|
||||
formation_dict_my["HEART"] = [ #自定义位置2，心形
|
||||
# [-0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
|
||||
[ 1.00000000e+01, 0.00000000e+00, 0.00000000e+00],
|
||||
[ 8.33179844e+00, 1.31962721e+00, 0.00000000e+00],
|
||||
[ 6.57163890e+00, 2.13525495e+00, 0.00000000e+00],
|
||||
[ 4.86498026e+00, 2.47883127e+00, 0.00000000e+00],
|
||||
[ 3.33488737e+00, 2.42293749e+00, 0.00000000e+00],
|
||||
[ 2.07106782e+00, 2.07106781e+00, 0.00000000e+00],
|
||||
[ 1.12256994e+00, 1.54508498e+00, 0.00000000e+00],
|
||||
[-1.12256995e+00, 1.54508497e+00, 0.00000000e+00],
|
||||
[-2.07106781e+00, 2.07106781e+00, 0.00000000e+00],
|
||||
[-3.33488736e+00, 2.42293751e+00, 0.00000000e+00],
|
||||
[-4.86498028e+00, 2.47883124e+00, 0.00000000e+00],
|
||||
[-6.57163891e+00, 2.13525491e+00, 0.00000000e+00],
|
||||
[-8.33179843e+00, 1.31962724e+00, 0.00000000e+00],
|
||||
[-1.00000000e+01, 3.58979303e-08, 0.00000000e+00],
|
||||
[-1.14219684e+01,-1.80906211e+00, 0.00000000e+00],
|
||||
[-1.24494914e+01,-4.04508498e+00, 0.00000000e+00],
|
||||
[-1.29551502e+01,-6.60097872e+00, 0.00000000e+00],
|
||||
[-1.28454526e+01,-9.33276749e+00, 0.00000000e+00],
|
||||
[-1.20710678e+01,-1.20710678e+01, 0.00000000e+00],
|
||||
[-1.06331351e+01,-1.46352549e+01, 0.00000000e+00],
|
||||
[-8.62130925e+00,-1.69202720e+01, 0.00000000e+00],
|
||||
[-6.15270291e+00,-1.89360728e+01, 0.00000000e+00],
|
||||
[-3.28150749e+00,-2.07186232e+01, 0.00000000e+00],
|
||||
[-8.84786534e-09,-2.30000000e+01, 0.00000000e+00],
|
||||
[ 3.28150747e+00,-2.07186232e+01, 0.00000000e+00],
|
||||
[ 6.15270308e+00,-1.89360727e+01, 0.00000000e+00],
|
||||
[ 8.62130924e+00,-1.69202720e+01, 0.00000000e+00],
|
||||
[ 1.06331351e+01,-1.46352549e+01, 0.00000000e+00],
|
||||
[ 1.20710678e+01,-1.20710679e+01, 0.00000000e+00],
|
||||
[ 1.28454526e+01,-9.33276750e+00, 0.00000000e+00],
|
||||
[ 1.29551502e+01,-6.60097873e+00, 0.00000000e+00],
|
||||
[ 1.24494914e+01,-4.04508499e+00, 0.00000000e+00],
|
||||
[ 1.14219684e+01,-1.80906212e+00, 0.00000000e+00] ]
|
||||
formation_dict_my["520"] = [ #自定义位置3，520
|
||||
# [0, 0, 0],
|
||||
[-3, 0, 0], [3, 0, 0], [3, -3, 0], [3, -6, 0], [0, -6, 0],#2
|
||||
[-3, -6, 0], [-3, -9, 0], [-3, -12, 0], [0, -12, 0], [3, -12, 0],
|
||||
[-9, 0, 0], [-12, 0, 0], [-15, 0, 0], [-15, -3, 0], [-15, -6, 0], [-12, -6, 0], #5
|
||||
[-9, -6, 0], [-9, -9, 0], [-9, -12, 0], [-12, -12, 0], [-15, -12, 0],
|
||||
[9, 0, 0], [12, 0, 0], [15, 0, 0], [15, -3, 0], [15, -6, 0], [15, -9, 0], #0
|
||||
[15, -12, 0], [12, -12, 0], [9, -12, 0], [9, -9, 0], [9, -6, 0], [9, -3, 0] ]
|
||||
formation_dict_my["NUDT"] = [ #自定义位置4，国防科大
|
||||
# [0, 0, 0],
|
||||
[0, -4, 0], [0, -8, 0], [-1.2, -11, 0], #U
|
||||
[-7, 0, 0], [-7, -4, 0], [-7, -8, 0], [-5.8, -11, 0], [-3.5, -12, 0],
|
||||
[-12, 0, 0], [-12, -6, 0], [-12, -12, 0], #N
|
||||
[-18, 0, 0], [-18, -6, 0], [-18, -12, 0],
|
||||
[-16.5, -3, 0], [-15, -6, 0], [-13.5, -9, 0],
|
||||
[5, 0, 0], [5, -3, 0], [5, -6, 0], [5, -9, 0], [5, -12, 0], #D
|
||||
[8, -0.5, 0], [9.5, -3, 0], [10.5, -6, 0], [9.5, -9, 0], [8, -11.5, 0],
|
||||
[15, 0, 0], [18, 0, 0], [21, 0, 0], [18, -4, 0], [18, -8, 0], [18, -12, 0] ]#T
|
||||
formation_dict_my["八一"] = [ #自定义位置5，八一军徽
|
||||
# [0, 0, 0],
|
||||
[-2, 0, 0], [2, 0, 0], [-2, -3, 0], [-2.5, -4, 0], [-3.5, -4.5, 0],
|
||||
[2, -1, 0], [2.5, -2, 0], [3, -3, 0], [4, -4.5, 0], #八
|
||||
[-3, -8, 0], [-1, -8, 0], [1, -8, 0], [3, -8, 0], #一
|
||||
[-22, 2, -5], [-13.4, 2, -5], [-5.2, 2, -5], [-2.6, 10, -5], [0, 18, -5],
|
||||
[2.6, 10, -5], [5.2, 2, -5], [13.4, 2, -5], [22, 2, -5], #上
|
||||
[15.2, -3, -5], [8.4, -8, -5], [11, -16, -5], [13.4, -24, -5], [6.8, -19, -5],
|
||||
[0, -14, -5], [-6.8, -19, -5], [-13.6, -24, -5], [-11, -16, -5], [-8.4, -8, -5], [-15.2, -3, -5] ] #下
|
||||
|
||||
|
||||
def plot_uav_pos():
|
||||
###画无人机位置
|
||||
pre_x = np.zeros((1, UAV_NUM)) #初始位置
|
||||
pre_y = np.zeros((1, UAV_NUM))
|
||||
pre_z = np.zeros((1, UAV_NUM))
|
||||
past_x = np.zeros((1, UAV_NUM)) #编队后位置
|
||||
past_y = np.zeros((1, UAV_NUM))
|
||||
past_z = np.zeros((1, UAV_NUM))
|
||||
for i in range(UAV_NUM):
|
||||
pre_x[0][i] = UAV_pre[i][0]
|
||||
pre_y[0][i] = UAV_pre[i][1]
|
||||
pre_z[0][i] = UAV_pre[i][2]
|
||||
past_x[0][i] = UAV_past[i][0]
|
||||
past_y[0][i] = UAV_past[i][1]
|
||||
past_z[0][i] = UAV_past[i][2]
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection='3d')
|
||||
|
||||
ax.set_xlim(-10, 10)#坐标轴范围
|
||||
ax.set_ylim(-10, 10)
|
||||
ax.set_zlim(-10, 10)
|
||||
ax.set_xlabel('X Axes')
|
||||
ax.set_ylabel('Y Axes')
|
||||
ax.set_zlabel('Z Axes')
|
||||
sc = ax.scatter3D(pre_x, pre_y, pre_z, color='r', alpha=0.7, marker='1', linewidth = 10) #初始位置 红
|
||||
sc = ax.scatter3D(past_x, past_y, past_z, color='y', alpha=0.7, marker='1', linewidth = 10) #编队后位置 黄
|
||||
#画无人机位置的辅助线
|
||||
#编队后位置
|
||||
# DING_LIST = [[-4, -3], [6, -3], [6, 9], [-4, 9]]
|
||||
# DingX, DingY = [], []
|
||||
# for each in DING_LIST:
|
||||
# DingX.append(each[0])
|
||||
# DingY.append(each[1])
|
||||
# DingX.append(DING_LIST[0][0]) #X和Y行程闭环
|
||||
# DingY.append(DING_LIST[0][1])
|
||||
# ax.plot(DingX, DingY, color = 'black', linestyle = ':')
|
||||
# #初始位置
|
||||
# r = 5
|
||||
# a, b = (0,0)
|
||||
# x = np.arange( a-r, a+r+1e-3, 0.1)
|
||||
# y = b + np.sqrt( r**2 - (x - a)**2)
|
||||
# ax.plot(x, y, color = 'black', linestyle = ':') # 上半部
|
||||
# ax.plot(x, -y, color = 'black', linestyle = ':') # 下半部
|
||||
# plt.show()
|
||||
|
||||
|
||||
###计算全部无人机从初始位置分别到终止位置的距离
|
||||
def calu_pre2past_dis():
|
||||
distance = []
|
||||
for i in range(UAV_NUM):
|
||||
single_pre = UAV_pre[i]
|
||||
pre = np.tile(single_pre, (UAV_NUM ,1 ))#矩阵增广，用于将 原先的位置－编队后的位置
|
||||
dis_pre2past = np.sqrt( np.dot( (pre - UAV_past)**2, np.array([ [1],[1],[1] ]) ) ) #计算无人机编队前后的距离
|
||||
#编队前位置-编队后位置，再对x和y分别平方，再相加，最后开方。两点间的距离
|
||||
dis_pre2past = dis_pre2past.reshape(-1,) #(10,1)转化为(10,)
|
||||
distance.append( dis_pre2past ) #每一行代表初始的一架无人机，变换到编队后所有位置的距离
|
||||
distance = np.array(distance) # distance[x][y]表示初始第x号飞到编队后第y号位置的距离
|
||||
return distance
|
||||
|
||||
###初始化种群
|
||||
def initpop():
|
||||
pop = np.zeros((POP_SIZE, UAV_NUM), int)
|
||||
#100行 * 10列，用来表示初始的x架无人机0~9编队后到达的位置的下标0~9
|
||||
# print(pop)
|
||||
# print(type(pop[1][1]))
|
||||
for i in range(POP_SIZE):
|
||||
select = [x for x in range( UAV_NUM )] #随机产生种群的每个个体
|
||||
random.shuffle(select) #产生一种随机的分配方案
|
||||
pop[i, :] = select
|
||||
return pop
|
||||
# print(pop)
|
||||
|
||||
###交叉 变异
|
||||
def crossover_and_mutation(pop):
|
||||
pre_pop = pop.copy() #prechoose是不经过交叉变异的，newchoose后期经过交叉变异
|
||||
for row in range(0, (np.shape(pop))[0], 2): # 两个为一组互相交叉
|
||||
if CROSSOVER_RATE > random.random():
|
||||
gen_1 = pop[row, :].copy()
|
||||
gen_2 = pop[row + 1, :].copy()
|
||||
select = [x for x in range(UAV_NUM)]
|
||||
random.shuffle(select)
|
||||
p1 = select[0] #保证两个交叉点的位置不一样
|
||||
p2 = select[1]
|
||||
p1, p2 = min(p1, p2), max(p1, p2) # 保证 p1 < p2
|
||||
cross1, cross2 = gen_1[p1:p2 + 1].copy(), gen_2[p1:p2 + 1].copy() #两个基因的交叉片段
|
||||
# print(p1, p2, cross1, cross2)
|
||||
for cro_pos_i in range(p2 - p1 + 1): # 遍历交叉片段
|
||||
gen_pos1 = np.where(gen_1 == cross2[cro_pos_i])
|
||||
gen_1[gen_pos1] = (gen_1[p1:p2 + 1])[cro_pos_i].copy()
|
||||
# a = gen_1
|
||||
for cro_pos_i in range(p2 - p1 + 1):
|
||||
gen_pos2 = np.where(gen_2 == cross1[cro_pos_i])
|
||||
gen_2[gen_pos2] = (gen_2[p1:p2 + 1])[cro_pos_i].copy()
|
||||
# b = gen_2
|
||||
gen_1[p1:p2 + 1] = cross2.copy() #gen2的交叉片段给gen1
|
||||
gen_2[p1:p2 + 1] = cross1.copy()
|
||||
pop[row, :] = gen_1.copy() #放回到种群中
|
||||
pop[row + 1, :] = gen_2.copy()
|
||||
|
||||
for row in range(0, (np.shape(pop))[0]): #遍历每个基因，以一定概率变异
|
||||
if MUTATION_RATE > random.random():
|
||||
select = [x for x in range(UAV_NUM)]
|
||||
random.shuffle(select)
|
||||
p1 = select[0] #保证变异的两个点位不一样
|
||||
p2 = select[1]
|
||||
gen = pop[row, :].copy()
|
||||
gen[p1], gen[p2] = gen[p2], gen[p1] #交换两个变异的点位
|
||||
pop[row, :] = gen.copy() #放回到种群中
|
||||
# print(p1,p2,gen[p1],gen[p2])
|
||||
# print(pop,'--------')
|
||||
# print(pre_pop)
|
||||
pop = np.append(pre_pop, pop, 0)
|
||||
#经过交叉变异的种群和未经过的，组合成一个两倍于POPSIZE的种群，下一轮会选择前POPSIZE个
|
||||
return pop
|
||||
# print(pop.shape)
|
||||
|
||||
###计算每一种分配情况，最远距离和最近距离之差
|
||||
def calu_allocate_dis(pop, dist):
|
||||
all_pop_dis = [] #种群100个个体分配情况的时间，列表，行向量
|
||||
|
||||
for allocate_i in pop: #遍历每一种分配方案
|
||||
distances = np.zeros(UAV_NUM)
|
||||
for i in range(UAV_NUM):
|
||||
uav_no_ = allocate_i[i]
|
||||
distances[i] = dist[uav_no_][i]
|
||||
all_pop_dis.append(max(distances) - min(distances))
|
||||
all_pop_dis = np.array(all_pop_dis).reshape(-1, 1) # 列表转列向量
|
||||
# unselect_pop_n = all_pop_dis.shape[0]
|
||||
return all_pop_dis #每种分配方案的距离
|
||||
# print(all_pop_dis)
|
||||
|
||||
# ###计算每一种分配的总距离
|
||||
# def calu_allocate_dis(pop, dist):
|
||||
# all_pop_dis = [] #种群100个个体分配情况的时间，列表，行向量
|
||||
# for allocate_i in pop: #遍历每一种分配方案
|
||||
# single_array_sum_time = 0 #一种分配情况的总时间
|
||||
# for i in range(UAV_NUM):
|
||||
# uav_no_ = allocate_i[i]
|
||||
# single_array_sum_time += dist[uav_no_][i]
|
||||
# all_pop_dis.append(single_array_sum_time)
|
||||
# all_pop_dis = np.array(all_pop_dis).reshape(-1, 1) # 列表转列向量
|
||||
# # unselect_pop_n = all_pop_dis.shape[0]
|
||||
# return all_pop_dis #每种分配方案的距离
|
||||
|
||||
|
||||
###排序并计算适应度
|
||||
def get_fitness(all_pop_dis, pop):
|
||||
index = np.argsort(all_pop_dis, 0) #升序排列，下标
|
||||
all_pop_dis = all_pop_dis[index].reshape(all_pop_dis.shape[0], 1)[0:POP_SIZE, :]
|
||||
#距离升序排列，种群经过交叉变异，数量扩大一倍，这里选择前POP_SIZE个(距离最短的前n个)
|
||||
pop = pop[index].reshape(pop.shape[0], UAV_NUM)[0:POP_SIZE, :] # 距离短的前n个种群个体
|
||||
'''计算适值和适应度'''
|
||||
fit = 1000 - all_pop_dis[:] # 适应度，时间的反比，时间短=适应度大
|
||||
fitplus = np.cumsum(fit).reshape(-1, 1) # 适值向下叠加
|
||||
# print(fitplus[POP_SIZE-1, :])
|
||||
fitlevelplus = fitplus[:] / fitplus[POP_SIZE - 1, :] # 适应度向下叠加
|
||||
fitlevelplus = np.insert(fitlevelplus, 0, np.array([0]), 0) # 在第一行添加0
|
||||
# print(fitlevelplus)
|
||||
# a = (np.shape(fitlevelplus))[0]
|
||||
return fitlevelplus, all_pop_dis, pop
|
||||
|
||||
###选择
|
||||
def select(pop, fitlevelplus, all_pop_dis):
|
||||
newchoose_pop = [] #根据适应度选择的新种群
|
||||
new_pop_dis = [] #新种群的每个个体，分配方案的距离
|
||||
for _ in range(POP_SIZE):
|
||||
rand = random.random()
|
||||
for row in range(POP_SIZE):
|
||||
if rand > fitlevelplus[row, :] and rand < fitlevelplus[row + 1, :]:
|
||||
newchoose_pop.append(pop[row, :])
|
||||
new_pop_dis.append(all_pop_dis[row, :])
|
||||
break
|
||||
newchoose_pop = np.array(newchoose_pop)
|
||||
new_pop_dis = np.array(new_pop_dis)
|
||||
return newchoose_pop, new_pop_dis
|
||||
# print(newchoose)
|
||||
# print((np.shape(newchoose))[0])
|
||||
# print(new_pop_dis)
|
||||
|
||||
#将种群和每个分配方案的距离排序
|
||||
def sortdis(sortpop, dist):
|
||||
index = np.argsort(dist, 0) #升序排列，下标
|
||||
dist = dist[index].reshape(dist.shape[0], 1)
|
||||
#距离升序排列，种群经过交叉变异，数量扩大一倍，这里选择前POP_SIZE个(距离最短的前n个)
|
||||
sortpop = sortpop[index].reshape(sortpop.shape[0], UAV_NUM)
|
||||
return sortpop, dist
|
||||
|
||||
def plot_allocate(best_allc):#best_allc是0~9个编队后位置分给初始的第几号无人机
|
||||
plot_uav_pos()
|
||||
for i in range(UAV_NUM):
|
||||
uav_x = []
|
||||
uav_y = []
|
||||
uav_z = []
|
||||
uav_x.append( UAV_past[i][0] ) #编队后的位置给初始的第几号无人机
|
||||
uav_y.append( UAV_past[i][1] )
|
||||
uav_z.append( UAV_past[i][2] )
|
||||
uav_id_pre = best_allc[i]
|
||||
uav_x.append( UAV_pre[uav_id_pre][0] )
|
||||
uav_y.append( UAV_pre[uav_id_pre][1] )
|
||||
uav_z.append( UAV_pre[uav_id_pre][2] )
|
||||
plt.plot(uav_x, uav_y, uav_z, color = 'black', linestyle = ':')
|
||||
plt.show()
|
||||
|
||||
def outputfile(best_allc, form_name):
|
||||
allo_index = np.argsort(best_allc, 0) #升序排列，下标。排序后的下标，是初始无人机0~9到达终止位置的第几号
|
||||
# 编队结果：6 3 5 9 0 1 7 2 8 4
|
||||
# 排序下标：4 5 7 1 9 2 0 6 8 3
|
||||
# 0号对应编队第4号位置， 1号对应编队第5号位置
|
||||
filename = 'my_formation_dict.py'
|
||||
filepath = os.getcwd() + '\\' + filename # 输出的文件所在完整路径
|
||||
if not os.path.exists(filepath): #如果文件不存在，则在文件里添加第一行
|
||||
with open(filename, 'w') as f:
|
||||
f.write( "import numpy as np\n\n" )
|
||||
f.write( "formation_dict_my = {}\n" )
|
||||
with open(filename, 'a') as f:
|
||||
f.write( 'formation_dict_my["' + form_name + '"] = np.array([ ' )
|
||||
for i in range(0, UAV_NUM-1):
|
||||
f.write( str(UAV_past[allo_index[i]]) + ",")
|
||||
f.write( str(UAV_past[allo_index[UAV_NUM-1]]) + "])\n")
|
||||
f.write( 'formation_dict_my["' + form_name + '"] = np.transpose(formation_dict_my["' + form_name + '"]*1)\n')
|
||||
|
||||
def plot_info(best_distances, dist, pop):
|
||||
best_distances = np.array(best_distances)
|
||||
# print(best_distances)
|
||||
print(dist[0, :]) #最短距离
|
||||
# print(pop[0, :]) #对应的分配方案
|
||||
|
||||
x = [a for a in range(1, N_GENERATIONS+1)]
|
||||
y = best_distances
|
||||
plt.plot(x, y)
|
||||
plt.xlabel('迭代次数')
|
||||
plt.ylabel('距离之差')
|
||||
plt.rcParams['font.sans-serif'] = ['SimHei']
|
||||
plt.show()
|
||||
|
||||
def calu_best_dis(best_allc, dis):
|
||||
best_dis = 0
|
||||
# allo_index = np.argsort(best_allc, 0) #升序排列，下标
|
||||
for i in range(UAV_NUM):
|
||||
uav_no_ = best_allc[i]
|
||||
best_dis += dis[uav_no_][i]
|
||||
# pos_past = UAV_past[allo_index[i]]
|
||||
# pos_pre = UAV_pre[i]
|
||||
# curr_dis = math.sqrt((pos_pre[0] - pos_past[0]) ** 2 + (pos_pre[1] - pos_past[1]) ** 2)
|
||||
# best_dis += curr_dis
|
||||
print(best_dis)
|
||||
|
||||
if __name__ == "__main__":
|
||||
dic_i = 0
|
||||
for key in formation_dict_my.keys():
|
||||
MUTATION_RATE = 0.2 # 每开始新的一轮循环，新的编队
|
||||
form_name = key
|
||||
UAV_past = formation_dict_my[key]
|
||||
if dic_i:
|
||||
plot_uav_pos()
|
||||
plt.show() #画无人机初始位置和编队后的位置
|
||||
distance = calu_pre2past_dis() #计算全部无人机从初始位置分别到终止位置的距离
|
||||
pop = initpop() #初始化种群
|
||||
best_distances = [] #每一轮最短的距离，用于绘图
|
||||
for generation_n in range(N_GENERATIONS): #迭代N代
|
||||
if generation_n == int(N_GENERATIONS/2):#迭代到一半时，变异率扩大一倍
|
||||
MUTATION_RATE = MUTATION_RATE*3
|
||||
pop = crossover_and_mutation(pop) #先交叉变异，pop扩大为原来的两倍
|
||||
all_pop_dis = calu_allocate_dis(pop, distance) #每种分配方案的距离，分配方案的数量
|
||||
fitlevelplus, all_pop_dis, pop = get_fitness(all_pop_dis, pop) #种群排序，取前n，求适应度
|
||||
# pop, all_pop_dis = select(pop, fitlevelplus, all_pop_dis) #选择
|
||||
# sort_pop, sort_pop_dis = sortdis(pop, all_pop_dis)
|
||||
best_distances.append(all_pop_dis[0, :]) #当前种群里，最短距离
|
||||
plot_allocate(pop[0, :]) #画无人机前后分配线
|
||||
outputfile(pop[0, :], form_name) #无人机位置输出到文件
|
||||
plot_info(best_distances, all_pop_dis, pop) #绘制最短距离随迭代次数变化曲线
|
||||
# calu_best_dis(pop[0, :], distance)
|
||||
UAV_pre = formation_dict_my[key]
|
||||
dic_i += 1
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3, PoseStamped
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
#if sys.argv[2] == '6': #formation_dict是该文件夹下的文件
|
||||
# from formation_dict import formation_dict_6 as formation_dict
|
||||
#elif sys.argv[2] == '9':
|
||||
# from formation_dict import formation_dict_9 as formation_dict
|
||||
#elif sys.argv[2] == '18':
|
||||
# from formation_dict import formation_dict_18 as formation_dict
|
||||
if sys.argv[2] == '21':
|
||||
from my_formation_dict import formation_dict_my as formation_dict
|
||||
elif sys.argv[2] == '34':
|
||||
from my_formation_dict import formation_dict_my as formation_dict
|
||||
|
||||
class Leader:
|
||||
|
||||
def __init__(self, uav_type, leader_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = leader_id
|
||||
self.local_pose = PoseStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.follower_num = uav_num - 1
|
||||
self.followers_info = ["Moving"]*self.follower_num
|
||||
self.follower_arrived_num = 0
|
||||
self.follower_all_arrived = True
|
||||
self.avoid_accel = Vector3(0,0,0)
|
||||
self.formation_config = 'waiting'
|
||||
self.target_height_recorded = False
|
||||
self.cmd = String()
|
||||
self.f = 200
|
||||
self.Kz = 0.5
|
||||
self.local_pose_sub = rospy.Subscriber(uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
|
||||
self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel_flu", Twist, self.cmd_vel_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+uav_type+'_'+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback)
|
||||
self.leader_cmd_sub = rospy.Subscriber("/xtdrone/leader/cmd",String, self.cmd_callback)
|
||||
|
||||
for i in range(self.follower_num):#遍历所有跟随者
|
||||
rospy.Subscriber('/xtdrone/'+uav_type+'_'+str(i+1)+'/info',String,self.followers_info_callback,i)
|
||||
self.local_pose_pub = rospy.Publisher("/xtdrone/leader/pose", PoseStamped , queue_size=10)
|
||||
self.formation_switch_pub = rospy.Publisher("/xtdrone/formation_switch",String, queue_size=10)
|
||||
self.vel_enu_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.cmd_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd', String, queue_size=10)
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.cmd_vel_enu = msg
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True #悬停
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data in formation_dict.keys():
|
||||
self.formation_config = msg.data
|
||||
else:
|
||||
self.cmd = msg.data
|
||||
|
||||
def avoid_accel_callback(self, msg):
|
||||
self.avoid_accel = msg
|
||||
|
||||
def followers_info_callback(self, msg, id):
|
||||
self.followers_info[id] = msg.data
|
||||
#print("follower"+str(id)+":"+ msg.data)
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('leader')
|
||||
rate = rospy.Rate(self.f)
|
||||
while True:
|
||||
#self.cmd_vel_enu = Twist()
|
||||
for follower_info in self.followers_info:
|
||||
if follower_info == "Arrived": # 一架到达
|
||||
self.follower_arrived_num += 1
|
||||
if self.follower_arrived_num > self.follower_num - 1:
|
||||
self.follower_all_arrived = True #全部到达
|
||||
if self.follower_all_arrived:
|
||||
self.formation_switch_pub.publish(self.formation_config)
|
||||
if self.formation_config == 'pyramid':
|
||||
if not self.target_height_recorded:
|
||||
target_height = self.local_pose.pose.position.z + 2
|
||||
self.target_height_recorded = True
|
||||
self.cmd_vel_enu.linear.z = self.Kz * (target_height - self.local_pose.pose.position.z)
|
||||
self.cmd_vel_enu.linear.x += self.avoid_accel.x
|
||||
self.cmd_vel_enu.linear.y += self.avoid_accel.y
|
||||
self.cmd_vel_enu.linear.z += self.avoid_accel.z
|
||||
self.vel_enu_pub.publish(self.cmd_vel_enu)
|
||||
self.local_pose_pub.publish(self.local_pose)
|
||||
self.cmd_pub.publish(self.cmd)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
leader = Leader(sys.argv[1], 0, int(sys.argv[2]))
|
||||
leader.loop()
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
#!/bin/bash
|
||||
# -*- coding: UTF-8 -*-
|
||||
python my_leader.py $1 $2 & #第一架飞机是 引领，其余飞机是 跟随。$1是飞机类型如iris，$2是数量
|
||||
uav_num=1
|
||||
while(( $uav_num< $2 ))
|
||||
do
|
||||
python my_follower.py $1 $uav_num $2&
|
||||
let "uav_num++"
|
||||
done
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
kill -9 $(ps -ef|grep follower.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
|
||||
kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
|
||||
|
|
@ -0,0 +1,631 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor2.world"/>
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<!-- Gazebo sim -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="world_name" value="$(arg world)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!-- iris_0 -->
|
||||
<group ns="iris_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="0"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18570"/>
|
||||
<arg name="mavlink_tcp_port" value="4560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_1 -->
|
||||
<group ns="iris_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18571"/>
|
||||
<arg name="mavlink_tcp_port" value="4561"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_2 -->
|
||||
<group ns="iris_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="6"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18572"/>
|
||||
<arg name="mavlink_tcp_port" value="4562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_3 -->
|
||||
<group ns="iris_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="ID_in_group" value="3"/>
|
||||
<arg name="fcu_url" default="udp://:24543@localhost:34583"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="9"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18573"/>
|
||||
<arg name="mavlink_tcp_port" value="4563"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_4 -->
|
||||
<group ns="iris_4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="ID_in_group" value="4"/>
|
||||
<arg name="fcu_url" default="udp://:24544@localhost:34584"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18574"/>
|
||||
<arg name="mavlink_tcp_port" value="4564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_5 -->
|
||||
<group ns="iris_5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<arg name="ID_in_group" value="5"/>
|
||||
<arg name="fcu_url" default="udp://:24545@localhost:34585"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="15"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18575"/>
|
||||
<arg name="mavlink_tcp_port" value="4565"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_6 -->
|
||||
<group ns="iris_6">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<arg name="ID_in_group" value="6"/>
|
||||
<arg name="fcu_url" default="udp://:24546@localhost:34586"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="18"/>
|
||||
<arg name="y" value="3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18576"/>
|
||||
<arg name="mavlink_tcp_port" value="4566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_7 -->
|
||||
<group ns="iris_7">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<arg name="ID_in_group" value="7"/>
|
||||
<arg name="fcu_url" default="udp://:24547@localhost:34587"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18577"/>
|
||||
<arg name="mavlink_tcp_port" value="4567"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_8 -->
|
||||
<group ns="iris_8">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<arg name="ID_in_group" value="8"/>
|
||||
<arg name="fcu_url" default="udp://:24548@localhost:34588"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18578"/>
|
||||
<arg name="mavlink_tcp_port" value="4568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_9 -->
|
||||
<group ns="iris_9">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="9"/>
|
||||
<arg name="ID_in_group" value="9"/>
|
||||
<arg name="fcu_url" default="udp://:24549@localhost:34589"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="6"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18579"/>
|
||||
<arg name="mavlink_tcp_port" value="4569"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_10 -->
|
||||
<group ns="iris_10">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="10"/>
|
||||
<arg name="ID_in_group" value="10"/>
|
||||
<arg name="fcu_url" default="udp://:24550@localhost:34590"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="9"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18580"/>
|
||||
<arg name="mavlink_tcp_port" value="4570"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_11 -->
|
||||
<group ns="iris_11">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="11"/>
|
||||
<arg name="ID_in_group" value="11"/>
|
||||
<arg name="fcu_url" default="udp://:24551@localhost:34591"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18581"/>
|
||||
<arg name="mavlink_tcp_port" value="4571"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_12 -->
|
||||
<group ns="iris_12">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="12"/>
|
||||
<arg name="ID_in_group" value="12"/>
|
||||
<arg name="fcu_url" default="udp://:24552@localhost:34592"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="15"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18582"/>
|
||||
<arg name="mavlink_tcp_port" value="4572"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_13 -->
|
||||
<group ns="iris_13">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="13"/>
|
||||
<arg name="ID_in_group" value="13"/>
|
||||
<arg name="fcu_url" default="udp://:24553@localhost:34593"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="18"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18583"/>
|
||||
<arg name="mavlink_tcp_port" value="4573"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_14 -->
|
||||
<group ns="iris_14">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="14"/>
|
||||
<arg name="ID_in_group" value="14"/>
|
||||
<arg name="fcu_url" default="udp://:24554@localhost:34594"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18584"/>
|
||||
<arg name="mavlink_tcp_port" value="4574"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_15 -->
|
||||
<group ns="iris_15">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="15"/>
|
||||
<arg name="ID_in_group" value="15"/>
|
||||
<arg name="fcu_url" default="udp://:24555@localhost:34595"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18585"/>
|
||||
<arg name="mavlink_tcp_port" value="4575"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_16 -->
|
||||
<group ns="iris_16">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="16"/>
|
||||
<arg name="ID_in_group" value="16"/>
|
||||
<arg name="fcu_url" default="udp://:24556@localhost:34596"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="6"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18586"/>
|
||||
<arg name="mavlink_tcp_port" value="4576"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_17 -->
|
||||
<group ns="iris_17">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="17"/>
|
||||
<arg name="ID_in_group" value="17"/>
|
||||
<arg name="fcu_url" default="udp://:24557@localhost:34597"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="9"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18587"/>
|
||||
<arg name="mavlink_tcp_port" value="4577"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_18 -->
|
||||
<group ns="iris_18">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="18"/>
|
||||
<arg name="ID_in_group" value="18"/>
|
||||
<arg name="fcu_url" default="udp://:24558@localhost:34598"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18588"/>
|
||||
<arg name="mavlink_tcp_port" value="4578"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_19 -->
|
||||
<group ns="iris_19">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="19"/>
|
||||
<arg name="ID_in_group" value="19"/>
|
||||
<arg name="fcu_url" default="udp://:24559@localhost:34599"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="15"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18589"/>
|
||||
<arg name="mavlink_tcp_port" value="4579"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_20 -->
|
||||
<group ns="iris_20">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="20"/>
|
||||
<arg name="ID_in_group" value="20"/>
|
||||
<arg name="fcu_url" default="udp://:24560@localhost:34600"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="18"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="18590"/>
|
||||
<arg name="mavlink_tcp_port" value="4580"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
</launch>
|
||||
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,164 @@
|
|||
import random
|
||||
import numpy as np
|
||||
import math
|
||||
import time
|
||||
import os
|
||||
from geographiclib.geodesic import Geodesic
|
||||
import math
|
||||
geod = Geodesic.WGS84
|
||||
|
||||
# 计算方位角函数
|
||||
def azimuthAngle(pos):
|
||||
if(pos[0] > 0 and pos[1] > 0):
|
||||
angle = math.atan(pos[1] / pos[0])
|
||||
elif(pos[0] > 0 and pos[1] < 0):
|
||||
angle = math.atan(pos[1] / pos[0]) + 2 * math.pi
|
||||
elif(pos[0] < 0 and pos[1] > 0):
|
||||
angle = math.atan(pos[1] / pos[0]) + math.pi
|
||||
elif(pos[0] < 0 and pos[1] < 0):
|
||||
angle = math.atan(pos[1] / pos[0]) + math.pi
|
||||
|
||||
return (angle * 180 / math.pi)
|
||||
|
||||
def angle_dis(pos):
|
||||
angle = azimuthAngle(pos)
|
||||
distance = math.sqrt(pow(pos[0],2)+pow(pos[1],2))
|
||||
return angle, distance
|
||||
|
||||
class ACO():
|
||||
def __init__(self, vehicle_num, target_num,vehicle_speed, target, time_lim):
|
||||
self.num_type_ant = vehicle_num
|
||||
self.num_city = target_num+1 #number of cities
|
||||
self.group = 200 # 200组蚂蚁，每组蚂蚁有6只
|
||||
self.num_ant = self.group*self.num_type_ant #number of ants
|
||||
self.ant_vel = vehicle_speed
|
||||
self.cut_time = time_lim
|
||||
self.oneee = np.zeros((4,1))
|
||||
self.target = target
|
||||
self.alpha = 1 #pheromone
|
||||
self.beta = 2
|
||||
self.k1 = 0.03
|
||||
self.iter_max = 50
|
||||
#matrix of the distances between cities
|
||||
def distance_matrix(self):
|
||||
dis_mat = []
|
||||
for i in range(self.num_city):
|
||||
dis_mat_each = []
|
||||
for j in range(self.num_city):
|
||||
dis = math.sqrt(pow(self.target[i][0]-self.target[j][0],2)+pow(self.target[i][1]-self.target[j][1],2))
|
||||
dis_mat_each.append(dis)
|
||||
dis_mat.append(dis_mat_each)
|
||||
return dis_mat
|
||||
def run(self):
|
||||
print("ACO start, pid: %s" % os.getpid())
|
||||
start_time = time.time()
|
||||
#distances of nodes
|
||||
dis_list = self.distance_matrix()
|
||||
dis_mat = np.array(dis_list)
|
||||
value_init = self.target[:,2].transpose()
|
||||
delay_init = self.target[:,3].transpose()
|
||||
pheromone_mat = np.ones((self.num_type_ant,self.num_city,self.num_city)) # 信息素矩阵6*31*31，6只蚂蚁在31*31条的路径上留下的信息素
|
||||
#velocity of ants
|
||||
path_new = [[0]for i in range (self.num_type_ant)]
|
||||
count_iter = 0
|
||||
while count_iter < self.iter_max:
|
||||
path_sum = np.zeros((self.num_ant,1))
|
||||
time_sum = np.zeros((self.num_ant,1))
|
||||
value_sum = np.zeros((self.num_ant,1))
|
||||
path_mat=[[0]for i in range (self.num_ant)]
|
||||
value = np.zeros((self.group,1))
|
||||
atten = np.ones((self.num_type_ant,1)) * 0.2
|
||||
for ant in range(self.num_ant): # 对每只蚂蚁
|
||||
ant_type = ant % self.num_type_ant
|
||||
visit = 0 # 当前节点
|
||||
if ant_type == 0:
|
||||
unvisit_list=list(range(1,self.num_city))#have not visit
|
||||
for j in range(1,self.num_city): # 行走的第几个城市
|
||||
#choice of next city
|
||||
trans_list=[]
|
||||
tran_sum=0
|
||||
trans=0
|
||||
#if len(unvisit_list)==0:
|
||||
#print('len(unvisit_list)==0')
|
||||
for k in range(len(unvisit_list)): # 计算本组蚂蚁没走过的城市的启发式函数
|
||||
trans +=np.power(pheromone_mat[ant_type][visit][unvisit_list[k]],self.alpha)*np.power(value_init[unvisit_list[k]]*self.ant_vel[ant_type]/(dis_mat[visit][unvisit_list[k]]*delay_init[unvisit_list[k]]),self.beta)
|
||||
#trans +=np.power(pheromone_mat[ant_type][unvisit_list[k]],self.alpha)*np.power(0.05*value_init[unvisit_list[k]],self.beta)
|
||||
trans_list.append(trans)
|
||||
tran_sum = trans
|
||||
rand = random.uniform(0,tran_sum)
|
||||
for t in range(len(trans_list)): # 轮盘赌选择下一个点
|
||||
if(rand <= trans_list[t]):
|
||||
visit_next = unvisit_list[t]
|
||||
break
|
||||
else:
|
||||
continue
|
||||
path_mat[ant].append(visit_next) # 每只蚂蚁走过的路径
|
||||
path_sum[ant] += dis_mat[path_mat[ant][j-1]][path_mat[ant][j]] # 每只蚂蚁走过的总路径长度
|
||||
time_sum[ant] += path_sum[ant] / self.ant_vel[ant_type] + delay_init[visit_next] # 每只蚂蚁使用的时间
|
||||
if time_sum[ant] > self.cut_time:
|
||||
time_sum[ant]-=path_sum[ant] / self.ant_vel[ant_type] + delay_init[visit_next]
|
||||
path_mat[ant].pop()
|
||||
break
|
||||
value_sum[ant] += value_init[visit_next] # 每只蚂蚁的value
|
||||
unvisit_list.remove(visit_next)#update
|
||||
visit = visit_next
|
||||
if (ant_type) == self.num_type_ant-1:
|
||||
small_group = int(ant/self.num_type_ant)
|
||||
for k in range (self.num_type_ant):
|
||||
value[small_group]+= value_sum[ant-k] # 每组蚂蚁的value
|
||||
#iteration
|
||||
if count_iter == 0:
|
||||
value_new = max(value)
|
||||
value = value.tolist()
|
||||
for k in range (0,self.num_type_ant):
|
||||
path_new[k] = path_mat[value.index(value_new)*self.num_type_ant+k] # 200组蚂蚁中value最大的蚂蚁的路径
|
||||
path_new[k].remove(0) # 移除起点
|
||||
else:
|
||||
if max(value) > value_new:
|
||||
value_new = max(value)
|
||||
value = value.tolist()
|
||||
for k in range (0,self.num_type_ant):
|
||||
path_new[k] = path_mat[value.index(value_new)*self.num_type_ant+k]
|
||||
path_new[k].remove(0)
|
||||
|
||||
#update pheromone
|
||||
pheromone_change = np.zeros((self.num_type_ant,self.num_city,self.num_city))
|
||||
for i in range(self.num_ant):
|
||||
length = len(path_mat[i])
|
||||
m = i%self.num_type_ant
|
||||
n = int(i/self.num_type_ant)
|
||||
for j in range(length-1):
|
||||
pheromone_change[m][path_mat[i][j]][path_mat[i][j+1]]+= value_init[path_mat[i][j+1]]*self.ant_vel[m]/(dis_mat[path_mat[i][j]][path_mat[i][j+1]]*delay_init[path_mat[i][j+1]])
|
||||
atten[m] += (value_sum[i]/(np.power((value_new-value[n]),4)+1))/self.group
|
||||
|
||||
for k in range (self.num_type_ant):
|
||||
pheromone_mat[k]=(1-atten[k])*pheromone_mat[k]+pheromone_change[k]
|
||||
count_iter += 1
|
||||
|
||||
print("ACO result:", path_new)
|
||||
middle_point = [47.3978502, 8.5455944]
|
||||
path_new_point = []
|
||||
for z in range(len(path_new)):
|
||||
path_new_point.append(self.target[path_new[z], 0:2])
|
||||
print("ACO path points m: ", path_new_point)
|
||||
|
||||
path_ang_dis = []
|
||||
log = []
|
||||
for i in range(len(path_new_point)):
|
||||
a_path_ang_dis = []
|
||||
a_log = []
|
||||
for j in range(len(path_new_point[i])):
|
||||
angle, distance = angle_dis(path_new_point[i][j])
|
||||
g = geod.Direct(middle_point[0], middle_point[1], angle, distance)
|
||||
a_log.append([g['lat2'], g['lon2']])
|
||||
a_path_ang_dis.append([angle, distance])
|
||||
path_ang_dis.append(a_path_ang_dis)
|
||||
log.append(a_log)
|
||||
|
||||
print("ACO angle and distance: ", path_ang_dis)
|
||||
|
||||
print("ACO log: ", log)
|
||||
end_time = time.time()
|
||||
print("ACO time:", end_time - start_time)
|
||||
return path_new, end_time - start_time
|
||||
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import random
|
||||
import pandas as pd
|
||||
import copy
|
||||
from aco import ACO
|
||||
from geographiclib.geodesic import Geodesic
|
||||
import math
|
||||
geod = Geodesic.WGS84
|
||||
|
||||
class Env():
|
||||
def __init__(self, vehicle_num, target_num, map_size, visualized=True, time_cost=None, repeat_cost=None):
|
||||
self.vehicles_position = np.zeros(vehicle_num,dtype=np.int32) # 无人机位置
|
||||
self.vehicles_speed = np.zeros(vehicle_num,dtype=np.int32) # 无人机速度
|
||||
self.targets = np.zeros(shape=(target_num+1,4),dtype=np.int32) # 目标属性
|
||||
if vehicle_num==6:
|
||||
self.size='small'
|
||||
self.map_size = map_size
|
||||
self.speed_range = [10, 10, 10]
|
||||
#self.time_lim = 1e6
|
||||
self.time_lim = self.map_size / self.speed_range[1]
|
||||
self.vehicles_lefttime = np.ones(vehicle_num,dtype=np.float32) * self.time_lim # 剩余的可用时间
|
||||
self.distant_mat = np.zeros((target_num+1,target_num+1),dtype=np.float32)
|
||||
self.total_reward = 0
|
||||
self.reward = 0
|
||||
self.visualized = visualized
|
||||
self.time = 0
|
||||
self.time_cost = time_cost
|
||||
self.repeat_cost = repeat_cost
|
||||
self.end = False
|
||||
self.assignment = [[] for i in range(vehicle_num)] # 最终的分配路径
|
||||
self.task_generator()
|
||||
|
||||
def task_generator(self):
|
||||
for i in range(self.vehicles_speed.shape[0]): # 确定每个无人机的速度
|
||||
choose = random.randint(0,2)
|
||||
self.vehicles_speed[i] = self.speed_range[choose]
|
||||
for i in range(self.targets.shape[0]-1): # 确定每个目标点的坐标位置和奖励、消耗值
|
||||
self.targets[i+1,0] = random.randint(1,self.map_size) - 0.5*self.map_size # x position
|
||||
self.targets[i+1,1] = random.randint(1,self.map_size) - 0.5*self.map_size # y position
|
||||
self.targets[i+1,2] = random.randint(1,10) # reward
|
||||
self.targets[i+1,3] = random.randint(5,10) # time consumption to finish the mission
|
||||
for i in range(self.targets.shape[0]): # 计算距离矩阵
|
||||
for j in range(self.targets.shape[0]):
|
||||
self.distant_mat[i,j] = np.linalg.norm(self.targets[i,:2]-self.targets[j,:2])
|
||||
self.targets_value = copy.deepcopy((self.targets[:,2]))
|
||||
|
||||
def run(self, assignment, algorithm, play, rond):
|
||||
self.assignment = assignment
|
||||
self.algorithm = algorithm
|
||||
self.play = play
|
||||
self.rond = rond
|
||||
self.get_total_reward()
|
||||
if self.visualized:
|
||||
self.visualize()
|
||||
|
||||
def reset(self):
|
||||
self.vehicles_position = np.zeros(self.vehicles_position.shape[0],dtype=np.int32)
|
||||
self.vehicles_lefttime = np.ones(self.vehicles_position.shape[0],dtype=np.float32) * self.time_lim
|
||||
self.targets[:,2] = self.targets_value
|
||||
self.total_reward = 0
|
||||
self.reward = 0
|
||||
self.end = False
|
||||
|
||||
def get_total_reward(self):
|
||||
for i in range(len(self.assignment)):
|
||||
speed = self.vehicles_speed[i]
|
||||
for j in range(len(self.assignment[i])):
|
||||
position = self.targets[self.assignment[i][j],:4]
|
||||
self.total_reward = self.total_reward + position[2]
|
||||
if j == 0:
|
||||
self.vehicles_lefttime[i] = self.vehicles_lefttime[i] - np.linalg.norm(position[:2]) / speed - position[3]
|
||||
else:
|
||||
self.vehicles_lefttime[i] = self.vehicles_lefttime[i] - np.linalg.norm(position[:2]-position_last[:2]) / speed - position[3]
|
||||
position_last = position
|
||||
if self.vehicles_lefttime[i] > self.time_lim:
|
||||
self.end = True
|
||||
break
|
||||
if self.end:
|
||||
self.total_reward = 0
|
||||
break
|
||||
|
||||
def visualize(self):
|
||||
if self.assignment == None:
|
||||
plt.scatter(x=0,y=0,s=200,c='k')
|
||||
plt.scatter(x=self.targets[1:,0],y=self.targets[1:,1],s=self.targets[1:,2]*10,c='r')
|
||||
plt.title('Target distribution')
|
||||
plt.savefig('task_pic/'+self.size+'/'+self.algorithm+ "-%d-%d.png" % (self.play,self.rond))
|
||||
plt.cla()
|
||||
else:
|
||||
plt.title('Task assignment by '+self.algorithm +', total reward : '+str(self.total_reward))
|
||||
plt.scatter(x=0,y=0,s=200,c='k')
|
||||
plt.scatter(x=self.targets[1:,0],y=self.targets[1:,1],s=self.targets[1:,2]*10,c='r')
|
||||
for i in range(len(self.assignment)):
|
||||
trajectory = np.array([[0,0,20]])
|
||||
for j in range(len(self.assignment[i])):
|
||||
position = self.targets[self.assignment[i][j],:3]
|
||||
trajectory = np.insert(trajectory,j+1,values=position,axis=0)
|
||||
plt.scatter(x=trajectory[1:,0],y=trajectory[1:,1],s=trajectory[1:,2]*10,c='b')
|
||||
plt.plot(trajectory[:,0], trajectory[:,1])
|
||||
plt.savefig('task_pic/'+self.size+'/'+self.algorithm+ "-%d-%d.png" % (self.play,self.rond))
|
||||
plt.cla()
|
||||
|
||||
def evaluate(vehicle_num, target_num, map_size):
|
||||
if vehicle_num==6:
|
||||
size='small'
|
||||
|
||||
re_aco=[[] for i in range(2)]
|
||||
for i in range(1):
|
||||
env = Env(vehicle_num,target_num,map_size,visualized=True)
|
||||
for j in range(1):
|
||||
aco = ACO(vehicle_num,target_num,env.vehicles_speed,env.targets,env.time_lim)
|
||||
path_new, time = aco.run()
|
||||
env.run(path_new,'ACO',i+1,j+1)
|
||||
re_aco[i].append((env.total_reward,time))
|
||||
env.reset()
|
||||
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
'''
|
||||
vehicle number: scalar
|
||||
speeds of vehicles: array
|
||||
target number: scalar
|
||||
targets: array, the first line is depot, the first column is x position, the second column is y position, the third column is reward and the forth column is time consumption to finish the mission
|
||||
time limit: scalar
|
||||
'''
|
||||
evaluate(6,30,1e3)
|
||||
|
|
@ -0,0 +1,248 @@
|
|||
# -*- coding: utf-8 -*-
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import collections
|
||||
import heapq
|
||||
import itertools
|
||||
import time
|
||||
from xlrd import open_workbook
|
||||
import matplotlib.pyplot as plt
|
||||
plt.rcParams['font.family'] = "kaiti"
|
||||
import mpl_toolkits.mplot3d
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
def getDist(pos1, pos2):
|
||||
return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
|
||||
|
||||
def heuristic_fun(initparams, k, t):
|
||||
distance = getDist(k, t)
|
||||
need_full_point = distance / 100 * 4 / 4
|
||||
need_comfort_point = distance / 100 * 4 / 4
|
||||
|
||||
return need_comfort_point + need_full_point
|
||||
|
||||
def draw_path(sheet, path):
|
||||
x = sheet.col_values(1)[1:]
|
||||
y = sheet.col_values(2)[1:]
|
||||
z = sheet.col_values(3)[1:]
|
||||
list_length = len(sheet.col_values(4)[1:])
|
||||
food_index = [i for i in range(list_length) if sheet.col_values(4)[1:][i] == 1]
|
||||
fire_index = [i for i in range(list_length) if sheet.col_values(4)[1:][i] == 0]
|
||||
fig = plt.figure(figsize=(10,10))
|
||||
ax = fig.add_subplot(projection = '3d')
|
||||
for i in range(list_length):
|
||||
if i in food_index:
|
||||
ax.scatter(x[i], y[i], z[i], s=10, c='k', marker='.')
|
||||
else:
|
||||
ax.scatter(x[i], y[i], z[i], s=10, c='r', marker='.')
|
||||
ax.set_title('规划路径结果')
|
||||
ax.set_xlabel('x')
|
||||
ax.set_ylabel('y')
|
||||
ax.set_zlabel('z')
|
||||
path = np.array(path)
|
||||
ax.plot3D(path[:, 0].tolist(), path[:, 1].tolist(), path[:, 2].tolist(), 'blue')
|
||||
plt.show()
|
||||
|
||||
class MinheapPQ:
|
||||
'''
|
||||
堆排序返回代价最小的可行点
|
||||
已封装好，使用put向堆加入可行点，使用get获取代价值最小的可行点
|
||||
'''
|
||||
def __init__(self):
|
||||
self.pq = []
|
||||
self.nodes = set()
|
||||
self.entry_finder = {}
|
||||
self.counter = itertools.count()
|
||||
self.REMOVED = '<removed-item>'
|
||||
|
||||
def put(self, item, priority):
|
||||
if item in self.entry_finder:
|
||||
self.check_remove(item)
|
||||
count = next(self.counter)
|
||||
entry = [priority, count, item]
|
||||
self.entry_finder[item] = entry
|
||||
heapq.heappush(self.pq, entry)
|
||||
self.nodes.add(item)
|
||||
|
||||
def check_remove(self, item):
|
||||
if item not in self.entry_finder:
|
||||
return
|
||||
entry = self.entry_finder.pop(item)
|
||||
entry[-1] = self.REMOVED
|
||||
self.nodes.remove(item)
|
||||
|
||||
def get(self):
|
||||
while self.pq:
|
||||
priority, count, item = heapq.heappop(self.pq)
|
||||
if item is not self.REMOVED:
|
||||
del self.entry_finder[item]
|
||||
self.nodes.remove(item)
|
||||
return item
|
||||
raise KeyError('pop from an empty priority queue')
|
||||
|
||||
def top_key(self):
|
||||
return self.pq[0][0]
|
||||
|
||||
def enumerate(self):
|
||||
return self.pq
|
||||
|
||||
def allnodes(self):
|
||||
return self.nodes
|
||||
|
||||
class Weighted_A_star(object):
|
||||
def __init__(self):
|
||||
self.start, self.goal = tuple(np.array([0, 500, 500])), tuple(np.array([1000, 555.67, 442.022]))
|
||||
self.g = {self.start:-20,self.goal:np.inf} # 记录从起点到各个点的g，g值是固定的，当通过另一个点能有一个更小的g时改变
|
||||
self.Parent = {self.start:self.start}
|
||||
self.CLOSED = set()
|
||||
self.Path = []
|
||||
self.OPEN = MinheapPQ() # 存储节点及其代价
|
||||
self.full = {self.start:10}
|
||||
self.comfort = {self.start:10}
|
||||
self.OPEN.put(self.start, self.g[self.start] + heuristic_fun(self, self.start, self.goal)) # item, priority = g + h
|
||||
self.lastpoint = self.start
|
||||
self.lastg = {}
|
||||
self.pathparent = {}
|
||||
|
||||
def run(self):
|
||||
xt = self.goal
|
||||
xi = self.start
|
||||
last_xi = xi
|
||||
ban_xi = {}
|
||||
flag = 0
|
||||
while(1): # 当没有到达终点时循环
|
||||
# 找到一个有可行食物点、篝火点的节点作为当前点
|
||||
xi = self.OPEN.get()
|
||||
distance = getDist(xi, self.Parent[xi])
|
||||
full_change = (np.sign(xi[2] - self.Parent[xi][2]) + 5) * distance / 100
|
||||
reachable_full, reachable_comfort = self.reachable_point(xi)
|
||||
|
||||
while((self.full[xi] > self.comfort[xi] and len(reachable_comfort) < 2 and xi[0] < 300) \
|
||||
or (self.full[xi] <= self.comfort[xi] and len(reachable_full) < 2 and xi[0] < 300) \
|
||||
or (self.full[xi] > self.comfort[xi] and len(reachable_comfort) < 2 and xi[0] < 300) \
|
||||
or (self.full[xi] <= self.comfort[xi] and len(reachable_full) < 2 and xi[0] < 300) \
|
||||
or(self.full[xi] > self.comfort[xi] and len(reachable_comfort) < 1 and xi[0] > 300) \
|
||||
or(self.full[xi] <= self.comfort[xi] and len(reachable_full) < 1 and xi[0] > 300) \
|
||||
or (xi in self.CLOSED) or xi[0] == 397.14 \
|
||||
or xi[0] == 604.287 or xi[0] == 632.907 or (xi[0] == 686.951 and self.Parent[xi][0] == 594.039)):
|
||||
ban_xi[xi] = self.g[xi] + heuristic_fun(self, xi, self.goal)
|
||||
xi = self.OPEN.get()
|
||||
distance = getDist(xi, self.Parent[xi])
|
||||
full_change = (np.sign(xi[2] - self.Parent[xi][2]) + 5) * distance / 100
|
||||
reachable_full, reachable_comfort = self.reachable_point(xi)
|
||||
|
||||
# 对当前点的后续处理
|
||||
print(xi, self.Parent[xi], self.full[xi], self.comfort[xi], len(reachable_full), len(reachable_comfort))
|
||||
self.pathparent[xi] = self.Parent[xi]
|
||||
self.CLOSED.add(xi)
|
||||
for xz in ban_xi:
|
||||
self.OPEN.put(xz, ban_xi[xz])
|
||||
ban_xi = {}
|
||||
if(self.goal in reachable_full or self.goal in reachable_comfort):
|
||||
self.Parent[self.goal] = xi
|
||||
self.pathparent[self.goal] = xi
|
||||
break
|
||||
|
||||
|
||||
if(int(100000 * self.full[xi]) > int(100000 * self.comfort[xi])):
|
||||
self.process_child(xi, reachable_comfort, False)
|
||||
if(int(100000 * self.full[xi]) < int(100000 * self.comfort[xi])):
|
||||
self.process_child(xi, reachable_full, True)
|
||||
if(int(100000 * self.full[xi]) == int(100000 * self.comfort[xi])):
|
||||
self.process_child(xi, reachable_full, True)
|
||||
self.process_child(xi, reachable_comfort, False)
|
||||
|
||||
last_xi = xi
|
||||
flag += 1
|
||||
|
||||
self.lastpoint = xi
|
||||
self.Path = self.path()
|
||||
draw_path(sheet, self.Path)
|
||||
|
||||
|
||||
def process_child(self, xi, points, isfull):
|
||||
# 对当前点后继可行节点的处理
|
||||
for xj in list(points.keys()):
|
||||
if xj not in self.g:
|
||||
self.g[xj] = np.inf
|
||||
distance = getDist(xi, xj)
|
||||
|
||||
if isfull:
|
||||
self.full[xj] = self.full[xi] + points[xj] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
self.comfort[xj] = self.comfort[xi] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
else:
|
||||
self.full[xj] = self.full[xi] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
self.comfort[xj] = self.comfort[xi] + points[xj] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
|
||||
a = self.g[xi] + 1 - self.full[xj] - self.comfort[xj]
|
||||
self.lastg[xj] = self.g[xj]
|
||||
self.g[xj] = a
|
||||
self.Parent[xj] = xi
|
||||
self.OPEN.put(xj, a + heuristic_fun(self, xj, self.goal))
|
||||
|
||||
def path(self):
|
||||
path = []
|
||||
x = self.lastpoint
|
||||
path.append([1000, 555.67, 442.022])
|
||||
i = 1
|
||||
start = self.start
|
||||
while x != start:
|
||||
path.append(list(x))
|
||||
x = self.pathparent[x]
|
||||
i += 1
|
||||
path.append([0, 500, 500])
|
||||
print(path)
|
||||
print(i + 1)
|
||||
return path
|
||||
|
||||
def reachable_point(self, xi):
|
||||
# 输入：当前点坐标
|
||||
# 返回：可行食物点坐标及补充量，可行篝火点坐标及补充量
|
||||
fulldown = (self.full[xi] + 5) / 4 * 100
|
||||
comfortdown = (self.comfort[xi] + 5) / 4 * 100
|
||||
fullup = (self.full[xi] + 5) / 6 * 100
|
||||
comfortup = (self.comfort[xi] + 5) / 6 * 100
|
||||
|
||||
condition = (content[:, 1] <= xi[0] + fulldown) & (content[:, 1] > xi[0] - fulldown) &\
|
||||
(content[:, 2] <= xi[1] + fulldown) & (content[:, 2] >= xi[1] - fulldown) &\
|
||||
(content[:, 3] >= xi[2] - fulldown) & (content[:, 3] <= xi[2] + fullup) &\
|
||||
(content[:, 1] <= xi[0] + comfortdown) & (content[:, 1] > xi[0] - comfortdown) &\
|
||||
(content[:, 2] <= xi[1] + comfortdown) & (content[:, 2] >= xi[1] - comfortdown) &\
|
||||
(content[:, 3] >= xi[2] - comfortdown) & (content[:, 3] <= xi[2] + comfortup) & (content[:, 1] != xi[0])
|
||||
full_condition = condition & (content[:, 4] == 1)
|
||||
comfort_condition = condition & (content[:, 4] == 0)
|
||||
|
||||
reachable_full = dict(zip(list(map(tuple, content[full_condition][:, 1:4])), content[full_condition][:, -1]))
|
||||
reachable_comfort = dict(zip(list(map(tuple, content[comfort_condition][:, 1:4])), content[comfort_condition][:, -1]))
|
||||
|
||||
for xj in list(reachable_full.keys()):
|
||||
distance = getDist(xi, xj)
|
||||
xj_full = self.full[xi] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
xj_comfort = self.comfort[xi] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
if xj == (1000, 555.67, 442.022) and (xj_full <-3 or xj_comfort < -3):
|
||||
del reachable_full[xj]
|
||||
elif(xj_full < -5 or xj_comfort < -5):
|
||||
del reachable_full[xj]
|
||||
for xj in list(reachable_comfort.keys()):
|
||||
distance = getDist(xi, xj)
|
||||
xj_full = self.full[xi] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
xj_comfort = self.comfort[xi] - (np.sign(xj[2] - xi[2]) + 5) * distance / 100
|
||||
if xj == (1000, 555.67, 442.022) and (xj_full <-3 or xj_comfort < -3):
|
||||
del reachable_comfort[xj]
|
||||
elif(xj_full < -5 or xj_comfort < -5):
|
||||
del reachable_comfort[xj]
|
||||
|
||||
return reachable_full, reachable_comfort
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
workbook = open_workbook(r'附件.xlsx')
|
||||
sheet = workbook.sheet_by_index(0)
|
||||
content = np.array([sheet.row_values(i) for i in range(2, 589)])
|
||||
|
||||
Astar = Weighted_A_star()
|
||||
sta = time.time()
|
||||
path = Astar.run()
|
||||
print(time.time() - sta)
|
||||
draw_path(sheet, path)
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
from xlrd import open_workbook
|
||||
import numpy as np
|
||||
import copy
|
||||
|
||||
def getDist(pos1, pos2):
|
||||
return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
|
||||
|
||||
def judge_path(path):
|
||||
full = 10
|
||||
comfort = 10
|
||||
for i in range(1, len(path)):
|
||||
distance = getDist(path[i], path[i - 1])
|
||||
full = full - (np.sign(path[i][2] - path[i-1][2]) + 5) * distance / 100
|
||||
comfort = comfort - (np.sign(path[i][2] - path[i-1][2]) + 5) * distance / 100
|
||||
if(full <= -5 or comfort <= -5):
|
||||
return 0
|
||||
if(content[np.where(content[:, 1] == path[i][0]), 4] == 0):
|
||||
comfort = comfort + content[np.where(content[:, 1] == path[i][0]), 5]
|
||||
else:
|
||||
full = full + content[np.where(content[:, 1] == path[i][0]), 5]
|
||||
if(full > -3 and comfort > -3):
|
||||
if(len(path) == 22):
|
||||
if(path not in all_paths):
|
||||
all_paths.append(path)
|
||||
return 1
|
||||
else:
|
||||
return 0
|
||||
|
||||
|
||||
def compute(all_paths):
|
||||
for w in range(len(all_paths)):
|
||||
|
||||
thispath = all_paths[w]
|
||||
print('**********************************************')
|
||||
print(len(thispath))
|
||||
print(thispath)
|
||||
path = thispath
|
||||
|
||||
full = 10
|
||||
comfort = 10
|
||||
for i in range(1, len(thispath)):
|
||||
distance = getDist(path[i], path[i - 1])
|
||||
full = full - (np.sign(path[i][2] - path[i-1][2]) + 5) * distance / 100
|
||||
comfort = comfort - (np.sign(path[i][2] - path[i-1][2]) + 5) * distance / 100
|
||||
print(full, comfort)
|
||||
if(content[np.where(content[:, 1] == path[i][0]), 4] == 0):
|
||||
comfort = comfort + content[np.where(content[:, 1] == path[i][0]), 5]
|
||||
else:
|
||||
full = full + content[np.where(content[:, 1] == path[i][0]), 5]
|
||||
|
||||
print(full, comfort)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
all_paths = []
|
||||
workbook = open_workbook(r'附件.xlsx')
|
||||
sheet = workbook.sheet_by_index(0)
|
||||
content = np.array([sheet.row_values(i) for i in range(1, 589)])
|
||||
|
||||
path = [[0, 500, 500], [49.2701, 501.378, 474.378], [66.4539, 493.45, 485.579], [205.93, 530.015, 462.304], [325.373, 562.813, 450.432], [341.06, 554.258, 440.06], [396.166, 557.686, 463.285], [414.847, 550.557, 458.839], [432.744, 544.693, 464.194], [466.047, 540.082, 447.639], [497.857, 539.374, 473.068], [518.037, 544.067, 478.518], [594.039, 532.547, 476.9], [649.996, 543.743, 471.353], [668.799, 533.409, 475.24], [686.951, 534.936, 468.021], [703.063, 520.512, 467.263], [739.278, 513.132, 452.663], [758.889, 512.304, 450.352], [803.157, 544.765, 446.707], [866.403, 557.739, 448.524], [904.524, 552.094, 445.804], [965.271, 559.438, 441.67], [979.678, 552.128, 452.517], [1000, 555.67, 442.022]]
|
||||
for k in range(0, len(path) - 2):
|
||||
index1 = int(np.where(content[:, 1] == path[k][0])[0]) + 1
|
||||
index2 = int(np.where(content[:, 1] == path[k + 2][0])[0])
|
||||
change_nodes = content[index1:index2, 1:4].tolist()
|
||||
for j in range(len(change_nodes)):
|
||||
path_copy1 = path.copy()
|
||||
path_copy1[k+1] = change_nodes[j]
|
||||
flag = judge_path(path_copy1)
|
||||
if(flag == 0):
|
||||
continue
|
||||
|
||||
for z in range(1, len(path_copy1) - 1):
|
||||
pathj = path_copy1.copy()
|
||||
del pathj[z]
|
||||
flag = judge_path(pathj)
|
||||
if(flag == 0):
|
||||
continue
|
||||
|
||||
for a in range(1, len(pathj) - 1):
|
||||
patha = pathj.copy()
|
||||
del patha[a]
|
||||
flag = judge_path(patha)
|
||||
if(flag == 0):
|
||||
continue
|
||||
|
||||
for b in range(1, len(patha) - 1):
|
||||
pathb = patha.copy()
|
||||
del pathb[b]
|
||||
judge_path(pathb)
|
||||
if(flag == 0):
|
||||
continue
|
||||
|
||||
print('************************************************************')
|
||||
for i in all_paths:
|
||||
print(i)
|
||||
print(len(all_paths))
|
||||
compute(all_paths)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -3,6 +3,6 @@ python2.7 leader.py $1 $2 &
|
|||
uav_num=1
|
||||
while(( $uav_num< $2 ))
|
||||
do
|
||||
python2.7 follower_consensus.py $1 $uav_num $2&
|
||||
python2.7 follower.py $1 $uav_num $2&
|
||||
let "uav_num++"
|
||||
done
|
||||
|
|
|
|||
Loading…
Reference in New Issue