diff --git a/README.md b/README.md index 93d5c87..56e3773 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # tello_driver +[![Build Status](http://build.ros.org/job/Ksrc_uX__tello_driver__ubuntu_xenial__source/badge/icon)](http://build.ros.org/job/Ksrc_uX__tello_driver__ubuntu_xenial__source/) + ## 1. Overview Communicating with the Tello drone can be done either using official [Tello SDK](https://dl-cdn.ryzerobotics.com/downloads/Tello/Tello%20SDK%202.0%20User%20Guide.pdf) or one of the unofficial libraries. The unofficial libraries originated from the reverse-engineering the raw packages broadcasted by the Tello. This ROS package is build on top of the unofficial [TelloPy](https://github.com/hanyazou/TelloPy) library. The [TelloPy](https://github.com/hanyazou/TelloPy) library is used at this moment since it offers more functionalities than the official [Tello SDK](https://dl-cdn.ryzerobotics.com/downloads/Tello/Tello%20SDK%202.0%20User%20Guide.pdf) or any other unofficial library. @@ -23,7 +25,6 @@ Developing of the tello_driver ROS package is inspired by [tello_driver](https:/ * Connect to drone's WiFi access point (```TELLO_XXXXXX)``` * ```$ roslaunch tello_driver tello_node.launch``` - ## 2. Nodes ### tello_driver_node.py @@ -40,19 +41,17 @@ Main node running as interface for the TelloPy library * ```/tello/takeoff``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) * ```/tello/throw_takeoff``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) -### Published topics +#### Published topics * ```/tello/camera/camera_info``` [sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) * ```/tello/image_raw``` [sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html) * ```/tello/imag/raw/h264``` [h264_image_transport/H264Packet](https://github.com/tilk/h264_image_transport/blob/master/msg/H264Packet.msg) * ```/tello/odom``` [nav_msgs/Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) * ```/tello/status``` [tello_driver/TelloStatus](https://github.com/appie-17/tello_driver/blob/development/msg/TelloStatus.msg) -### Services +#### Services +TODO -### Parameters -* ```~/image_raw/compressed/format``` -* ```~/image_raw/compressed/jpeg_quality``` -* ```~/image_raw/compressed/png_level``` +#### Parameters * ```~/tello_driver_node/connect_timeout_sec``` * ```~/tello_driver_node/fixed_video_rate``` * ```~/tello_driver_node/local_cmd_client_port``` @@ -63,12 +62,51 @@ Main node running as interface for the TelloPy library * ```~/tello_driver_node/vel_cmd_scale``` * ```~/tello_driver_node/video_req_sps_hz``` +### gamepad_teleop_node.py +Converting gamepad input controls from ```joy_node``` to commands for ```tello_driver_node.py``` + +#### Subscribed topics +* ```/joy``` [sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) +* ```/tello/agent_cmd_vel_in``` [geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) + +#### Published topic +* ```/tello/cmd_vel``` [geometry_msgs/Twist](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) +* ```/tello/fast_mode``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) +* ```/tello/flattrim``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) +* ```/tello/flip``` [std_msgs/Uint8](http://docs.ros.org/api/std_msgs/html/msg/UInt8.html) +* ```/tello/land``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) +* ```/tello/palm_land``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) +* ```/tello/reset``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) +* ```/tello/takeoff``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) +* ```/tello/throw_takeoff``` [std_msgs/Empty](http://docs.ros.org/api/std_msgs/html/msg/Empty.html) + +#### Services +None + +#### Parameters + +### joy_node +Receive input from gamepad controller and publish into ```sensor_msgs/Joy``` message + +### Subscribed topics +None + +### Published topics +* ```/joy``` [sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) + +### Services +None + +### Parameters +* ```~/joy_node/deadzone``` +* ```~/joy_node/dev``` ## 3. Troubleshooting ## 4. Notes +*When using PyAV to decode raw video instead of streaming h264 video, required to relaunch the ```tello_driver_node.py``` to continue the video stream after disconnected WiFi connection. -## 5. WIP +## 5. Work-in-progress ## 6. License diff --git a/cfg/960x720.yaml b/cfg/960x720.yaml new file mode 100644 index 0000000..bd5c971 --- /dev/null +++ b/cfg/960x720.yaml @@ -0,0 +1,20 @@ +image_width: 960 +image_height: 720 +camera_name: camera_front +camera_matrix: + rows: 3 + cols: 3 + data: [929.562627, 0.000000, 487.474037, 0.000000, 928.604856, 363.165223, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.016272, 0.093492, 0.000093, 0.002999, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [937.878723, 0.000000, 489.753885, 0.000000, 0.000000, 939.156738, 363.172139, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/cfg/Tello.cfg b/cfg/Tello.cfg index 5edefe7..20f2898 100755 --- a/cfg/Tello.cfg +++ b/cfg/Tello.cfg @@ -17,7 +17,10 @@ gen.add("video_req_sps_hz", double_t, 0, "Rate for regularly requesting SPS data from drone (0: disabled)", 0.5, 0.0, 4.0) -# Limit velocity control (Jordy) +# Scale velocity control (Jordy) gen.add("vel_cmd_scale", double_t, 0, "Scale (down) vel_cmd value", 0.5, 0.01, 1.0) +# Limit attitude control (Jordy) +gen.add("att_limit", double_t, 0, "Limit attitude of Tello", 15, 5, 90) + exit(gen.generate(PACKAGE, "tello_driver_node.py", "Tello")) diff --git a/launch/joy_teleop.launch b/launch/joy_teleop.launch index b95449c..86e84b4 100644 --- a/launch/joy_teleop.launch +++ b/launch/joy_teleop.launch @@ -9,7 +9,7 @@ - + diff --git a/launch/logger.launch b/launch/logger.launch index 3a62273..614fdf1 100644 --- a/launch/logger.launch +++ b/launch/logger.launch @@ -1,3 +1,4 @@ + - - - - - + + + + + + - - - - - - - - - + + + + + + + + + + + + + + + - - diff --git a/launch/two_drones_test.launch b/launch/two_drones_test.launch deleted file mode 100644 index ab5ba41..0000000 --- a/launch/two_drones_test.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/package.xml b/package.xml index 2bbc150..4bea109 100644 --- a/package.xml +++ b/package.xml @@ -2,6 +2,7 @@ tello_driver 0.2.0 + >This package provides a ROS interface for the TelloPy library. Development of this ROS package pursues not to modify the TelloPy library, instead apply any modification or addition to the ros_driver package in an encapsulated manner. @@ -12,8 +13,10 @@ + http://www.ros.org/wiki/tello_driver https://github.com/appie-17/tello_driver - + https://github.com/appie-17/tello_driver/issues + Apache2 diff --git a/src/gamepad_marshall_node.py b/src/gamepad_teleop_node.py similarity index 74% rename from src/gamepad_marshall_node.py rename to src/gamepad_teleop_node.py index 9f564bc..238304b 100755 --- a/src/gamepad_marshall_node.py +++ b/src/gamepad_teleop_node.py @@ -7,10 +7,10 @@ class GamepadState: def __init__(self): - self.A = False - self.B = False - self.X = False - self.Y = False + self.Cr = False + self.Ci = False + self.Sq = False + self.Tr = False self.Start = False self.Select = False self.Sync = False @@ -35,10 +35,10 @@ def parse_ps3_usb(self, msg): if len(msg.buttons) != 17 or len(msg.axes) != 6: raise ValueError('Invalid number of buttons (%d) or axes (%d)' % ( len(msg.buttons), len(msg.axes))) - self.A = msg.buttons[0] - self.B = msg.buttons[1] - self.Y = msg.buttons[2] - self.X = msg.buttons[3] + self.Cr = msg.buttons[0] + self.Ci = msg.buttons[1] + self.Tr = msg.buttons[2] + self.Sq = msg.buttons[3] self.L1 = msg.buttons[4] self.R1 = msg.buttons[5] self.L2 = msg.buttons[6] @@ -75,10 +75,10 @@ def parse_ps3_usb2(self, msg): self.R2 = msg.buttons[9] self.L1 = msg.buttons[10] self.R1 = msg.buttons[11] - self.Y = msg.buttons[12] - self.B = msg.buttons[13] - self.A = msg.buttons[14] - self.X = msg.buttons[15] + self.Tr = msg.buttons[12] + self.Ci = msg.buttons[13] + self.Cr = msg.buttons[14] + self.Sq = msg.buttons[15] self.Sync = msg.buttons[16] self.LX = msg.axes[0] self.LY = msg.axes[1] @@ -100,7 +100,7 @@ def parse(self, msg): raise err -class GamepadMarshallNode: +class GamepadTeleopNode: MAX_FLIP_DIR = 7 def __init__(self): @@ -109,9 +109,10 @@ def __init__(self): # if None then not in agent mode, otherwise contains time of latest enable/ping self.agent_mode_t = None self.flip_dir = 0 + self.fast_mode = False # Start ROS node - rospy.init_node('gamepad_marshall_node') + rospy.init_node('gamepad_teleop_node') # Load parameters self.agent_mode_timeout_sec = rospy.get_param( @@ -119,14 +120,16 @@ def __init__(self): self.pub_takeoff = rospy.Publisher( 'takeoff', Empty, queue_size=1, latch=False) + self.pub_manual_takeoff = rospy.Publisher( + 'manual_takeoff', Empty, queue_size=1, latch=False) self.pub_throw_takeoff = rospy.Publisher( 'throw_takeoff', Empty, queue_size=1, latch=False) self.pub_land = rospy.Publisher( 'land', Empty, queue_size=1, latch=False) self.pub_palm_land = rospy.Publisher( 'palm_land', Empty, queue_size=1, latch=False) - self.pub_reset = rospy.Publisher( - 'reset', Empty, queue_size=1, latch=False) + self.pub_emergency = rospy.Publisher( + 'emergency', Empty, queue_size=1, latch=False) self.pub_flattrim = rospy.Publisher( 'flattrim', Empty, queue_size=1, latch=False) self.pub_flip = rospy.Publisher( @@ -134,11 +137,13 @@ def __init__(self): self.pub_cmd_out = rospy.Publisher( 'cmd_vel', Twist, queue_size=10, latch=False) self.pub_fast_mode = rospy.Publisher( - 'fast_mode', Bool, queue_size=1, latch=False) + 'fast_mode', Empty, queue_size=1, latch=False) + self.pub_video_mode = rospy.Publisher( + 'video_mode', Empty, queue_size=1, latch=False) self.sub_agent_cmd_in = rospy.Subscriber( 'agent_cmd_vel_in', Twist, self.agent_cmd_cb) self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_cb) - rospy.loginfo('Gamepad marshall node initialized') + rospy.loginfo('Gamepad teleop node initialized') def agent_cmd_cb(self, msg): if self.agent_mode_t is not None: @@ -153,23 +158,28 @@ def joy_cb(self, msg): self.joy_state.parse(msg) # Process emergency stop - if not self.joy_state_prev.B and self.joy_state.B: - self.pub_reset.publish() + if not self.joy_state_prev.R2 and self.joy_state.R2: + self.pub_emergency.publish() #rospy.logwarn('Issued RESET') return # Process takeoff - if not self.joy_state_prev.Start and self.joy_state.Start: + if not self.joy_state_prev.L1 and self.joy_state.L1: self.pub_takeoff.publish() #rospy.logwarn('Issued TAKEOFF') + # Process manual takeoff + if self.joy_state.L2: + self.pub_manual_takeoff.publish() + #rospy.logwarn('Issued MANUAL_TAKEOFF') + # Process throw takeoff if not self.joy_state_prev.DU and self.joy_state.DU: self.pub_throw_takeoff.publish() #rospy.logwarn('Issued THROW_TAKEOFF') # Process land - if not self.joy_state_prev.Select and self.joy_state.Select: + if not self.joy_state_prev.R1 and self.joy_state.R1: self.pub_land.publish() #rospy.logwarn('Issued LAND') @@ -178,11 +188,17 @@ def joy_cb(self, msg): self.pub_palm_land.publish() #rospy.logwarn('Issued PALM_LAND') - if not self.joy_state_prev.X and self.joy_state.X: + # Process flat trim + if not self.joy_state_prev.Sq and self.joy_state.Sq: self.pub_flattrim.publish() #rospy.logwarn('Issued FLATTRIM') + + # Process video mode + if not self.joy_state_prev.Tr and self.joy_state.Tr: + self.pub_video_mode.publish() + #rospy.logwarn('Issued VIDEO_MODE') - if not self.joy_state_prev.Y and self.joy_state.Y: + if not self.joy_state_prev.Sq and self.joy_state.Sq: self.pub_flip.publish(self.flip_dir) #rospy.logwarn('Issued FLIP %d' % self.flip_dir) self.flip_dir += 1 @@ -190,23 +206,21 @@ def joy_cb(self, msg): self.flip_dir = 0 # Update agent bypass mode - if self.joy_state.L2: + if self.joy_state.Select: self.agent_mode_t = rospy.Time.now() else: self.agent_mode_t = None # Manual control mode if self.agent_mode_t is None: - if not self.joy_state_prev.R2 and self.joy_state.R2: - self.pub_fast_mode.publish(True) - elif self.joy_state_prev.R2 and not self.joy_state.R2: - self.pub_fast_mode.publish(False) + if not self.joy_state_prev.Start and self.joy_state.Start: + self.pub_fast_mode.publish() cmd = Twist() - cmd.linear.x = self.joy_state.LY - cmd.linear.y = self.joy_state.LX - cmd.linear.z = self.joy_state.RY - cmd.angular.z = self.joy_state.RX + cmd.linear.x = -self.joy_state.RX + cmd.linear.y = self.joy_state.RY + cmd.linear.z = self.joy_state.LY + cmd.angular.z = -self.joy_state.LX self.pub_cmd_out.publish(cmd) # Copy to previous state @@ -218,7 +232,7 @@ def spin(self): if __name__ == '__main__': try: - node = GamepadMarshallNode() + node = GamepadTeleopNode() node.spin() except rospy.ROSInterruptException: pass diff --git a/src/standalone_two_drone_aerobatics.py b/src/standalone_two_drone_aerobatics.py deleted file mode 100755 index 8babe6f..0000000 --- a/src/standalone_two_drone_aerobatics.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/usr/bin/env python2 -from time import sleep -import tellopy - - -def test(): - drone1 = tellopy.Tello( - local_cmd_client_port=8890, - local_vid_server_port=6038, - tello_ip='172.17.0.2', - ) - drone2 = tellopy.Tello( - local_cmd_client_port=9890, - local_vid_server_port=7038, - tello_ip='172.17.0.3', - ) - try: - drone1.connect() - drone2.connect() - drone1.wait_for_connection(10.0) - drone2.wait_for_connection(10.0) - print('Connected to both drones') - sleep(2.0) - - drone1.takeoff() - drone2.takeoff() - sleep(3.5) - - drone1.set_vspeed(1.0) - drone2.set_vspeed(1.0) - sleep(0.8) - drone1.set_vspeed(0.0) - drone2.set_vspeed(0.0) - - sleep(7.0) - - drone1.set_pitch(0.5) - drone1.set_yaw(1.0) - drone2.set_pitch(-0.5) - drone2.set_yaw(1.0) - sleep(2.0) - drone1.set_pitch(0.0) - drone1.set_yaw(0.0) - drone2.set_pitch(0.0) - drone2.set_yaw(0.0) - - sleep(5.0) - - drone1.set_vspeed(-0.5) - drone2.set_vspeed(-0.5) - sleep(0.6) - drone1.set_vspeed(0.0) - drone2.set_vspeed(0.0) - - sleep(1.0) - - drone1.palm_land() - drone2.palm_land() - sleep(3.0) - except BaseException as ex: - print(ex) - finally: - drone1.set_roll(0.0) - drone1.set_pitch(0.0) - drone1.set_yaw(0.0) - drone1.set_vspeed(0.0) - drone2.set_roll(0.0) - drone2.set_pitch(0.0) - drone2.set_yaw(0.0) - drone2.set_vspeed(0.0) - drone1.land() - drone2.land() - sleep(5.0) - drone1.quit() - drone2.quit() - - -if __name__ == '__main__': - test() diff --git a/src/tello_driver_node.py b/src/tello_driver_node.py index a51476c..c22b217 100755 --- a/src/tello_driver_node.py +++ b/src/tello_driver_node.py @@ -2,7 +2,7 @@ import rospy from std_msgs.msg import Empty, UInt8, Bool from std_msgs.msg import UInt8MultiArray -from sensor_msgs.msg import Image, CompressedImage +from sensor_msgs.msg import Image, CompressedImage, Imu from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from dynamic_reconfigure.server import Server @@ -37,7 +37,7 @@ from tellopy._internal import event ## Added statics to 'Tello' object, leave 'TelloPy' package untouched (Jordy) ## -VIDEO_MODE_CMD = 0x0031 +THROW_AND_GO_CMD = 0x005d EMERGENCY_CMD = 'emergency' ########################################END####################################### @@ -77,7 +77,7 @@ def notify_cmd_success(cmd, success): class TelloNode(tello.Tello): #######################################BEGIN####################################### - ## Add event variables to leave 'TelloPy' package untouched (Jordy) ## + ## Add event variable(s) to leave 'TelloPy' package untouched (Jordy) ## EVENT_VIDEO_FRAME_H264 = event.Event('video frame h264') #########################################END####################################### @@ -97,8 +97,13 @@ def __init__(self): self.frame_thread = None # Connect to drone - log = RospyLogger('Tello') - log.set_level(self.LOG_WARN) + self.log = RospyLogger('Tello') + +#######################################BEGIN####################################### + # fast_mode attribute before inheritance from TelloPy object (see override __send_stick_command) + self.fast_mode = False +#########################################END####################################### + super(TelloNode, self).__init__(port=9000) rospy.loginfo('Connecting to drone @ %s:%d' % self.tello_addr) self.connect() @@ -128,6 +133,7 @@ def __init__(self): 'image_raw', Image, queue_size=10) self.sub_takeoff = rospy.Subscriber('takeoff', Empty, self.cb_takeoff) + self.sub_manual_takeoff = rospy.Subscriber('manual_takeoff', Empty, self.cb_manual_takeoff) self.sub_throw_takeoff = rospy.Subscriber( 'throw_takeoff', Empty, self.cb_throw_takeoff) self.sub_land = rospy.Subscriber('land', Empty, self.cb_land) @@ -138,9 +144,9 @@ def __init__(self): self.sub_flip = rospy.Subscriber('flip', UInt8, self.cb_flip) self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.cb_cmd_vel) self.sub_fast_mode = rospy.Subscriber( - 'fast_mode', Bool, self.cb_fast_mode) + 'fast_mode', Empty, self.cb_fast_mode) - self.subscribe(self.EVENT_FLIGHT_DATA, self.cb_status_telem) + self.subscribe(self.EVENT_FLIGHT_DATA, self.cb_status_log) #########################################BEGIN##################################### @@ -148,16 +154,18 @@ def __init__(self): self.zoom = False # Reconstruction H264 video frames self.prev_seq_id = None - self.seq_block_count = 0 + self.seq_block_count = 0 - # agile flight mode state - self.fast_mode = False + # Height from EVENT_FLIGHT_DATA more accurate than MVO (monocular visual odometry) + self.height = 0 - #EVENT_LOG Odometry from 'TelloPy' package - # self.pub_odom = rospy.Publisher( - # 'odom', UInt8MultiArray, queue_size=1, latch=True) + #EVENT_LOG_DATA from 'TelloPy' package self.pub_odom = rospy.Publisher('odom', Odometry, queue_size=1, latch=True) - self.subscribe(self.EVENT_LOG_DATA, self.cb_odom_log) + self.pub_imu = rospy.Publisher('imu', Imu, queue_size=1, latch=True) + + self.subscribe(self.EVENT_LOG_DATA, self.cb_data_log) + + self.sub_zoom = rospy.Subscriber('video_mode', Empty, self.cb_video_mode, queue_size=1) if self.stream_h264_video: self.start_video() @@ -182,10 +190,10 @@ def __init__(self): ###########################################BEGIN################################### ## Add 'Tello' compositions, leave 'TelloPy' package untouched (Jordy) ## - - def set_fast_mode(self, enabled): - self.fast_mode = enabled + def set_fast_mode(self, enabled): + self.fast_mode = enabled + def reset_cmd_vel(self): self.left_x = 0. self.left_y = 0. @@ -196,8 +204,9 @@ def reset_cmd_vel(self): # scaling for velocity command def __scale_vel_cmd(self, cmd_val): return self.vel_cmd_scale * cmd_val - - def __send_stick_command(self): + + # Override TelloPy '__send_stick_command' to add 'fast_mode' functionality + def _Tello__send_stick_command(self): pkt = Packet(STICK_CMD, 0x60) axis1 = int(1024 + 660.0 * self.right_x) & 0x7ff @@ -234,6 +243,16 @@ def __send_stick_command(self): byte_to_hexstring(pkt.get_buffer())) return self.send_packet(pkt) + def manual_takeoff(self): + # Hold max 'yaw' and min 'pitch', 'roll', 'throttle' for several seconds + self.set_pitch(-1) + self.set_roll(-1) + self.set_yaw(1) + self.set_throttle(-1) + self.fast_mode = False + + return self._Tello__send_stick_command() + def cb_video_data(self, event, sender, data, **args): now = time.time() @@ -275,7 +294,7 @@ def __send_video_mode(self, mode): def set_video_mode(self, zoom=False): """Tell the drone whether to capture 960x720 4:3 video, or 1280x720 16:9 zoomed video. 4:3 has a wider field of view (both vertically and horizontally), 16:9 is crisper.""" - log.info('set video mode zoom=%s (cmd=0x%02x seq=0x%04x)' % ( + self.log.info('set video mode zoom=%s (cmd=0x%02x seq=0x%04x)' % ( zoom, VIDEO_START_CMD, self.pkt_seq_num)) self.zoom = zoom return self.__send_video_mode(int(zoom)) @@ -301,7 +320,7 @@ def emergency(self): def flip(self, cmd): """ tell drone to perform a flip in directions [0,8] """ - log.info('flip (cmd=0x%02x seq=0x%04x)' % (FLIP_CMD, self.pkt_seq_num)) + self.log.info('flip (cmd=0x%02x seq=0x%04x)' % (FLIP_CMD, self.pkt_seq_num)) pkt = Packet(FLIP_CMD, 0x70) pkt.add_byte(cmd) pkt.fixup() @@ -309,7 +328,7 @@ def flip(self, cmd): ##### Additions to 'tello_driver_node' (Jordy) ##### - def cb_videomode(self, msg): + def cb_video_mode(self, msg): if not self.zoom: self.set_video_mode(True) else: @@ -317,7 +336,7 @@ def cb_videomode(self, msg): def cb_emergency(self, msg): success = self.emergency() - notify_cmd_success('Emergency', success) + notify_cmd_success('Emergency', success) ##### Modifications to 'tello_driver_node' (Jordy) ##### @@ -336,20 +355,22 @@ def cb_dyncfg(self, config, level): req_sps_pps = True if update_all or self.cfg.vel_cmd_scale != config.vel_cmd_scale: self.vel_cmd_scale = config.vel_cmd_scale - + if update_all or self.cfg.att_limit != config.att_limit: + self.set_attitude_limit(config.att_limit) if req_sps_pps: self.send_req_video_sps_pps() self.cfg = config return self.cfg - def cb_status_telem(self, event, sender, data, **args): + def cb_status_log(self, event, sender, data, **args): speed_horizontal_mps = math.sqrt( data.north_speed*data.north_speed+data.east_speed*data.east_speed)/10. # TODO: verify outdoors: anecdotally, observed that: # data.east_speed points to South # data.north_speed points to East + self.height = data.height/10. msg = TelloStatus( height_m=data.height/10., speed_northing_mps=-data.east_speed/10., @@ -392,26 +413,48 @@ def cb_status_telem(self, event, sender, data, **args): ) self.pub_status.publish(msg) - def cb_odom_log(self, event, sender, data, **args): + def cb_data_log(self, event, sender, data, **args): + time_cb = rospy.Time.now() + odom_msg = Odometry() - - # Height received as negative distance to floor in mm - odom_msg.pose.pose.position.z = -data.mvo.pos_z*1000 + odom_msg.child_frame_id = rospy.get_namespace() + 'base_link' + odom_msg.header.stamp = time_cb + odom_msg.header.frame_id = rospy.get_namespace() + 'local_origin' + + # Height from MVO received as negative distance to floor + odom_msg.pose.pose.position.z = self.height #-data.mvo.pos_z odom_msg.pose.pose.position.x = data.mvo.pos_x odom_msg.pose.pose.position.y = data.mvo.pos_y odom_msg.pose.pose.orientation.w = data.imu.q0 odom_msg.pose.pose.orientation.x = data.imu.q1 odom_msg.pose.pose.orientation.y = data.imu.q2 odom_msg.pose.pose.orientation.z = data.imu.q3 - # Forward/Backward/Sides- speed received in dm/sec + # Linear speeds from MVO received in dm/sec odom_msg.twist.twist.linear.x = data.mvo.vel_y/10 odom_msg.twist.twist.linear.y = data.mvo.vel_x/10 odom_msg.twist.twist.linear.z = -data.mvo.vel_z/10 - - odom_msg.child_frame_id = rospy.get_namespace() + 'base_link' - odom_msg.header.stamp = rospy.Time.now() + odom_msg.twist.twist.angular.x = data.imu.gyro_x + odom_msg.twist.twist.angular.y = data.imu.gyro_y + odom_msg.twist.twist.angular.z = data.imu.gyro_z self.pub_odom.publish(odom_msg) + + imu_msg = Imu() + imu_msg.header.stamp = time_cb + imu_msg.header.frame_id = rospy.get_namespace() + 'base_link' + + imu_msg.orientation.w = data.imu.q0 + imu_msg.orientation.x = data.imu.q1 + imu_msg.orientation.y = data.imu.q2 + imu_msg.orientation.z = data.imu.q3 + imu_msg.angular_velocity.x = data.imu.gyro_x + imu_msg.angular_velocity.y = data.imu.gyro_y + imu_msg.angular_velocity.z = data.imu.gyro_z + imu_msg.linear_acceleration.x = data.imu.acc_x + imu_msg.linear_acceleration.y = data.imu.acc_y + imu_msg.linear_acceleration.z = data.imu.acc_z + + self.pub_imu.publish(imu_msg) def cb_cmd_vel(self, msg): self.set_pitch( self.__scale_vel_cmd(msg.linear.y) ) @@ -479,9 +522,13 @@ def framegrabber_loop(self): def cb_takeoff(self, msg): success = self.takeoff() notify_cmd_success('Takeoff', success) + + def cb_manual_takeoff(self, msg): + success = self.manual_takeoff() + notify_cmd_success('Manual takeoff', success) def cb_throw_takeoff(self, msg): - success = self.throw_takeoff() + success = self.throw_and_go() if success: rospy.loginfo('Drone set to auto-takeoff when thrown') else: @@ -500,7 +547,10 @@ def cb_flattrim(self, msg): notify_cmd_success('FlatTrim', success) def cb_fast_mode(self, msg): - self.set_fast_mode(msg.data) + if self.fast_mode: + self.set_fast_mode(False) + elif not self.fast_mode: + self.set_fast_mode(True) def main(): diff --git a/src/test.py b/src/test.py deleted file mode 100755 index f80ba75..0000000 --- a/src/test.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python2 -import time -import traceback -import tellopy - - -def test(): - drone1 = tellopy.Tello( - local_cmd_client_port=8890, - local_vid_server_port=6038, - tello_ip='192.168.10.1', - ) - try: - drone1.connect() - drone1.wait_for_connection(10.0) - print('Connected to drone') - - # Test raw video - if True: - drone1.start_video() - while True: - time.sleep(1.0) - - # Test video decoding - if False: - import av - import cv2 - import numpy - - container = av.open(drone1.get_video_stream()) - for frame in container.decode(video=0): - image = cv2.cvtColor(numpy.array( - frame.to_image()), cv2.COLOR_RGB2BGR) - cv2.imshow('Frame', image) - key = cv2.waitKey(1) - if key and chr(key) in ('q', 'Q', 'x', 'X'): - break - - # Test flying - if False: - time.sleep(2.0) - drone1.takeoff() - time.sleep(3.5) - - drone1.set_vspeed(1.0) - time.sleep(0.6) - drone1.set_vspeed(0.0) - - time.sleep(2.0) - - drone1.set_pitch(0.4) - drone1.set_yaw(1.0) - time.sleep(0.5) - drone1.set_pitch(0.0) - drone1.set_yaw(0.0) - - time.sleep(2.0) - - drone1.flip(1) - - time.sleep(2.0) - - drone1.set_vspeed(-0.5) - time.sleep(0.6) - drone1.set_vspeed(0.0) - - time.sleep(1.0) - - drone1.palm_land() - time.sleep(3.0) - - except BaseException: - traceback.print_exc() - finally: - if False: - drone1.reset_cmd_vel() - drone1.land() - drone1.quit() - - -if __name__ == '__main__': - test()