Merge pull request #167 from JdeRobot/issue-165

Driver adaptation to new DW
Closes #165

Author: pariaspe

drone_wrapper/src/drone_wrapper/drone_wrapper_class.py

@@ -437,7 +437,7 @@ def set_cmd_vel(self, vx=0, vy=0, vz=0, az=0, frame="flu"):
         if abs(vz) <= EPSILON:
             self.is_z = True
         else:
-            self.setpoint_raw.velocity.z = vtwist.linear.z
+            self.setpoint_raw.velocity.z = twist.linear.z
             self.is_z = False
 
         if self.is_xy:

tello_driver/src/tello_driver_node.py

@@ -6,6 +6,7 @@
 from sensor_msgs.msg import BatteryState, NavSatFix, Image, NavSatStatus
 from geometry_msgs.msg import PoseStamped, TwistStamped, Pose, Point, Quaternion, Twist, Vector3
 from cv_bridge import CvBridge
+import tf
 from tf.transformations import quaternion_from_euler
 
 import cv2
@@ -27,6 +28,18 @@ def process_mask(mask):
         return 1991  # vx vy vz yaw_rate
 
 
+class TelloConstraints:
+    MIN_SPEED = 10
+    MAX_SPEED = 100
+
+    def constraint_speed(self, s):
+        '''
+        s: speed (cm/s)
+        '''
+        return self.MIN_SPEED if s < self.MIN_SPEED else self.MAX_SPEED if s > self.MAX_SPEED else s
+
+
+
 class TelloConnectionError(Exception):
     pass
 
@@ -35,11 +48,13 @@ class TelloConnectionError(Exception):
 class TelloDriver:
     STILL_ALIVE_RATE = 10  # sec
     PUB_RATE = 100000000  # nsec --> 0.1 secs
+    TF_RATE = 100000000  # nsec --> 0.1 secs
     RESP_TIMEOUT = 7  # sec
     CMD_ADDRESS = ('192.168.10.1', 8889)
     STATE_ADDRESS = ('0.0.0.0', 8890)
     VIDEO_ADDRESS = ('0.0.0.0', 11111)
     LOCAL_ADDRESS = ('0.0.0.0', 9000)
+    CONSTRAINTS = TelloConstraints()
 
     def __init__(self, emergency_disabled=False):
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -63,6 +78,7 @@ def __init__(self, emergency_disabled=False):
         self.__vy = 0.0  # cm/s
         self.__vz = 0.0  # cm/s
         self.__yaw_rate = 0.0  # degrees/s
+        self.__speed = 100.0  # cm/s (DEFAULT VALUE)
 
         self.state_pub = rospy.Publisher('mavros/state', State, queue_size=10)
         self.ext_state_pub = rospy.Publisher('mavros/extended_state', ExtendedState, queue_size=10)
@@ -108,14 +124,20 @@ def __init__(self, emergency_disabled=False):
             rospy.logerr(ex)
             self.shutdown()
 
-        rospy.Timer(rospy.Duration(nsecs=self.PUB_RATE), self.send_data)
+        self.pub_timer = rospy.Timer(rospy.Duration(nsecs=self.PUB_RATE), self.send_data)
+        
+        self.tb_map = tf.TransformBroadcaster()
+        self.tb_map_timer = rospy.Timer(rospy.Duration(nsecs=self.TF_RATE), self.br_map2ned)
+        self.tb_base = tf.TransformBroadcaster()
+        self.tb_base_timer = rospy.Timer(rospy.Duration(nsecs=self.TF_RATE), self.br_base2frd)
+
         if emergency_disabled:
-            rospy.Timer(rospy.Duration(secs=self.STILL_ALIVE_RATE), self.__still_alive)
+            self.still_alive_timer = rospy.Timer(rospy.Duration(secs=self.STILL_ALIVE_RATE), self.__still_alive)
 
     def __still_alive(self, event):
         self.__send_cmd("battery?")
 
-    def __send_cmd(self, cmd, blocking=True, timeout=RESP_TIMEOUT):
+    def __send_cmd(self, cmd, reading=False, blocking=True, timeout=RESP_TIMEOUT):
         print("Sending... {}".format(cmd))  # DEBUG
         self.sock.sendto(cmd.encode(encoding="utf-8"), self.CMD_ADDRESS)
 
@@ -128,6 +150,8 @@ def __send_cmd(self, cmd, blocking=True, timeout=RESP_TIMEOUT):
                 time.sleep(0.1)  # waits a bit
 
             resp = self.__responses.pop(0).decode("utf-8")
+            if reading:
+                return resp
             if resp.lower() == "ok":
                 return True
             else:
@@ -213,12 +237,35 @@ def tello_land(self, req):
         else:
             return False, 1
 
+    def tello_change_speed(self, speed):
+        '''speed in m/s'''
+        speed = self.CONSTRAINTS.constraint_speed(speed*100)  # to cm/s
+
+        if self.__send_cmd("speed {}".format(str(speed))):
+            rospy.loginfo("Tello speed updated to " + str(speed))
+            rospy.set_param("/tello/speed", speed)
+            return True
+        else:
+            return False
+
     def tello_param_set(self, req):
         # string param_id
         # mavros_msgs/ParamValue value
         # ----
         # bool success
         # mavros_msgs/ParamValue value
+
+        if req.param_id == "MPC_XY_VEL_MAX":
+            if req.value.integer != 0:
+                speed = req.value.integer
+            elif req.value.real != 0.0:
+                speed = req.value.real
+            else:
+                speed = 0
+
+            self.tello_change_speed(int(speed))
+            return True, req.value
+
         return False, ParamValue(integer=0, real=0.0)
 
     def tello_param_get(self, req):
@@ -292,9 +339,9 @@ def setpoint_cb(self, msg):
                 self.__x += target_x
 
             if x > 0:
-                self.__send_cmd("forward {}".format(abs(x)), False)  # cm
+                self.__send_cmd("forward {}".format(abs(x)), blocking=False)  # cm
             elif x < 0:
-                self.__send_cmd("back {}".format(abs(x)), False)  # cm
+                self.__send_cmd("back {}".format(abs(x)), blocking=False)  # cm
             else:
                 pass  # already at target x
                 # print("Already at target x")
@@ -310,9 +357,9 @@ def setpoint_cb(self, msg):
             cmd_right = "right" if is_frd else "left"
             cmd_left = "left" if is_frd else "right"
             if y > 0:
-                self.__send_cmd(cmd_right + " {}".format(abs(y)), False)  # cm
+                self.__send_cmd(cmd_right + " {}".format(abs(y)), blocking=False)  # cm
             elif y < 0:
-                self.__send_cmd(cmd_left + " {}".format(abs(y)), False)  # cm
+                self.__send_cmd(cmd_left + " {}".format(abs(y)), blocking=False)  # cm
             else:
                 pass  # already at target y
                 # print("Already at target y")
@@ -328,9 +375,9 @@ def setpoint_cb(self, msg):
             cmd_down = "down" if is_frd else "up"
             cmd_up = "up" if is_frd else "down"
             if z > 0:
-                self.__send_cmd(cmd_down + " {}".format(abs(z)), False)  # cm
+                self.__send_cmd(cmd_down + " {}".format(abs(z)), blocking=False)  # cm
             elif z < 0:
-                self.__send_cmd(cmd_up + " {}".format(abs(z)), False)  # cm
+                self.__send_cmd(cmd_up + " {}".format(abs(z)), blocking=False)  # cm
             else:
                 pass  # already at target z
                 # print("Already at target z")
@@ -377,9 +424,9 @@ def setpoint_cb(self, msg):
                 self.__yaw += target_yaw
 
             if yaw > 0:
-                self.__send_cmd("cw {}".format(abs(yaw)), False)  # degrees
+                self.__send_cmd("cw {}".format(abs(yaw)), blocking=False)  # degrees
             elif yaw < 0:
-                self.__send_cmd("ccw {}".format(abs(yaw)), False)  # degrees
+                self.__send_cmd("ccw {}".format(abs(yaw)), blocking=False)  # degrees
             else:
                 pass  # already at target yaw
                 # print("Already at target yaw")
@@ -549,6 +596,12 @@ def rcv_video(self):
         cap.release()
         cv2.destroyAllWindows()
 
+    def br_map2ned(self, event):
+        self.tb_map.sendTransform((0,0,0), (0.707, 0.707, 0.000, 0.000), rospy.Time.now(), "map_ned", "map")
+
+    def br_base2frd(self, event):
+        self.tb_base.sendTransform((0,0,0), (1, 0, 0, 0), rospy.Time.now(), "base_link_frd", "base_link")
+
     def shutdown(self):
         if self.__is_flying:
             self.tello_land(CommandTOL())
@@ -571,6 +624,15 @@ def shutdown(self):
         if self.video_handler.is_alive():
             self.video_handler.join()
 
+        try:
+            self.pub_timer.shutdown()
+            self.tb_map_timer.shutdown()
+            self.tb_base_timer.shutdown()
+            if self.__emergency_disabled:
+                self.still_alive_timer.shutdown()
+        except AttributeError:
+            pass
+
         rospy.loginfo("Tello driver shutting down")
 
     def __del__(self):