Robobot MQTT-client
Back to Robobot_B
MQTT-client in Python
This skeleton is intended as the mission controller app.
mqtt-client.py
This is the primary file in the mission control skeleton.
It is intended to run on the Raspberry Pi, using the command:
cd ~/svn/robobot/mqtt_python python3 mqtt-client.py
All the interfaces have a network interface except the GPIO (General Purpose Input Output). This allows the app to run anywhere with network access to the robot.
Command line parameters
The app has defined a few command line parameters, among these help:
python3 mqtt-client.py --help % Starting usage: mqtt-client.py [-h] [-w] [-g] [-l] [-s] [-n] Robobot app 2024 options: -h, --help show this help message and exit -w, --white Calibrate white tape level -g, --gyro Calibrate gyro -l, --level Calibrate horizontal -s, --silent Print less to console -n, --now Start drive now (do not wait for start button)
The command line options include some sensor calibration. The distance sensor calibration is missing (but needed, see the Robobot_teensy_firmware).
Main loop
The main loop is a state machine with a state for each mission part. The default version looks something like this (simplified):
import time as t
from spose import pose
from sgpio import gpio
from uservice import service
def loop():
state = 0
# main state machine
while not (service.stop or gpio.stop()):
if state == 0: # wait for start signal
start = gpio.start() or service.args.now
if start:
state = 12 # go straight
service.send(service.topicCmd + "ti/rc","0.25 0") # go straight
pose.tripBreset() # use trip counter B
elif state == 12: # go straight
if pose.tripB > 0.3 or pose.tripBtimePassed() > 10:
state = 14 # turn left
service.send(service.topicCmd + "ti/rc","0.25 0.5") # turn left
....
....
else: # abort
print(f"% Mission finished/aborted; state={state}")
break
# do not loop too fast
t.sleep(0.1)
# tell interface that we are alive
service.send(service.topicCmd + "ti/alive","")
pass # end of while loop
# end of mission, stop
service.send(service.topicCmd + "ti/rc","0 0")
t.sleep(0.05)
pass
############################################################
if __name__ == "__main__":
print("% Starting")
service.setup('10.197.218.172')
if service.connected:
loop()
service.terminate()
print("% Main Terminated")
The system is set up at the bottom of the code. The main setup parameter is the robot IP. If running on the Raspberry Pi, this should be 127.0.0.1; otherwise, it must be the robot's IP as displayed by the robot.
At the top are the other modules imported.
The "while" loop checks if anyone has pressed the stop button. Then, each state decides when to move on to the next state and what to do.
The commands to the robot, the MQTT messages, are a service function call:
service.send(service.topicCmd + "ti/rc","0.25 0") # go straight
The first parameter (service.topicCmd + "ti/rc") is the topic. The service module has the system and function part of the topic. Then, "ti/rc" is added to indicate that this is for the teensy_interface and a remote-control message.
The second parameter ("0.25 0") is the desired forward velocity in meters per second and the turn rate in radians per second (positive is CCV).
A delay, t.sleep(0.1), is inserted in the loop to save power and give the processor more time to handle incoming MQTT messages.
MQTT service
This module handles the MQTT communication and the app's general setup and termination.
Robot
This module decodes some of the robot's basic data, e.g., battery voltage.
All the data interfaces have an update counter that estimates the interval between updates. This is intended as a debug measure to detect potential slow network situations.
Pose
The robot pose has data related to the robot's movement.
The data is provided by requesting configuration data directly from Teensy and listening to the data streams configured by the teensy_interface.
The streamed data is
- Wheel velocity (message topic robobot/drive/T0/vel) in m/s.
- Motor velocity (message topic robobot/drive/T0/mvel) before the gear. The unit is radians/sec. This stream is probably not needed.
- The robot pose (message topic robobot/drive/T0/pose) is the x,y position and the robot heading as calculated in the Teensy.
Distance
The IR distances are received from Teensy and should be in meters, but this requires some calibration.
The distance sensor can give unreliable values if the infrared beam is not reflected within about 1m. This is why the sensor's default position points slightly down towards the floor.
Line
The line sensor has 8 reflection channels. This module receives a data stream with normalized values (assuming the sensor is calibrated).
Calibration on a white line is needed and can be done with a command line option.
There is a tape line position estimator in this module. It is based on an average calculation and is probably not good to follow a line edge in junctions.
See a data example in the next section.
Camera
The camera module captures images from the camera server stream.
The server sets the resolution and framerate.
A small example of using openCV to manipulate image frames is included.
The annotations in the image are the line sensor data with a white tape line to the left.
GPIO
Eight of the Raspberry Pi pins are available as general-purpose input-output. Two are dedicated as start and stop buttons.
The GPIO board has drivers capable of driving a high current (pull a relay or similar).
A button is available on all lines to give a signal to an input line and to simulate an output line.
See the circuit diagram for the IO board here Robobot circuits.
Log
A simple logging module is provided. The intention is to make a file with essential data that can be used for Matlab plotting.
The first lines are intended to describe the data columns. They are preceded by a "%" that will be seen as a comment by Matlab.
Remarks, like state change, can be added, and will be saved in the file with the "%" and a timestamp.
The default log looks like (logfile.txt):
% logfile for Python side % 1 time (sec) % 2 Mission state % 3,4,5 (x,y,h) (m,m,rad) % 6 Camera frame count % 7 Line sensor line position % 8,9 trip A (distance and heading change) % 10,11 trip B (distance and heading change) 1737224641.0107589 0 0.000 0.000 0.000 2 -1.22 0.000 0.0000.0000 0.0000 1737224641.036046 0 0.000 0.000 0.000 2 -1.27 0.000 0.0000.0000 0.0000 1737224641.0549598 20 0.000 0.000 0.000 2 -1.25 0.000 0.0000.0000 0.0000 % 1737224641.05497 % State change from 0 to 20 1737224641.061122 20 0.000 0.000 0.000 2 -1.25 0.000 0.0000.0000 0.0000 1737224641.0862672 20 0.000 0.000 0.000 2 -1.22 0.000 0.0000.0000 0.0000 1737224641.1113825 20 0.000 0.000 0.000 2 -1.26 0.000 0.0000.0000 0.0000 1737224641.1366208 20 0.000 0.000 0.000 2 -1.21 0.000 0.0000.0000 0.0000 1737224641.201906 20 0.000 0.000 0.000 2 -1.20 0.000 0.0000.0000 0.0000 1737224641.2038832 20 0.000 0.000 0.000 2 -1.22 0.000 0.0000.0000 0.0000
The robot is not moving, but the state and line sensor estimates are visible.
Drive command data flow
The data flow from the MQTT client to the wheels and back is illustrated in this figure:
Behave
The Behave block sends a 'remote-control' MQTT message, like:
robobot/cmd/ti/rc 0.1 0.5
where the topic robobot/cmd/ti/rc indicates that it is for the teensy_interface and is a remote-control rc. The parameters are a forward velocity of 0.1 m/s and a turn rate of 0.5 rad/s.
The related Python command is
service.send(service.topicCmd + "ti/rc","0.1 0.5")
MQTT
The yellow MQTT block handles all publish and subscribe messages and is shown as transparent.
All communication is through a network server on the robot's Raspberry PI. The port is 1883. The result is that the Python MQTT client can run both on the Raspberry Py and on any other PC on the network with access to the robot.
Teensy_interface
The teensy_interface subscribes to all topics starting with robobot/cmd/ti. The remote control message values are mixed to a desired velocity for the left and right wheels. The closed loop velocity control block generates a motor voltage sent to the Teensy 4.1 board.
The robot.ini configuration file allows you to select many sensor messages available from the Teensy 4.1 board. These messages can be logged for debugging or data analysis. All messages are further published to the MQTT server.
Other data can be subscribed directly from the Teensy and are then published directly as MQTT topics.
Sensor data
Sensor data in the MQTT client can be used to generate behaviour. Data like driven distance, heading change, line validity, distance sensor, and IMU data can be included.
Line drive
Line drive is slightly different.
The communication is mainly like the odometry data flow. The difference is that the turn rate is dependent on the line sensor.
Line sensor data
The line sensor data from MQTT looks like this.
'robobot/drive/T0/livn' 1738420382.4767 637 612 649 810 984 872 706 639 136 'robobot/drive/T0/livn' 1738420382.4837 638 612 650 812 985 876 710 641 140 'robobot/drive/T0/livn' 1738420382.4908 639 613 651 813 985 878 713 642 143
The topic part livn is 'line sensor value, normalized'. The first value is a timestamp and is ignored.
The values are normalized to 1000, where 1000 is the calibrated reflectance for a white tape line. In this case, sensor 5 shows a value of about 980; this sensor sees a tape line. The far-right sensor (8) has a value of about 140; this is not a tape line (0 is black).
Line detect
There is a (basic) line detect function that uses the centre of gravity for the bright part to determine the position of the tape line.
The figure shows 10 line sensor measurements in 2 situations. The red curves show steady measurements, and the blue shows a situation with more variation. Sensor level 0 is black (no reflection). In this case, the background is about 600, i.e., rather bright but not as bright as the tape line (calibrated to 1000). The vertical lines show the detector's estimate of the line position. Note the variation in measurements on the floor (a wooden floor with some variation).
The line position is zero if the tape line is centred below the robot, between sensors 4 and 5. The value is the number of sensors away from the centre. The red line position measurement is about -0.6, and the blue is about +0.7.
For every update of the line sensor data, a new turn rate is calculated and published on the MQTT server, which controls the wheels.
A forward velocity of zero will end the line drive.
Other detections can be added, such as line crossing to one side or line edge.
Servo control
The servo can be controlled by a message directly to the Teensy
robobot/drive/T0/servo 1 400 200
where the topic robobot/drive/T0/servo is a topic that is transferred directly to the teensy as a servo 1 400 200 command.
The command parameters are 1 for servo number one and 400 for the position (range is ca. -800 to 800 (us). The third parameter, 200, is velocity, the number of values per second. If the starting position is zero and the destination is 400, it takes about 2 seconds to reach position 400.
Velocity 0 gives the maximum velocity the servo can achieve.
A position outside the valid range will make the servo relax.
