From c07ee727d76d0f7c60ead83a25e3b4ca5b993fbf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Mon, 21 Aug 2023 15:18:44 +0200 Subject: [PATCH] try to solve issues: possible to force disable the robot via controller. + ChassisFB --- rmp220_middleware/rmp220_middleware.py | 50 +++++++++++++++++++++----- 1 file changed, 42 insertions(+), 8 deletions(-) diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py index 5d863d2..7e2fe43 100644 --- a/rmp220_middleware/rmp220_middleware.py +++ b/rmp220_middleware/rmp220_middleware.py @@ -7,15 +7,17 @@ from geometry_msgs.msg import Twist from sensor_msgs.msg import Joy from enum import Enum from segway_msgs.srv import RosSetChassisEnableCmd +from segway_msgs.msg import ChassisModeFb import atexit import signal import sys -class State(Enum): +class State(Enum): # is this best-practice? DISABLED = 0 - ENABLED = 1 + PAUSED = 1 + ENABLED = 2 class StateMachineNode(Node): def __init__(self): @@ -45,16 +47,46 @@ class StateMachineNode(Node): self.get_logger().info('Service not available, waiting for chassis enable service...') self.get_logger().info('Chassis enable service available.') + # Create a subscriber for the chassis status topic + self.chassis_mode_sub = self.create_subscription( + ChassisModeFb, + '/chassis_mode_fb', + self.chassis_mode_callback, + 10 + ) + + self.chassis_mode = State.DISABLED + + def chassis_mode_callback(self, msg): + # Handle the incoming chassis status message + # Need to figure out the type + if msg.chassis_mode == 1: # Assuming 1 represents enabled and 0 represents disabled + self.chassis_mode = State.ENABLED + else: + self.chassis_mode = State.DISABLED + + def get_chassis_mode(self): + return self.chassis_mode + def enable_chassis(self): req = RosSetChassisEnableCmd.Request() req.ros_set_chassis_enable_cmd = True self.chassis_enable_client.call_async(req) + self.state = State.ENABLED self.get_logger().info('Enabling chassis...') + def pause_chassis(self): + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = False + self.chassis_enable_client.call_async(req) + self.state = State.PAUSED + self.get_logger().info('Pausing chassis...') + def disable_chassis(self): req = RosSetChassisEnableCmd.Request() req.ros_set_chassis_enable_cmd = False self.chassis_enable_client.call_async(req) + self.state = State.DISABLED self.get_logger().info('Disabling chassis...') def joy_callback(self, msg): @@ -82,20 +114,22 @@ class StateMachineNode(Node): self.timeout = 20.0 def timer_callback(self): - if self.state == State.ENABLED: + #if self.state == State.ENABLED: + if self.chassis_mode == State.ENABLED: if self.timeout <= 0: - self.state = State.DISABLED - self.get_logger().info("State: DISABLED (Timeout)") + self.state = State.PAUSED + self.get_logger().info("State: PAUSED (Timeout)") self.disable_chassis() else: self.timeout -= 0.01 self.cmd_vel_pub.publish(self.latest_cmd_vel) - 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 + #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.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 self.state = State.ENABLED self.get_logger().info("State: ENABLED (cmd_vel)") self.enable_chassis() - if self.state == State.DISABLED and (self.abs_x < 0.1 and self.abs_z < 0.1): - self.cmd_vel_pub.publish(self.twist) + # 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)