from statemachine import StateChart, State
class BoundedPatrol(StateChart):
allow_event_without_transition = False
forward = State(initial=True)
left = State()
finished = State(final=True)
out_of_bounds = (forward.to(left) | left.to(left))
in_bounds = (forward.to(forward) | left.to(forward))
quit = (forward.to(finished) | left.to(finished))
if __name__ == '__main__':
b = BoundedPatrol()
print(f"{b:md}")
while b.configuration != {b.finished}:
cmd = input("'out_of_bounds', 'in_bounds', or 'quit': ")
b.send(cmd)
print(b.configuration)
def state_name(self):
return next(iter(self.configuration)).id
while b.state_name() != 'finished':
Make these two modifications, then test the program and ensure it still
works.
from patrol_machine_1 import BoundedPatrol
import sys, curses, math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from geometry_msgs.msg import TwistStamped
from robot_pose import RobotPose, odom2pose, normalize_angle_symmetric
from nav_msgs.msg import Odometry
ANGULAR_VELOCITY = 0.5
LINEAR_VELOCITY = 1.0
DISTANCE_THRESHOLD = 0.5
HEADING_THRESHOLD = 0.25
def add_twist(machine: BoundedPatrol, t: TwistStamped):
state = machine.state_name()
if state == 'forward':
t.twist.linear.x = LINEAR_VELOCITY
else:
t.twist.angular.z = ANGULAR_VELOCITY
class BoundedPatrolNode(Node):
def __init__(self, robot_name: str, distance_threshold: float, heading_threshold: float):
super().__init__(f"{robot_name}_BoundedPatrolNode")
self.motors = self.create_publisher(TwistStamped, f"/{robot_name}/cmd_vel_stamped", qos_profile_sensor_data)
self.create_subscription(Odometry, f"/{robot_name}/odom", self.odom_callback, qos_profile_sensor_data)
self.machine = BoundedPatrol()
self.distance_threshold = distance_threshold
self.heading_threshold = heading_threshold
self.start = None
self.msg = None
self.distance = 0.0
self.heading = 0.0
def running(self):
return self.machine.state_name() != 'finished'
def odom_callback(self, odom: Odometry):
pose = odom2pose(odom)
if self.start is None:
self.start = pose
else:
self.send_odom_transition(pose)
self.publish_twist()
def send_odom_transition(self, pose: RobotPose):
self.distance = pose.distance_to(self.start.x, self.start.y)
self.heading = pose.turn_to(self.start.x, self.start.y)
if self.distance > self.distance_threshold and abs(self.heading) > self.heading_threshold:
self.msg = "out_of_bounds"
else:
self.msg = "in_bounds"
self.machine.send(self.msg)
def publish_twist(self):
t = TwistStamped()
t.header.frame_id = "base_link"
t.header.stamp = self.get_clock().now().to_msg()
add_twist(self.machine, t)
self.motors.publish(t)
def process_keystroke(self, key: str):
if key == 'q':
self.machine.send("quit")
def display(self, stdscr):
stdscr.addstr(1, 0, f"state: {self.machine.state_name()}{' ' * 10}")
stdscr.addstr(2, 0, f"transition: {self.msg}{' ' * 10}")
stdscr.addstr(3, 0, f"distance: {self.distance:.2f}{' ' * 10}")
stdscr.addstr(4, 0, f"heading: {self.heading:.2f}{' ' * 10}")
stdscr.refresh()
def main(stdscr):
rclpy.init()
node = BoundedPatrolNode(sys.argv[1], DISTANCE_THRESHOLD, HEADING_THRESHOLD)
curses.cbreak()
stdscr.nodelay(True)
stdscr.clear()
while node.running():
try:
k = stdscr.getch()
if k != -1:
node.process_keystroke(chr(k))
node.display(stdscr)
rclpy.spin_once(node, timeout_sec=0.0)
except curses.error as e:
if str(e) != 'no input':
stdscr.addstr(0, 0, traceback.format_exc())
rclpy.shutdown()
node.destroy_node()
curses.nocbreak()
curses.echo()
stdscr.refresh()
if __name__ == '__main__':
if len(sys.argv) < 2:
print("Usage: python3 patrol_node_1.py robot_name")
else:
curses.wrapper(main)