forked from xtdrone/XTDrone
实现固定翼的多机offboard控制
This commit is contained in:
parent
e00541e9f3
commit
e348874e81
|
|
@ -18,7 +18,7 @@ class Communication:
|
|||
|
||||
def __init__(self, vehicle_id):
|
||||
|
||||
if vehicle_type == 'iris' or 'typhoon' or 'solo':
|
||||
if vehicle_type == 'iris' or vehicle_type == 'typhoon' or vehicle_type == 'solo':
|
||||
self.vehicle_type = vehicle_type
|
||||
else:
|
||||
print('only support iris, typhoon and solo for multirotors')
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@ class Communication:
|
|||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.target_motion = PositionTarget()
|
||||
self.global_target = None
|
||||
self.arm_state = False
|
||||
|
|
@ -35,7 +34,7 @@ class Communication:
|
|||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
#self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
|
|
@ -50,9 +49,9 @@ class Communication:
|
|||
'''
|
||||
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)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
|
||||
|
||||
print(self.vehicle_type+self.vehicle_id+": "+"communication initialized")
|
||||
print("communication initialized")
|
||||
|
||||
def start(self):
|
||||
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
|
||||
|
|
@ -63,7 +62,7 @@ class Communication:
|
|||
while(rospy.is_shutdown):
|
||||
self.target_motion_pub.publish(self.target_motion)
|
||||
try:
|
||||
response = self.gazeboModelstate (self.vehicle+self.vehicle_id,'ground_plane')
|
||||
response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
|
||||
except rospy.ServiceException, e:
|
||||
print "Gazebo model state service call failed: %s"%e
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
|
@ -75,8 +74,6 @@ class Communication:
|
|||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def construct_target(self, x=0, y=0, z=0):
|
||||
target_raw_pose = PositionTarget()
|
||||
|
|
@ -98,10 +95,10 @@ class Communication:
|
|||
return target_raw_pose
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=9,motion_type=0)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=1,motion_type=0)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
|
|
@ -117,8 +114,12 @@ class Communication:
|
|||
self.arm_state = False
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
else:
|
||||
elif msg.data == 'takeoff' or msg.data == 'land' or msg.data == 'loiter' or msg.data == 'idle':
|
||||
self.mission = msg.data
|
||||
print(self.mission)
|
||||
else:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
|
|
@ -133,6 +134,13 @@ class Communication:
|
|||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
|
||||
return False
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
communication = Communication(sys.argv[1])
|
||||
|
|
|
|||
|
|
@ -25,7 +25,6 @@ class Communication:
|
|||
self.target_motion = PositionTarget()
|
||||
self.global_target = None
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.motion_type = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
|
@ -35,7 +34,6 @@ class Communication:
|
|||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
|
|
@ -81,9 +79,6 @@ class Communication:
|
|||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, yaw=0, coordinate_frame=1, motion_type=1):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.coordinate_frame = coordinate_frame
|
||||
|
|
@ -100,25 +95,25 @@ class Communication:
|
|||
|
||||
if(motion_type == 0):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE
|
||||
if(motion_type == 1):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||
+ IGNORE_YAW_RATE
|
||||
+ PositionTarget.IGNORE_YAW_RATE
|
||||
|
||||
return target_raw_pose
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=9,motion_type=0)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=0,coordinate_frame=9,motion_type=0)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=msg.angular.z,coordinate_frame=1,motion_type=0)
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=0,coordinate_frame=1,motion_type=0)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=msg.angular.z,coordinate_frame=8,motion_type=1)
|
||||
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=0,coordinate_frame=8,motion_type=1)
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=msg.angular.z,coordinate_frame=1,motion_type=1)
|
||||
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw=0,coordinate_frame=1,motion_type=1)
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
|
|
@ -143,27 +138,11 @@ class Communication:
|
|||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
|
|
@ -180,7 +159,6 @@ class Communication:
|
|||
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
self.hover_flag = 0
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
|
|
|
|||
|
|
@ -0,0 +1,148 @@
|
|||
import rospy
|
||||
import tf
|
||||
import yaml
|
||||
from mavros_msgs.msg import GlobalPositionTarget, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandVtolTransition, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, Pose
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
from multiprocessing import Process
|
||||
import sys
|
||||
|
||||
class Communication:
|
||||
|
||||
def __init__(self, vehicle_type, vehicle_id):
|
||||
|
||||
self.vehicle_type = vehicle_type
|
||||
self.vehicle_id = vehicle_id
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.target_motion = PositionTarget()
|
||||
self.global_target = None
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.motion_type = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
#self.imu_sub = rospy.Subscriber(self.vehicle_type+self.vehicle_id+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+self.vehicle_id+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher(self.vehicle_type+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy(self.vehicle_type+self.vehicle_id+"/mavros/set_mode", SetMode)
|
||||
self.gazeboModelstate = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
|
||||
|
||||
print("communication initialized")
|
||||
|
||||
def start(self):
|
||||
rospy.init_node(self.vehicle_type+self.vehicle_id+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
self.target_motion_pub.publish(self.target_motion)
|
||||
try:
|
||||
response = self.gazeboModelstate (self.vehicle_type+'_'+self.vehicle_id,'ground_plane')
|
||||
except rospy.ServiceException, e:
|
||||
print "Gazebo model state service call failed: %s"%e
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
if(self.disarm()):
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
|
||||
def construct_target(self, x=0, y=0, z=0):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.coordinate_frame = 1
|
||||
|
||||
target_raw_pose.position.x = x
|
||||
target_raw_pose.position.y = y
|
||||
target_raw_pose.position.z = z
|
||||
|
||||
if self.mission == 'takeoff':
|
||||
target_raw_pose.type_mask = 4096
|
||||
elif self.mission == 'land':
|
||||
target_raw_pose.type_mask = 8192
|
||||
elif self.mission == 'loiter':
|
||||
target_raw_pose.type_mask = 12288
|
||||
else:
|
||||
target_raw_pose.type_mask = 16384
|
||||
|
||||
return target_raw_pose
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print(self.vehicle_type+self.vehicle_id+": "+'armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'takeoff' or msg.data == 'land' or msg.data == 'loiter' or msg.data == 'idle':
|
||||
self.mission = msg.data
|
||||
print(self.mission)
|
||||
else:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": arming failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": disarming failed!")
|
||||
return False
|
||||
def flight_mode_switch(self):
|
||||
if self.flightModeService(custom_mode=self.flight_mode):
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print(self.vehicle_type+self.vehicle_id+": "+self.flight_mode+"failed")
|
||||
return False
|
||||
|
||||
if __name__ == '__main__':
|
||||
communication = Communication(sys.argv[1],sys.argv[2])
|
||||
communication.start()
|
||||
|
||||
|
|
@ -5,7 +5,7 @@ import tty, termios
|
|||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 20
|
||||
MAX_LIN_VEL = 10
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.1
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
|
@ -116,13 +116,14 @@ if __name__=="__main__":
|
|||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rover_num = int(sys.argv[1])
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*rover_num
|
||||
multi_cmd_pub = [None]*rover_num
|
||||
for i in range(rover_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/rover'+str(i)+'/cmd',String,queue_size=10)
|
||||
multirotor_type = sys.argv[1]
|
||||
multirotor_num = int(sys.argv[2])
|
||||
rospy.init_node('multirotor_keyboard_multi_control')
|
||||
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_pub = rospy.Publisher("/xtdrone/leader/cmd_vel", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
cmd= String()
|
||||
|
|
@ -240,7 +241,7 @@ if __name__=="__main__":
|
|||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
for i in range(rover_num):
|
||||
for i in range(multirotor_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(twist)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
|
|
@ -257,7 +258,7 @@ if __name__=="__main__":
|
|||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(rover_num):
|
||||
for i in range(multirotor_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(twist)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
|
|
@ -0,0 +1,273 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False
|
||||
uav_num = int(sys.argv[1])
|
||||
if uav_num == 9:
|
||||
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
if uav_num == 6:
|
||||
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
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 accelebration (-1~1)
|
||||
a/d : increase/decrease leftward accelebration (-1~1)
|
||||
i/, : increase/decrease upward accelebration (-1~1)
|
||||
j/l : increase/decrease angular accelebration (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff/land
|
||||
b : offboard
|
||||
s : hover(offboard mode) and remove the mask of keyboard control
|
||||
k : hover(hover mode) and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
msg2leader = """
|
||||
Control Your XTDrone!
|
||||
To the leader (press g to control all drones)
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d g j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward accelebration (-1~1)
|
||||
a/d : increase/decrease leftward accelebration (-1~1)
|
||||
i/, : increase/decrease upward accelebration (-1~1)
|
||||
j/l : increase/decrease angular accelebration (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover and remove the mask of keyboard control
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control all drones
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg():
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*uav_num
|
||||
multi_cmd_pub = [None]*uav_num
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd_accel_enu', Vector3, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_accel", Vector3, queue_size=10)
|
||||
formation_switch_pub = rospy.Publisher("/gcs/formation_switch", String, queue_size=10)
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
accel = Vector3()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
#cmd = 'AUTO.TAKEOFF'
|
||||
cmd = ''
|
||||
print(msg)
|
||||
print('Takeoff mode is disenabled now')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('Landing')
|
||||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = formation_configs[i]
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
accel = twist.linear
|
||||
|
||||
for i in range(uav_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(accel)
|
||||
if not cmd == '':
|
||||
formation_switch_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
multi_cmd_vel_flu_pub[i].publish(accel)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
accel.x = 0.0; accel.y = 0.0; accel.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(accel)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -0,0 +1,249 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Pose
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 20
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.1
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
|
||||
ctrl_leader = False
|
||||
send_flag = 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 north setpoint
|
||||
a/d : increase/decrease east setpoint
|
||||
i/, : increase/decrease upward setpoint
|
||||
j/l : increase/decrease orientation
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : AUTO.TAKEOFF/AUTO.LAND
|
||||
b : offboard
|
||||
s : loiter
|
||||
k : idle
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
this will mask the keyboard control
|
||||
g : control the leader
|
||||
o : send setpoint
|
||||
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 north setpoint
|
||||
a/d : increase/decrease east setpoint
|
||||
i/, : increase/decrease upward setpoint
|
||||
j/l : increase/decrease orientation
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : AUTO.TAKEOFF/AUTO.LAND
|
||||
b : offboard
|
||||
s : loiter
|
||||
k : idle
|
||||
0~9 : extendable mission(eg.different formation configuration)
|
||||
g : control all drones
|
||||
o : send setpoint
|
||||
CTRL-C to quit
|
||||
"""
|
||||
|
||||
e = """
|
||||
Communications Failed
|
||||
"""
|
||||
|
||||
def getKey():
|
||||
tty.setraw(sys.stdin.fileno())
|
||||
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
|
||||
if rlist:
|
||||
key = sys.stdin.read(1)
|
||||
else:
|
||||
key = ''
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
return key
|
||||
|
||||
def print_msg():
|
||||
if ctrl_leader:
|
||||
print(msg2leader)
|
||||
else:
|
||||
print(msg2all)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_orientation_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t orientation %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_orientation_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkpositionLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkorientationLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
plane_num = int(sys.argv[1])
|
||||
rospy.init_node('plane_keyboard_multi_control')
|
||||
multi_cmd_vel_flu_pub = [None]*plane_num
|
||||
multi_cmd_pub = [None]*plane_num
|
||||
for i in range(plane_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/plane'+str(i)+'/cmd_pose_enu', Pose, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/plane'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_pose", Pose, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
cmd= String()
|
||||
pose = Pose()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_orientation_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_orientation_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkpositionLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkpositionLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkpositionLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkpositionLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkpositionLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkpositionLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'j':
|
||||
target_orientation_vel = checkorientationLimitVelocity(target_orientation_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'l':
|
||||
target_orientation_vel = checkorientationLimitVelocity(target_orientation_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_orientation_vel))
|
||||
elif key == 'r':
|
||||
cmd = 'AUTO.RTL'
|
||||
print_msg()
|
||||
print('Returning home')
|
||||
elif key == 't':
|
||||
cmd = 'ARM'
|
||||
print_msg()
|
||||
print('Arming')
|
||||
elif key == 'y':
|
||||
cmd = 'DISARM'
|
||||
print_msg()
|
||||
print('Disarming')
|
||||
elif key == 'v':
|
||||
cmd = 'AUTO.TAKEOFF'
|
||||
print_msg()
|
||||
print('AUTO.TAKEOFF')
|
||||
elif key == 'b':
|
||||
cmd = 'OFFBOARD'
|
||||
print_msg()
|
||||
print('Offboard')
|
||||
elif key == 'n':
|
||||
cmd = 'AUTO.LAND'
|
||||
print_msg()
|
||||
print('AUTO.LAND')
|
||||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 's':
|
||||
cmd = 'loiter'
|
||||
print_msg()
|
||||
print('loiter')
|
||||
elif key == 'k' :
|
||||
cmd = 'idle'
|
||||
print_msg()
|
||||
print('idle')
|
||||
elif key == 'o':
|
||||
send_flag = True
|
||||
print_msg()
|
||||
print('send setpoint')
|
||||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = 'mission'+key
|
||||
print_msg()
|
||||
print(cmd)
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
pose.position.x = control_forward_vel; pose.position.y = control_leftward_vel; pose.position.z = control_upward_vel
|
||||
|
||||
control_orientation_vel = makeSimpleProfile(control_orientation_vel, target_orientation_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = control_orientation_vel
|
||||
for i in range(plane_num):
|
||||
if ctrl_leader:
|
||||
if send_flag:
|
||||
leader_cmd_vel_pub.publish(pose)
|
||||
leader_cmd_pub.publish(cmd)
|
||||
else:
|
||||
if send_flag:
|
||||
multi_cmd_vel_flu_pub[i].publish(pose)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -117,7 +117,7 @@ if __name__=="__main__":
|
|||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rover_num = int(sys.argv[1])
|
||||
rospy.init_node('keyboard_control')
|
||||
rospy.init_node('rover_keyboard_multi_control')
|
||||
multi_cmd_vel_flu_pub = [None]*rover_num
|
||||
multi_cmd_pub = [None]*rover_num
|
||||
for i in range(rover_num):
|
||||
|
|
|
|||
Loading…
Reference in New Issue