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

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