modify formation_demo
This commit is contained in:
parent
30326783ef
commit
1e22feaf92
|
|
@ -0,0 +1,277 @@
|
|||
import rospy
|
||||
from mavros_msgs.msg import State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, Pose, Twist
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import math
|
||||
import numpy
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
class Communication:
|
||||
|
||||
def __init__(self, vehicle_type, vehicle_id):
|
||||
|
||||
self.vehicle_type = vehicle_type
|
||||
self.vehicle_id = vehicle_id
|
||||
self.current_position = None
|
||||
self.current_yaw = 0
|
||||
self.hover_flag = 0
|
||||
self.target_motion = PositionTarget()
|
||||
self.arm_state = False
|
||||
self.motion_type = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
initial pose (simutaneously set in indoor?.launch)
|
||||
'''
|
||||
#self.x_origin = 0
|
||||
#self.y_origin = 0
|
||||
#self.z_origin = 0
|
||||
#self.r_origin = 0
|
||||
#self.p_origin = 0
|
||||
#self.y_origin = 0
|
||||
|
||||
'''
|
||||
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.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+'_'+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_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback)
|
||||
|
||||
'''
|
||||
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/'+self.vehicle_type+'_'+self.vehicle_id+'/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)
|
||||
|
||||
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
|
||||
'''
|
||||
# count = 0
|
||||
while not rospy.is_shutdown():
|
||||
# count = count + 1
|
||||
self.target_motion_pub.publish(self.target_motion)
|
||||
|
||||
if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
|
||||
if(self.disarm()):
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
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
|
||||
odom = Odometry()
|
||||
odom.header = response.header
|
||||
odom.pose.pose = response.pose
|
||||
odom.twist.twist = response.twist
|
||||
self.odom_groundtruth_pub.publish(odom)
|
||||
|
||||
# if count >= 10:
|
||||
# bc = tf2.TransformBroadcaster()
|
||||
# # pO = (self.x_origin, self.y_origin, self.z_origin, 0)
|
||||
# # qO = tfc.transformations.quaternion_from_euler(self.r_origin, self.p_origin, self.y_origin)
|
||||
# p02C = (response.pose.position.x, response.pose.position.y, response.pose.position.z, 0)
|
||||
# q02C = (response.pose.orientation.x, response.pose.orientation.y, response.pose.orientation.z, response.pose.orientation.w)
|
||||
# transform = self.__genTf("current_state", "state_0", p02C, q02C)
|
||||
# bc.sendTransform(transform)
|
||||
# count = 0
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def __genTf(self, frameT, frameS, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameT
|
||||
ts.child_frame_id = frameS
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.current_position = msg.pose.position
|
||||
self.current_yaw = self.q2yaw(msg.pose.orientation)
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
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):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.coordinate_frame = self.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.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_RATE
|
||||
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.coordinate_frame = 9
|
||||
self.motion_type = 0
|
||||
yaw = self.q2yaw(msg.orientation)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.coordinate_frame = 1
|
||||
self.motion_type = 0
|
||||
yaw = self.q2yaw(msg.orientation)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 8
|
||||
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_vel_enu_callback(self, msg):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 1
|
||||
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):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 8
|
||||
self.motion_type = 2
|
||||
self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
|
||||
|
||||
def cmd_accel_enu_callback(self, msg):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 1
|
||||
self.motion_type = 2
|
||||
self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
|
||||
|
||||
def hover_state_transition(self,x,y,z,w):
|
||||
if abs(x) > 0.005 or abs(y) > 0.005 or abs(z) > 0.005 or abs(w) > 0.005:
|
||||
self.hover_flag = 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':
|
||||
self.arm_state = not self.disarm()
|
||||
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 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.coordinate_frame = 1
|
||||
self.motion_type = 0
|
||||
self.target_motion = self.construct_target(x=self.current_position.x,y=self.current_position.y,z=self.current_position.z,yaw=self.current_yaw)
|
||||
|
||||
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):
|
||||
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.current_position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
communication = Communication('iris', '0') # Communication(sys.argv[1],sys.argv[2])
|
||||
communication.start()
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
import rospy
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
import math
|
||||
import numpy
|
||||
|
||||
def qRot(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(q, p), qInv)
|
||||
return pNew
|
||||
|
||||
def qRotInv(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(qInv, p), q)
|
||||
return pNew
|
||||
|
||||
|
||||
def __genTf(frameT, frameS, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameT
|
||||
ts.child_frame_id = frameS
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def __main():
|
||||
rospy.init_node("A2C")
|
||||
rate = rospy.Rate(1)
|
||||
tbf = tf2.Buffer()
|
||||
tl = tf2.TransformListener(tbf)
|
||||
|
||||
a = 0
|
||||
pp = 0
|
||||
while not tbf.can_transform("C", "D", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{C-D}"
|
||||
rate.sleep()
|
||||
|
||||
while not tbf.can_transform("D", "C", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{D-C}"
|
||||
rate.sleep()
|
||||
|
||||
while not tbf.can_transform("A", "B", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{A-B}"
|
||||
rate.sleep()
|
||||
|
||||
while not tbf.can_transform("B", "A", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{B-A}"
|
||||
rate.sleep()
|
||||
|
||||
print ("all static tf ready")
|
||||
|
||||
roundCount = 0
|
||||
rateFast = rospy.Rate(10)
|
||||
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
while not rospy.is_shutdown():
|
||||
print("begin: " + str(roundCount))
|
||||
roundCount += 1
|
||||
# lookup_transform(desFrame, srcFrame, time)
|
||||
tfD2C = tbf.lookup_transform("C", "D", rospy.Time())
|
||||
tfC2D = tbf.lookup_transform("D", "C", rospy.Time())
|
||||
tfB2A = tbf.lookup_transform("A", "B", rospy.Time())
|
||||
tfA2B = tbf.lookup_transform("B", "A", rospy.Time())
|
||||
|
||||
bc = tf2.TransformBroadcaster()
|
||||
|
||||
tD2B = (2 * math.cos(a), 1 + 2 * math.sin(a), -1, 0)
|
||||
qD2B = tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)
|
||||
|
||||
ts = __genTf("B", "DB", tD2B, qD2B)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
# db -> da
|
||||
tfm = tfB2A
|
||||
r = tfm.transform.rotation
|
||||
ts = tfm.transform.translation
|
||||
qB2A = numpy.array([r.x, r.y, r.z, r.w])
|
||||
tB2A = numpy.array([ts.x, ts.y, ts.z, 0])
|
||||
qD2A = qMultiply(qB2A, qD2B)
|
||||
tD2A = qRot(tD2B, qB2A) + tB2A
|
||||
|
||||
ts = __genTf("A", "DA", tD2A, qD2A)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
# da -> ca
|
||||
tfm = tfC2D
|
||||
r = tfm.transform.rotation
|
||||
ts = tfm.transform.translation
|
||||
qC2D = numpy.array([r.x, r.y, r.z, r.w])
|
||||
tC2D = numpy.array([ts.x, ts.y, ts.z, 0])
|
||||
qC2A = qMultiply(qD2A, qC2D)
|
||||
tC2A = qRot(tC2D, qD2A) + tD2A
|
||||
ts = __genTf("A", "C", tC2A, qC2A)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
if a < math.pi * 2:
|
||||
a += 0.01
|
||||
else:
|
||||
a = 0
|
||||
|
||||
rateFast.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
__main()
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
import rospy
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
import math
|
||||
import numpy
|
||||
|
||||
def __genTf(frameT, frameS, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameT
|
||||
ts.child_frame_id = frameS
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def __main():
|
||||
rospy.init_node("AB2CD")
|
||||
rate = rospy.Rate(100)
|
||||
tbf = tf2.Buffer()
|
||||
|
||||
t = TFMessage()
|
||||
sbc = tf2.StaticTransformBroadcaster()
|
||||
qA2B = tfc.transformations.quaternion_from_euler(math.pi, 0, math.pi / 2)
|
||||
ts1 = __genTf("A", "B", (-1, 0, 0.2), qA2B)
|
||||
t.transforms.append(ts1)
|
||||
|
||||
qC2D = tfc.transformations.quaternion_from_euler(math.pi, 0, 0)
|
||||
ts2 = __genTf("C", "D", (0, 0, 1), qC2D)
|
||||
t.transforms.append(ts2)
|
||||
|
||||
sbc.pub_tf.publish(t)
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
__main()
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
|
||||
#vrpn节点，相当于驱动，转换数据到ros话题
|
||||
roscd vrpn_client_ros/
|
||||
cd launch
|
||||
roslaunch sample.launch
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,491 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
from nav_msgs.msg import Odometry
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import math
|
||||
import numpy as np
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
MAX_LINEAR = 0.6
|
||||
MAX_ANG_VEL = 0.25 #0.15
|
||||
LINEAR_STEP_SIZE = 0.01
|
||||
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/k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
o : init---offboard, arm, takeoff and hover
|
||||
p : pose control
|
||||
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():
|
||||
print(msg2all)
|
||||
|
||||
def current_state_callback(data):
|
||||
global cur_state
|
||||
cur_state = data
|
||||
|
||||
def target_state_callback(data):
|
||||
global tar_state
|
||||
tar_state = data
|
||||
|
||||
def qprod(q1, q2):
|
||||
return tfc.transformations.quaternion_multiply(q1, q2)
|
||||
|
||||
# def qqprod(q1, q2):
|
||||
# return np.hstack((q1[3]*q2[:3]+q2[3]*q1[:3]+np.cross(q1[:3], q2[:3]), q1[3]*q2[3] - np.dot(q1[:3], q2[:3])))
|
||||
|
||||
def qconj(q):
|
||||
return np.hstack((-q[:3],q[3]))
|
||||
|
||||
def qAd(q, p):
|
||||
return qprod(qprod(q, p), qconj(q))
|
||||
|
||||
def qcrossProd(p1, p2):
|
||||
return np.hstack((np.cross(p1[:3], p2[:3]), [0]))
|
||||
|
||||
def qdotProd(p1, p2):
|
||||
return np.array([p1[0]*p2[0], p1[1]*p2[1], p1[2]*p2[2], 0])
|
||||
|
||||
def dqdotProd(xi1, xi2):
|
||||
return np.hstack((qdotProd(xi1[:4], xi2[:4]), qdotProd(xi1[4:8], xi2[4:8])))
|
||||
|
||||
def dqprod(dq1, dq2):
|
||||
return np.hstack((qprod(dq1[:4],dq2[:4]),qprod(dq1[4:8],dq2[:4])+qprod(dq1[:4],dq2[4:8])))
|
||||
|
||||
def dqconj(dq):
|
||||
return np.hstack((qconj(dq[:4]), qconj(dq[4:8])))
|
||||
|
||||
def dqAd(dq, xi):
|
||||
return dqprod(dqprod(dq, xi), dqconj(dq))
|
||||
|
||||
def dqconstr(q, p_s):
|
||||
return np.hstack((q, qprod(p_s, q)/2))
|
||||
|
||||
def lambdadq(dq):
|
||||
if dq[3] >= 0:
|
||||
return dq
|
||||
else:
|
||||
return -dq
|
||||
|
||||
def q2nvartheta(q):
|
||||
if q[3] > 1:
|
||||
q[3] = 1
|
||||
elif q[3] < -1:
|
||||
q[3] = -1
|
||||
vartheta = 2 * np.arccos(q[3])
|
||||
sin_varthataD2 = np.sin(vartheta/2)
|
||||
if sin_varthataD2 == 0:
|
||||
sin_varthataD2 = sin_varthataD2 + 0.0000001
|
||||
n = np.array([q[0]/sin_varthataD2, q[1]/sin_varthataD2, q[2]/sin_varthataD2])
|
||||
return np.hstack((n, np.array(vartheta)))
|
||||
|
||||
def dq2nvarthetaps(dq):
|
||||
p_s = qprod(2 * dq[4:8], qconj(dq[:4]))
|
||||
return np.hstack((q2nvartheta(dq[:4]), p_s))
|
||||
|
||||
def dqln(dq):
|
||||
nvarthetaps = dq2nvarthetaps(dq)
|
||||
return np.hstack((nvarthetaps[:3]*nvarthetaps[3]*0.5, [0], nvarthetaps[4:8]*0.5))
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
multirotor_type = 'iris' #sys.argv[1]
|
||||
multirotor_num = 1 #int(sys.argv[2])
|
||||
control_type = 'vel' #sys.argv[3]
|
||||
|
||||
# if multirotor_num == 18:
|
||||
# formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond']
|
||||
# elif multirotor_num == 9:
|
||||
# formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
# elif multirotor_num == 6:
|
||||
# formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
rospy.init_node('multirotor_keyboard_UDQ_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)
|
||||
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/odom", Odometry, current_state_callback)
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/target", Odometry, current_state_callback)
|
||||
global tar_state
|
||||
tar_state = Odometry()
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
upward = 0.0
|
||||
angular = 0.0
|
||||
count = 0
|
||||
print_msg()
|
||||
while not rospy.is_shutdown(): #(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')
|
||||
elif key == 'o':
|
||||
#offboard
|
||||
cmd = 'OFFBOARD'
|
||||
print('Offboard')
|
||||
# while count < 5:
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# count = count + 1
|
||||
# rospy.Rate(5).sleep()
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
rospy.sleep(1.0)
|
||||
count = 0
|
||||
#arm
|
||||
cmd = 'ARM'
|
||||
print('Arming')
|
||||
# while count < 5:
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# count = count + 1
|
||||
# rospy.Rate(5).sleep()
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
rospy.sleep(1.0)
|
||||
count = 0
|
||||
#takeoff
|
||||
cmd = ''
|
||||
upward = 0.4
|
||||
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
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
print('takingoff')
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
while cur_state.pose.pose.position.z < 2.0 :
|
||||
rospy.Rate(5).sleep()
|
||||
# while cur_pose.pose.pose.position.z < 1.5 :
|
||||
# multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# rospy.Rate(5).sleep()
|
||||
#hover
|
||||
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')
|
||||
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
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
elif key == 'p':
|
||||
# set
|
||||
dk = np.array([1,1,2,0,2,1,1,0])
|
||||
tbf = tf2.Buffer()
|
||||
listener = tf2.TransformListener(tbf)
|
||||
pdot_bsat_last = np.array([0,0,0,0])
|
||||
omega_bsat_last = np.array([0,0,0,0])
|
||||
while getKey() != 's' :
|
||||
#####listen to target pose
|
||||
tf02t = geometry_msgs.msg.TransformStamped()
|
||||
try:
|
||||
tf02t = tbf.lookup_transform("state_0", "target_state", rospy.Time())
|
||||
except(tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException):
|
||||
rospy.Rate(10).sleep()
|
||||
rot02t = tf02t.transform.rotation
|
||||
tsl02t = tf02t.transform.translation
|
||||
#####tar_pose
|
||||
q_d = np.array([rot02t.x, rot02t.y, rot02t.z, rot02t.w])
|
||||
# print("destiny:\t q_d x %.2f\t q_d y %.2f\t q_d z %.2f\t q_d w %.2f" % (
|
||||
# q_d[0], q_d[1], q_d[2], q_d[3]))
|
||||
p_sd = np.array([tsl02t.x, tsl02t.y, tsl02t.z, 0])
|
||||
# print("destiny:\t p_sd x %.2f\t p_sd y %.2f\t p_sd z %.2f" % (
|
||||
# p_sd[0], p_sd[1], p_sd[2]))
|
||||
#####tar_state
|
||||
lin02t = tar_state.twist.twist.linear
|
||||
ang02t = tar_state.twist.twist.angular
|
||||
omega_sd = np.array([ang02t.x, ang02t.y, ang02t.z, 0])
|
||||
pdot_sd = np.array([lin02t.x, lin02t.y, lin02t.z, 0])
|
||||
# omega_sd = np.array([0, 0, 0, 0])
|
||||
# pdot_sd = np.array([0, 0, 0, 0])
|
||||
xi_sd = np.hstack((omega_sd, pdot_sd + qcrossProd(p_sd, omega_sd)))
|
||||
#####callback from current state
|
||||
tsl02c = cur_state.pose.pose.position
|
||||
rot02c = cur_state.pose.pose.orientation
|
||||
# lin02c = cur_state.twist.twist.linear
|
||||
# ang02c = cur_state.twist.twist.angular
|
||||
######cur_pose and cur_twist
|
||||
q = np.array([rot02c.x, rot02c.y, rot02c.z, rot02c.w])
|
||||
# print("currently:\t q x %.2f\t q y %.2f\t q z %.2f\t q w %.2f" % (
|
||||
# q[0], q[1], q[2], q[3]))
|
||||
p_s = np.array([tsl02c.x, tsl02c.y, tsl02c.z, 0])
|
||||
# print("currently:\t p_s x %.2f\t p_s y %.2f\t p_s z %.2f" % (
|
||||
# p_s[0], p_s[1], p_s[2]))
|
||||
# omega_sC = np.array([ang02c.x, ang02c.y, ang02c.z, 0])
|
||||
# pdot_sC = np.array([lin02c.x, lin02c.y, lin02c.z, 0])
|
||||
#####control scheme
|
||||
q_er = qprod(q_d, qconj(q))
|
||||
# print("error:\t q_er x %.2f\t q_er y %.2f\t q_er z %.2f\t q_er w %.2f" % (
|
||||
# q_er[0], q_er[1], q_er[2], q_er[3]))
|
||||
# omega_se = omega_sd - qAd(q_er, omega_s)
|
||||
p_se = p_sd - qAd(q_er, p_s)
|
||||
# print("error:\t p_se x %.2f\t p_se y %.2f\t p_se z %.2f" % (
|
||||
# p_se[0], p_se[1], p_se[2]))
|
||||
# pdot_se = pdot_sd - qcrossProd(omega_se, qAd(q_er, p_s)) - qAd(q_er, pdot_s)
|
||||
dq_er = dqconstr(q_er, p_se)
|
||||
xi_s = dqAd(dqconj(dq_er), xi_sd + 2 * dqdotProd(dk, dqln(lambdadq(dq_er))))
|
||||
omega_s = xi_s[:4]
|
||||
pdot_s = xi_s[4:8] + qcrossProd(p_s, omega_s)
|
||||
# print(
|
||||
# "currently:\t linear vel x %.2f\t linear vel y %.2f\t linear vel z %.2f\t angular vel x %.2f\t angular vel y %.2f\t angular vel z %.2f" % (
|
||||
# pdot_s[0], pdot_s[1], pdot_s[2], omega_s[0], omega_s[1], omega_s[2]))
|
||||
#####saturate
|
||||
ang = np.sqrt(np.dot(omega_s, omega_s.T))
|
||||
lin = np.sqrt(np.dot(pdot_s, pdot_s.T))
|
||||
# print("linear vel: %.2f\t angular vel: %.2f" %(lin, ang))
|
||||
if ang > MAX_ANG_VEL:
|
||||
omega_ssat = omega_s/ang*MAX_ANG_VEL
|
||||
else:
|
||||
omega_ssat = omega_s
|
||||
# omega_ssat = omega_s
|
||||
if lin > MAX_LINEAR:
|
||||
pdot_ssat = pdot_s/lin*MAX_LINEAR
|
||||
else:
|
||||
pdot_ssat = pdot_s
|
||||
# pdot_ssat = pdot_s
|
||||
# # print("omega_ssat: %.2f\t %.2f\t %.2f\t pdot_ssat: %.2f\t %.2f\t %.2f" % (omega_ssat[0], omega_ssat[1],omega_ssat[2], pdot_ssat[0],pdot_ssat[1],pdot_ssat[2]))
|
||||
#####convert ang vel and lin vel to body frame
|
||||
pdot_bsat = qAd(qconj(q), pdot_ssat)
|
||||
omega_bsat = qAd(qconj(q), omega_ssat)
|
||||
#####Compare to last vel cmd supply && make vel cmd smooth
|
||||
if (pdot_bsat[0] == 0) & (pdot_bsat[1] == 0):
|
||||
pdot_bsat = pdot_bsat_last
|
||||
else:
|
||||
pdot_inc = pdot_bsat - pdot_bsat_last
|
||||
pdot_inc_mag = np.sqrt(np.dot(pdot_inc, pdot_inc.T))
|
||||
if pdot_inc_mag > MAX_LINEAR*0.1:
|
||||
pdot_bsat = pdot_bsat_last + pdot_inc/pdot_inc_mag*MAX_LINEAR*0.1
|
||||
if (omega_bsat[2] == 0) & (omega_bsat[1] == 0):
|
||||
omega_bsat = omega_bsat_last
|
||||
else:
|
||||
omega_inc = omega_bsat - omega_bsat_last
|
||||
omega_inc_mag = np.sqrt(np.dot(omega_inc, omega_inc.T))
|
||||
if omega_inc_mag > MAX_ANG_VEL*0.1:
|
||||
omega_bsat = omega_bsat_last + omega_inc/omega_inc_mag*MAX_ANG_VEL*0.1
|
||||
####output
|
||||
cmd = ''
|
||||
twist.linear.x = pdot_bsat[0]
|
||||
twist.linear.y = pdot_bsat[1]
|
||||
twist.linear.z = pdot_bsat[2]
|
||||
twist.angular.x = omega_bsat[0]
|
||||
twist.angular.y = omega_bsat[1]
|
||||
twist.angular.z = omega_bsat[2]
|
||||
print('control scheme: press "s" to hover')
|
||||
print("currently:\t linear vel x %.2f\t linear vel y %.2f\t linear vel z %.2f\t angular vel x %.2f\t angular vel y %.2f\t angular vel z %.2f" % (
|
||||
twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z))
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
#####remember
|
||||
pdot_bsat_last = pdot_bsat
|
||||
omega_bsat_last = omega_bsat
|
||||
rospy.Rate(100).sleep()
|
||||
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_accel_flu_pub.publish(twist)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
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,149 @@
|
|||
import rospy
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
from geometry_msgs.msg import PoseStamped, TransformStamped
|
||||
from tf2_msgs.msg import TFMessage
|
||||
import math
|
||||
import numpy
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
def current_state_callback(data):
|
||||
global cur_state
|
||||
cur_state = data
|
||||
# print('upward vel %.2f', cur_state.pose.pose.orientation.x)
|
||||
|
||||
def qRot(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(q, p), qInv)
|
||||
return pNew
|
||||
|
||||
def qRotInv(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(qInv, p), q)
|
||||
return pNew
|
||||
|
||||
|
||||
def __genTf(frameS, frameT, posTs, q):
|
||||
ts = TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameS
|
||||
ts.child_frame_id = frameT
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def __main():
|
||||
rospy.init_node("tf_broadcaster")
|
||||
# rate = rospy.Rate(1)
|
||||
# tbf = tf2.Buffer()
|
||||
# tl = tf2.TransformListener(tbf)
|
||||
|
||||
a = 0
|
||||
|
||||
rospy.Subscriber("/vrpn_client_node/parallel_f450_3/pose", PoseStamped, current_state_callback)
|
||||
target_groundtruth_pub = rospy.Publisher(
|
||||
"/xtdrone/iris_0/ground_truth/target", Odometry, queue_size=10)
|
||||
global cur_state
|
||||
cur_state = PoseStamped()
|
||||
roundCount = 0
|
||||
rateFast = rospy.Rate(10)
|
||||
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
while not rospy.is_shutdown():
|
||||
print("begin: " + str(roundCount))
|
||||
roundCount += 1
|
||||
# # lookup_transform(desFrame, srcFrame, time)
|
||||
# tfD2C = tbf.lookup_transform("C", "D", rospy.Time())
|
||||
# tfC2D = tbf.lookup_transform("D", "C", rospy.Time())
|
||||
# tfB2A = tbf.lookup_transform("A", "B", rospy.Time())
|
||||
# tfA2B = tbf.lookup_transform("B", "A", rospy.Time())
|
||||
|
||||
bc = tf2.TransformBroadcaster()
|
||||
# # t20
|
||||
#####set-point
|
||||
# p02t = numpy.array([1, 2, 2, 0])
|
||||
# q02t = tfc.transformations.quaternion_from_euler(0, 0, math.pi/2)
|
||||
#####tracking
|
||||
# p02t = numpy.array([2 * math.cos(a), -1 + 2 * math.sin(a), 1, 0])
|
||||
# q02t = numpy.array(tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)) #math.pi / 2 +
|
||||
p02t = numpy.array([2 * math.cos(a), -1 + 2 * math.sin(a), 2+math.sin(5*a), 0])
|
||||
q02t = numpy.array(tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)) #math.pi / 2 +
|
||||
ts = __genTf("state_0", "target_state", p02t, q02t)
|
||||
# qt20 = qInvert(q02t)
|
||||
# pt20 = qRotInv(-p02t, q02t)
|
||||
# ts = __genTf("target_state", "state_0", pt20, qt20)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
if cur_state.pose.orientation.x != 0.0:
|
||||
# # 02c
|
||||
p02c = numpy.array([cur_state.pose.position.x, cur_state.pose.position.y, cur_state.pose.position.z, 0])
|
||||
q02c = (cur_state.pose.orientation.x, cur_state.pose.orientation.y, cur_state.pose.orientation.z, cur_state.pose.orientation.w)
|
||||
# print("currently:\t x %.2f\t y %.2f\t z %.2f" % (
|
||||
# cur_state.pose.pose.position.x, cur_state.pose.pose.position.y, cur_state.pose.pose.position.z))
|
||||
# ts = __genTf("current_state", "state_0", p02c, q02c)
|
||||
# bc.sendTransform(ts)
|
||||
# # c20
|
||||
pc20 = qRotInv(-p02c, q02c)
|
||||
qc20 = qInvert(q02c)
|
||||
ts = __genTf("current_state", "state_0", pc20, qc20)
|
||||
bc.sendTransform(ts)
|
||||
# # # c2t
|
||||
qc2t = qMultiply(qInvert(q02c), q02t)
|
||||
print("qc2t %.2f\t %.2f\t %.2f\t %.2f", qc2t[0], qc2t[1],qc2t[2],qc2t[3])
|
||||
pc2t = qRotInv((p02t - p02c), q02c)
|
||||
ts = __genTf("current_state", "target_state1", pc2t, qc2t)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
#####set-point
|
||||
# qdot02t = numpy.array([0, 0, 0, 0])
|
||||
# pdot02t = numpy.array([0, 0, 0, 0])
|
||||
#####tracking
|
||||
# qdot02t = numpy.array([0, 0, 0.05*numpy.cos(a/2), -0.05*numpy.sin(a/2)])
|
||||
# pdot02t = numpy.array([-0.2*numpy.sin(a), 0.2*numpy.cos(a), 0, 0])
|
||||
qdot02t = numpy.array([0, 0, 0.05*numpy.cos(a/2), -0.05*numpy.sin(a/2)])
|
||||
pdot02t = numpy.array([-0.2*numpy.sin(a), 0.2*numpy.cos(a), 0.25*numpy.cos(5*a), 0])
|
||||
|
||||
omega02t = 2 * qMultiply(qdot02t, qInvert(q02t))
|
||||
|
||||
tar = Odometry()
|
||||
tar.header.stamp = rospy.Time.now()
|
||||
tar.header.frame_id = "state_0"
|
||||
tar.pose.pose.position.x = p02t[0]
|
||||
tar.pose.pose.position.y = p02t[1]
|
||||
tar.pose.pose.position.z = p02t[2]
|
||||
tar.pose.pose.orientation.x = q02t[0]
|
||||
tar.pose.pose.orientation.y = q02t[1]
|
||||
tar.pose.pose.orientation.z = q02t[2]
|
||||
tar.pose.pose.orientation.w = q02t[3]
|
||||
tar.twist.twist.linear.x = pdot02t[0]
|
||||
tar.twist.twist.linear.y = pdot02t[1]
|
||||
tar.twist.twist.linear.z = pdot02t[2]
|
||||
tar.twist.twist.angular.x = omega02t[0]
|
||||
tar.twist.twist.angular.y = omega02t[1]
|
||||
tar.twist.twist.angular.z = omega02t[2]
|
||||
target_groundtruth_pub.publish(tar)
|
||||
|
||||
|
||||
if a < math.pi * 2:
|
||||
a += 0.01
|
||||
else:
|
||||
a = 0
|
||||
|
||||
rateFast.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
__main()
|
||||
|
|
@ -0,0 +1,500 @@
|
|||
import rospy
|
||||
from mavros_msgs.msg import State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, Pose, Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
from nav_msgs.msg import Odometry
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import math
|
||||
import numpy as np
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
class Controller:
|
||||
def __init__(self, vehicle_type, vehicle_id):
|
||||
self.MAX_LINEAR = 0.6
|
||||
self.MAX_ANG_VEL = 0.25 #0.15
|
||||
self.LINEAR_STEP_SIZE = 0.01
|
||||
self.ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
self.cmd_vel_mask = False
|
||||
self.ctrl_leader = False
|
||||
|
||||
self.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
|
||||
o : init---offboard, arm, takeoff and hover
|
||||
p : pose control
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
self.e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
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, settings)
|
||||
return key
|
||||
|
||||
def print_msg(self):
|
||||
print(msg2all)
|
||||
|
||||
def qprod(self, q1, q2):
|
||||
return tfc.transformations.quaternion_multiply(q1, q2)
|
||||
|
||||
# def qqprod(self, q1, q2):
|
||||
# return np.hstack((q1[3]*q2[:3]+q2[3]*q1[:3]+np.cross(q1[:3], q2[:3]), q1[3]*q2[3] - np.dot(q1[:3], q2[:3])))
|
||||
|
||||
def qconj(self, q):
|
||||
return np.hstack((-q[:3], q[3]))
|
||||
|
||||
def qAd(self, q, p):
|
||||
return qprod(qprod(q, p), qconj(q))
|
||||
|
||||
def qcrossProd(self, p1, p2):
|
||||
return np.hstack((np.cross(p1[:3], p2[:3]), [0]))
|
||||
|
||||
def qdotProd(self, p1, p2):
|
||||
return np.array([p1[0] * p2[0], p1[1] * p2[1], p1[2] * p2[2], 0])
|
||||
|
||||
def dqdotProd(self, xi1, xi2):
|
||||
return np.hstack((qdotProd(xi1[:4], xi2[:4]), qdotProd(xi1[4:8], xi2[4:8])))
|
||||
|
||||
def dqprod(self, dq1, dq2):
|
||||
return np.hstack((qprod(dq1[:4], dq2[:4]), qprod(dq1[4:8], dq2[:4]) + qprod(dq1[:4], dq2[4:8])))
|
||||
|
||||
def dqconj(self, dq):
|
||||
return np.hstack((qconj(dq[:4]), qconj(dq[4:8])))
|
||||
|
||||
def dqAd(self, dq, xi):
|
||||
return dqprod(dqprod(dq, xi), dqconj(dq))
|
||||
|
||||
def dqconstr(self, q, p_s):
|
||||
return np.hstack((q, qprod(p_s, q) / 2))
|
||||
|
||||
def lambdadq(self, dq):
|
||||
if dq[3] >= 0:
|
||||
return dq
|
||||
else:
|
||||
return -dq
|
||||
|
||||
def q2nvartheta(self, q):
|
||||
if q[3] > 1:
|
||||
q[3] = 1
|
||||
elif q[3] < -1:
|
||||
q[3] = -1
|
||||
vartheta = 2 * np.arccos(q[3])
|
||||
sin_varthataD2 = np.sin(vartheta / 2)
|
||||
if sin_varthataD2 == 0:
|
||||
sin_varthataD2 = sin_varthataD2 + 0.0000001
|
||||
n = np.array([q[0] / sin_varthataD2, q[1] / sin_varthataD2, q[2] / sin_varthataD2])
|
||||
return np.hstack((n, np.array(vartheta)))
|
||||
|
||||
def dq2nvarthetaps(self, dq):
|
||||
p_s = qprod(2 * dq[4:8], qconj(dq[:4]))
|
||||
return np.hstack((q2nvartheta(dq[:4]), p_s))
|
||||
|
||||
def dqln(dq):
|
||||
nvarthetaps = dq2nvarthetaps(self, dq)
|
||||
return np.hstack((nvarthetaps[:3] * nvarthetaps[3] * 0.5, [0], nvarthetaps[4:8] * 0.5))
|
||||
|
||||
|
||||
def current_state_callback(data):
|
||||
global cur_state
|
||||
cur_state = data
|
||||
|
||||
def target_state_callback(data):
|
||||
global tar_state
|
||||
tar_state = data
|
||||
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
multirotor_type = 'iris' #sys.argv[1]
|
||||
multirotor_num = 1 #int(sys.argv[2])
|
||||
control_type = 'vel' #sys.argv[3]
|
||||
|
||||
# if multirotor_num == 18:
|
||||
# formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond']
|
||||
# elif multirotor_num == 9:
|
||||
# formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
# elif multirotor_num == 6:
|
||||
# formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
rospy.init_node('multirotor_keyboard_UDQ_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)
|
||||
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/odom", Odometry, current_state_callback)
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/target", Odometry, current_state_callback)
|
||||
global tar_state
|
||||
tar_state = Odometry()
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
upward = 0.0
|
||||
angular = 0.0
|
||||
count = 0
|
||||
print_msg()
|
||||
while not rospy.is_shutdown(): #(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')
|
||||
elif key == 'o':
|
||||
#offboard
|
||||
cmd = 'OFFBOARD'
|
||||
print('Offboard')
|
||||
# while count < 5:
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# count = count + 1
|
||||
# rospy.Rate(5).sleep()
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
rospy.sleep(1.0)
|
||||
count = 0
|
||||
#arm
|
||||
cmd = 'ARM'
|
||||
print('Arming')
|
||||
# while count < 5:
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# count = count + 1
|
||||
# rospy.Rate(5).sleep()
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
rospy.sleep(1.0)
|
||||
count = 0
|
||||
#takeoff
|
||||
cmd = ''
|
||||
upward = 0.4
|
||||
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
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
print('takingoff')
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
while cur_state.pose.pose.position.z < 2.0 :
|
||||
rospy.Rate(5).sleep()
|
||||
# while cur_pose.pose.pose.position.z < 1.5 :
|
||||
# multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# rospy.Rate(5).sleep()
|
||||
#hover
|
||||
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')
|
||||
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
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
elif key == 'p':
|
||||
# set
|
||||
dk = np.array([1,1,2,0,2,1,1,0])
|
||||
tbf = tf2.Buffer()
|
||||
listener = tf2.TransformListener(tbf)
|
||||
pdot_bsat_last = np.array([0,0,0,0])
|
||||
omega_bsat_last = np.array([0,0,0,0])
|
||||
while getKey() != 's' :
|
||||
#####listen to target pose
|
||||
tf02t = geometry_msgs.msg.TransformStamped()
|
||||
try:
|
||||
tf02t = tbf.lookup_transform("state_0", "target_state", rospy.Time())
|
||||
except(tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException):
|
||||
rospy.Rate(10).sleep()
|
||||
rot02t = tf02t.transform.rotation
|
||||
tsl02t = tf02t.transform.translation
|
||||
#####tar_pose
|
||||
q_d = np.array([rot02t.x, rot02t.y, rot02t.z, rot02t.w])
|
||||
# print("destiny:\t q_d x %.2f\t q_d y %.2f\t q_d z %.2f\t q_d w %.2f" % (
|
||||
# q_d[0], q_d[1], q_d[2], q_d[3]))
|
||||
p_sd = np.array([tsl02t.x, tsl02t.y, tsl02t.z, 0])
|
||||
# print("destiny:\t p_sd x %.2f\t p_sd y %.2f\t p_sd z %.2f" % (
|
||||
# p_sd[0], p_sd[1], p_sd[2]))
|
||||
#####tar_state
|
||||
lin02t = tar_state.twist.twist.linear
|
||||
ang02t = tar_state.twist.twist.angular
|
||||
omega_sd = np.array([ang02t.x, ang02t.y, ang02t.z, 0])
|
||||
pdot_sd = np.array([lin02t.x, lin02t.y, lin02t.z, 0])
|
||||
# omega_sd = np.array([0, 0, 0, 0])
|
||||
# pdot_sd = np.array([0, 0, 0, 0])
|
||||
xi_sd = np.hstack((omega_sd, pdot_sd + qcrossProd(p_sd, omega_sd)))
|
||||
#####callback from current state
|
||||
tsl02c = cur_state.pose.pose.position
|
||||
rot02c = cur_state.pose.pose.orientation
|
||||
# lin02c = cur_state.twist.twist.linear
|
||||
# ang02c = cur_state.twist.twist.angular
|
||||
######cur_pose and cur_twist
|
||||
q = np.array([rot02c.x, rot02c.y, rot02c.z, rot02c.w])
|
||||
# print("currently:\t q x %.2f\t q y %.2f\t q z %.2f\t q w %.2f" % (
|
||||
# q[0], q[1], q[2], q[3]))
|
||||
p_s = np.array([tsl02c.x, tsl02c.y, tsl02c.z, 0])
|
||||
# print("currently:\t p_s x %.2f\t p_s y %.2f\t p_s z %.2f" % (
|
||||
# p_s[0], p_s[1], p_s[2]))
|
||||
# omega_sC = np.array([ang02c.x, ang02c.y, ang02c.z, 0])
|
||||
# pdot_sC = np.array([lin02c.x, lin02c.y, lin02c.z, 0])
|
||||
#####control scheme
|
||||
q_er = qprod(q_d, qconj(q))
|
||||
# print("error:\t q_er x %.2f\t q_er y %.2f\t q_er z %.2f\t q_er w %.2f" % (
|
||||
# q_er[0], q_er[1], q_er[2], q_er[3]))
|
||||
# omega_se = omega_sd - qAd(q_er, omega_s)
|
||||
p_se = p_sd - qAd(q_er, p_s)
|
||||
# print("error:\t p_se x %.2f\t p_se y %.2f\t p_se z %.2f" % (
|
||||
# p_se[0], p_se[1], p_se[2]))
|
||||
# pdot_se = pdot_sd - qcrossProd(omega_se, qAd(q_er, p_s)) - qAd(q_er, pdot_s)
|
||||
dq_er = dqconstr(q_er, p_se)
|
||||
xi_s = dqAd(dqconj(dq_er), xi_sd + 2 * dqdotProd(dk, dqln(lambdadq(dq_er))))
|
||||
omega_s = xi_s[:4]
|
||||
pdot_s = xi_s[4:8] + qcrossProd(p_s, omega_s)
|
||||
# print(
|
||||
# "currently:\t linear vel x %.2f\t linear vel y %.2f\t linear vel z %.2f\t angular vel x %.2f\t angular vel y %.2f\t angular vel z %.2f" % (
|
||||
# pdot_s[0], pdot_s[1], pdot_s[2], omega_s[0], omega_s[1], omega_s[2]))
|
||||
#####saturate
|
||||
ang = np.sqrt(np.dot(omega_s, omega_s.T))
|
||||
lin = np.sqrt(np.dot(pdot_s, pdot_s.T))
|
||||
# print("linear vel: %.2f\t angular vel: %.2f" %(lin, ang))
|
||||
if ang > MAX_ANG_VEL:
|
||||
omega_ssat = omega_s/ang*MAX_ANG_VEL
|
||||
else:
|
||||
omega_ssat = omega_s
|
||||
# omega_ssat = omega_s
|
||||
if lin > MAX_LINEAR:
|
||||
pdot_ssat = pdot_s/lin*MAX_LINEAR
|
||||
else:
|
||||
pdot_ssat = pdot_s
|
||||
# pdot_ssat = pdot_s
|
||||
# # print("omega_ssat: %.2f\t %.2f\t %.2f\t pdot_ssat: %.2f\t %.2f\t %.2f" % (omega_ssat[0], omega_ssat[1],omega_ssat[2], pdot_ssat[0],pdot_ssat[1],pdot_ssat[2]))
|
||||
#####convert ang vel and lin vel to body frame
|
||||
pdot_bsat = qAd(qconj(q), pdot_ssat)
|
||||
omega_bsat = qAd(qconj(q), omega_ssat)
|
||||
#####Compare to last vel cmd supply && make vel cmd smooth
|
||||
if (pdot_bsat[0] == 0) & (pdot_bsat[1] == 0):
|
||||
pdot_bsat = pdot_bsat_last
|
||||
else:
|
||||
pdot_inc = pdot_bsat - pdot_bsat_last
|
||||
pdot_inc_mag = np.sqrt(np.dot(pdot_inc, pdot_inc.T))
|
||||
if pdot_inc_mag > MAX_LINEAR*0.1:
|
||||
pdot_bsat = pdot_bsat_last + pdot_inc/pdot_inc_mag*MAX_LINEAR*0.1
|
||||
if (omega_bsat[2] == 0) & (omega_bsat[1] == 0):
|
||||
omega_bsat = omega_bsat_last
|
||||
else:
|
||||
omega_inc = omega_bsat - omega_bsat_last
|
||||
omega_inc_mag = np.sqrt(np.dot(omega_inc, omega_inc.T))
|
||||
if omega_inc_mag > MAX_ANG_VEL*0.1:
|
||||
omega_bsat = omega_bsat_last + omega_inc/omega_inc_mag*MAX_ANG_VEL*0.1
|
||||
####output
|
||||
cmd = ''
|
||||
twist.linear.x = pdot_bsat[0]
|
||||
twist.linear.y = pdot_bsat[1]
|
||||
twist.linear.z = pdot_bsat[2]
|
||||
twist.angular.x = omega_bsat[0]
|
||||
twist.angular.y = omega_bsat[1]
|
||||
twist.angular.z = omega_bsat[2]
|
||||
print('control scheme: press "s" to hover')
|
||||
print("currently:\t linear vel x %.2f\t linear vel y %.2f\t linear vel z %.2f\t angular vel x %.2f\t angular vel y %.2f\t angular vel z %.2f" % (
|
||||
twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z))
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
#####remember
|
||||
pdot_bsat_last = pdot_bsat
|
||||
omega_bsat_last = omega_bsat
|
||||
rospy.Rate(100).sleep()
|
||||
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_accel_flu_pub.publish(twist)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
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,19 @@
|
|||
vrpn节点--->vicon坐标系下的位姿速加速，左我上
|
||||
|
||||
1。 让px4知道自己是什么状态
|
||||
vicon坐标系下的pose（vicon给的原始的） ----> vrpn_transfer节点（与mavros通信demo） ----> tx2里的mavros节点
|
||||
|
||||
2。 发布目标，顺便可视化跟踪过程
|
||||
vicon坐标系下的pose ----> tf_broadcaster节点，以0系虚拟建立tar系与0系的关系， 再建立0系与cur系的关系
|
||||
|
||||
3。 速度指令求解与发布
|
||||
vicon坐标系下的pose+twist(来自mavros融合后的）， 听target -----> udq_control节点 -----> cur与tar误差求解速度控制量 ----> tx2里的mavros节点
|
||||
|
||||
下一步工作：
|
||||
1。读取自身位姿速、目标速，听目标位姿
|
||||
2。整合communication和速度输出（包括键盘控制）
|
||||
3。发布速度控制话题给tx2运行的mavros，测试
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,280 +0,0 @@
|
|||
# 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,277 @@
|
|||
import rospy
|
||||
from mavros_msgs.msg import State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, Pose, Twist
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import math
|
||||
import numpy
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
class Communication:
|
||||
|
||||
def __init__(self, vehicle_type, vehicle_id):
|
||||
|
||||
self.vehicle_type = vehicle_type
|
||||
self.vehicle_id = vehicle_id
|
||||
self.current_position = None
|
||||
self.current_yaw = 0
|
||||
self.hover_flag = 0
|
||||
self.target_motion = PositionTarget()
|
||||
self.arm_state = False
|
||||
self.motion_type = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
initial pose (simutaneously set in indoor?.launch)
|
||||
'''
|
||||
#self.x_origin = 0
|
||||
#self.y_origin = 0
|
||||
#self.z_origin = 0
|
||||
#self.r_origin = 0
|
||||
#self.p_origin = 0
|
||||
#self.y_origin = 0
|
||||
|
||||
'''
|
||||
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.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+'_'+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_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback)
|
||||
self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback)
|
||||
|
||||
'''
|
||||
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/'+self.vehicle_type+'_'+self.vehicle_id+'/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)
|
||||
|
||||
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
|
||||
'''
|
||||
# count = 0
|
||||
while not rospy.is_shutdown():
|
||||
# count = count + 1
|
||||
self.target_motion_pub.publish(self.target_motion)
|
||||
|
||||
if (self.flight_mode is "LAND") and (self.current_position.z < 0.15):
|
||||
if(self.disarm()):
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
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
|
||||
odom = Odometry()
|
||||
odom.header = response.header
|
||||
odom.pose.pose = response.pose
|
||||
odom.twist.twist = response.twist
|
||||
self.odom_groundtruth_pub.publish(odom)
|
||||
|
||||
# if count >= 10:
|
||||
# bc = tf2.TransformBroadcaster()
|
||||
# # pO = (self.x_origin, self.y_origin, self.z_origin, 0)
|
||||
# # qO = tfc.transformations.quaternion_from_euler(self.r_origin, self.p_origin, self.y_origin)
|
||||
# p02C = (response.pose.position.x, response.pose.position.y, response.pose.position.z, 0)
|
||||
# q02C = (response.pose.orientation.x, response.pose.orientation.y, response.pose.orientation.z, response.pose.orientation.w)
|
||||
# transform = self.__genTf("current_state", "state_0", p02C, q02C)
|
||||
# bc.sendTransform(transform)
|
||||
# count = 0
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def __genTf(self, frameT, frameS, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameT
|
||||
ts.child_frame_id = frameS
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.current_position = msg.pose.position
|
||||
self.current_yaw = self.q2yaw(msg.pose.orientation)
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
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):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.coordinate_frame = self.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.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_RATE
|
||||
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.coordinate_frame = 9
|
||||
self.motion_type = 0
|
||||
yaw = self.q2yaw(msg.orientation)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.coordinate_frame = 1
|
||||
self.motion_type = 0
|
||||
yaw = self.q2yaw(msg.orientation)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 8
|
||||
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_vel_enu_callback(self, msg):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 1
|
||||
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):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 8
|
||||
self.motion_type = 2
|
||||
self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
|
||||
|
||||
def cmd_accel_enu_callback(self, msg):
|
||||
self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
|
||||
if self.hover_flag == 0:
|
||||
self.coordinate_frame = 1
|
||||
self.motion_type = 2
|
||||
self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
|
||||
|
||||
def hover_state_transition(self,x,y,z,w):
|
||||
if abs(x) > 0.005 or abs(y) > 0.005 or abs(z) > 0.005 or abs(w) > 0.005:
|
||||
self.hover_flag = 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':
|
||||
self.arm_state = not self.disarm()
|
||||
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 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.coordinate_frame = 1
|
||||
self.motion_type = 0
|
||||
self.target_motion = self.construct_target(x=self.current_position.x,y=self.current_position.y,z=self.current_position.z,yaw=self.current_yaw)
|
||||
|
||||
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):
|
||||
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.current_position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
communication = Communication('iris', '0') # Communication(sys.argv[1],sys.argv[2])
|
||||
communication.start()
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
import rospy
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
import math
|
||||
import numpy
|
||||
|
||||
def qRot(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(q, p), qInv)
|
||||
return pNew
|
||||
|
||||
def qRotInv(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(qInv, p), q)
|
||||
return pNew
|
||||
|
||||
|
||||
def __genTf(frameT, frameS, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameT
|
||||
ts.child_frame_id = frameS
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def __main():
|
||||
rospy.init_node("A2C")
|
||||
rate = rospy.Rate(1)
|
||||
tbf = tf2.Buffer()
|
||||
tl = tf2.TransformListener(tbf)
|
||||
|
||||
a = 0
|
||||
pp = 0
|
||||
while not tbf.can_transform("C", "D", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{C-D}"
|
||||
rate.sleep()
|
||||
|
||||
while not tbf.can_transform("D", "C", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{D-C}"
|
||||
rate.sleep()
|
||||
|
||||
while not tbf.can_transform("A", "B", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{A-B}"
|
||||
rate.sleep()
|
||||
|
||||
while not tbf.can_transform("B", "A", rospy.Time()) and not rospy.is_shutdown():
|
||||
print "waiting for static tf ready.{B-A}"
|
||||
rate.sleep()
|
||||
|
||||
print ("all static tf ready")
|
||||
|
||||
roundCount = 0
|
||||
rateFast = rospy.Rate(10)
|
||||
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
while not rospy.is_shutdown():
|
||||
print("begin: " + str(roundCount))
|
||||
roundCount += 1
|
||||
# lookup_transform(desFrame, srcFrame, time)
|
||||
tfD2C = tbf.lookup_transform("C", "D", rospy.Time())
|
||||
tfC2D = tbf.lookup_transform("D", "C", rospy.Time())
|
||||
tfB2A = tbf.lookup_transform("A", "B", rospy.Time())
|
||||
tfA2B = tbf.lookup_transform("B", "A", rospy.Time())
|
||||
|
||||
bc = tf2.TransformBroadcaster()
|
||||
|
||||
tD2B = (2 * math.cos(a), 1 + 2 * math.sin(a), -1, 0)
|
||||
qD2B = tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)
|
||||
|
||||
ts = __genTf("B", "DB", tD2B, qD2B)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
# db -> da
|
||||
tfm = tfB2A
|
||||
r = tfm.transform.rotation
|
||||
ts = tfm.transform.translation
|
||||
qB2A = numpy.array([r.x, r.y, r.z, r.w])
|
||||
tB2A = numpy.array([ts.x, ts.y, ts.z, 0])
|
||||
qD2A = qMultiply(qB2A, qD2B)
|
||||
tD2A = qRot(tD2B, qB2A) + tB2A
|
||||
|
||||
ts = __genTf("A", "DA", tD2A, qD2A)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
# da -> ca
|
||||
tfm = tfC2D
|
||||
r = tfm.transform.rotation
|
||||
ts = tfm.transform.translation
|
||||
qC2D = numpy.array([r.x, r.y, r.z, r.w])
|
||||
tC2D = numpy.array([ts.x, ts.y, ts.z, 0])
|
||||
qC2A = qMultiply(qD2A, qC2D)
|
||||
tC2A = qRot(tC2D, qD2A) + tD2A
|
||||
ts = __genTf("A", "C", tC2A, qC2A)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
if a < math.pi * 2:
|
||||
a += 0.01
|
||||
else:
|
||||
a = 0
|
||||
|
||||
rateFast.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
__main()
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
import rospy
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
import math
|
||||
import numpy
|
||||
|
||||
def __genTf(frameT, frameS, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameT
|
||||
ts.child_frame_id = frameS
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def __main():
|
||||
rospy.init_node("AB2CD")
|
||||
rate = rospy.Rate(100)
|
||||
tbf = tf2.Buffer()
|
||||
|
||||
t = TFMessage()
|
||||
sbc = tf2.StaticTransformBroadcaster()
|
||||
qA2B = tfc.transformations.quaternion_from_euler(math.pi, 0, math.pi / 2)
|
||||
ts1 = __genTf("A", "B", (-1, 0, 0.2), qA2B)
|
||||
t.transforms.append(ts1)
|
||||
|
||||
qC2D = tfc.transformations.quaternion_from_euler(math.pi, 0, 0)
|
||||
ts2 = __genTf("C", "D", (0, 0, 1), qC2D)
|
||||
t.transforms.append(ts2)
|
||||
|
||||
sbc.pub_tf.publish(t)
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
__main()
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
cd PX4_Firmware/launch/
|
||||
roslaunch px4 indoor6.launch
|
||||
|
||||
cd XTDrone/control/sim_UDQcontrol/
|
||||
python communication.py
|
||||
|
||||
cd XTDrone/control/sim_UDQcontrol/
|
||||
python tf_broadcaster.py
|
||||
|
||||
cd XTDrone/control/sim_UDQcontrol/
|
||||
python udq_control.py
|
||||
|
|
@ -0,0 +1,171 @@
|
|||
import rospy
|
||||
import sys
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
import math
|
||||
import numpy
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
def current_state_callback(data):
|
||||
global cur_state
|
||||
cur_state = data
|
||||
# print('upward vel %.2f', cur_state.pose.pose.orientation.x)
|
||||
|
||||
def qRot(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(q, p), qInv)
|
||||
return pNew
|
||||
|
||||
def qRotInv(p, q):
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
qInv = qInvert(q)
|
||||
pNew = qMultiply(qMultiply(qInv, p), q)
|
||||
return pNew
|
||||
|
||||
|
||||
def __genTf(frameS, frameT, posTs, q):
|
||||
ts = geometry_msgs.msg.TransformStamped()
|
||||
ts.header.stamp = rospy.Time.now()
|
||||
ts.header.frame_id = frameS
|
||||
ts.child_frame_id = frameT
|
||||
|
||||
ts.transform.translation.x = posTs[0]
|
||||
ts.transform.translation.y = posTs[1]
|
||||
ts.transform.translation.z = posTs[2]
|
||||
|
||||
ts.transform.rotation.x = q[0]
|
||||
ts.transform.rotation.y = q[1]
|
||||
ts.transform.rotation.z = q[2]
|
||||
ts.transform.rotation.w = q[3]
|
||||
return ts
|
||||
|
||||
def __main():
|
||||
rospy.init_node("tf_broadcaster")
|
||||
# rate = rospy.Rate(1)
|
||||
# tbf = tf2.Buffer()
|
||||
# tl = tf2.TransformListener(tbf)
|
||||
|
||||
a = 0
|
||||
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/odom", Odometry, current_state_callback)
|
||||
target_groundtruth_pub = rospy.Publisher(
|
||||
"/xtdrone/iris_0/ground_truth/target", Odometry, queue_size=10)
|
||||
global cur_state
|
||||
cur_state = Odometry()
|
||||
roundCount = 0
|
||||
rateFast = rospy.Rate(10)
|
||||
|
||||
qInvert = tfc.transformations.quaternion_inverse
|
||||
qMultiply = tfc.transformations.quaternion_multiply
|
||||
while not rospy.is_shutdown():
|
||||
print("begin: " + str(roundCount))
|
||||
roundCount += 1
|
||||
# # lookup_transform(desFrame, srcFrame, time)
|
||||
# tfD2C = tbf.lookup_transform("C", "D", rospy.Time())
|
||||
# tfC2D = tbf.lookup_transform("D", "C", rospy.Time())
|
||||
# tfB2A = tbf.lookup_transform("A", "B", rospy.Time())
|
||||
# tfA2B = tbf.lookup_transform("B", "A", rospy.Time())
|
||||
|
||||
bc = tf2.TransformBroadcaster()
|
||||
# # t20
|
||||
#####set-point
|
||||
p02t = numpy.array([1, 2, 1, 0])
|
||||
q02t = tfc.transformations.quaternion_from_euler(0, 0, math.pi/2)
|
||||
#####tracking
|
||||
# p02t = numpy.array([2 * math.cos(a), -1 + 2 * math.sin(a), 1, 0])
|
||||
# q02t = numpy.array(tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)) #math.pi / 2 +
|
||||
# p02t = numpy.array([2 * math.cos(a), -1 + 2 * math.sin(a), 2+math.sin(5*a), 0])
|
||||
# q02t = numpy.array(tfc.transformations.quaternion_from_euler(0, 0, math.pi / 2 + a)) #math.pi / 2 +
|
||||
ts = __genTf("state_0", "target_state", p02t, q02t)
|
||||
# qt20 = qInvert(q02t)
|
||||
# pt20 = qRotInv(-p02t, q02t)
|
||||
# ts = __genTf("target_state", "state_0", pt20, qt20)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
if cur_state.pose.pose.orientation.x != 0.0:
|
||||
# # 02c
|
||||
p02c = numpy.array([cur_state.pose.pose.position.x, cur_state.pose.pose.position.y, cur_state.pose.pose.position.z, 0])
|
||||
q02c = (cur_state.pose.pose.orientation.x, cur_state.pose.pose.orientation.y, cur_state.pose.pose.orientation.z, cur_state.pose.pose.orientation.w)
|
||||
# print("currently:\t x %.2f\t y %.2f\t z %.2f" % (
|
||||
# cur_state.pose.pose.position.x, cur_state.pose.pose.position.y, cur_state.pose.pose.position.z))
|
||||
# ts = __genTf("current_state", "state_0", p02c, q02c)
|
||||
# bc.sendTransform(ts)
|
||||
# # c20
|
||||
pc20 = qRotInv(-p02c, q02c)
|
||||
qc20 = qInvert(q02c)
|
||||
ts = __genTf("current_state", "state_0", pc20, qc20)
|
||||
bc.sendTransform(ts)
|
||||
# # c2t
|
||||
qc2t = qMultiply(qInvert(q02c), q02t)
|
||||
# print("qc2t %.2f\t %.2f\t %.2f\t %.2f", qc2t[0], qc2t[1],qc2t[2],qc2t[3])
|
||||
pc2t = qRotInv((p02t - p02c), q02c)
|
||||
ts = __genTf("current_state", "target_state1", pc2t, qc2t)
|
||||
bc.sendTransform(ts)
|
||||
|
||||
#####set-point
|
||||
qdot02t = numpy.array([0, 0, 0, 0])
|
||||
pdot02t = numpy.array([0, 0, 0, 0])
|
||||
#####tracking
|
||||
# qdot02t = numpy.array([0, 0, 0.05*numpy.cos(a/2), -0.05*numpy.sin(a/2)])
|
||||
# pdot02t = numpy.array([-0.2*numpy.sin(a), 0.2*numpy.cos(a), 0, 0])
|
||||
# qdot02t = numpy.array([0, 0, 0.05*numpy.cos(a/2), -0.05*numpy.sin(a/2)])
|
||||
# pdot02t = numpy.array([-0.2*numpy.sin(a), 0.2*numpy.cos(a), 0.5*numpy.cos(5*a), 0])
|
||||
|
||||
omega02t = 2 * qMultiply(qdot02t, qInvert(q02t))
|
||||
|
||||
tar = Odometry()
|
||||
tar.header.stamp = rospy.Time.now()
|
||||
tar.header.frame_id = "state_0"
|
||||
tar.pose.pose.position.x = p02t[0]
|
||||
tar.pose.pose.position.y = p02t[1]
|
||||
tar.pose.pose.position.z = p02t[2]
|
||||
tar.pose.pose.orientation.x = q02t[0]
|
||||
tar.pose.pose.orientation.y = q02t[1]
|
||||
tar.pose.pose.orientation.z = q02t[2]
|
||||
tar.pose.pose.orientation.w = q02t[3]
|
||||
tar.twist.twist.linear.x = pdot02t[0]
|
||||
tar.twist.twist.linear.y = pdot02t[1]
|
||||
tar.twist.twist.linear.z = pdot02t[2]
|
||||
tar.twist.twist.angular.x = omega02t[0]
|
||||
tar.twist.twist.angular.y = omega02t[1]
|
||||
tar.twist.twist.angular.z = omega02t[2]
|
||||
target_groundtruth_pub.publish(tar)
|
||||
|
||||
# # db -> da
|
||||
# tfm = tfB2A
|
||||
# r = tfm.transform.rotation
|
||||
# ts = tfm.transform.translation
|
||||
# qB2A = numpy.array([r.x, r.y, r.z, r.w])
|
||||
# tB2A = numpy.array([ts.x, ts.y, ts.z, 0])
|
||||
# qD2A = qMultiply(qB2A, qD2B)
|
||||
# tD2A = qRot(tD2B, qB2A) + tB2A
|
||||
#
|
||||
# ts = __genTf("A", "DA", tD2A, qD2A)
|
||||
# bc.sendTransform(ts)
|
||||
#
|
||||
# # da -> ca
|
||||
# tfm = tfC2D
|
||||
# r = tfm.transform.rotation
|
||||
# ts = tfm.transform.translation
|
||||
# qC2D = numpy.array([r.x, r.y, r.z, r.w])
|
||||
# tC2D = numpy.array([ts.x, ts.y, ts.z, 0])
|
||||
# qC2A = qMultiply(qD2A, qC2D)
|
||||
# tC2A = qRot(tC2D, qD2A) + tD2A
|
||||
# ts = __genTf("A", "C", tC2A, qC2A)
|
||||
# bc.sendTransform(ts)
|
||||
|
||||
if a < math.pi * 2:
|
||||
a += 0.01
|
||||
else:
|
||||
a = 0
|
||||
|
||||
rateFast.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
__main()
|
||||
|
|
@ -0,0 +1,495 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
from nav_msgs.msg import Odometry
|
||||
import tf2_ros as tf2
|
||||
import tf_conversions as tfc
|
||||
import math
|
||||
import numpy as np
|
||||
import geometry_msgs.msg
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
MAX_LINEAR = 0.6
|
||||
MAX_ANG_VEL = 0.3 #0.15
|
||||
LINEAR_STEP_SIZE = 0.01
|
||||
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/k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
o : init---offboard, arm, takeoff and hover
|
||||
p : pose control
|
||||
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():
|
||||
print(msg2all)
|
||||
|
||||
def current_state_callback(data):
|
||||
global cur_state
|
||||
cur_state = data
|
||||
|
||||
def target_state_callback(data):
|
||||
global tar_state
|
||||
tar_state = data
|
||||
|
||||
def qprod(q1, q2):
|
||||
return tfc.transformations.quaternion_multiply(q1, q2)
|
||||
|
||||
# def qqprod(q1, q2):
|
||||
# return np.hstack((q1[3]*q2[:3]+q2[3]*q1[:3]+np.cross(q1[:3], q2[:3]), q1[3]*q2[3] - np.dot(q1[:3], q2[:3])))
|
||||
|
||||
def qconj(q):
|
||||
return np.hstack((-q[:3],q[3]))
|
||||
|
||||
def qAd(q, p):
|
||||
return qprod(qprod(q, p), qconj(q))
|
||||
|
||||
def qcrossProd(p1, p2):
|
||||
return np.hstack((np.cross(p1[:3], p2[:3]), [0]))
|
||||
|
||||
def qdotProd(p1, p2):
|
||||
return np.array([p1[0]*p2[0], p1[1]*p2[1], p1[2]*p2[2], 0])
|
||||
|
||||
def dqdotProd(xi1, xi2):
|
||||
return np.hstack((qdotProd(xi1[:4], xi2[:4]), qdotProd(xi1[4:8], xi2[4:8])))
|
||||
|
||||
def dqprod(dq1, dq2):
|
||||
return np.hstack((qprod(dq1[:4],dq2[:4]),qprod(dq1[4:8],dq2[:4])+qprod(dq1[:4],dq2[4:8])))
|
||||
|
||||
def dqconj(dq):
|
||||
return np.hstack((qconj(dq[:4]), qconj(dq[4:8])))
|
||||
|
||||
def dqAd(dq, xi):
|
||||
return dqprod(dqprod(dq, xi), dqconj(dq))
|
||||
|
||||
def dqconstr(q, p_s):
|
||||
return np.hstack((q, qprod(p_s, q)/2))
|
||||
|
||||
def lambdadq(dq):
|
||||
if dq[3] >= 0:
|
||||
return dq
|
||||
else:
|
||||
return -dq
|
||||
|
||||
def q2nvartheta(q):
|
||||
if q[3] > 1:
|
||||
q[3] = 1
|
||||
elif q[3] < -1:
|
||||
q[3] = -1
|
||||
vartheta = 2 * np.arccos(q[3])
|
||||
sin_varthataD2 = np.sin(vartheta/2)
|
||||
if sin_varthataD2 == 0:
|
||||
sin_varthataD2 = sin_varthataD2 + 0.0000001
|
||||
n = np.array([q[0]/sin_varthataD2, q[1]/sin_varthataD2, q[2]/sin_varthataD2])
|
||||
return np.hstack((n, np.array(vartheta)))
|
||||
|
||||
def dq2nvarthetaps(dq):
|
||||
p_s = qprod(2 * dq[4:8], qconj(dq[:4]))
|
||||
return np.hstack((q2nvartheta(dq[:4]), p_s))
|
||||
|
||||
def dqln(dq):
|
||||
nvarthetaps = dq2nvarthetaps(dq)
|
||||
return np.hstack((nvarthetaps[:3]*nvarthetaps[3]*0.5, [0], nvarthetaps[4:8]*0.5))
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
multirotor_type = 'iris' #sys.argv[1]
|
||||
multirotor_num = 1 #int(sys.argv[2])
|
||||
control_type = 'vel' #sys.argv[3]
|
||||
|
||||
# if multirotor_num == 18:
|
||||
# formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond']
|
||||
# elif multirotor_num == 9:
|
||||
# formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
# elif multirotor_num == 6:
|
||||
# formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
rospy.init_node('multirotor_keyboard_UDQ_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)
|
||||
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/odom", Odometry, current_state_callback)
|
||||
rospy.Subscriber("/xtdrone/iris_0/ground_truth/target", Odometry, target_state_callback)
|
||||
global tar_state
|
||||
tar_state = Odometry()
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
upward = 0.0
|
||||
angular = 0.0
|
||||
count = 0
|
||||
print_msg()
|
||||
while not rospy.is_shutdown(): #(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')
|
||||
elif key == 'o':
|
||||
#offboard
|
||||
cmd = 'OFFBOARD'
|
||||
print('Offboard')
|
||||
# while count < 5:
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# count = count + 1
|
||||
# rospy.Rate(5).sleep()
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
rospy.sleep(1.0)
|
||||
count = 0
|
||||
#arm
|
||||
cmd = 'ARM'
|
||||
print('Arming')
|
||||
# while count < 5:
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# count = count + 1
|
||||
# rospy.Rate(5).sleep()
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
rospy.sleep(1.0)
|
||||
count = 0
|
||||
#takeoff
|
||||
cmd = ''
|
||||
upward = 0.4
|
||||
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
|
||||
print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular))
|
||||
print('takingoff')
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
while cur_state.pose.pose.position.z < 2.0 :
|
||||
rospy.Rate(5).sleep()
|
||||
# while cur_pose.pose.pose.position.z < 1.5 :
|
||||
# multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
# multi_cmd_pub[0].publish(cmd)
|
||||
# rospy.Rate(5).sleep()
|
||||
#hover
|
||||
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')
|
||||
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
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
multi_cmd_pub[0].publish(cmd)
|
||||
elif key == 'p':
|
||||
# set
|
||||
dk = np.array([1,1,1,0,1,1,1,0])
|
||||
tbf = tf2.Buffer()
|
||||
listener = tf2.TransformListener(tbf)
|
||||
pdot_bsat_last = np.array([0,0,0,0])
|
||||
omega_bsat_last = np.array([0,0,0,0])
|
||||
while getKey() != 's' :
|
||||
#####listen to target pose
|
||||
tf02t = geometry_msgs.msg.TransformStamped()
|
||||
try:
|
||||
tf02t = tbf.lookup_transform("state_0", "target_state", rospy.Time())
|
||||
except(tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException):
|
||||
rospy.Rate(10).sleep()
|
||||
rot02t = tf02t.transform.rotation
|
||||
tsl02t = tf02t.transform.translation
|
||||
#####tar_pose
|
||||
q_d = np.array([rot02t.x, rot02t.y, rot02t.z, rot02t.w])
|
||||
# print("destiny:\t q_d x %.2f\t q_d y %.2f\t q_d z %.2f\t q_d w %.2f" % (
|
||||
# q_d[0], q_d[1], q_d[2], q_d[3]))
|
||||
p_sd = np.array([tsl02t.x, tsl02t.y, tsl02t.z, 0])
|
||||
# print("destiny:\t p_sd x %.2f\t p_sd y %.2f\t p_sd z %.2f" % (
|
||||
# p_sd[0], p_sd[1], p_sd[2]))
|
||||
#####tar_state
|
||||
lin02t = tar_state.twist.twist.linear
|
||||
ang02t = tar_state.twist.twist.angular
|
||||
omega_sd = np.array([ang02t.x, ang02t.y, ang02t.z, 0])
|
||||
pdot_sd = np.array([lin02t.x, lin02t.y, lin02t.z, 0])
|
||||
# print("destiny:\t pdot_sd x %.2f\t pdot_sd y %.2f\t pdot_sd z %.2f" % (
|
||||
# pdot_sd[0], pdot_sd[1], pdot_sd[2]))
|
||||
# omega_sd = np.array([0, 0, 0, 0])
|
||||
# pdot_sd = np.array([0, 0, 0, 0])
|
||||
xi_sd = np.hstack((omega_sd, pdot_sd + qcrossProd(p_sd, omega_sd)))
|
||||
print("destiny:\t xi_sd = (%.2f\t, %.2f\t, %.2f\t, %.2f\t, %.2f\t, %.2f\t, %.2f\t, %.2f") % (
|
||||
xi_sd[0], xi_sd[1], xi_sd[2], xi_sd[3], xi_sd[4], xi_sd[5], xi_sd[6], xi_sd[7])
|
||||
#####callback from current state
|
||||
tsl02c = cur_state.pose.pose.position
|
||||
rot02c = cur_state.pose.pose.orientation
|
||||
# lin02c = cur_state.twist.twist.linear
|
||||
# ang02c = cur_state.twist.twist.angular
|
||||
######cur_pose and cur_twist
|
||||
q = np.array([rot02c.x, rot02c.y, rot02c.z, rot02c.w])
|
||||
# print("currently:\t q x %.2f\t q y %.2f\t q z %.2f\t q w %.2f" % (
|
||||
# q[0], q[1], q[2], q[3]))
|
||||
p_s = np.array([tsl02c.x, tsl02c.y, tsl02c.z, 0])
|
||||
# print("currently:\t p_s x %.2f\t p_s y %.2f\t p_s z %.2f" % (
|
||||
# p_s[0], p_s[1], p_s[2]))
|
||||
# omega_sC = np.array([ang02c.x, ang02c.y, ang02c.z, 0])
|
||||
# pdot_sC = np.array([lin02c.x, lin02c.y, lin02c.z, 0])
|
||||
#####control scheme
|
||||
q_er = qprod(q_d, qconj(q))
|
||||
# print("error:\t q_er x %.2f\t q_er y %.2f\t q_er z %.2f\t q_er w %.2f" % (
|
||||
# q_er[0], q_er[1], q_er[2], q_er[3]))
|
||||
# omega_se = omega_sd - qAd(q_er, omega_s)
|
||||
p_se = p_sd - qAd(q_er, p_s)
|
||||
# print("error:\t p_se x %.2f\t p_se y %.2f\t p_se z %.2f" % (
|
||||
# p_se[0], p_se[1], p_se[2]))
|
||||
# pdot_se = pdot_sd - qcrossProd(omega_se, qAd(q_er, p_s)) - qAd(q_er, pdot_s)
|
||||
dq_er = dqconstr(q_er, p_se)
|
||||
xi_s = dqAd(dqconj(dq_er), xi_sd + 2 * dqdotProd(dk, dqln(lambdadq(dq_er))))
|
||||
omega_s = xi_s[:4]
|
||||
pdot_s = xi_s[4:8] + qcrossProd(p_s, omega_s)
|
||||
# print(
|
||||
# "currently:\t linear vel x %.2f\t linear vel y %.2f\t linear vel z %.2f\t angular vel x %.2f\t angular vel y %.2f\t angular vel z %.2f" % (
|
||||
# pdot_s[0], pdot_s[1], pdot_s[2], omega_s[0], omega_s[1], omega_s[2]))
|
||||
#####saturate
|
||||
ang = np.sqrt(np.dot(omega_s, omega_s.T))
|
||||
lin = np.sqrt(np.dot(pdot_s, pdot_s.T))
|
||||
print("linear vel: %.2f\t angular vel: %.2f" %(lin, ang))
|
||||
if ang > MAX_ANG_VEL:
|
||||
omega_ssat = omega_s/ang*MAX_ANG_VEL
|
||||
else:
|
||||
omega_ssat = omega_s
|
||||
# omega_ssat = omega_s
|
||||
if lin > MAX_LINEAR:
|
||||
pdot_ssat = pdot_s/lin*MAX_LINEAR
|
||||
else:
|
||||
pdot_ssat = pdot_s
|
||||
# pdot_ssat = pdot_s
|
||||
# # print("omega_ssat: %.2f\t %.2f\t %.2f\t pdot_ssat: %.2f\t %.2f\t %.2f" % (omega_ssat[0], omega_ssat[1],omega_ssat[2], pdot_ssat[0],pdot_ssat[1],pdot_ssat[2]))
|
||||
#####convert ang vel and lin vel to body frame
|
||||
pdot_bsat = qAd(qconj(q), pdot_ssat)
|
||||
omega_bsat = qAd(qconj(q), omega_ssat)
|
||||
#####Compare to last vel cmd supply && make vel cmd smooth
|
||||
# if (pdot_bsat[0] == 0) & (pdot_bsat[1] == 0):
|
||||
# pdot_bsat = pdot_bsat_last
|
||||
# else:
|
||||
# pdot_inc = pdot_bsat - pdot_bsat_last
|
||||
# pdot_inc_mag = np.sqrt(np.dot(pdot_inc, pdot_inc.T))
|
||||
# if pdot_inc_mag > MAX_LINEAR*0.1:
|
||||
# pdot_bsat = pdot_bsat_last + pdot_inc/pdot_inc_mag*MAX_LINEAR*0.1
|
||||
# if (omega_bsat[2] == 0) & (omega_bsat[1] == 0):
|
||||
# omega_bsat = omega_bsat_last
|
||||
# else:
|
||||
# omega_inc = omega_bsat - omega_bsat_last
|
||||
# omega_inc_mag = np.sqrt(np.dot(omega_inc, omega_inc.T))
|
||||
# if omega_inc_mag > MAX_ANG_VEL*0.1:
|
||||
# omega_bsat = omega_bsat_last + omega_inc/omega_inc_mag*MAX_ANG_VEL*0.1
|
||||
####output
|
||||
cmd = ''
|
||||
twist.linear.x = pdot_bsat[0]
|
||||
twist.linear.y = pdot_bsat[1]
|
||||
twist.linear.z = pdot_bsat[2]
|
||||
twist.angular.x = omega_bsat[0]
|
||||
twist.angular.y = omega_bsat[1]
|
||||
twist.angular.z = omega_bsat[2]
|
||||
print('control scheme: press "s" to hover')
|
||||
print("currently:\t linear vel x %.2f\t linear vel y %.2f\t linear vel z %.2f\t angular vel x %.2f\t angular vel y %.2f\t angular vel z %.2f" % (
|
||||
twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z))
|
||||
multi_cmd_vel_flu_pub[0].publish(twist)
|
||||
#####remember
|
||||
pdot_bsat_last = pdot_bsat
|
||||
omega_bsat_last = omega_bsat
|
||||
rospy.Rate(100).sleep()
|
||||
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_accel_flu_pub.publish(twist)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
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)
|
||||
|
|
@ -1,136 +1,194 @@
|
|||
#!/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
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
import heapq
|
||||
import copy
|
||||
|
||||
# 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
|
||||
import numpy
|
||||
import numpy
|
||||
import Queue
|
||||
|
||||
class Follower:
|
||||
|
||||
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
|
||||
self.f = 30 # 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 = 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
|
||||
self.arrive_print = True
|
||||
self.following_ids = []
|
||||
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
|
||||
self.Kp = 1
|
||||
self.velxy_max = 1 #0.8
|
||||
self.following_count = 0 # the number of followers of this uav
|
||||
self.Kp = 0.7
|
||||
self.velxy_max = 1 # 0.8
|
||||
self.velz_max = 1
|
||||
self.following_local_pose = [PoseStamped() for i in range(self.uav_num)]
|
||||
self.following_local_pose_sub = [None]*self.uav_num
|
||||
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 = [[] for i in range(
|
||||
self.uav_num)]
|
||||
self.arrive_count = 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.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)
|
||||
for i in range(self.uav_num):
|
||||
self.following_local_pose_sub[i] = rospy.Subscriber(
|
||||
self.uav_type + '_' + str(i) + "/mavros/local_position/pose", PoseStamped,
|
||||
self.following_local_pose_callback, i)
|
||||
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 = copy.deepcopy(msg)
|
||||
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:
|
||||
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] = copy.deepcopy(msg)
|
||||
|
||||
# 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
|
||||
self.formation_config = msg.data
|
||||
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
self.avoid_vel = copy.deepcopy(msg)
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
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")
|
||||
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):
|
||||
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)
|
||||
if self.formation_config=='waiting':
|
||||
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) #self.get_L_central_matrix()
|
||||
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_matrix(formation_dict[self.formation_config])
|
||||
#self.L_matrix = self.get_L_central_matrix()
|
||||
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])
|
||||
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.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.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()
|
||||
self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config])
|
||||
# Get a new formation pattern of UAVs based on KM.
|
||||
# self.L_matrix = self.get_L_matrix(self.orig_formation)
|
||||
self.new_formation = self.get_new_formation(self.change_id,
|
||||
formation_dict[self.formation_config])
|
||||
self.L_matrix = self.get_L_matrix(self.new_formation)
|
||||
#self.L_matrix = self.get_L_central_matrix()
|
||||
self.orig_formation=self.new_formation
|
||||
self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1)
|
||||
# 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
|
||||
|
||||
# for i in range(self.uav_num):
|
||||
# if not self.following_local_pose_sub[i] == None:
|
||||
# self.following_local_pose_sub[i].unregister()
|
||||
|
||||
self.following_count = len(self.following_ids)
|
||||
print(self.id, "UAV's following count: ",self.following_count)
|
||||
|
||||
self.cmd_vel_enu.linear = Vector3(0, 0, 0)
|
||||
input_vel = Vector3(0, 0, 0)
|
||||
# Code of the consensus protocol, see details on the paper.
|
||||
for following_id in 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 following_id[0] == 0:
|
||||
input_vel.x += self.following_local_pose[
|
||||
following_id[0]].pose.position.x - self.local_pose.pose.position.x + \
|
||||
self.new_formation[0, self.id - 1]
|
||||
input_vel.y += self.following_local_pose[
|
||||
following_id[0]].pose.position.y - self.local_pose.pose.position.y + \
|
||||
self.new_formation[1, self.id - 1]
|
||||
input_vel.z += self.following_local_pose[
|
||||
following_id[0]].pose.position.z - self.local_pose.pose.position.z + \
|
||||
self.new_formation[2, self.id - 1]
|
||||
else:
|
||||
input_vel.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.new_formation[0, following_id[0] - 1]
|
||||
input_vel.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.new_formation[1, following_id[0] - 1]
|
||||
input_vel.z += self.following_local_pose[
|
||||
following_id[0]].pose.position.z - self.local_pose.pose.position.z + \
|
||||
self.new_formation[2, self.id - 1] - self.new_formation[2, following_id[0] - 1]
|
||||
if self.id == 4:
|
||||
print(following_id[0],"'s pose: ", self.following_local_pose[
|
||||
following_id[0]].pose.position)
|
||||
print(self.id , "'s my pose: ", self.local_pose.pose.position)
|
||||
print(input_vel)
|
||||
if self.following_count >0:
|
||||
self.cmd_vel_enu.linear.x = self.Kp * (input_vel.x/self.following_count) + self.avoid_vel.x
|
||||
self.cmd_vel_enu.linear.y = self.Kp * (input_vel.y/self.following_count) + self.avoid_vel.y
|
||||
self.cmd_vel_enu.linear.z = self.Kp * (input_vel.z/self.following_count) + self.avoid_vel.z
|
||||
else:
|
||||
self.cmd_vel_enu.linear.x = self.Kp * input_vel.x+ self.avoid_vel.x
|
||||
self.cmd_vel_enu.linear.y = self.Kp * input_vel.y + self.avoid_vel.y
|
||||
self.cmd_vel_enu.linear.z = self.Kp * input_vel.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:
|
||||
|
|
@ -146,199 +204,204 @@ class Follower:
|
|||
|
||||
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:
|
||||
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()
|
||||
|
||||
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)]
|
||||
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])
|
||||
# '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)]
|
||||
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
|
||||
|
||||
def find_path(self,i):
|
||||
|
||||
# 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])
|
||||
#print('1',slack_right)
|
||||
return False
|
||||
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
|
||||
|
||||
def KM(self):
|
||||
# Main body of KM algorithm.
|
||||
|
||||
for i in range(self.uav_num-1):
|
||||
#print(i)
|
||||
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
|
||||
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
|
||||
#print ('2',slack_right)
|
||||
for j, slack in enumerate(self.slack_right):
|
||||
if not self.visit_right[j] :
|
||||
d = min(d,slack)
|
||||
#print(d)
|
||||
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
|
||||
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
|
||||
self.slack_right[k] -= d
|
||||
return self.match_right
|
||||
|
||||
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]
|
||||
#print (change_id)
|
||||
for i in range(0,self.uav_num-1):
|
||||
position[:,i]=change_formation[:,i]
|
||||
# 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]
|
||||
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
|
||||
|
||||
#函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
|
||||
def get_L_matrix(self,rel_posi):
|
||||
|
||||
c_num=int((self.uav_num-1)/2)
|
||||
min_num_index_list = [0]*c_num
|
||||
|
||||
comm=[[]for i in range (self.uav_num)]
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
node_mid_flag=[]
|
||||
# Laplacian matrix
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
def get_L_matrix(self, rel_posi):
|
||||
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
c_num = int((self.uav_num - 1) / 2)
|
||||
min_num_index_list = [0] * c_num
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
comm = [[] for i in range(self.uav_num)]
|
||||
w = numpy.ones((self.uav_num, self.uav_num)) * 0
|
||||
nodes_next = []
|
||||
node_flag = [self.uav_num - 1]
|
||||
node_mid_flag = []
|
||||
|
||||
c=numpy.copy(rel_d)
|
||||
rel_d = [0] * (self.uav_num - 1)
|
||||
|
||||
for i in range(0, self.uav_num - 1):
|
||||
rel_d[i] = pow(rel_posi[0][i], 2) + pow(rel_posi[1][i], 2) + pow(rel_posi[2][i], 2)
|
||||
|
||||
c = numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
count = 0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
for j in range(0, c_num):
|
||||
for i in range(0, self.uav_num - 1):
|
||||
if rel_d[i] == c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count] = i
|
||||
node_mid_flag.append(i)
|
||||
count = count + 1
|
||||
if count == c_num:
|
||||
break
|
||||
if count == c_num:
|
||||
break
|
||||
|
||||
|
||||
for j in range(0,c_num):
|
||||
for j in range(0, c_num):
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[self.uav_num-1].append(min_num_index_list[j])
|
||||
|
||||
size_=len(node_flag)
|
||||
comm[self.uav_num - 1].append(min_num_index_list[j])
|
||||
|
||||
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
|
||||
size_ = len(node_flag)
|
||||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
min_num_index_list = [0]*c_num
|
||||
node_mid_flag=[]
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000
|
||||
while (nodes_next != []) and (size_ < (self.uav_num - 1)):
|
||||
|
||||
next_node = nodes_next[0]
|
||||
nodes_next = nodes_next[1:]
|
||||
min_num_index_list = [0] * c_num
|
||||
node_mid_flag = []
|
||||
rel_d = [0] * (self.uav_num - 1)
|
||||
for i in range(0, self.uav_num - 1):
|
||||
|
||||
if i == next_node or i in node_flag:
|
||||
|
||||
rel_d[i] = 2000
|
||||
else:
|
||||
|
||||
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
|
||||
rel_d[i] = pow((rel_posi[0][i] - rel_posi[0][next_node]), 2) + pow(
|
||||
(rel_posi[1][i] - rel_posi[1][next_node]), 2) + pow((rel_posi[2][i] - rel_posi[2][next_node]),
|
||||
2)
|
||||
c = numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count = 0
|
||||
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
for j in range(0, c_num):
|
||||
for i in range(0, self.uav_num - 1):
|
||||
if rel_d[i] == c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count] = i
|
||||
node_mid_flag.append(i)
|
||||
count = count + 1
|
||||
if count == c_num:
|
||||
break
|
||||
if count == c_num:
|
||||
break
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
size_ = len(node_flag)
|
||||
|
||||
for j in range(0,c_num):
|
||||
for j in range(0, c_num):
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
nodes_next = nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
nodes_next = nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
||||
if i==0:
|
||||
if j in comm[self.uav_num-1]:
|
||||
w[j+1][i]=1
|
||||
for i in range(0, self.uav_num):
|
||||
for j in range(0, self.uav_num - 1):
|
||||
if i == 0:
|
||||
if j in comm[self.uav_num - 1]:
|
||||
w[j + 1][i] = 1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
w[j + 1][i] = 0
|
||||
else:
|
||||
if j in comm[i-1]:
|
||||
w[j+1][i]=1
|
||||
if j in comm[i - 1] and i < (j+1):
|
||||
w[j + 1][i] = 1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w
|
||||
for i in range (0,self.uav_num):
|
||||
w[j + 1][i] = 0
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
L = w
|
||||
for i in range(0, self.uav_num):
|
||||
L[i][i] = -sum(w[i])
|
||||
|
||||
return L
|
||||
#central-station control
|
||||
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
|
||||
|
||||
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],[1,1,-2,0,0,0],[1,1,1,-3,0,0],[1,1,1,1,-4,0],[1,1,1,1,1,-5]])
|
||||
# L=numpy.array([[-0., 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0.], [ 1. ,-1., 0., 0., 0., 0., 0., 0., 0.], [ 1., 1., -2., 0., 0., 0. , 0. , 0. , 0.],
|
||||
# [ 1., 1., 1., -3., 0., 0., 0., 0., 0.],[ 1., 1., 1., 1., -4., 0., 0., 0., 0.],
|
||||
# [0., 1., 1., 0., 1., - 3., 0., 0., 0.],[ 0., 0., 1., 1., 1., 1., -4., 0., 0.],
|
||||
# [0., 0., 0., 1., 1., 1., 1., - 4., 0.], [ 0., 0., 0., 1., 1., 1., 1., 1., -5.]])
|
||||
|
||||
return L
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
follower = Follower(sys.argv[1],int(sys.argv[2]),int(sys.argv[3]))
|
||||
follower.loop()
|
||||
follower = Follower(sys.argv[1], int(sys.argv[2]), int(sys.argv[3]))
|
||||
follower.loop()
|
||||
|
|
@ -3,6 +3,6 @@ python2.7 leader.py $1 $2 &
|
|||
uav_num=1
|
||||
while(( $uav_num< $2 ))
|
||||
do
|
||||
python2.7 follower.py $1 $uav_num $2&
|
||||
python2.7 follower_consensus.py $1 $uav_num $2&
|
||||
let "uav_num++"
|
||||
done
|
||||
|
|
|
|||
|
|
@ -1,2 +1,2 @@
|
|||
kill -9 $(ps -ef|grep follower.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
|
||||
kill -9 $(ps -ef|grep follower_consensus.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,602 @@
|
|||
<?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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="0"/>
|
||||
<arg name="y" value="12"/>
|
||||
<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="3"/>
|
||||
<arg name="y" value="12"/>
|
||||
<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="6"/>
|
||||
<arg name="y" value="12"/>
|
||||
<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="9"/>
|
||||
<arg name="y" value="12"/>
|
||||
<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="12"/>
|
||||
<arg name="y" value="12"/>
|
||||
<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>
|
||||
</launch>
|
||||
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->
|
||||
|
|
@ -0,0 +1,428 @@
|
|||
<?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>
|
||||
</launch>
|
||||
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->
|
||||
Loading…
Reference in New Issue