ellensohn/rmp220_middleware
1
0
Fork 0
mirror of https://github.com/bjoernellens1/rmp220_middleware.git synced 2024-11-21 15:33:47 +00:00
Code Issues Packages Projects Releases Wiki Activity

cleanup

Browse Source
This commit is contained in:
Björn Ellensohn 2023-08-22 17:48:39 +02:00
parent 374351c59e
commit e72ecc9ec6
1 changed files with 14 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
rmp220_middleware/rmp220_middleware.py
View File

@ -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)