The central logic of this project is in controller.py. It ingests data via Ros2 broadcasts from simulated sensors or other processes simulated in emergency_publicher.py and proximity_publisher.py. In turn, it broadcasts to a Node running in plotter.py that does visualization via matplotlib.
- Ros2 (this was written using Jazzy)
- matplotlib
Run the shell script run.sh. If it is not executable it can be made executable via chmod +x run.sh. This script is quite hacky but it gets the job done without much fuss. This must be run from the base directory of this project as it does make some assumptions about script locations. It will source the requisite messages and will source Ros2 for you. It assumes that the Ros2 setup script is at /opt/ros/jazzy/setup.bash; adjust accordingly if that is not the case in your installation. The script will start all of the python scripts and the simulation will run for 60 seconds. The data plot will remain open after that if you want to look more closely.
The proximity sensor output is sectioned off into 15 second chunks:
- 15 seconds at 200
- 15 seconds at 600
- 15 seconds at 1000
- 15 seconds rapidly oscillating between 805 and 795
I did not think it was important to publish other proximities because it's ultimately discrete. There is literally no difference between a broadcast of 1 and a broadcast of 399.
For the first 45 seconds, the emergency stop will be activated randomly. Every 5 seconds it will be deactivated. Once deactivated, a random number of seconds between 1 and 3 will be chosen and it will be activated again after that amount of time. This is certainly overkill. The last 15 seconds we deactivate it so that we can test the response to the dithering of the proximity around SLOW and FULL SPEED
Publishes proximities at 5Hz for 60 seconds in "meters". It publishes a Proximity.msg message on the topic /proximity. The first 15 seconds it will publish 200m. The second 15 seconds it will publish 600m. The third 15 seconds it will publish 1000m. The last 15 seconds it will publish oscillating values of 805m and 795m. There's nothing special about the values being close to the threshold, it's just to test that the speed controller does not rapidly respond to dithering that quickly.
Publishes emergency stop at semi-random invervals. It publishes an EStop.msg message on the topic /e_stop. It starts with the emergency stop deactivated. It will publish a deactivated e-stop message every 5 seconds. Each time it publishes the deactivated message, it will randomly pick a value between 1 and 3 seconds and publish an emergency stop after that random amount of time. It will finish publishing after 45 seconds after which it will make sure to broadcast a deactivate message and exit.
The "brains" of the simulation. It subscribes to the Proximity and EStop broadcasts. Internally it maintains the "desired" state and the "actual" state determined by whether or not the estop is active and how recently the state has been changed previously. There is a very simple histeresis timer that makes sure that the actual commanded state does not change more often than 0.5Hz. It publishes the actual state and the e-stop status (redundant but it makes plotting nicer) on /speed_control which is ingested by the plotter.
Plots the three signals (proximity, output state, and e-stop state) against an x axis of the timestamp via the std_msgs/Header message. Each time a new message is ingested, the all three plots are destroyed and re-plotted. This is horrifically inefficient but I did not have time to do it more efficiently. It would be more efficient to simply set the x and y data of the relevantly changed plot but that requires some fiddling with the limits that I just did not have the time to mess with. This gets the job done well enough to give you a good view of what is going on with the simulation.