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

rewrite for bot_mini

Browse Source
This commit is contained in:
Björn Ellensohn 2023-05-08 11:42:40 +02:00
parent d5758fe47f
commit 97bf198c2a
1 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
rmp220_teleop/rmp220_teleop.py
View File

@ -2,14 +2,14 @@ import rclpy
from rclpy.node import Node 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
from segway_msgs.srv import RosSetChassisEnableCmd #from segway_msgs.srv import RosSetChassisEnableCmd
class TeleopTwistJoy(Node): class TeleopTwistJoy(Node):
def __init__(self): def __init__(self):
super().__init__('teleop_twist_joy') super().__init__('teleop_twist_joy')
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) 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.joy_sub = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
self.timer = self.create_timer(0.01, self.timer_callback) self.timer = self.create_timer(0.01, self.timer_callback)
self.twist = Twist() self.twist = Twist()
@ -26,49 +26,49 @@ class TeleopTwistJoy(Node):
if self.limit > 3: if self.limit > 3:
self.limit = 3 self.limit = 3
self.twist.linear.x = self.limit * joy_msg.axes[1] self.twist.linear.x = self.limit * joy_msg.axes[1]
self.twist.angular.z = self.limit *2 * joy_msg.axes[0] self.twist.angular.z = -self.limit *2 * joy_msg.axes[0]
if joy_msg.buttons[7]: #start # if joy_msg.buttons[7]: #start
self.enable = True # self.enable = True
enable_chassis(self) # enable_chassis(self)
if joy_msg.buttons[6]: #back # if joy_msg.buttons[6]: #back
self.enable = False # self.enable = False
disable_chassis(self) # disable_chassis(self)
def timer_callback(self): def timer_callback(self):
self.cmd_vel_pub.publish(self.twist) self.cmd_vel_pub.publish(self.twist)
def enable_chassis(node): # def enable_chassis(node):
chassis_enable = node.create_client(RosSetChassisEnableCmd, '/set_chassis_enable') # chassis_enable = node.create_client(RosSetChassisEnableCmd, '/set_chassis_enable')
req = RosSetChassisEnableCmd.Request() # req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = True # req.ros_set_chassis_enable_cmd = True
while not chassis_enable.wait_for_service(timeout_sec=1.0): # while not chassis_enable.wait_for_service(timeout_sec=1.0):
print('Service not available, waiting again...') # print('Service not available, waiting again...')
chassis_enable.call_async(req) # chassis_enable.call_async(req)
def disable_chassis(node): # def disable_chassis(node):
chassis_disable = node.create_client(RosSetChassisEnableCmd, '/set_chassis_enable') # chassis_disable = node.create_client(RosSetChassisEnableCmd, '/set_chassis_enable')
req = RosSetChassisEnableCmd.Request() # req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = False # req.ros_set_chassis_enable_cmd = False
while not chassis_disable.wait_for_service(timeout_sec=1.0): # while not chassis_disable.wait_for_service(timeout_sec=1.0):
print('Service not available, waiting again...') # print('Service not available, waiting again...')
chassis_disable.call_async(req) # chassis_disable.call_async(req)
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
try: try:
teleop_twist_joy = TeleopTwistJoy() teleop_twist_joy = TeleopTwistJoy()
enable_chassis(teleop_twist_joy) # Call the function to enable the chassis #enable_chassis(teleop_twist_joy) # Call the function to enable the chassis
rclpy.spin(teleop_twist_joy) rclpy.spin(teleop_twist_joy)
except KeyboardInterrupt: except KeyboardInterrupt:
disable_chassis(teleop_twist_joy) # chassis disable for safe close #disable_chassis(teleop_twist_joy) # chassis disable for safe close
req = RosSetChassisEnableCmd.Request() #req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = False #req.ros_set_chassis_enable_cmd = False
#rclpy.spin_until_future_complete(teleop_twist_joy, future) #rclpy.spin_until_future_complete(teleop_twist_joy, future)
finally:
teleop_twist_joy.destroy_node() teleop_twist_joy.destroy_node()
rclpy.shutdown() rclpy.shutdown()
if __name__ == '__main__': if __name__ == '__main__':