From e348874e81d4a205fb636b1c0cb16091ed20b814 Mon Sep 17 00:00:00 2001 From: Robin Shaun Date: Thu, 7 May 2020 16:53:02 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=9B=BA=E5=AE=9A=E7=BF=BC?= =?UTF-8?q?=E7=9A=84=E5=A4=9A=E6=9C=BAoffboard=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../1.11/multirotor_communication.py | 2 +- communication/1.11/plane_communication.py | 28 +- communication/1.11/rover_communication.py | 34 +-- communication/1.11/vtol_communication.py | 148 ++++++++++ ... mulitrotor_keyboard_multi_control_vel.py} | 21 +- ...multirotor_keyboard_multi_control_accel.py | 273 ++++++++++++++++++ .../1.11/plane_keyboard_multi_control.py | 249 ++++++++++++++++ .../1.11/rover_keyboard_multi_control.py | 2 +- 8 files changed, 707 insertions(+), 50 deletions(-) create mode 100644 communication/1.11/vtol_communication.py rename coordination/1.11/{mulitrotor_keyboard_multi_control copy.py => mulitrotor_keyboard_multi_control_vel.py} (93%) create mode 100755 coordination/1.11/multirotor_keyboard_multi_control_accel.py create mode 100755 coordination/1.11/plane_keyboard_multi_control.py diff --git a/communication/1.11/multirotor_communication.py b/communication/1.11/multirotor_communication.py index 9549e87..7c5a759 100644 --- a/communication/1.11/multirotor_communication.py +++ b/communication/1.11/multirotor_communication.py @@ -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') diff --git a/communication/1.11/plane_communication.py b/communication/1.11/plane_communication.py index 423f1d0..0477504 100644 --- a/communication/1.11/plane_communication.py +++ b/communication/1.11/plane_communication.py @@ -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]) diff --git a/communication/1.11/rover_communication.py b/communication/1.11/rover_communication.py index adecf6a..c451f8d 100644 --- a/communication/1.11/rover_communication.py +++ b/communication/1.11/rover_communication.py @@ -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: diff --git a/communication/1.11/vtol_communication.py b/communication/1.11/vtol_communication.py new file mode 100644 index 0000000..657861f --- /dev/null +++ b/communication/1.11/vtol_communication.py @@ -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() + diff --git a/coordination/1.11/mulitrotor_keyboard_multi_control copy.py b/coordination/1.11/mulitrotor_keyboard_multi_control_vel.py similarity index 93% rename from coordination/1.11/mulitrotor_keyboard_multi_control copy.py rename to coordination/1.11/mulitrotor_keyboard_multi_control_vel.py index 263d2f9..00c9271 100755 --- a/coordination/1.11/mulitrotor_keyboard_multi_control copy.py +++ b/coordination/1.11/mulitrotor_keyboard_multi_control_vel.py @@ -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) diff --git a/coordination/1.11/multirotor_keyboard_multi_control_accel.py b/coordination/1.11/multirotor_keyboard_multi_control_accel.py new file mode 100755 index 0000000..e67c79d --- /dev/null +++ b/coordination/1.11/multirotor_keyboard_multi_control_accel.py @@ -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) diff --git a/coordination/1.11/plane_keyboard_multi_control.py b/coordination/1.11/plane_keyboard_multi_control.py new file mode 100755 index 0000000..5139137 --- /dev/null +++ b/coordination/1.11/plane_keyboard_multi_control.py @@ -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) diff --git a/coordination/1.11/rover_keyboard_multi_control.py b/coordination/1.11/rover_keyboard_multi_control.py index 263d2f9..f15ca9d 100755 --- a/coordination/1.11/rover_keyboard_multi_control.py +++ b/coordination/1.11/rover_keyboard_multi_control.py @@ -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):