From e72ecc9ec6824ff0c380c4e5cde98d93feb5e9a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Tue, 22 Aug 2023 17:48:39 +0200 Subject: [PATCH] cleanup --- rmp220_middleware/rmp220_middleware.py | 39 +++++++++----------------- 1 file changed, 14 insertions(+), 25 deletions(-) diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py index e106262..cca8d9f 100644 --- a/rmp220_middleware/rmp220_middleware.py +++ b/rmp220_middleware/rmp220_middleware.py @@ -26,15 +26,8 @@ class StateMachineNode(Node): super().__init__('state_machine_node') # Initialize state and other variables - self.state = State.DISABLED + self.state = State.DISABLED # is this necessary? self.timeout = 20.0 # Timeout in seconds - #self.limit = 0.5 # Limit for linear and angular velocity - - # Create publishers, subscribers, timers, and service clients - 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.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10) - self.timer = self.create_timer(0.01, self.timer_callback) # Create twist class for publishing velocities self.twist = Twist() @@ -43,6 +36,14 @@ class StateMachineNode(Node): self.abs_x = 0.0 self.abs_z = 0.0 + #self.limit = 0.5 # Limit for linear and angular velocity + + # Create publishers, subscribers, timers, and service clients + 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.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10) + self.timer = self.create_timer(0.01, self.timer_callback) + # Create service clients for chassis enable and disable self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') while not self.chassis_enable_client.wait_for_service(timeout_sec=1.0): @@ -57,33 +58,24 @@ class StateMachineNode(Node): 10 ) - self.chassis_mode = State.DISABLED - def chassis_mode_callback(self, msg): + # Need to evaluate that for possible errors # Handle the incoming chassis status message, will update every second - - if self.state == State.PAUSED: + if self.state == State.PAUSED: # trying to save processing time by skipping directly if 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 def enable_chassis(self): req = RosSetChassisEnableCmd.Request() @@ -113,11 +105,13 @@ class StateMachineNode(Node): if start_button == 1: self.get_logger().info("State: ENABLED (Button 'start')") self.enable_chassis() + self.timeout = 20 if select_button == 1: self.get_logger().info("State: DISABLED (Button 'select')") self.pause_chassis() def cmd_vel_callback(self, msg): + # Should be called everytime a new cmd_vel is received on /cmd_vel_mux # 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 @@ -131,23 +125,18 @@ class StateMachineNode(Node): def timer_callback(self): if self.state == State.PAUSED or self.state == State.STOPPED or self.state == State.PASSIVE: return # Do nothing if chassis is disabled, stopped or passive --> should save processing power - # if self.state == State.PAUSED: - # return # Do nothing if state is paused --> should save processing power if self.state == State.ENABLED: if self.timeout <= 0: self.state = State.DISABLED self.get_logger().info("State: DISABLED (Timeout)") self.disable_chassis() else: - self.timeout -= 0.01 + 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) - #if self.state == State.PAUSED 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 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.get_logger().info("State: ENABLED (cmd_vel)") self.enable_chassis() - # if self.state == State.PAUSED and (self.abs_x < 0.1 and self.abs_z < 0.1): # Is this even necessary? - # self.cmd_vel_pub.publish(self.twist) def main(args=None): rclpy.init(args=args)