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

optimize

Browse Source
This commit is contained in:
Björn Ellensohn 2023-08-18 10:36:18 +02:00
parent 18a2d08ba8
commit 88ab6a4d28
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
rmp220_middleware/rmp220_middleware.py
View File

@ -36,6 +36,8 @@ class StateMachineNode(Node):
self.twist = Twist() self.twist = Twist()
self.latest_cmd_vel = Twist() self.latest_cmd_vel = Twist()
self.abs_x = 0.0
self.abs_z = 0.0
# 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')
@ -70,6 +72,9 @@ class StateMachineNode(Node):
def cmd_vel_callback(self, msg): def cmd_vel_callback(self, msg):
# 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. # 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 self.latest_cmd_vel = msg
self.abs_x = abs(msg.linear.x)
self.abs_z = abs(msg.angular.z)
self.timeout = 20.0 # Reset timeout when receiving commands self.timeout = 20.0 # Reset timeout when receiving commands
def timer_callback(self): def timer_callback(self):
@ -81,7 +86,7 @@ class StateMachineNode(Node):
else: else:
self.timeout -= 0.01 self.timeout -= 0.01
self.cmd_vel_pub.publish(self.latest_cmd_vel) self.cmd_vel_pub.publish(self.latest_cmd_vel)
if self.state == State.DISABLED and (abs(self.latest_cmd_vel.linear.x) > 0.1 or abs(self.latest_cmd_vel.angular.z > 0.1)): # 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.1 or self.abs_z > 0.1): # 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()