Update joy function (#95)

Update joy function (#95)

auv_teleop/config/joy.yaml

@@ -0,0 +1,19 @@
+buttons:
+  launch_torpedo1: 4
+  launch_torpedo2: 2
+  drop_ball: 0
+  z_control: 1
+
+axes:
+  x_axis:
+    index: 1
+    gain: 0.4
+  y_axis:
+    index: 0
+    gain: 0.4
+  z_axis:
+    index: 1
+    gain: 0.4
+  yaw_axis:
+    index: 2
+    gain: 0.5

auv_teleop/config/ps5.yaml

@@ -0,0 +1,19 @@
+buttons:
+  launch_torpedo1: 0
+  launch_torpedo2: 2
+  drop_ball: 3
+  z_control: 1
+
+axes:
+  x_axis:
+    index: 1
+    gain: 0.4
+  y_axis:
+    index: 0
+    gain: 0.4
+  z_axis:
+    index: 1
+    gain: 0.4
+  yaw_axis:
+    index: 3
+    gain: 0.5

auv_teleop/launch/start_teleop.launch

@@ -3,9 +3,16 @@
   <arg name="namespace" default="taluy" />
   <arg name="id" default="0" />
   <arg name="control_rate" default="25.0" />
+  <arg name="controller" default="joy" /> 
 
   <group ns="$(arg namespace)">
-    <node pkg="joy" type="joy_node" name="joy_node" args=" --num $(arg id)" output="screen"></node>
-    <node pkg="auv_teleop" type="joy_manager.py" name="joy_manager_node" output="screen"></node>
+
+    <node pkg="joy" type="joy_node" name="joy_node" args=" --num $(arg id)" output="screen" />
+
+    <node pkg="auv_teleop" type="joy_manager.py" name="joystick_node" output="screen">
+
+      <rosparam command="load" file="$(find auv_teleop)/config/$(arg controller).yaml" />
+
+    </node>
   </group>
 </launch>

auv_teleop/scripts/joy_manager.py

@@ -1,13 +1,11 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import rospy
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import Twist
-from std_msgs.msg import Header
 from std_msgs.msg import Bool
 import threading
-from std_srvs.srv import Trigger, TriggerResponse, TriggerRequest
-
+from std_srvs.srv import Trigger, TriggerRequest
 
 class JoystickEvent:
     def __init__(self, change_threshold, callback):
@@ -18,7 +16,6 @@ def __init__(self, change_threshold, callback):
     def update(self, value):
         if (value - self.previous_value) > self.change_threshold:
             self.callback()
-
         self.previous_value = value
 
 
@@ -34,10 +31,13 @@ def __init__(self):
 
         self.publish_rate = 50  # 50 Hz
         self.rate = rospy.Rate(self.publish_rate)
+
+        self.buttons = rospy.get_param("~buttons")
+        self.axes = rospy.get_param("~axes")
 
         self.torpedo1_button_event = JoystickEvent(0.1, self.launch_torpedo1)
         self.torpedo2_button_event = JoystickEvent(0.1, self.launch_torpedo2)
-        self.droper_button_event = JoystickEvent(0.1, self.drop_droper)
+        self.dropper_button_event = JoystickEvent(0.1, self.drop_dropper)
 
         self.dropper_service = rospy.ServiceProxy(
             "actuators/ball_dropper/drop", Trigger
@@ -54,62 +54,58 @@ def __init__(self):
         self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_callback)
         rospy.loginfo("Joystick node initialized")
 
-    def call_service_if_available(self, service):
-        # check  if service is available
+
+    def call_service_if_available(self, service, success_message, failure_message):
         try:
             service.wait_for_service(timeout=1)
+            response = service(TriggerRequest())
+            if response.success:
+                rospy.loginfo(success_message)
+            else:
+                rospy.logwarn(failure_message)
         except rospy.exceptions.ROSException:
-            rospy.logwarn("Dropper service is not available")
-            return
-
-        # call dropper service
-        response = service(TriggerRequest())
-        if response.success:
-            rospy.loginfo("Dropper dropped")
-        else:
-            rospy.loginfo("Dropper failed to drop")
+            rospy.logwarn(f"Service {service.resolved_name} is not available")
 
     def launch_torpedo1(self):
-        self.call_service_if_available(self.torpedo1_service)
-        rospy.loginfo("Launching torpedo 1")
+        self.call_service_if_available(
+            self.torpedo1_service, "Torpedo 1 launched", "Failed to launch torpedo 1"
+        )
 
     def launch_torpedo2(self):
-        self.call_service_if_available(self.torpedo2_service)
-        rospy.loginfo("Launching torpedo 2")
+        self.call_service_if_available(
+            self.torpedo2_service, "Torpedo 2 launched", "Failed to launch torpedo 2"
+        )
+
+    def drop_dropper(self):
+        self.call_service_if_available(
+            self.dropper_service, "Ball dropped", "Failed to drop the ball"
+        )
 
-    def drop_droper(self):
-        rospy.loginfo("Dropping dropper")
-        self.call_service_if_available(self.dropper_service)
 
     def joy_callback(self, msg):
         with self.lock:
             self.joy_data = msg
 
-            self.torpedo1_button_event.update(self.joy_data.buttons[0])
-            self.torpedo2_button_event.update(self.joy_data.buttons[1])
-            self.droper_button_event.update(self.joy_data.buttons[3])
+            self.torpedo1_button_event.update(self.joy_data.buttons[self.buttons["launch_torpedo1"]])
+            self.torpedo2_button_event.update(self.joy_data.buttons[self.buttons["launch_torpedo2"]])
+            self.dropper_button_event.update(self.joy_data.buttons[self.buttons["drop_ball"]])
 
     def run(self):
         while not rospy.is_shutdown():
             twist = Twist()
 
             with self.lock:
                 if self.joy_data:
-
-                    # if button 1 pressed, control z axis
-                    if self.joy_data.buttons[1]:
+                    # Use axes with gain
+                    if self.joy_data.buttons[self.buttons["z_control"]]:
                         twist.linear.x = 0.0
-                        twist.linear.z = self.joy_data.axes[1] * 0.4
+                        twist.linear.z = self.joy_data.axes[self.axes["z_axis"]["index"]] * self.axes["z_axis"]["gain"]
                     else:
-                        twist.linear.x = self.joy_data.axes[1] * 0.4
+                        twist.linear.x = self.joy_data.axes[self.axes["x_axis"]["index"]] * self.axes["x_axis"]["gain"]
                         twist.linear.z = 0.0
 
-                    # control y and angular z axis
-                    twist.linear.y = (
-                        self.joy_data.axes[0] * 0.4
-                    )
-                    twist.angular.z = self.joy_data.axes[3] * 0.5
-
+                    twist.linear.y = self.joy_data.axes[self.axes["y_axis"]["index"]] * self.axes["y_axis"]["gain"]
+                    twist.angular.z = self.joy_data.axes[self.axes["yaw_axis"]["index"]] * self.axes["yaw_axis"]["gain"]
                 else:
                     twist.linear.x = 0.0
                     twist.angular.z = 0.0