aah98
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

开始扩充更多机型

This commit is contained in:
Robin Shaun 2020-05-05 21:16:54 +08:00
parent a5b8cdbba6
commit e00541e9f3
13 changed files with 1588 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
communication/1.11/config.yaml Normal file
View File

@ -0,0 +1,9 @@
# number of different kinds of vehicles
rover: 2
plane: 2
typhoon: 2
solo: 3
iris: 3
tiltrotor: 2
tailsitter: 2
standard_vtol: 2

266
communication/1.11/multirotor_communication.py Normal file
View File

@ -0,0 +1,266 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from geometry_msgs.msg import PoseStamped, Pose, TwistStamped, Twist, Vector3Stamped, Vector3
from gazebo_msgs.srv import GetModelState
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
from multiprocessing import Process
import sys
class Communication:
def __init__(self, vehicle_id):
if vehicle_type == 'iris' or 'typhoon' or 'solo':
self.vehicle_type = vehicle_type
else:
print('only support iris, typhoon and solo for multirotors')
sys.exit()
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.takeoff_height = 1
self.hover_flag = 0
self.target_motion = PositionTarget()
self.global_target = None
self.arm_state = False
self.offboard_state = False
self.motion_type = 0
self.flight_mode = None
self.mission = None
self.transition_state = None
self.transition = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.mavros_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_accel_flu", Vector3, self.cmd_accel_flu_callback)
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+str(self.vehicle_id)+"/cmd_accel_enu", Vector3, self.cmd_accel_enu_callback)
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/ground_truth/odom', Odometry, queue_size=10)
'''
ros publishers
'''
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/ground_truth/odom', Odometry, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
if self.vehicle_type == 'tiltrotor' or 'tailsitter' or 'standard_vtol':
self.transition = rospy.ServiceProxy('/mavros/cmd/vtol_transition', CommandVtolTransition)
self.transition_state = 'multirotor'
print(self.transition(state = 3))
print(self.vehicle_type+self.vehicle_id+": "+"communication initialized")
def start(self):
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
rate = rospy.Rate(100)
'''
main ROS thread
'''
while(rospy.is_shutdown):
self.target_motion_pub.publish(self.target_motion)
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
try:
response = self.gazeboModelstate (self.vehicle+self.vehicle_id,'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s"%e
odom = Odometry()
odom.header = response.header
odom.pose.pose = response.pose
odom.twist.twist = response.twist
self.odom_groundtruth_pub.publish(odom)
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
def imu_callback(self, msg):
self.current_heading = self.q2yaw(msg.orientation)
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
if self.vehicle_type == 'iris' or self.vehicle_type = 'solo' or self.vehicle_type = 'typhoon':
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = 1
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
if(self.motion_type == 0):
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
if(self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW
if(self.motion_type == 2):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_YAW
return target_raw_pose
def cmd_pose_flu_callback(self, msg):
self.motion_type = 0
target_x_enu, target_y_enu = self.flu2enu(msg.position.x, msg.position.y)
target_z_enu = msg.position.z
#target_yaw = self.q2yaw(msg.orientation)+self.current_heading
self.target_motion = self.construct_target(x=target_x_enu,y=target_y_enu,z=target_z_enu)
def cmd_pose_enu_callback(self, msg):
self.motion_type = 0
#target_yaw = self.q2yaw(msg.orientation)+self.current_heading
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
def cmd_vel_flu_callback(self, msg):
if self.hover_flag == 0:
self.motion_type = 1
target_vx_enu, target_vy_enu = self.flu2enu(msg.linear.x, msg.linear.y)
target_vz_enu = msg.linear.z
target_yaw_rate = msg.angular.z
self.target_motion = self.construct_target(vx=target_vx_enu,vy=target_vy_enu,vz=target_vz_enu,yaw_rate=target_yaw_rate)
def cmd_vel_enu_callback(self, msg):
if self.hover_flag == 0:
self.motion_type = 1
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)
def cmd_accel_flu_callback(self, msg):
if self.hover_flag == 0:
self.motion_type = 2
target_afx_enu, target_afy_enu = self.flu2enu(msg.x, msg.y)
target_afz_enu = msg.z
self.target_motion = self.construct_target(afx=target_afx_enu,afy=target_afy_enu,afz=target_afz_enu)
def cmd_accel_enu_callback(self, msg):
if self.hover_flag == 0:
self.motion_type = 2
self.target_motion = self.construct_target(afx=msg.x,afy=msg.y,afz=msg.z)
def cmd_callback(self, msg):
if msg.data == '':
return
elif msg.data == 'ARM':
self.arm_state =self.arm()
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
elif msg.data == 'DISARM':
disarm_state =self.disarm()
if disarm_state:
self.arm_state = False
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
self.mission = msg.data
print(self.vehicle_type+self.vehicle_id+": "+msg.data)
elif not msg.data == self.flight_mode:
self.flight_mode = msg.data
self.flight_mode_switch()
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 flu2enu(self, x_flu, y_flu):
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
return x_enu, y_enu
def cmd_yaw(self, yaw):
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
self.target_pose.pose.orientation.x = quaternion[0]
self.target_pose.pose.orientation.y = quaternion[1]
self.target_pose.pose.orientation.z = quaternion[2]
self.target_pose.pose.orientation.w = quaternion[3]
def arm(self):
if self.armService(True):
return True
else:
print(self.vehicle_type+self.vehicle_id+": arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
return False
def hover(self):
self.motion_type = 0
self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z)
def flight_mode_switch(self):
if self.flight_mode == 'HOVER':
self.hover_flag = 1
self.hover()
print(self.vehicle_type+self.vehicle_id+":"+self.flight_mode)
elif self.flightModeService(custom_mode=self.flight_mode):
self.hover_flag = 0
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
return True
else:
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
return False
def takeoff_detection(self):
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
return True
else:
return False
if __name__ == '__main__':
communication = Communication(sys.argv[1],sys.argv[2])
communication.start()

140
communication/1.11/plane_communication.py Normal file
View File

@ -0,0 +1,140 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from geometry_msgs.msg import PoseStamped, Pose
from gazebo_msgs.srv import GetModelState
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
from multiprocessing import Process
import sys
class Communication:
def __init__(self, vehicle_id):
self.vehicle_type = 'plane'
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.target_motion = PositionTarget()
self.global_target = None
self.arm_state = False
self.offboard_state = False
self.motion_type = 0
self.flight_mode = None
self.mission = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
'''
ros publishers
'''
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
print(self.vehicle_type+self.vehicle_id+": "+"communication initialized")
def start(self):
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
rate = rospy.Rate(100)
'''
main ROS thread
'''
while(rospy.is_shutdown):
self.target_motion_pub.publish(self.target_motion)
try:
response = self.gazeboModelstate (self.vehicle+self.vehicle_id,'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s"%e
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg
def imu_callback(self, msg):
self.current_heading = self.q2yaw(msg.orientation)
def construct_target(self, x=0, y=0, z=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = 1
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
if self.mission == 'takeoff':
target_raw_pose.type_mask = 4096
elif self.mission == 'land':
target_raw_pose.type_mask = 8192
elif self.mission == 'loiter':
target_raw_pose.type_mask = 12288
else:
target_raw_pose.type_mask = 16384
return target_raw_pose
def cmd_pose_flu_callback(self, msg):
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=9,motion_type=0)
def cmd_pose_enu_callback(self, msg):
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=1,motion_type=0)
def cmd_callback(self, msg):
if msg.data == '':
return
elif msg.data == 'ARM':
self.arm_state =self.arm()
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
elif msg.data == 'DISARM':
disarm_state =self.disarm()
if disarm_state:
self.arm_state = False
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
else:
self.mission = msg.data
def arm(self):
if self.armService(True):
return True
else:
print(self.vehicle_type+self.vehicle_id+": arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
return False
if __name__ == '__main__':
communication = Communication(sys.argv[1])
communication.start()

192
communication/1.11/rover_communication.py Normal file
View File

@ -0,0 +1,192 @@
import rospy
import tf
import yaml
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
from geometry_msgs.msg import PoseStamped, Pose, TwistStamped, Twist, Vector3Stamped, Vector3
from gazebo_msgs.srv import GetModelState
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
from multiprocessing import Process
import sys
class Communication:
def __init__(self, vehicle_id):
self.vehicle_type = 'rover'
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.target_motion = PositionTarget()
self.global_target = None
self.arm_state = False
self.offboard_state = False
self.motion_type = 0
self.flight_mode = None
self.mission = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.mavros_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
'''
ros publishers
'''
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
print(self.vehicle_type+self.vehicle_id+": "+"communication initialized")
def start(self):
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
rate = rospy.Rate(100)
'''
main ROS thread
'''
while(rospy.is_shutdown):
self.target_motion_pub.publish(self.target_motion)
try:
response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s"%e
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
def imu_callback(self, msg):
self.current_heading = self.q2yaw(msg.orientation)
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, yaw=0, coordinate_frame=1, motion_type=1):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = coordinate_frame
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.yaw = yaw
if(motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ
if(motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ IGNORE_YAW_RATE
return target_raw_pose
def cmd_pose_flu_callback(self, msg):
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=9,motion_type=0)
def cmd_pose_enu_callback(self, msg):
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=1,motion_type=0)
def cmd_vel_flu_callback(self, msg):
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=msg.angular.z,coordinate_frame=8,motion_type=1)
def cmd_vel_enu_callback(self, msg):
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=msg.angular.z,coordinate_frame=1,motion_type=1)
def cmd_callback(self, msg):
if msg.data == '':
return
elif msg.data == 'ARM':
self.arm_state =self.arm()
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
elif msg.data == 'DISARM':
disarm_state =self.disarm()
if disarm_state:
self.arm_state = False
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
self.mission = msg.data
print(self.vehicle_type+self.vehicle_id+": "+msg.data)
elif not msg.data == self.flight_mode:
self.flight_mode = msg.data
self.flight_mode_switch()
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 flu2enu(self, x_flu, y_flu):
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
return x_enu, y_enu
def cmd_yaw(self, yaw):
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
self.target_pose.pose.orientation.x = quaternion[0]
self.target_pose.pose.orientation.y = quaternion[1]
self.target_pose.pose.orientation.z = quaternion[2]
self.target_pose.pose.orientation.w = quaternion[3]
def arm(self):
if self.armService(True):
return True
else:
print(self.vehicle_type+self.vehicle_id+": arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
return False
def flight_mode_switch(self):
if self.flightModeService(custom_mode=self.flight_mode):
self.hover_flag = 0
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
return True
else:
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
return False
if __name__ == '__main__':
communication = Communication(sys.argv[1])
communication.start()

0
communication/__init__.py Normal file
View File

5
communication/px4_communication.py
View File

@ -10,6 +10,7 @@ from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
import sys
class PX4Communication:
@ -25,10 +26,12 @@ class PX4Communication:
self.target_vel = TwistStamped()
self.global_target = None
self.vehicle = sys.argv[1]
self.arm_state = False
self.offboard_state = False
self.flag = 0
self.flight_mode = None
'''
ros subscribers
@ -79,7 +82,7 @@ class PX4Communication:
self.flight_mode = "DISARMED"
try:
response = self.gazeboModelstate ('iris','ground_plane')
response = self.gazeboModelstate (self.vehicle,'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s"%e
odom = Odometry()

4
communication/px4_multi_communication_new.py
View File

@ -42,8 +42,8 @@ class PX4Communication:
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_flu", Vector3, self.cmd_accel_flu_callback)
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_enu", Vector3, self.cmd_accel_enu_callback)
self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_flu", Vector3, self.cmd_accel_flu_callback)
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_enu", Vector3, self.cmd_accel_enu_callback)
self.cmd_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd",String,self.cmd_callback)
'''

17
communication/vtol_transition_test.py Normal file
View File

@ -0,0 +1,17 @@
import rospy
import tf
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandVtolTransition
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
from nav_msgs.msg import Odometry
from gazebo_msgs.srv import GetModelState
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
import sys
trans = rospy.ServiceProxy('/mavros/cmd/vtol_transition', CommandVtolTransition)
print(trans(state = 3))

22
control/1.11/plane_control.py Normal file
View File

@ -0,0 +1,22 @@
import rospy
import tf
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Twist
from nav_msgs.msg import Odometry
from gazebo_msgs.srv import GetModelState
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import String
import time
from pyquaternion import Quaternion
import math
import sys
from px4_communication import PX4Communication
px4_com = PX4Communication()
rospy.init_node("px4_communication")
rate = rospy.Rate(100)
cnt = 0
while(rospy.is_shutdown):
px4_com.pose_target_pub.publish(px4_com.target_pose)
if cnt == 1000:
px4_com.target_pose.pose.position.x = 10

264
coordination/1.11/mulitrotor_keyboard_multi_control copy.py Executable file
View File

@ -0,0 +1,264 @@
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import String
MAX_LIN_VEL = 20
MAX_ANG_VEL = 0.1
LIN_VEL_STEP_SIZE = 0.1
ANG_VEL_STEP_SIZE = 0.01
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 : hover(offboard mode) and remove the mask of keyboard control
k : hover(hover mode) 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 or 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)
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
def makeSimpleProfile(output, input, slop):
if input > output:
output = min( input, output + slop )
elif input < output:
output = max( input, output - slop )
else:
output = input
return output
def constrain(input, low, high):
if input < low:
input = low
elif input > high:
input = high
else:
input = input
return input
def checkLinearLimitVelocity(vel):
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
return vel
def checkAngularLimitVelocity(vel):
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
return vel
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rover_num = int(sys.argv[1])
rospy.init_node('keyboard_control')
multi_cmd_vel_flu_pub = [None]*rover_num
multi_cmd_pub = [None]*rover_num
for i in range(rover_num):
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd',String,queue_size=10)
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
cmd= String()
twist = Twist()
target_forward_vel = 0.0
target_leftward_vel = 0.0
target_upward_vel = 0.0
target_angular_vel = 0.0
control_forward_vel = 0.0
control_leftward_vel = 0.0
control_upward_vel = 0.0
control_angular_vel = 0.0
count = 0
try:
print_msg()
while(1):
key = getKey()
if key == 'w' :
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'x' :
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'a' :
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'd' :
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'i' :
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == ',' :
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'j':
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'l':
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
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 == 'k':
cmd = 'HOVER'
cmd_vel_mask = False
print_msg()
print('Hover')
elif key == 's' :
cmd_vel_mask = False
target_forward_vel = 0.0
target_leftward_vel = 0.0
target_upward_vel = 0.0
target_angular_vel = 0.0
control_forward_vel = 0.0
control_leftward_vel = 0.0
control_upward_vel = 0.0
control_angular_vel = 0.0
print_msg()
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
else:
for i in range(10):
if key == str(i):
cmd = 'mission'+key
print_msg()
print(cmd)
cmd_vel_mask = True
if (key == '\x03'):
break
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
for i in range(rover_num):
if ctrl_leader:
leader_cmd_vel_pub.publish(twist)
leader_cmd_pub.publish(cmd)
else:
if not cmd_vel_mask:
multi_cmd_vel_flu_pub[i].publish(twist)
multi_cmd_pub[i].publish(cmd)
cmd = ''
except:
print(e)
finally:
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
cmd = ''
for i in range(rover_num):
multi_cmd_vel_flu_pub[i].publish(twist)
multi_cmd_pub[i].publish(cmd)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

264
coordination/1.11/rover_keyboard_multi_control.py Executable file
View File

@ -0,0 +1,264 @@
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
import tty, termios
from std_msgs.msg import String
MAX_LIN_VEL = 20
MAX_ANG_VEL = 0.1
LIN_VEL_STEP_SIZE = 0.1
ANG_VEL_STEP_SIZE = 0.01
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 : hover(offboard mode) and remove the mask of keyboard control
k : hover(hover mode) 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 or 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)
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
def makeSimpleProfile(output, input, slop):
if input > output:
output = min( input, output + slop )
elif input < output:
output = max( input, output - slop )
else:
output = input
return output
def constrain(input, low, high):
if input < low:
input = low
elif input > high:
input = high
else:
input = input
return input
def checkLinearLimitVelocity(vel):
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
return vel
def checkAngularLimitVelocity(vel):
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
return vel
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rover_num = int(sys.argv[1])
rospy.init_node('keyboard_control')
multi_cmd_vel_flu_pub = [None]*rover_num
multi_cmd_pub = [None]*rover_num
for i in range(rover_num):
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd',String,queue_size=10)
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
cmd= String()
twist = Twist()
target_forward_vel = 0.0
target_leftward_vel = 0.0
target_upward_vel = 0.0
target_angular_vel = 0.0
control_forward_vel = 0.0
control_leftward_vel = 0.0
control_upward_vel = 0.0
control_angular_vel = 0.0
count = 0
try:
print_msg()
while(1):
key = getKey()
if key == 'w' :
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'x' :
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'a' :
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'd' :
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'i' :
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == ',' :
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'j':
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
elif key == 'l':
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
print_msg()
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
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 == 'k':
cmd = 'HOVER'
cmd_vel_mask = False
print_msg()
print('Hover')
elif key == 's' :
cmd_vel_mask = False
target_forward_vel = 0.0
target_leftward_vel = 0.0
target_upward_vel = 0.0
target_angular_vel = 0.0
control_forward_vel = 0.0
control_leftward_vel = 0.0
control_upward_vel = 0.0
control_angular_vel = 0.0
print_msg()
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
else:
for i in range(10):
if key == str(i):
cmd = 'mission'+key
print_msg()
print(cmd)
cmd_vel_mask = True
if (key == '\x03'):
break
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
for i in range(rover_num):
if ctrl_leader:
leader_cmd_vel_pub.publish(twist)
leader_cmd_pub.publish(cmd)
else:
if not cmd_vel_mask:
multi_cmd_vel_flu_pub[i].publish(twist)
multi_cmd_pub[i].publish(cmd)
cmd = ''
except:
print(e)
finally:
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
cmd = ''
for i in range(rover_num):
multi_cmd_vel_flu_pub[i].publish(twist)
multi_cmd_pub[i].publish(cmd)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

116
coordination/launch/multi_uav.launch
View File

@ -196,4 +196,120 @@
</include>
</group>
<!-- UAV7-->
<group ns="uav7">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="7"/>
<!--default="14540@localhost:14557"/>-->
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.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="$(arg vehicle)"/>
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_7"/>
<arg name="mavlink_udp_port" value="24572"/>
<arg name="ID" value="$(arg ID)"/>
</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="$(arg ID)"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- UAV8-->
<group ns="uav8">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="8"/>
<!--default="14540@localhost:14557"/>-->
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34586"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="-7"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_8"/>
<arg name="mavlink_udp_port" value="24574"/>
<arg name="ID" value="$(arg ID)"/>
</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="$(arg ID)"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- UAV9-->
<group ns="uav9">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="9"/>
<!--default="14540@localhost:14557"/>-->
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34588"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="8"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_9"/>
<arg name="mavlink_udp_port" value="24576"/>
<arg name="ID" value="$(arg ID)"/>
</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="$(arg ID)"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- UAV10-->
<group ns="uav10">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="10"/>
<!--default="14540@localhost:14557"/>-->
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34590"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.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="$(arg vehicle)"/>
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_10"/>
<arg name="mavlink_udp_port" value="24578"/>
<arg name="ID" value="$(arg ID)"/>
</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="$(arg ID)"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>

292
coordination/launch_1.9/multi_uav_10.launch Normal file
View File

@ -0,0 +1,292 @@
<?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="vehicle" default="iris"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.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>
<!-- UAV0 -->
<group ns="uav0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="fcu_url" default="udp://:14540@localhost:34571"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="-1"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24560"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV1-->
<group ns="uav1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="fcu_url" default="udp://:14541@127.0.0.1:34572"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="1"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24562"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV2-->
<group ns="uav2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="-2"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24564"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV3-->
<group ns="uav3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="3"/>
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.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="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24566"/>
<arg name="mavlink_tcp_port" value="4563"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV4-->
<group ns="uav4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="4"/>
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="-4"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24568"/>
<arg name="mavlink_tcp_port" value="4564"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV5-->
<group ns="uav5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="5"/>
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="5"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24570"/>
<arg name="mavlink_tcp_port" value="4565"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV6-->
<group ns="uav6">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="6"/>
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.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="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24572"/>
<arg name="mavlink_tcp_port" value="4566"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV7-->
<group ns="uav7">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="7"/>
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="7"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24574"/>
<arg name="mavlink_tcp_port" value="4567"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV8-->
<group ns="uav8">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="8"/>
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0"/>
<arg name="y" value="-8"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24576"/>
<arg name="mavlink_tcp_port" value="4568"/>
<arg name="ID" value="$(arg ID)"/>
</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>
<!-- UAV9-->
<group ns="uav9">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="9"/>
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34588"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.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="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="24578"/>
<arg name="mavlink_tcp_port" value="4569"/>
<arg name="ID" value="$(arg ID)"/>
</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>