From 1104c8024c68e0f7fa7a0925deeb5ce39c6a4778 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Tue, 22 Aug 2023 10:25:57 +0200 Subject: [PATCH] changes in operation: testing --- rmp220_middleware/rmp220_middleware.py | 35 ++++++++++++++------------ 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py index 3bf9219..47b4f49 100644 --- a/rmp220_middleware/rmp220_middleware.py +++ b/rmp220_middleware/rmp220_middleware.py @@ -65,22 +65,25 @@ class StateMachineNode(Node): self.get_logger().info('Got chassis_mode from chassis_mode_fb topic') self.get_logger().info(str(msg.chassis_mode)) - if msg.chassis_mode == 0: - self.chassis_mode = State.DISABLED - self.state = State.DISABLED - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) - if msg.chassis_mode == 1: # Assuming 1 represents enabled and 0 represents disabled - self.chassis_mode = State.ENABLED - self.state = State.ENABLED - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) - if msg.chassis_mode == 2: - self.chassis_mode = State.PASSIVE - self.state = State.PASSIVE - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) - if msg.chassis_mode == 3: - self.chassis_mode = State.STOPPED - self.state = State.STOPPED - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) + if self.state == State.PAUSED: + return + else: + if msg.chassis_mode == 0: + self.chassis_mode = State.DISABLED + self.state = State.DISABLED + self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) + if msg.chassis_mode == 1: # Assuming 1 represents enabled and 0 represents disabled + self.chassis_mode = State.ENABLED + self.state = State.ENABLED + self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) + if msg.chassis_mode == 2: + self.chassis_mode = State.PASSIVE + self.state = State.PASSIVE + self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) + if msg.chassis_mode == 3: + self.chassis_mode = State.STOPPED + self.state = State.STOPPED + self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) def get_chassis_mode(self): return self.chassis_mode