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

Merge pull request #1 from bjoernellens1/optimizations

Browse Source 
Update rmp220_middleware.py
This commit is contained in:
bjoernellens1 2023-08-15 18:32:11 +02:00 committed by GitHub
commit 7e89c525a0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
rmp220_middleware/rmp220_middleware.py
View File

@ -76,26 +76,30 @@ class StateMachineNode(Node):
self.disable_chassis() self.disable_chassis()
def cmd_vel_callback(self, msg): def cmd_vel_callback(self, msg):
if self.state == State.ENABLED and (abs(msg.linear.x) >= 0.03 or abs(msg.angular.z) >= 0.03): # if self.state == State.ENABLED and (abs(msg.linear.x) >= 0.03 or abs(msg.angular.z) >= 0.03):
self.cmd_vel_pub.publish(msg) # self.cmd_vel_pub.publish(msg)
self.timeout = 20.0 # Reset timeout when receiving commands # self.timeout = 20.0 # Reset timeout when receiving commands
if msg.linear.x == 0.0 and msg.angular.z == 0.0: # if msg.linear.x == 0.0 and msg.angular.z == 0.0:
# No data received on cmd_vel_mux, publish zeros # # No data received on cmd_vel_mux, publish zeros
twist_msg = Twist() # twist_msg = Twist()
self.cmd_vel_pub.publish(twist_msg) # self.cmd_vel_pub.publish(twist_msg)
else: # else:
# Forward received data to cmd_vel_out # # Forward received data to cmd_vel_out
self.cmd_vel_pub.publish(msg) # self.cmd_vel_pub.publish(msg)
self.timeout = 10.0 # Reset timeout when receiving commands # self.timeout = 10.0 # Reset timeout when receiving commands
# Forward received data to cmd_vel_out # # Forward received data to cmd_vel_out
self.cmd_vel_pub.publish(msg) # self.cmd_vel_pub.publish(msg)
self.timeout = 10.0 # Reset timeout when receiving commands # self.timeout = 10.0 # Reset timeout when receiving commands
if abs(msg.linear.x) > 0.03 or abs(msg.angular.z) > 0.03: # if abs(msg.linear.x) > 0.03 or abs(msg.angular.z) > 0.03:
self.latest_cmd_vel = msg # self.latest_cmd_vel = msg
else: # else:
self.latest_cmd_vel = Twist() # self.latest_cmd_vel = Twist()
# This method shall only update the latest_cmd_vel attribute so it can be republished by the timer_callback with 100 HZ. Should have a look at performance though.
self.latest_cmd_vel = msg
# TODO: Add setting chassis state to enabled automatically upon receiving commands (with filter maybe)
def timer_callback(self): def timer_callback(self):
#self.cmd_vel_pub.publish(self.twist) #self.cmd_vel_pub.publish(self.twist)
@ -107,7 +111,7 @@ class StateMachineNode(Node):
self.disable_chassis() self.disable_chassis()
else: else:
self.timeout -= 0.1 self.timeout -= 0.1
self.cmd_vel_pub.publish(self.latest_cmd_vel) self.cmd_vel_pub.publish(self.latest_cmd_vel)
if self.state == State.DISABLED: if self.state == State.DISABLED:
self.cmd_vel_pub.publish(self.twist) self.cmd_vel_pub.publish(self.twist)