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

update

Browse Source
This commit is contained in:
Björn Ellensohn 2023-09-04 10:33:02 +02:00
parent 7486acf14e
commit c29a04f06e
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
rmp220_middleware/rmp220_middleware.py
View File

@ -42,7 +42,7 @@ class StateMachineNode(Node):
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10) self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10)
self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10) self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 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.02, self.timer_callback) # changed to 50 Hz for testing purposes
# Create service clients for chassis enable and disable # Create service clients for chassis enable and disable
self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable')
@ -133,7 +133,7 @@ class StateMachineNode(Node):
else: else:
self.timeout -= 0.01 # at a rate of 100 Hz this equals to -1 per second. self.timeout -= 0.01 # at a rate of 100 Hz this equals to -1 per second.
self.cmd_vel_pub.publish(self.latest_cmd_vel) self.cmd_vel_pub.publish(self.latest_cmd_vel)
if self.state == State.DISABLED and (self.abs_x > 0.02 or self.abs_z > 0.02): # This is a hack to enable the chassis when receiving commands e.g. from Nav2 if self.state == State.DISABLED and (self.abs_x > 0.002 or self.abs_z > 0.002): # This is a hack to enable the chassis when receiving commands e.g. from Nav2
self.state = State.ENABLED self.state = State.ENABLED
self.get_logger().info("State: ENABLED (cmd_vel)") self.get_logger().info("State: ENABLED (cmd_vel)")
self.enable_chassis() self.enable_chassis()