Update and simplify ual teleoperation

ual_teleop/launch/joy_teleop.launch

@@ -1,11 +1,15 @@
 <launch>
-
+	<!-- This launch must be called inside ual namespace:
+	    - from console, e.g: roslaunch ual_teleop joy_teleop.launch __ns:=uav_1 
+		- from another launch, e.g: include with ns="uav_1"
+	-->
 	<arg name="joy" default="js0"/>
 
-	<node name="joy_teleop" pkg="ual_teleop" type="joy_teleop.py"/>
+	<node name="joy_teleop" pkg="ual_teleop" type="joy_teleop.py" output="screen"
+	args="-joy_file=$(find ual_teleop)/config/saitek_p3200.yaml"/>
 
-	<!-- joy node -->
-	<node respawn="true" pkg="joy" type="joy_node" name="joy" >
+	<!-- joy node: Use namespace to avoid conflicts with simulate_safety_pilot -->
+	<node ns="ual_teleop" respawn="true" pkg="joy" type="joy_node" name="joy" >
 		<param name="dev" type="string" value="/dev/input/$(arg joy)" />
 		<param name="autorepeat_rate" value="5" />
 	</node>

ual_teleop/scripts/joy_teleop.py

@@ -1,172 +1,103 @@
 #!/usr/bin/env python
-import yaml
+import argparse
 import rospy
 import rospkg
 import math
+from joy_handle import JoyHandle, ButtonState
 from sensor_msgs.msg import Joy
-from enum import Enum
 from uav_abstraction_layer.srv import TakeOff, Land, SetVelocity 
 from geometry_msgs.msg import TwistStamped
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import String
-import PyKDL
-from tf_conversions import posemath
-
-ButtonState = Enum('ButtonState', 'UNKNOWN JUST_PRESSED PRESSED JUST_RELEASED RELEASED')
-gains = [0.5, 0.8, 1.0, 1.3, 1.8, 2.1, 2.5]
-
-def update_button_state(prev_state, value):
-    if prev_state is ButtonState.UNKNOWN:
-        if value:
-            return ButtonState.PRESSED
-        else:
-            return ButtonState.RELEASED
-    elif prev_state is ButtonState.JUST_PRESSED:
-        if value:
-            return ButtonState.PRESSED
-        else:
-            return ButtonState.JUST_RELEASED
-    elif prev_state is ButtonState.PRESSED:
-        if value:
-            return ButtonState.PRESSED
-        else:
-            return ButtonState.JUST_RELEASED
-    elif prev_state is ButtonState.JUST_RELEASED:
-        if value:
-            return ButtonState.JUST_PRESSED
-        else:
-            return ButtonState.RELEASED
-    elif prev_state is ButtonState.RELEASED:
-        if value:
-            return ButtonState.JUST_PRESSED
-        else:
-            return ButtonState.RELEASED
-    else:
-        raise IOError("Unexpected button state")
-
-class JoyHandle:
-
-    def __init__(self, config_file):
-        self.ros_data = Joy()
-        self.buttons_state = []
-        with open(config_file, 'r') as config:
-            self.layout = yaml.load(config)['joy_layout']
-
-    def update(self, data):
-        self.ros_data = data
-        # Update buttons
-        if len(self.ros_data.buttons) is not len(self.buttons_state):
-            self.buttons_state = [ButtonState.UNKNOWN] * len(self.ros_data.buttons)
-        for i, state in enumerate(self.buttons_state):
-            self.buttons_state[i] = update_button_state(state, self.ros_data.buttons[i])
-
-    # TODO(franreal): Study other interface functions like is_pressed(id)
-    def get_axis(self, id):
-        # TODO(franreal): Check id is an axis first?
-        if self.layout[id]['reversed']:
-            return -self.ros_data.axes[self.layout[id]['index']]
-        else:
-            return self.ros_data.axes[self.layout[id]['index']]
-
-    def get_button(self, id):
-        # TODO(franreal): Check id is a button first?
-        return self.ros_data.buttons[self.layout[id]['index']]
-
-    def get_button_state(self, id):
-        # TODO(franreal): Check id is a button first?
-        return self.buttons_state[self.layout[id]['index']]
-
-    def __str__(self):
-        output = '---\n'
-        for button in ['a', 'b', 'x', 'y', 
-                       'left_shoulder', 'right_shoulder',
-                       'left_trigger', 'right_trigger',
-                       'select', 'start',
-                       'left_thumb', 'right_thumb']:
-            output = output + button + ': ' + str(self.get_button_state(button)) + '\n'
-        for axis in ['left_analog_x', 'left_analog_y',
-                     'right_analog_x', 'right_analog_y',
-                     'dpad_x', 'dpad_y']:
-            output = output + axis + ': ' + str(self.get_axis(axis)) + '\n'
-        return output
 
 class JoyTeleop:
 
-    def __init__(self, joy_file, robot_id):
+    def __init__(self, joy_file):
         self.joy_handle = JoyHandle(joy_file)
-        take_off_url = robot_id + '/ual/take_off'
-        land_url =     robot_id + '/ual/land'
-        velocity_url = robot_id + '/ual/set_velocity'
+        take_off_url = 'ual/take_off'
+        land_url =     'ual/land'
+        velocity_url = 'ual/set_velocity'
         rospy.wait_for_service(take_off_url)
         rospy.wait_for_service(land_url)
-        rospy.wait_for_service(velocity_url)
         self.take_off = rospy.ServiceProxy(take_off_url, TakeOff)
         self.land     = rospy.ServiceProxy(land_url,     Land)
-        self.velocity = rospy.ServiceProxy(velocity_url, SetVelocity)
+        self.velocity_pub = rospy.Publisher(velocity_url, TwistStamped, queue_size=1)
         self.ual_state = String()
         self.headless = True
-        self.ual_yaw = 0.0
-        self.gain_value = 2
+        self.uav_yaw = 0.0
+        self.gains_table = [0.5, 0.8, 1.0, 1.3, 1.8, 2.1, 2.5]
+        self.gain_index = 2
 
     def state_callback(self, data):
         self.ual_state = data
 
-    def motion_callback(self, data):
-        rot = PyKDL.Rotation.Quaternion(data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w)
-        self.ual_yaw = rot.GetRPY()[2]
-        #print "Yaw = " + str(self.ual_yaw) # DEBUG
+    def pose_callback(self, data):
+        self.uav_yaw = 2.0 * math.atan2(data.pose.orientation.z, data.pose.orientation.w)
 
     def joy_callback(self, data):
         self.joy_handle.update(data)
         # print self.joy_handle  # DEBUG
         if self.joy_handle.get_button('left_shoulder'):
-            if self.joy_handle.get_button_state('x') is ButtonState.JUST_PRESSED and self.ual_state.data == 'LANDED':
-                print "TAKING OFF"
+            if self.joy_handle.get_button_state('x') is ButtonState.JUST_PRESSED and self.ual_state.data == 'LANDED_ARMED':
+                rospy.loginfo("Taking off")
                 self.take_off(2.0, False)  # TODO(franreal): takeoff height?
-            if self.joy_handle.get_button_state('b') is ButtonState.JUST_PRESSED and self.ual_state.data == 'FLYING':
-                print "LANDING"
+            if self.joy_handle.get_button_state('b') is ButtonState.JUST_PRESSED and self.ual_state.data == 'FLYING_AUTO':
+                rospy.loginfo("Landing")
                 self.land(False)
-        
+
         if self.headless == True and (self.joy_handle.get_button_state('right_shoulder') is ButtonState.JUST_PRESSED):
-            print "EXITING \"HEADLESS\" MODE"
+            rospy.loginfo("Exiting headless mode")
             self.headless = False
         elif self.headless == False and (self.joy_handle.get_button_state('right_shoulder') is ButtonState.JUST_PRESSED):
-            print "ENTERING \"HEADLESS\" MODE"
+            rospy.loginfo("Entering headless mode")
             self.headless = True
 
         if self.joy_handle.get_button_state('left_trigger') is ButtonState.JUST_PRESSED:
-            self.gain_value = self.gain_value - 1 if self.gain_value > 0 else 0
-            print "SPEED LEVEL: " + str(self.gain_value)
+            self.gain_index = self.gain_index - 1 if self.gain_index > 0 else 0
+            rospy.loginfo("Speed level: %d", self.gain_index)
         if self.joy_handle.get_button_state('right_trigger') is ButtonState.JUST_PRESSED:
-            self.gain_value = self.gain_value + 1 if self.gain_value < 6 else 6
-            print "SPEED LEVEL: " + str(self.gain_value)
+            max_index = len(self.gains_table) - 1
+            self.gain_index = self.gain_index + 1 if self.gain_index < max_index else max_index
+            rospy.loginfo("Speed level: %d", self.gain_index)
 
-        if self.ual_state.data == 'FLYING':
+        if self.ual_state.data == 'FLYING_AUTO':
             vel_cmd = TwistStamped()
             vel_cmd.header.stamp = rospy.Time.now()
+            # TODO: Use frame_id = 'uav_1' in not-headless mode?
             vel_cmd.header.frame_id = 'map'
-            if self.headless == True:
-                vel_cmd.twist.linear.x = 0.5 * gains[self.gain_value] * self.joy_handle.get_axis('right_analog_x') #NOTE: For ds4 controller X axis must be inverted
-                vel_cmd.twist.linear.y = 0.5 * gains[self.gain_value] * self.joy_handle.get_axis('right_analog_y')
-                vel_cmd.twist.linear.z = 0.2 * gains[self.gain_value] * self.joy_handle.get_axis('left_analog_y')
+            if self.headless:
+                vel_cmd.twist.linear.x = 0.5 * self.gains_table[self.gain_index] * self.joy_handle.get_axis('right_analog_x')
+                vel_cmd.twist.linear.y = 0.5 * self.gains_table[self.gain_index] * self.joy_handle.get_axis('right_analog_y')
+                vel_cmd.twist.linear.z = 0.2 * self.gains_table[self.gain_index] * self.joy_handle.get_axis('left_analog_y')
                 vel_cmd.twist.angular.z =     -self.joy_handle.get_axis('left_analog_x')
             else:
-                x = 0.5 * gains[self.gain_value] * self.joy_handle.get_axis('right_analog_x') #NOTE: For ds4 controller X axis must be inverted
-                y = 0.5 * gains[self.gain_value] * self.joy_handle.get_axis('right_analog_y')
-                vel_cmd.twist.linear.x = (x*math.cos(self.ual_yaw) - y*math.sin(self.ual_yaw))
-                vel_cmd.twist.linear.y = (x*math.sin(self.ual_yaw) + y*math.cos(self.ual_yaw))
-                vel_cmd.twist.linear.z = 0.2 * gains[self.gain_value] * self.joy_handle.get_axis('left_analog_y')
+                x =  0.5 * self.gains_table[self.gain_index] * self.joy_handle.get_axis('right_analog_y')
+                y = -0.5 * self.gains_table[self.gain_index] * self.joy_handle.get_axis('right_analog_x')
+                vel_cmd.twist.linear.x = (x*math.cos(self.uav_yaw) - y*math.sin(self.uav_yaw))
+                vel_cmd.twist.linear.y = (x*math.sin(self.uav_yaw) + y*math.cos(self.uav_yaw))
+                vel_cmd.twist.linear.z = 0.2 * self.gains_table[self.gain_index] * self.joy_handle.get_axis('left_analog_y')
                 vel_cmd.twist.angular.z =     -self.joy_handle.get_axis('left_analog_x')
-            self.velocity(vel_cmd)
+            self.velocity_pub.publish(vel_cmd)
 
 def main():
+    # Parse arguments
+    parser = argparse.ArgumentParser(description='Teleoperate ual with a joystick')
+    parser.add_argument('-joy_file', type=str, default=None,
+                        help='Configuration yaml file describing joystick buttons mapping')
+    args, unknown = parser.parse_known_args()
+    # utils.check_unknown_args(unknown)
+
     rospy.init_node('joy_teleop', anonymous=True)
-    joy_file = rospkg.RosPack().get_path('ual_teleop') + '/config/saitek_p3200.yaml'  # TODO(franreal): argument!
-    robot_id = 'uav_1'  # TODO(franreal): argument!
-    teleop = JoyTeleop(joy_file, robot_id)
-    rospy.Subscriber(robot_id + '/ual/state', String, teleop.state_callback)
-    rospy.Subscriber(robot_id + '/ual/pose', PoseStamped, teleop.motion_callback) #TODO: Use ground truth
-    rospy.Subscriber('joy', Joy, teleop.joy_callback)
+
+    if args.joy_file is None:
+        default_joy_file = rospkg.RosPack().get_path('ual_teleop') + '/config/saitek_p3200.yaml'
+        rospy.loginfo("Using default joy map file [%s]", default_joy_file)
+        args.joy_file = default_joy_file
+    teleop = JoyTeleop(args.joy_file)
+
+    rospy.Subscriber('ual/state', String, teleop.state_callback)
+    rospy.Subscriber('ual/pose', PoseStamped, teleop.pose_callback)  # TODO: Use ground truth
+    rospy.Subscriber('ual_teleop/joy', Joy, teleop.joy_callback)
     rospy.spin()
 
 if __name__ == '__main__':