djicapt bringup dds discovery server + djicapt joystick interaction

Author: KKalem

scripts/smarc_bringups/scripts/dji_bringup.sh

@@ -49,7 +49,8 @@ if [ "$USE_SIM_TIME" = "False" ]; then
     tmux send-keys "ros2 topic echo /$ROBOT_NAME/captain_status std_msgs/msg/String --field data" C-m
 
     tmux select-pane -t $SESSION:0.3
-    tmux send-keys "cd ~ && ./record_bag_ex_camComp.sh" C-m
+    # tmux send-keys "cd ~ && ./record_bag_ex_camComp.sh" C-m
+    tmux send-keys "fast-discovery-server -i 0" C-m
 else
     tmux select-window -t $SESSION:0
     tmux send-keys "ros2 topic pub /$ROBOT_NAME/smarc/vehicle_health std_msgs/msg/Int8 \"data: 0\" -r 5" C-m

vehicles/hardware/dji/dji_captain/dji_captain/dji_captain.py

@@ -69,15 +69,22 @@ class DjiCaptain():
     def __init__(self, node: Node):
         self._node = node
         self._node.declare_parameter("robot_name", "M350")
+        self._node.declare_parameter("controller_deadzone", 0.1)
 
 
-        self._TF_NS = f"{self._node.get_parameter('robot_name')}/"
+        self._TF_NS : str = f"{self._node.get_parameter('robot_name')}/"
+
+        self._CONTROLLER_DEADZONE : float = 0.1
+        v = self._node.get_parameter("controller_deadzone").value 
+        if v is not None: self._CONTROLLER_DEADZONE = v
 
 
         self._control_mode = ControlModes.FLUvel
         self._move_to_setpoint : PoseStamped | None = None
         self._joy_timer : None | Timer = None
-        self._joy_pub = node.create_publisher(Joy, PSDKTopics.FLUvel_JOY.value, qos_profile=10)
+        self._FLU_vel_joy_pub = node.create_publisher(Joy, PSDKTopics.FLUvel_JOY.value, qos_profile=10)
+        self._ENU_vel_joy_pub = node.create_publisher(Joy, PSDKTopics.ENUvel_JOY.value, qos_profile=10)
+        self._ENU_pos_joy_pub = node.create_publisher(Joy, PSDKTopics.ENUpos_JOY.value, qos_profile=10)
         self._setpoint_received_at : float|None = None
 
         self.MOVE_TO_SETPOINT_MAX_AGE : float = 0.5 # seconds, how long we keep the move to setpoint before we consider it stale
@@ -253,34 +260,22 @@ def __init__(self, node: Node):
         self._land_srv = node.create_client(Trigger, PSDKTopics.LAND_SRV.value)
 
 
-        while True:
+        while rclpy.ok():
             commands = "Commands:\n"
             commands += "  1: Take control  \n"
             commands += "  2: Release control\n"
             commands += "  3: Take off\n"
             commands += "  4: Land\n"
-            commands += "  5: Print status (also available on $ROBOT_NAME/captain_status topic) \n"
-            commands += "  8: EXIT \n"
+            commands += f"\n  8: Print status (also available on {self._TF_NS}captain_status topic) \n"
             commands += "  9: Set max joy to (DANGEROUS, DONT USE UNLESS YOUR NAME STARTS WITH O)\n"
+            commands += "  0: EXIT \n"
             try:
                 self.log(commands)
                 n = int(input("Enter number for command: \n"))
                 if n == 1: #Take control
-                    if not self._take_control_srv.wait_for_service(timeout_sec=5.0):
-                        self.log("Take control service not available...")
-                        continue
-                    future = self._take_control_srv.call_async(Trigger.Request())
-                    future.add_done_callback(
-                        lambda f: self.log(f"Take control service called, success: {f.result().success}, message: {f.result().message}")
-                    )
+                    self._take_control()
                 elif n == 2: #Release control
-                    if not self._release_control_srv.wait_for_service(timeout_sec=5.0):
-                        self.log("Release control service not available...")
-                        continue
-                    future = self._release_control_srv.call_async(Trigger.Request())
-                    future.add_done_callback(
-                        lambda f: self.log(f"Release control service called, success: {f.result().success}, message: {f.result().message}")
-                    )
+                    self._release_control()
                 elif n == 3: #Take off
                     if self._got_control is False:
                         self.log("You must take control first!")
@@ -315,9 +310,13 @@ def __init__(self, node: Node):
                     self.log(self.status_str)
                 elif n == 9: # set max joy
                     self.JOY_PUB_MAX = float(input("Enter new max joy value: ") or "0")
-                    self.log(f"Set max joy to {self.JOY_PUB_MAX:.2f} (m/s?)")
+                    self.log(f"Set max joy to {self.JOY_PUB_MAX:.2f} (m/s)")
                 elif n == 0:
-                    self.log("Exiting captain")
+                    try:
+                        self.log("Exiting captain")
+                        self._speak("Bye bye.")
+                    except:
+                        print("Exiting captain.")
                     break
 
             except:
@@ -374,6 +373,41 @@ def log(self, msg: str):
         self._node.get_logger().info(msg)
 
 
+    def _take_control(self):
+        def on_result(f):
+            self.log(f"Take control service called, success: {f.result().success}, message: {f.result().message}")
+            if f.result().success:
+                self._speak("Got control.")
+            else:
+                self._speak("Failed to get control.")
+
+        self.log("Taking control.")
+        self._speak("Taking control.")
+        if not self._take_control_srv.wait_for_service(timeout_sec=5.0):
+            self.log("Take control service not available...")
+            self._speak("Take control service not available.")
+            return
+        future = self._take_control_srv.call_async(Trigger.Request())
+        future.add_done_callback(on_result)
+
+
+    def _release_control(self):
+        def on_result(f):
+            self.log(f"Release control service called, success: {f.result().success}, message: {f.result().message}")
+            if f.result().success:
+                self._speak("Gave up control.")
+            else:
+                self._speak("Failed to release control.")
+
+        self.log("Releasing control.")
+        self._speak("Releasing control.")
+        if not self._release_control_srv.wait_for_service(timeout_sec=5.0):
+            self.log("Release control service not available...")
+            self._speak("Release control service not available.")
+            return
+        future = self._release_control_srv.call_async(Trigger.Request())
+        future.add_done_callback(on_result)
+
 
     def _geo_alt_cb(self, msg: Float32):
         self._geo_altitude = msg.data
@@ -417,7 +451,13 @@ def _move_to_setpoint_callback(self, msg: PoseStamped):
         self.log(f"Move to setpoint received: {format_pose_stamped(self._move_to_setpoint)}")
 
         if self._joy_timer is None:
-            self._joy_timer = self._node.create_timer(self.JOY_PUB_PERIOD, self._move_towards_setpoint_FLUvel)
+            if self._control_mode == ControlModes.FLUvel:
+                self._joy_timer = self._node.create_timer(self.JOY_PUB_PERIOD, self._move_towards_setpoint_FLUvel)
+            elif self._control_mode == ControlModes.ENUvel:
+                self._joy_timer = self._node.create_timer(self.JOY_PUB_PERIOD, self.move_towards_setpoint_ENUvel)
+            elif self._control_mode == ControlModes.ENUpos:
+                self._joy_timer = self._node.create_timer(self.JOY_PUB_PERIOD, self.move_towards_setpoint_ENUpos)
+
             self._setpoint_received_at = time.time()
             self.log("Joy timer started to move with joy.")
 
@@ -427,41 +467,149 @@ def _buzz(self, intensity: float = 1.0):
                 type=JoyFeedback.TYPE_RUMBLE,
                 id=0,
                 intensity=intensity))
+        
+    def _speak(self, msg: str):
+        self._speak_pub.publish(String(data=msg))
 
+    def _controller_callback(self, msg: Joy):
+        if msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0:
+            # malformed...
+            return
+        
+        now = self.now_stamp
+        # Check if the message is older than 0.1 seconds
+        msg_age = (now.sec - msg.header.stamp.sec) + (now.nanosec - msg.header.stamp.nanosec) * 1e-9
+        if msg_age > 0.1:
+            self.log(f"Controller message is older than 0.1s ({msg_age:.3f}s), ignoring.")
+            return
 
 
-    def _controller_callback(self, msg: Joy):
         # right stick = horizontal movement, left stick = vertical movement + yaw
         # like the DJI RC controller
+        LH = msg.axes[0]  # left stick horizontal
+        LV = msg.axes[1]  # left stick vertical
+        RH = msg.axes[2]  # right stick horizontal
+        RV = msg.axes[3]  # right stick vertical
+        L2:float = msg.axes[4]  # L2 button 
+        R2:float = msg.axes[5]  # R2 button
+        south = msg.buttons[0]  # south button
+        east = msg.buttons[1]   # east button
+        west = msg.buttons[2]   # west button
+        north = msg.buttons[3]  # north button
+        select = msg.buttons[4] # select button
+        ps_button = msg.buttons[5]  # PS button
+        start = msg.buttons[6]  # start button
+        left_stick_in = msg.buttons[7]  # left stick in
+        right_stick_in = msg.buttons[8]  # right stick in
+        L1 = msg.buttons[9]  # L1 button
+        R1 = msg.buttons[10]  # R1 button
+        up = msg.buttons[11]  # up button
+        down = msg.buttons[12]  # down button
+        left = msg.buttons[13]  # left button
+        right = msg.buttons[14]  # right button
+
+        sticks_pushed = any(
+            [
+                abs(LH) > self._CONTROLLER_DEADZONE,
+                abs(LV) > self._CONTROLLER_DEADZONE,
+                abs(RH) > self._CONTROLLER_DEADZONE,
+                abs(RV) > self._CONTROLLER_DEADZONE
+            ]
+        )
+
         if not self._got_control:
-            self._buzz()
-            return
+            if start == 1 and ps_button == 1:
+                self._take_control()
+                return
+            
+            if sticks_pushed:
+                self.log("Sticks pushed without control!")
+                self._speak("You must first take control with the PS button and Start button.")
+                self._buzz()
+                return
+            
+        if self._move_to_setpoint is not None:
+            if sticks_pushed:
+                self.log("Sticks pushed, cancelling move to setpoint.")
+                self._speak("Move to setpoint cancelled because you touched the controller sticks!")
+                self._cancel_joy_timer()                
 
+
+        if up:
+            self.JOY_PUB_MAX += 0.2
+            if self.JOY_PUB_MAX > 5.0:
+                self.JOY_PUB_MAX = 5.0
+            self.log(f"Joy max increased to {self.JOY_PUB_MAX:.2f} (m/s)")
+            self._speak(f"Joy max {self.JOY_PUB_MAX:.1f}")
+        if down:
+            self.JOY_PUB_MAX -= 0.2
+            if self.JOY_PUB_MAX < 0.0:
+                self.JOY_PUB_MAX = 0.0
+            self.log(f"Joy max decreased to {self.JOY_PUB_MAX:.2f} (m/s)")
+            self._speak(f"Joy max {self.JOY_PUB_MAX:.1f}")
+
+
+        control_modes = list(ControlModes)
+        if left:
+            self._control_mode = control_modes[(control_modes.index(self._control_mode) - 1) % len(control_modes)]
+            self.log(f"Control mode changed to {self._control_mode.value}")
+            self._speak(f"Control mode {self._control_mode.value}")
+        if right:
+            self._control_mode = control_modes[(control_modes.index(self._control_mode) + 1) % len(control_modes)]
+            self.log(f"Control mode changed to {self._control_mode.value}")
+            self._speak(f"Control mode {self._control_mode.value}")
 
 
+            
+        if self._got_control and sticks_pushed:
+            joy_msg = Joy()
+            joy_msg.header.stamp = self.now_stamp
+            if self._control_mode == ControlModes.FLUvel:
+                # DJI expects Axes: [forward, left, up, yawrate]
+                joy_msg.axes = [RV, RH, LV, LH]
+                self._FLU_vel_joy_pub.publish(joy_msg)
+
+            elif self._control_mode == ControlModes.ENUvel:
+                self.log("Moving with ENU velocity control mode, be careful! Right stick is real East/North!")
+                # DJI expects Axes: [east, north, up, yawrate]
+                joy_msg.axes = [RV, RH, LV, LH]
+                self._ENU_vel_joy_pub.publish(joy_msg)
+
+            elif self._control_mode == ControlModes.ENUpos:
+                #TODO
+                pass
 
-    def _move_towards_setpoint_FLUvel(self):
-        def cancel_joy_timer():
-            if self._joy_timer is not None:
-                self._joy_timer.cancel()
-                self._joy_timer = None
-                self._setpoint_received_at = None
-                self.log("Joy timer cancelled.")
+        
+        
+    def _cancel_joy_timer(self):
+        self._setpoint_received_at = None
+        self._move_to_setpoint = None
+        self.log("Setpoint discarded.")
+        if self._joy_timer is not None:
+            self._joy_timer.cancel()
+            self._joy_timer = None
+            self.log("Joy timer cancelled.")
 
+
+      
+
+
+    def _move_towards_setpoint_FLUvel(self):
         if self._move_to_setpoint is None or self._setpoint_received_at is None:
             self.log("No move to setpoint set, cannot move with joy.")
+            self._cancel_joy_timer()
             return
 
         if (self.now_stamp.sec - self._move_to_setpoint.header.stamp.sec) + \
            (self.now_stamp.nanosec - self._move_to_setpoint.header.stamp.nanosec) * 1e-9 > self.MOVE_TO_SETPOINT_MAX_AGE:
             self.log(f"Move to setpoint message is older than {self.MOVE_TO_SETPOINT_MAX_AGE}s, cancelling joy timer.")
             self._move_to_setpoint = None
-            cancel_joy_timer()
+            self._cancel_joy_timer()
             return
 
         if not self._got_control:
             self.log("Not got control, cannot move with joy.")
-            cancel_joy_timer()
+            self._cancel_joy_timer()
             return
 
         try:
@@ -473,7 +621,7 @@ def cancel_joy_timer():
             target_in_base = do_transform_pose_stamped(self._move_to_setpoint, tf_diff)
         except:
             self.log(f"Failed to transform move to setpoint from {self._move_to_setpoint.header.frame_id} to {self.BASE_FLAT_FRAME}, cancelling joy timer.")
-            cancel_joy_timer()
+            self._cancel_joy_timer()
             return
 
 
@@ -496,9 +644,17 @@ def cancel_joy_timer():
         joy_msg.axes = [e_forw, e_left, e_updn, 0.0]  # Axes: [forward, left, up/down, yaw]
         joy_msg.buttons = []
 
-        self._joy_pub.publish(joy_msg)
+        self._FLU_vel_joy_pub.publish(joy_msg)
+
 
+    def move_towards_setpoint_ENUpos(self):
+        self.log("move_towards_setpoint_ENUpos not implemented yet.")
+        self._cancel_joy_timer()
 
+    def move_towards_setpoint_ENUvel(self):
+        self.log("move_towards_setpoint_ENUvel not implemented yet.")
+        self._cancel_joy_timer()
+    
 
     def _dji_rc_cb(self, msg: Joy):
         # if RC is touched by user, we give up control
@@ -545,7 +701,22 @@ def _angular_rate_ground_callback(self, msg: Vector3Stamped):
     def _control_mode_callback(self, msg: ControlMode):
         # hardcoded numbers from the psdk_ros2 interface
         # 1 = Has control authority, 4 = PSDK
-        self._got_control = msg.control_auth == 1 and msg.device_mode == 4
+        just_got_control = msg.control_auth == 1 and msg.device_mode == 4
+        if self._got_control == just_got_control:
+            return
+        
+        if self._got_control and not just_got_control:
+            self.log("Released control authority, stopping joy timer, discarding setpoint.")
+            self._speak("Released control.")
+            self._cancel_joy_timer()
+            self._got_control = False
+
+        elif not self._got_control and just_got_control:
+            self.log("Gained control authority.")
+            self._speak("Taken control.")
+            self._got_control = True
+        
+
 
     def _battery_callback(self, msg: BatteryState):
         self._battery_percent = msg.percentage*100
@@ -590,11 +761,9 @@ def _attitude_callback(self, msg: QuaternionStamped):
         self._heading_deg = 90 - math.degrees(rpy_enu[2])
         self._base_pose_in_home.pose.orientation = msg.quaternion
 
-        rpy_enu_flat = [0.0, 0.0, rpy_enu[2]]  # we want a frame without roll and pitch for control reasons
         flat_quat = Quaternion()
         flat_quat.x, flat_quat.y, flat_quat.z, flat_quat.w = quaternion_from_euler(0, 0, rpy_enu[2])
         self._base_pose_flat_in_home.pose.orientation = flat_quat
-        rpy_enu_flat_no_yaw = [0.0, 0.0, 0.0]
         ENU_quat = Quaternion()
         ENU_quat.x, ENU_quat.y, ENU_quat.z, ENU_quat.w = quaternion_from_euler(0, 0, 0)
         self._base_pose_ENU_in_home.pose.orientation = ENU_quat