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

code cleanup

Browse Source
This commit is contained in:
Björn Ellensohn 2023-07-25 12:57:59 +02:00
parent e750766103
commit 6361e36d44
1 changed files with 1 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
rmp220_teleop/rmp220_teleop.py
View File

@ -2,8 +2,6 @@ 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
class TeleopTwistJoy(Node):
@ -13,7 +11,6 @@ class TeleopTwistJoy(Node):
self.joy_sub = self.create_subscription(Joy, 'joy', self.joy_callback, 10)
self.timer = self.create_timer(0.01, self.timer_callback) # 100 Hz
self.twist = Twist()
self.enable = False
self.vFactor = 1 # if compensation is needed. Not needed anymore for bot_mini
self.limit = 1 * self.vFactor
self.max_vel = 2 * self.vFactor
@ -28,49 +25,21 @@ class TeleopTwistJoy(Node):
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 #trying to compensate slower rotational movement? Must find out why!
# 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 * joy_msg.axes[0] * 1 * 3.141592
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 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
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
#rclpy.spin_until_future_complete(teleop_twist_joy, future)
teleop_twist_joy.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()