ellensohn/rmp220_teleop
1
0
Fork 0
mirror of https://github.com/bjoernellens1/rmp220_teleop.git synced 2024-11-22 07:53:47 +00:00
Code Issues Packages Projects Releases Wiki Activity

update joy teleop code: rebase

Browse Source
This commit is contained in:
Björn Ellensohn 2023-07-26 08:52:58 +02:00
parent 7a5923139b
commit 8ba88f311e
1 changed files with 34 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
rmp220_teleop/rmp220_teleop.py
View File

@ -3,41 +3,47 @@ from rclpy.node import Node
from sensor_msgs.msg import Joy from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
class TeleopTwistJoy(Node): class JoyToTwistNode(Node):
def __init__(self): def __init__(self):
super().__init__('teleop_twist_joy') super().__init__('joy_to_twist_node')
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) # Subscribe to the joy topic
self.twist = Twist() self.joy_subscriber = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
self.vFactor = 1 # if compensation is needed. Not needed anymore for bot_mini
self.limit = 1 * self.vFactor # Publish twist messages to the cmd_vel topic
self.max_vel = 2 * self.vFactor 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): def joy_callback(self, joy_msg):
if joy_msg.buttons[4]: #lb # Check if we have enough elements in the axes array
self.limit -= 0.1 * self.vFactor if len(joy_msg.axes) < 2:
if self.limit < 0.2 * self.vFactor: self.get_logger().warn("Not enough elements in the axes array. Ignoring joy message.")
self.limit = 0.2 * self.vFactor return
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
# Publish the twist message only when joystick keys are pressed # Check the LB and RB buttons (indices 4 and 5 in the buttons array)
if joy_msg.buttons[4] or joy_msg.buttons[5] or joy_msg.axes[1] != 0.0 or joy_msg.axes[0] != 0.0: if joy_msg.buttons[4]: # LB button pressed
self.cmd_vel_pub.publish(self.twist) 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): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
try: joy_to_twist_node = JoyToTwistNode()
teleop_twist_joy = TeleopTwistJoy() rclpy.spin(joy_to_twist_node)
rclpy.spin(teleop_twist_joy) joy_to_twist_node.destroy_node()
except KeyboardInterrupt:
teleop_twist_joy.destroy_node()
rclpy.shutdown() rclpy.shutdown()
if __name__ == '__main__': if __name__ == '__main__':