From 8ba88f311ed1bf650b11886330c252e0ed2e56d7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Wed, 26 Jul 2023 08:52:58 +0200
Subject: [PATCH] update joy teleop code: rebase

---
 rmp220_teleop/rmp220_teleop.py | 62 +++++++++++++++++++---------------
 1 file changed, 34 insertions(+), 28 deletions(-)

diff --git a/rmp220_teleop/rmp220_teleop.py b/rmp220_teleop/rmp220_teleop.py
index 67461cd..5f52a5a 100644
--- a/rmp220_teleop/rmp220_teleop.py
+++ b/rmp220_teleop/rmp220_teleop.py
@@ -3,42 +3,48 @@ from rclpy.node import Node
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import Twist
 
-class TeleopTwistJoy(Node):
-
+class JoyToTwistNode(Node):
     def __init__(self):
-        super().__init__('teleop_twist_joy')
-        self.cmd_vel_pub = self.create_publisher(Twist, '/diffbot_base_controller/cmd_vel_unstamped', 10)
-        self.joy_sub = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
-        self.twist = Twist()
-        self.vFactor = 1 # if compensation is needed. Not needed anymore for bot_mini
-        self.limit = 1 * self.vFactor
-        self.max_vel = 2 * self.vFactor
+        super().__init__('joy_to_twist_node')
+
+        # Subscribe to the joy topic
+        self.joy_subscriber = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
+
+        # Publish twist messages to the cmd_vel topic
+        self.twist_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
+
+        # Initialize maximum linear and angular velocities
+        self.max_linear_velocity = 0.5  # You can adjust this to your desired initial maximum linear velocity
+        self.max_angular_velocity = 1.0  # You can adjust this to your desired initial maximum angular velocity
 
     def joy_callback(self, joy_msg):
-        if joy_msg.buttons[4]: #lb
-            self.limit -= 0.1 * self.vFactor
-            if self.limit < 0.2 * self.vFactor:
-                self.limit = 0.2 * self.vFactor
-        if joy_msg.buttons[5]: #rb
-            self.limit += 0.1 * self.vFactor
-            if self.limit > self.max_vel:
-                self.limit = self.max_vel
-        self.twist.linear.x = self.limit * joy_msg.axes[1]
-        self.twist.angular.z = self.limit * joy_msg.axes[0] * 1 * 3.141592
+        # Check if we have enough elements in the axes array
+        if len(joy_msg.axes) < 2:
+            self.get_logger().warn("Not enough elements in the axes array. Ignoring joy message.")
+            return
 
-        # Publish the twist message only when joystick keys are pressed
-        if joy_msg.buttons[4] or joy_msg.buttons[5] or joy_msg.axes[1] != 0.0 or joy_msg.axes[0] != 0.0:
-            self.cmd_vel_pub.publish(self.twist)
+        # Check the LB and RB buttons (indices 4 and 5 in the buttons array)
+        if joy_msg.buttons[4]:  # LB button pressed
+            self.max_linear_velocity -= 0.1
+            self.get_logger().info(f"Decreased max linear velocity to {self.max_linear_velocity}")
+        if joy_msg.buttons[5]:  # RB button pressed
+            self.max_linear_velocity += 0.1
+            self.get_logger().info(f"Increased max linear velocity to {self.max_linear_velocity}")
 
+        # Create a twist message and fill it with the joy data
+        twist_msg = Twist()
+        twist_msg.linear.x = joy_msg.axes[1] * self.max_linear_velocity  # Use the second axis (forward/backward)
+        twist_msg.angular.z = joy_msg.axes[0] * self.max_angular_velocity  # Use the first axis (left/right)
+
+        # Publish the twist message to the cmd_vel topic
+        self.twist_publisher.publish(twist_msg)
 
 def main(args=None):
     rclpy.init(args=args)
-    try:
-        teleop_twist_joy = TeleopTwistJoy()
-        rclpy.spin(teleop_twist_joy)
-    except KeyboardInterrupt:
-        teleop_twist_joy.destroy_node()
-        rclpy.shutdown()
+    joy_to_twist_node = JoyToTwistNode()
+    rclpy.spin(joy_to_twist_node)
+    joy_to_twist_node.destroy_node()
+    rclpy.shutdown()
 
 if __name__ == '__main__':
     main()