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

fixes

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

BIN
rmp220_middleware/__pycache__/__init__.cpython-310.pyc
View File

Binary file not shown.

BIN
rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc
View File

Binary file not shown.

96
rmp220_middleware/rmp220_middleware copy.py
View File

@ -1,96 +0,0 @@
#!/usr/bin/env python3
import rclpy
from enum import Enum
from rclpy.node import Node
from std_msgs.msg import Bool
from geometry_msgs.msg import Twist
#from sensor_msgs.msg import Joy
from segway_msgs.srv import RosSetChassisEnableCmd
class State(Enum):
DISABLED = 0
ENABLED = 1
class StateMachineNode(Node):
def __init__(self):
super().__init__('state_machine_node')
# Initialize state and other variables
self.state = State.DISABLED
self.timeout = 2.0 # Timeout in seconds
# Create publishers, subscribers, timers, and service clients here
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.1, 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):
self.get_logger().info('Service not available, waiting for chassis enable service...')
self.chassis_disable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable')
while not self.chassis_disable_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('Service not available, waiting for chassis disable service...')
def joy_callback(self, msg):
# Implement logic to detect joystick button presses (start/select) and update state
# ...
if msg.buttons[7] == 1: # Joystick button 'start'
self.state = State.ENABLED
self.get_logger().info("State: ENABLED (Button 'start')")
self.enable_chassis()
if msg.buttons[6] == 1: # Joystick button 'select'
self.state = State.DISABLED
self.get_logger().info("State: DISABLED (Button 'select')")
self.disable_chassis()
def enable_chassis(self):
req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = True
self.chassis_enable_client.call_async(req)
def disable_chassis(self):
req = RosSetChassisEnableCmd.Request()
req.ros_set_chassis_enable_cmd = False
self.chassis_disable_client.call_async(req)
def cmd_vel_callback(self, msg):
# Update state to ENABLED upon receiving a command on /cmd_vel_mux
# ...
if self.state == State.ENABLED:
self.cmd_vel_pub.publish(msg)
self.timeout = 2.0 # Reset timeout when receiving commands
def timer_callback(self):
# Republish the cmd_vel_mux command to cmd_vel_out topic
# ...
# Reset the timeout counter
# ...
# Check if the timeout has been exceeded, and if so, switch to DISABLED
# ...
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.1
def main(args=None):
rclpy.init(args=args)
node = StateMachineNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

2
rmp220_middleware/rmp220_middleware.py
View File

@ -81,7 +81,7 @@ class StateMachineNode(Node):
else:
self.timeout -= 0.01
self.cmd_vel_pub.publish(self.latest_cmd_vel)
if self.state == State.DISABLED and (abs(self.latest_cmd_vel.linear) > 0.1 or abs(self.latest_cmd_vel.angular > 0.1)): # This is a hack to enable the chassis when receiving commands e.g. from Nav2
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
self.state = State.ENABLED
self.get_logger().info("State: ENABLED (cmd_vel)")
self.enable_chassis()