GUI example
Introduction
Files are available as teleop_gui here
In this section we are going to create a GUI Dashboard that
can control the robot either in an automated mode
(very primitive, as autonomous driving is not our objective)
or in responce to buttons that user presses.
It will also display lidar and odometry information.
Finally, it will receive and display data from a video
camera.
GUI library
In order to create GUI, am going to use GUI library called tkinter. It is neither powerful nor convenient, but simple enough to save us time: after all, we are not going to create anything complex.
$ sudo apt-get install python3-tk
Directory structure
The project is called teleop_gui, so we are going to create a folder for it under harsh (which is a name of my ROS2 workspace, you can use your own) / src.
The project is almost identical to the ros2_teleop above, as the mater of fact, I simply copied it over and went through all files, changing the name of an old package to a new one.
Alternatively, you can create a new package and add necessary files:
$ ros2 pkg create --build-type ament_python teleop_gui
Compared to ros2_teleop, the only new file is controller_gui.py that contains GUI code:
$ cd ~/SnowCron/ros_projects/harsh
$ touch src/teleop_gui/teleop_gui/controller_gui.py
The directory structure looks like this:
$ cd cd ~/SnowCron/ros_projects/harsh
$ tree --dirsfirst src/teleop_gui
src/teleop_gui
├── launch
│ └── teleop_gui.launch.py
├── resource
│ └── teleop_gui # Empty
├── teleop_gui
│ ├── controller_gui.py
│ ├── __init__.py # Empty
│ └── robot_controller.py
├── package.xml
├── setup.cfg
└── setup.py
teleop_gui.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(package='teleop_gui', executable='controller_gui',
output='screen', emulate_tty=True)
])
As you can see, all this launch file does is starting teleop_gui node, looking for a main function in a controller_gui file.
package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>teleop_gui</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="contact@mail.com">author</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
This file contains your contact/Copyright data and dependencies the project requires.
setup.cfg
[develop]
script-dir=$base/lib/teleop_gui
[install]
install-scripts=$base/lib/teleop_gui
setup.py
import os
from setuptools import setup
from glob import glob
package_name = 'teleop_gui'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name), glob('launch/*.launch.py'))
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='harsh',
maintainer_email='contact@mail.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'controller_gui = teleop_gui.controller_gui:main'
],
},
)
Note that if you (like myself) make a copy of ros2_teleop instead of creating it, you will have to pay attention to package names, otherwise it just will not work.
robot_controller.py
This is a modified copy of a file from ros2_teleop. It now has odometry, that is not required for the code to run, but we use it to display coordinates on our GUI dashboard.
It also has some additional member variables. There are many ways to exchange data between robot_controller that receives/publishes data and controller_gui that displays data in GUI and reacts to user's actions.
A hardcore ROS2 practitioner would probably send ROS2 messages between them... while I just access member variables directly: not very structured approach, but definitely a simple one.
# Python math library
import math
# ROS client library for Python
import rclpy
# Enables pauses in the execution of code
from time import sleep
# Used to create nodes
from rclpy.node import Node
# Enables the use of the string message type
# from std_msgs.msg import String
# Twist is linear and angular velocity
from geometry_msgs.msg import Twist
# Position, orientation, linear velocity, angular velocity
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from PIL import Image as pil_image
from PIL import ImageTk
# Handles LaserScan messages to sense distance to obstacles
from sensor_msgs.msg import LaserScan
# Handles quality of service for LaserScan data
from rclpy.qos import qos_profile_sensor_data
# Handle Pose messages
from geometry_msgs.msg import Pose
# Handle float64 arrays
from std_msgs.msg import Float64MultiArray
# Scientific computing library
import numpy as np
class Controller(Node):
def __init__(self):
super().__init__('Controller')
# When the user clicks the middle button in
# Lidar (top) panel, we shitch between automatic
# and manual control modes.
self.bManualControl = False
self.nCameraResX = 640
self.nCameraResY = 480
# ROS2 uses its own image format, so we need to
# convert it to something Python understands
self.bridge = CvBridge()
# Here we keep a last image from the camera.
# We need to initialize it, as TKinter crashes
# otherwise
self.cv_image = pil_image.new('RGB',
(self.nCameraResX, self.nCameraResY))
# front, left-front, left... corresponds to
# buttons on a Dashboard, filled from Lidar msg
self.arrLidarRanges = []
# Subscribe to messages of type nav_msgs/Odometry
# that provides position and orientation of the robot
# Nice to have, but NOT required to control our robot.
# We only use it to display data.
self.odom_subscriber = self.create_subscription(
Odometry,
'/odom',
self.odom_callback,
10)
# Subscribe to camera data
self.subscription = self.create_subscription(
Image,
'/camera/image_raw',
self.camera_callback,
qos_profile_sensor_data)
self.subscription # prevent unused variable warning
# Subscribe to messages of type sensor_msgs/LaserScan
self.scan_subscriber = self.create_subscription(
LaserScan,
'/laser_controller/out',
self.scan_callback,
qos_profile=qos_profile_sensor_data)
# Publish the desired linear and angular velocity of the robot
# (in the robot chassis coordinate frame)
# to the /cmd_vel_unstamped topic.
# The diff_drive controller that our robot uses will read this topic
# and move the robot accordingly.
self.publisher_ = self.create_publisher(
Twist,
'/diff_cont/cmd_vel_unstamped',
10)
# Initialize the LaserScan sensor readings to some large values
# Values are in meters.
self.left_dist = 999999.9 # Left
self.leftfront_dist = 999999.9 # Left-front
self.front_dist = 999999.9 # Front
self.rightfront_dist = 999999.9 # Right-front
self.right_dist = 999999.9 # Right
# Maximum forward speed of the robot in meters per second
self.forward_speed = 0.5
# Current position and orientation of the robot in the global
# reference frame
# Note that on a flat surface, not all values are required,
# but we still want to display them all.
self.current_x = 0.0
self.current_y = 0.0
self.current_z = 0.0
self.current_roll = 0.0
self.current_pitch = 0.0
self.current_yaw = 0.0
# Note that we are implementing very primitive nav.
# algorithm, that can barely handle simple navigation.
# Speed in rad/s
self.turning_speed_wf_fast = 3.0
# Wall following distance threshold.
# We want to stay within this distance from the wall.
self.dist_thresh_wf = 1.0 # in meters
def odom_callback(self, msg):
"""
The position is x, y, z.
The orientation is a x,y,z,w quaternion.
Thanks to a wonderfull web site of automaticaddison.com
"""
roll, pitch, yaw = self.euler_from_quaternion(
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
self.current_x = msg.pose.pose.position.x
self.current_y = msg.pose.pose.position.y
# On a horizontal plane, we do not need z
self.current_z = msg.pose.pose.position.z
# On a horizontal plane, we only need yaw
self.current_roll = roll
self.current_pitch = pitch
self.current_yaw = yaw
def camera_callback(self, msg):
try:
im = pil_image.fromarray(self.bridge.imgmsg_to_cv2(
msg, desired_encoding='passthrough'))
img = ImageTk.PhotoImage(image=im)
self.app.camera_label.configure(image=img)
self.cv_image = img
except CvBridgeError as e:
print(e)
def euler_from_quaternion(self, x, y, z, w):
"""
Convert a quaternion into euler angles (roll, pitch, yaw)
roll is rotation around x in radians (counterclockwise)
pitch is rotation around y in radians (counterclockwise)
yaw is rotation around z in radians (counterclockwise)
"""
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
def scan_callback(self, msg):
"""
This method gets called every time a LaserScan message is
received on the '/demo/laser/out' topic
"""
self.left_dist = msg.ranges[90]
self.leftfront_dist = msg.ranges[135]
self.front_dist = msg.ranges[180]
self.rightfront_dist = msg.ranges[225]
self.right_dist = msg.ranges[270]
self.arrLidarRanges = [ msg.ranges[225], msg.ranges[180], msg.ranges[135],
msg.ranges[270], 0.0, msg.ranges[90],
msg.ranges[315], msg.ranges[0], msg.ranges[45]]
# -1 here means "automatic"
self.follow_wall(-1)
# nCommand: -1 - auto, 0 - manual:left,
# 1 - manual-forward, 2 - manual:right
def follow_wall(self, nCommand):
if(self.bManualControl and nCommand == -1):
return
# Create a geometry_msgs/Twist message
msg = Twist()
msg.linear.x = 0.0
msg.linear.y = 0.0
msg.linear.z = 0.0
msg.angular.x = 0.0
msg.angular.y = 0.0
msg.angular.z = 0.0
d = self.dist_thresh_wf
# Very, very simplified!!!
if(nCommand == 1 or (nCommand == -1 and self.leftfront_dist > d and
self.front_dist > d and
self.rightfront_dist > d)):
self.wall_following_state = "go forward"
msg.linear.x = self.forward_speed
msg.angular.z = 0.
elif(nCommand == 2):
msg.linear.x = 0.
msg.angular.z = -self.turning_speed_wf_fast
else:
msg.linear.x = 0.
msg.angular.z = self.turning_speed_wf_fast
# Send velocity command to the robot
self.publisher_.publish(msg)
controller_gui.py
This file creates a Dashboard with Lidar, Odometry and Video.
# This is our file
from teleop_gui import robot_controller
# ROS2
import rclpy
import tkinter as tk
from PIL import Image, ImageTk
# There should be a way of using only a main thread, as
# ROS2 spin() does nothing but sleeping, so we don't need it
# if we have loop provided by tkinter. But I wasn't able to
# make it work.
import threading, os
class App(threading.Thread):
def __init__(self, controller):
# Reference to robot_controller, this is
# how we access data it receives from ROS2
self.controller = controller
# 9 buttons (8 Lidar distances and one START/STOP
# in the center)
self.arrLidarButtons = []
# Odometry GUI elements
self.arrOdomValues = []
threading.Thread.__init__(self)
self.start()
def after(self):
# Update Lidar buttons once every 0.5 seconds,
# Except button 4 (START/STOP)
for i, dRange in enumerate(self.controller.arrLidarRanges):
if(i == 4):
continue
if(dRange < 999999.9):
self.arrLidarButtons[i]["text"] = "%.2f" % (dRange)
# Update Odometry controls
self.arrOdomValues[0]["text"] = "%.2f" % (self.controller.current_x)
self.arrOdomValues[1]["text"] = "%.2f" % (self.controller.current_roll)
self.arrOdomValues[2]["text"] = "%.2f" % (self.controller.current_y)
self.arrOdomValues[3]["text"] = "%.2f" % (self.controller.current_pitch)
self.arrOdomValues[4]["text"] = "%.2f" % (self.controller.current_z)
self.arrOdomValues[5]["text"] = "%.2f" % (self.controller.current_yaw)
# Call it again, 0.5 seconds later
self.root.after(500, self.after)
def run(self):
self.root = tk.Tk()
# We use tkinter's approach to creating UI, see tutorial in
# "Sources" below.
frame_row_1 = tk.Frame(master=self.root)
frame_div_1 = tk.Frame(master=self.root, bg="gray", height=5)
frame_row_2 = tk.Frame(master=self.root)
frame_div_2 = tk.Frame(master=self.root, bg="gray", height=5)
frame_row_3 = tk.Frame(master=self.root)
self.root.title("Controller GUI")
self.root.resizable(width=False, height=False)
dInit = 0.0
# Create buttons and set initial values
for i in range(3):
for j in range(3):
frame = tk.Frame(
master=frame_row_1,
relief=tk.RAISED,
borderwidth=1
)
frame.grid(row=i, column=j, padx=5, pady=5)
strText = f"{dInit}"
if(i == 1 and j == 1):
strText = "STOP"
button = tk.Button(master=frame, text=strText, width=10,
command = lambda m=(i,j) : self.processNavCommand(m))
button.pack()
self.arrLidarButtons.append(button)
frame_row_1.pack(fill=tk.BOTH)#side=tk.LEFT)
frame_div_1.pack(fill="x")
# ---
# Create Odometry controls and set
# initial values
self.arrOdomLabels = [ ["x:", "roll:"],
["y:", "pitch:"], ["z:", "yaw:"] ]
for nRow in range(3):
for nCol in range(2):
frame = tk.Frame(
master=frame_row_2,
relief=tk.RAISED,
borderwidth=1
)
frame.grid(row=nRow, column=nCol, padx=5, pady=5)
label = tk.Label(master=frame, text=self.arrOdomLabels[nRow][nCol],
width=4)
label.pack(padx=5, pady=5, side=tk.LEFT)
# ---
value = tk.Label(master=frame, text=f"{dInit}", width=8, bg="white",
anchor="w")
value.pack(padx=5, pady=5, side=tk.LEFT)
self.arrOdomValues.append(value)
frame_row_2.pack(fill=tk.BOTH)#, side=tk.LEFT)
frame_div_2.pack(fill="x")
# Create a control that will hold video
self.camera_label = tk.Label(master=frame_row_3)
self.camera_label.pack()
frame_row_3.pack(fill=tk.BOTH)
# This is an ugly and unreliable way to
# shut it all down. Has to be improved.
self.root.after(500, self.after)
self.root.mainloop()
print("Shutting down", flush=True)
self.controller.destroy_node()
rclpy.shutdown()
os._exit(1)
# Swith between auto and manual control modes,
# if in manual mode, send nav. commands by calling
# controller's function directly.
def processNavCommand(self, nav):
if(nav[0] == 1 and nav[1] == 1): # Switch mode
if(self.controller.bManualControl):
self.controller.bManualControl = False
self.arrLidarButtons[4]["text"] = "STOP"
else:
self.controller.bManualControl = True
self.arrLidarButtons[4]["text"] = "START"
elif(self.controller.bManualControl):
if(nav[0] == 0 and nav[1] == 1): # Forward
self.controller.follow_wall(1)
elif(nav[0] == 1 and nav[1] == 0): # Left
self.controller.follow_wall(0)
elif(nav[0] == 1 and nav[1] == 2): # Right
self.controller.follow_wall(2)
else:
pass
# Note that here we call GUI in a separate thread.
# It shouldn't be this way: in theory, rclpy.spin(controller)
# is not required... but it is. Unfortunately, neither
# Python version of ROS2, not tkinter have spin_once,
# so here goes multithreading.
def main(args=None):
rclpy.init(args=args)
controller = robot_controller.Controller()
app = App(controller)
controller.app = app
rclpy.spin(controller)
if __name__ == '__main__':
main()
Running the code
# In Terminal 1, bring up GUI Dashboard:
$ cd ~/SnowCron/ros_projects/harsh
$ colcon build --packages-select teleop_gui
$ source install/setup.bash
$ ros2 launch teleop_gui teleop_gui.launch.py
# In Terminal 2, bring up Gazebo:
$ colcon build --packages-select diff_drive_controller_bot
$ source install/setup.bash
$ ros2 launch diff_drive_controller_bot launch_sim.launch world:=src/worlds/maze.sdf
To watch messages as they are being published, in a new terminal:
$ ros2 topic echo /diff_cont/cmd_vel_unstamped
linear:
x: 0.25
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0