diff --git a/wiki/system-design-development/how-to-design-a-robotic-state-machine.md b/wiki/system-design-development/how-to-design-a-robotic-state-machine.md new file mode 100644 index 00000000..3b8e5908 --- /dev/null +++ b/wiki/system-design-development/how-to-design-a-robotic-state-machine.md @@ -0,0 +1,169 @@ +--- +# Jekyll 'Front Matter' goes here. Most are set by default, and should NOT be +# overwritten except in special circumstances. +# You should set the date the article was last updated like this: +date: 2023-12-04 # YYYY-MM-DD +# This will be displayed at the bottom of the article +# You should set the article's title: +title: How to design a robotic state machine +# The 'title' is automatically displayed at the top of the page +# and used in other parts of the site. +--- + +# Motivation + +One key aspect of designing complex robotic systems is the effective +implementation of state machines. This article will explore the best +practices and design considerations for designing state machines +tailored for robotic applications. We begin by describing an example +state machine, then talk about linear versus asynchronous state +machines, publishers and subscribers versus services in ROS, and finally +discuss strategies for error handling in state machines. To learn more +about state machines, read these articles: + +- [Finite-state_machine](https://en.wikipedia.org/wiki/Finite-state_machine) + +- [State Machine Basics](https://www.freecodecamp.org/news/state-machines-basics-of-computer-science-d42855debc66/) + +# Example + +Let us consider a green pepper harvesting robot which consists of a +manipulator arm and a custom end-effector. The major subsystems are +perception, motion planning, and end-effector. This system is structured +in ROS with the following nodes: + +- Perception: takes an image and outputs 6D pose for point-of-interaction (POI) + + +- Kalman filtering: filters the noisy 6D poses of the POI to provide filtered 6D pose + +- Motion planning: generates a plan for manipulator arm and actuates it to the POI + +- End-effector: actuates a gripper and cutter mechanism to harvest a pepper + +Two other nodes jointly control these subsystems. A state machine node +handles the definitions of different states and transitions between +them. This node accesses all the information published or made available +by other nodes to decide when we transition from one state to another. +On the other hand, a planner node constantly listens to the current +state to execute actions that need to be performed in that state. This +node may utilize any data that it needs. + +- State machine: defines different states and transitions between them + +- Planner: listens to the current state to execute actions that need to be performed + +# Linear vs Asynchronous + +The easiest way to structure a state machine is to linearly move forward +from one state to another upon completion of tasks in the current state. +Doing so, however, can limit the system's decision-making abilities and +limit scalability to complex architectures. In the case of the pepper +harvesting robot, a linear state machine will move the arm to multiple +positions one by one, capture images to estimate noisy 6D poses for POI, +then obtain a filtered pose, move to the POI, and harvest the pepper +with the end-effector. All these actions are completed one at a time. +The state machine will wait for one state to complete its actions before +moving on. + +A direct consequence of this is an increase in harvesting time with an +increased number of positions the arm is moved to. Furthermore, this +system cannot scale to have a mobile base that transports the arm to a +new pepper plant location by detecting peppers. With a linear state +machine, base motion and pepper detection cannot be performed at the +same time. + +In the case of an asynchronous state machine, some nodes are structured +to constantly process their inputs to publish outputs at all times. The +perception node can continuously generate POIs from the input images +published by a camera. This would allow the Kalman filtering node to +keep track of existing POIs and update them at the same frequency as the +perception node. One immediate advantage of this structure is that +images are processed in parallel with arm motion. This would drastically +reduce the overall harvesting time. Similarly, a mobile base can easily +be added to the system now. Another advantage of such a state machine is +that it can make decisions asynchronously. Given that some nodes are +always publishing data and some perform actuation based on the state, +the state machine can utilize current data to quickly update the next +action, instead of waiting for a state to be completed. + +# Choosing Communication Paradigm for State Machines in ROS + +When implementing a state machine in ROS, a crucial decision is whether +to use ROS publishers and subscribers or ROS services for state +communication. Let\'s explore the pros and cons of each approach. + +## Publisher & Subscriber +### Pro +1. Decentralized Communication: Enables asynchronous communication, allowing multiple nodes to be informed of the state independently. +2. Real-time Suitability: Non-blocking communication ensures timely updates for decisions requiring real-time computation. +3. Scalability: Easily scalable by adding more nodes as subscribers due to the decentralized system. + +### Con +1. No Guarantee of Delivery: Lack of confirmation on message receipt necessitates manual checks for communication. +2. Synchronization Issues: Asynchronous nature may require additional synchronization mechanisms for timely information retrieval. +3. Limited to Point-to-Point Communication: Restriction to one-to-one communication impedes multi-node awareness of the state. +4. Enforces a Linear State Machine: The point-to-point limitation results in a linear state model. +5. Communication Overhead: Frequent state changes with request-response interactions introduce performance overhead. + +## Ros service +### Pro + +1. Blocks Communication: Code blocking ensures coordination between nodes during communication. + +### Con +1. Limited to Point-to-Point Communication: Similar to publishers and subscribers, ROS services only allow communication between two nodes. +2. Enforces a Linear State Machine: Restricts the state machine to a linear model. +3. Overhead in Communication: Frequent state changes with request-response interactions introduce performance overhead. + + +As many networking errors can arise in a robotic system, it is usually +better to have a decentralized communication system rather than a +centralized, linear system. Using a rosservice often causes the system +to be blocked and slows down the entire system, and it is challenging to +debug where the blockage is happening. Thanks to the increased +computational speed, although publishers and subscribers do not +guarantee delivery, we generally do not have to worry about whether or +not the message gets delivered to the nodes. + +# Frequent errors that occur in state machines + +As mentioned, there are two ways of implementing the state machine. The +errors that occur vary depending on the implementation. + +## Publishing & Subscribing method: + +- The publisher and subscriber frequency may differ. If the publishing node and subscribing node are running at different frequencies, and the publisher is publishing at 500 Hz and the subscriber is listening at 1hz, there might be some messages (states) that are lost on the listener end. + +## Ros service method: + +- The system performance is slow: As Ros services block the code from moving on to the next block until it ensures a successful communication, you can easily face your code slowing down to ensure this successful communication. If this communication occurs across multiple nodes, the problem becomes worse. If you go down this route, ensuring no unnecessary blockers is crucial to system performance. + +## How to separate scripts + +Assuming you have decided to move forward with the publisher and +subscriber, the question of how you will design the script arises. We +suggest having a state_machine.py that just listens to all the other +node's current status and moves from state to state. In this case, all +the subsystem nodes should also publish their status through publishers. + +## Handling Errors in State Machine Design: + +Consider whether a singular error state requiring human intervention is +acceptable. Alternatively, separate error-handling states can automate +resolution for certain non-critical errors. Design your states +accordingly. + +Here are some example cases we encountered. + +1. When the depth image was sparse and could not get a value on that pixel, the system would go into an error state. Instead, you could handle this by getting a new depth image until there is a non-null value + +2. Plt was throwing an error because it was not running in the mainthread. This error can be neglected, and the system can perform without handling this error. Not all errors are crucial to the system and can be ignored. + +3. The end-effector subsystem threw a motor failure error that halted the entire system. You can reset the motors using the motor API instead of intervening when you are in this particular error state. + +Some errors make sense for humans to intervene, however, there are many +more cases that make sense to be handled separately through code. When +designing states, especially error states, carefully decide on the +number and nature of error states to optimize system resilience and +efficiency. diff --git a/wiki/tools/ROSgui.md b/wiki/tools/ROSgui.md new file mode 100644 index 00000000..86c74a20 --- /dev/null +++ b/wiki/tools/ROSgui.md @@ -0,0 +1,369 @@ +--- +# Jekyll 'Front Matter' goes here. Most are set by default, and should NOT be +# overwritten except in special circumstances. +# You should set the date the article was last updated like this: +date: 2023-05-02 # YYYY-MM-DD +# This will be displayed at the bottom of the article +# You should set the article's title: +title: Framework GUI for robotic system using ROS +# The 'title' is automatically displayed at the top of the page +# and used in other parts of the site. +--- +PyQt is a easy to implement user interface that can be easily integrated with ROS. It is easily customizable and can be deployed on multiple platforms. This documentation provides a basic GUI framework that all robotics system using ROS can start on. + +## Requirements + +- This application assumes your workspace has ROS installed. [Install ROS](http://wiki.ros.org/ROS/Installation) + +- This application can be used on any Operating System including Linux, Windows and Mac. + +- This application can be used for ROS1 and ROS2 + + +### Installation + +To use PyQt5, + +```bash +$ pip install PyQt5 +``` + +## Overview of Final Application + +This is the final application you can get by following this tutorial. Blocks that need customization will be explained throughout the documentation. + +![Final QT](assets/PyQt-final.png) + +## Application Walkthrough + +### Timers + +The application has two timers + +1. System level timer: allows you to keep track of your entire system operation time. + +2. Task level timer: allows you to keep track of your task operation time. + +### System Level Timer + +The system level timer should only be triggered upon starting and ending the entire system. This can be done by pressing on the timer as the timer is a button. The color of the timer will change based on the ` color_change_times ` and ` color_change_colors`. This may change depending on each system's requirements. + +The color change for the timers are as follows: + +Green Button +![Green Button](assets/PyQt-green.png) +Yellow Button +![Yellow Button](assets/PyQt-yellow.png) +Orange Button +![Orange Button](assets/PyQt-orange.png) +Red Button +![Red Button](assets/PyQt-red.png) + +The code block for +``` +def SystemTimerBlock(self): + self.system_count = 0 + self.system_start = False + self.system_end = False + + self.system_timer_button = QPushButton("Start System Timer", self) + self.system_timer_button.setFixedSize(self.win.frameGeometry().width(),self.win.frameGeometry().height()//4) + self.grid.addWidget(self.system_timer_button, 0, 2) + self.system_timer_button.clicked.connect(self.system_timer_controller) + + system_timer = QTimer(self) + system_timer.timeout.connect(self.system_timer) + system_timer.start(1000) # modify to match system needs +``` + +### Task Level Timer + +The task level timer should only be triggered upon starting and ending one task. This can be done by pressing on the timer as the timer is a button. This button also changes color as time requirements are closely reached. + +One additional feature that the task level timer has is that it saves the task logs and outputs it in the `Task Times` block. + +Logs Block filled as tasks done accumulates. +![Red Button](assets/PyQt-logs.png) + +``` +def TaskTimerBlock(self): + self.task_times = {} + self.task_count = 0 + self.task_start = False + self.task_end = False + + self.task_timer_button = QPushButton("Start Task Timer", self) + self.task_timer_button.setFixedSize(self.win.frameGeometry().width(),self.win.frameGeometry().height()//4) + self.grid.addWidget(self.task_timer_button, 1, 2) + self.task_timer_button.clicked.connect(self.task_timer_controller) + + task_timer = QTimer(self) + task_timer.timeout.connect(self.task_timer) + task_timer.start(1000) # modify to match system needs + + self.task_times_label = QLabel("Task Times", self) + self.grid.addWidget(self.task_times_label, 2, 2, 2, 1) + self.task_times_label.setStyleSheet("border : 3px solid black") + self.task_times_label.setFont(QFont('Times', 10)) + self.task_times_label.setAlignment(Qt.AlignCenter) +``` + +### EStop Button + +The EStop button is a ROS publisher that will publish to a topic + +You need to change +``` +self.estop_pub = rospy.Publisher('/mrsd/estop', Bool, queue_size=10) +``` +and customize it for your system. Your ROS system main code should also have a estop subscriber that will shut down the entire system once the button is pressed. + + +### System Status + +The system status block subscribes to the +``` +self.status_sub = rospy.Subscriber('/mrsd/status', String, self.status_callback) +``` +topic. Thus your main system should publish a `/mrsd/status` topic. Ideally, your state machine will have a topic publisher that this application can subscribe to. + +### System Log + +This block is left for your system customization. It subscribes to + +``` +self.system_log_sub = rospy.Subscriber('/mrsd/system_log', String, self.system_log_callback) +``` + +and you should add some code to format the output. This section could be used to display + +1. How many peppers you have harvested +2. How many people you have saved +3. What process are up next +4. etc + + +### Visualization Block + +The visualization block is there for any of your team's visuals. Use cases can be + +1. Robot localization within the map +2. 3D point clouds +3. Object detection results +4. etc. + + +### Entire code +
+ pyqt-ros.py + + ``` + # importing libraries +from PyQt5.QtWidgets import * +from PyQt5.QtGui import * +from PyQt5.QtCore import * +import sys, emoji, rospy +from PyQt5.QtGui import QPixmap + +# system level requirements +total_demo_time = 60*20 # assuming SVD is 20 minutes +one_task_max = 60 # assuming each task is 60 seconds +color_change_times = [0.25, 0.5, 0.75, 1.0] +color_change_colors = ['green', 'yellow', 'orange', 'red'] + +gui_x, gui_y = 700, 600 + +class Window(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle("Python ") + self.win = QWidget() + self.grid = QGridLayout() + + self.UiComponents() + self.win.setLayout(self.grid) + self.win.setGeometry(0, 0, gui_x, gui_y) + self.win.show() + + + # self.status_sub = rospy.Subscriber('/mrsd/status', String, self.status_callback) + # self.estop_pub = rospy.Publisher('/mrsd/estop', Bool, queue_size=10) + # self.system_log_sub = rospy.Subscriber('/mrsd/system_log', String, self.system_log_callback) + + def SystemTimerBlock(self): + self.system_count = 0 + self.system_start = False + self.system_end = False + + self.system_timer_button = QPushButton("Start System Timer", self) + self.system_timer_button.setFixedSize(self.win.frameGeometry().width(),self.win.frameGeometry().height()//4) + self.grid.addWidget(self.system_timer_button, 0, 2) + self.system_timer_button.clicked.connect(self.system_timer_controller) + + system_timer = QTimer(self) + system_timer.timeout.connect(self.system_timer) + system_timer.start(1000) # modify to match system needs + + def TaskTimerBlock(self): + self.task_times = {} + self.task_count = 0 + self.task_start = False + self.task_end = False + + self.task_timer_button = QPushButton("Start Task Timer", self) + self.task_timer_button.setFixedSize(self.win.frameGeometry().width(),self.win.frameGeometry().height()//4) + self.grid.addWidget(self.task_timer_button, 1, 2) + self.task_timer_button.clicked.connect(self.task_timer_controller) + + task_timer = QTimer(self) + task_timer.timeout.connect(self.task_timer) + task_timer.start(1000) # modify to match system needs + + self.task_times_label = QLabel("Task Times", self) + self.grid.addWidget(self.task_times_label, 2, 2, 2, 1) + self.task_times_label.setStyleSheet("border : 3px solid black") + self.task_times_label.setFont(QFont('Times', 10)) + self.task_times_label.setAlignment(Qt.AlignCenter) + + def EStopBlock(self): + self.estop_button = QPushButton("E-Stop", self) + self.estop_button.setStyleSheet("background-color: red; border-radius: 15px") + self.estop_button.setFixedWidth(self.win.frameGeometry().width()) + self.estop_button.setFixedHeight(self.win.frameGeometry().height()//4) + self.grid.addWidget(self.estop_button, 3, 0, 1, 1) + self.estop_button.clicked.connect(self.estop_button_clicked) + + def SystemLogsBlock(self): + self.system_logs = QLabel("System Logs", self) + self.grid.addWidget(self.system_logs, 1, 0, 2, 1) + self.system_logs.setStyleSheet("border : 3px solid black") + self.system_logs.setFont(QFont('Times', 8)) + self.system_logs.setAlignment(Qt.AlignCenter) + + def VisualizationBlock(self): + self.pixmap = QPixmap('turtlesim.png') + self.image_label = QLabel(self) + self.image_label.setPixmap(self.pixmap) + self.image_label.setStyleSheet("border : 3px solid black") + self.grid.addWidget(self.image_label, 1, 1, 3, 1) + + def SystemStatusBlock(self): + self.system_status = QLabel("System Status", self) + self.system_status.setStyleSheet("border : 3px solid black") + self.system_status.setFont(QFont('Times', 10)) + self.system_status.setAlignment(Qt.AlignCenter) + self.grid.addWidget(self.system_status, 0, 0, 1, 2) + + def UiComponents(self): + self.SystemTimerBlock() + self.TaskTimerBlock() + self.EStopBlock() + self.SystemLogsBlock() + self.VisualizationBlock() + self.SystemStatusBlock() + + + def format_time(self, seconds): + return f'{seconds // 60} Minutes {seconds % 60} Seconds' + def change_system_color(self): + if self.system_count/total_demo_time < color_change_times[0]: + color = color_change_colors[0] + elif self.system_count/total_demo_time < color_change_times[1]: + color = color_change_colors[1] + elif self.system_count/total_demo_time < color_change_times[2]: + color = color_change_colors[2] + else: + color = color_change_colors[3] + self.system_timer_button.setStyleSheet(f"background-color: {color}") + def system_timer(self): + if self.system_start == True: + self.system_count += 1 + text = self.format_time(self.system_count) + self.system_timer_button.setText(text) + self.change_system_color() + + if self.system_end == True: + self.system_start = False + self.system_end = False + def system_timer_controller(self): + if self.system_start == False: + self.system_start = True + self.system_end = False + else: + self.system_start = False + self.system_end = True + self.system_timer_button.setText("Start System Timer") + def change_task_color(self): + if self.task_count/one_task_max < color_change_times[0]: + color = color_change_colors[0] + emoji = '😀' + elif self.task_count/one_task_max < color_change_times[1]: + color = color_change_colors[1] + emoji = '😐' + elif self.task_count/one_task_max < color_change_times[2]: + color = color_change_colors[2] + emoji = '😕' + else: + color = color_change_colors[3] + emoji = '😡' + self.task_timer_button.setStyleSheet(f"background-color: {color}") + return emoji + + def task_timer(self): + if self.task_start == True: + self.task_count += 1 + text = self.format_time(self.task_count) + self.task_timer_button.setText(text) + self.change_task_color() + + if self.task_end == True: + self.task_start = False + self.task_end = False + self.task_times[len(self.task_times)] = (self.task_count, self.change_task_color()) + self.task_count = 0 + self.task_times_label.setText(self.timer_label_format()) + def task_timer_controller(self): + if self.task_start == False: + self.task_start = True + self.task_end = False + else: + self.task_start = False + self.task_end = True + self.task_timer_button.setText("Start Task Timer") + def timer_label_format(self): + text = "" + for i in range(len(self.task_times)): + text += f"[{i+1}]:{self.task_times[i][1]}: {self.format_time(self.task_times[i][0])}\n" + return text + def estop_button_clicked(self): + print("estop clicked") + # self.estop_pub.publish(True) + + def status_callback(self, msg): + # updates the system status when status information is received + if msg.data != '': + self.system_status_label.setText(msg.data) + + def system_output_callback(self, msg): + # updates the system output when system information is received + # should modify to display the system output in a more readable format for each team + if msg.data != '': + self.system_logs.setText(msg.data) + +App = QApplication(sys.argv) +window = Window() +sys.exit(App.exec()) + ``` + +
+ + +## Summary +This article provides a walkthrough of a basic framework code structure for PyQt5 ROS UI development. It highlights parts that need to be modified in your system level ROS code as well as suggests possible modifications. + +## See Also: +- [PyQt5 Official Documentation](https://doc.qt.io/qtforpython-5/) + +## Further Reading +- Links to articles of interest outside the Wiki (that are not references) go here. +