Creating a Graphical User Interface for Joint Position Control in Controlit!

Original Author: John H. Price

Intro

This example is written in python and makes use of TKinter, Regular Expressions, and rospy to create a joint position control GUI. Note that while this tutorial will cover everything that you need for this application, the previously mentioned libraries contain many tools that cannot be reasonably covered in the scope of this page. If you wish to expand or customize your GUI, you may wish to explore this TKinter index. There are also resources available for the use of Regular Expressions and rospy.

For this program to function, we must perform a few tasks. Note that creating the GUI itself is only one step.

  • Create a GUI using TKinter.
  • Get the ROS topic to publish the goal under.
  • Get the name, range, and publication order for the DOFs.
  • Create the ROS publisher.
  • Convert the data obtained from the GUI to the appropriate data type.
  • Publish the formatted data to the appropriate topic

Imports and Initialization

#!/usr/bin/env python

import rospy						#for interacting with ROS topics and parameters
import sys, getopt					#for parameters and sys.exit()
from Tkinter import *				#for GUI elements
from std_msgs.msg import Float64MultiArray, MultiArrayDimension
import re							#for findall()
import string 						#for split()
from controlit_core.srv import *	#for indices service (used to sort joint indices)

#initialize ROS node
rospy.init_node('GUIControl', anonymous=True)

#Get the GUI width from the user
width = input("Please input the number of columns desired:")

# initialize Tkinter master
master = Tk()
master.title("Control GUI")

Here we import the necessary libraries, initialize the ROS node so that we can interact with ROS topics, and initialize the TKinter master which will host the GUI. We take the user input before Initializing the TKinter master to avoid a window resize bug in TKinter.

Finding and Setting the ROS Topic

# get the robot controller location
topics = rospy.get_published_topics(namespace = "/")
for topic in topics:
	robotname = re.findall("/(.+?)/JPosTask/+?",topic[0])
	if robotname !=[]:
		namespace = robotname[0]
		break

if (len(sys.argv) > 1): #publish to specified topic
	publishTopic = sys.argv[1]
else: #No argument, publish to joint goal position
	publishTopic = "/" + namespace + "/JPosTask/goalPosition" # set publisher location

print("publishing to " + publishTopic)

To send goals to the controller, we must know where to publish our goal message. A GUI that publishes to a specific topic for a specific model can skip this step and hard code the publisher location into the ROS publisher. This method is best suited for a GUI to be used with models that use the same subtopic but different namespaces.
The program will first find the namespace by utilizing rospy's get_published_topics. The variable "topics" above will look something like: [[/namespace1/topic1, string], [/namespace1/topic2,int], [/namespace2/topic1, int]]. We will then use Regular Expression's findall to sift through these lists for the data that we need. The namespace we seek can be found between the initial "/" and "/JPosTask/", and the topic we need to publish to is in that namespace under "/JPosTask/goalPosition". findall will search through the name of each ROS topic found by get_published_topics(). Here, as soon as we find something that matches our search, we set a string "robotname" that we will use for our publisher location, and exit the iterable.

Alternatively, if any parameters are given, The GUI will use the first parameter provided as the publisher topic. This is included so that if you wish to publish to the topic of a path planner or alternative control, you don't need to hard-code the change into the GUI.

To look for search criteria for other topics, simply use: print (rospy.get_published_topics(namespace = "/"))

ROS Publisher

#initialize publisher
goalPublisher = rospy.Publisher(publishTopic, Float64MultiArray)

Initialize the ROS publisher using the ROS topic location from the previous step.

Creating the Joint Class

#The joint class will create the scale and entry elements in the GUI for each DOF
class Joint:
	nameList = []
	jointList = [] # contains all of the joint elements
	def __init__(self,Label,Min,Max,outFrame):
		self.frame = Frame(outFrame)
		self.scale = Scale(self.frame, from_=Min, to=Max, orient = HORIZONTAL, resolution = 0.05)
		self.scale.config(length = 200, label = Label)
		self.scale.bind()
		self.scale.pack(side = LEFT)
		self.en = Entry(self.frame)
		self.en.bind("<return>", lambda e: self.setSlider()) #hitting ENTER in the entry box will run setSlider() on the joint
		self.en.config(width = 3)
		self.en.pack(side = LEFT)
		self.frame.pack(side = LEFT)
		Joint.nameList.append(Label)
		Joint.jointList.append(self)
	def getVal(self):
		return self.scale.get() # will return the scale value
	def setSlider(Joint):
		Joint.scale.set(float(Joint.en.get())) # get the entry value and set the slider to it
		Joint.en.delete(0,END) # clear the entry box

By making a class for joint creation, the GUI can support a variable number of joints without requiring any change in code. jointlist holds the GUI joint objects so that we can access their values and nameList stores the name in order of input so that they can be sorted. __init__ creates a piece of the GUI to represent a joint and adds it to the GUI. Each piece consists of a scale (slider) and an entry box, and the name and range for each joint will be acquired from the URDF through the ROS parameter server as shown below.

getVal is a simple get function that returns the value of the scale.

setSlider runs when ENTER is hit in the entry box. It sets the scale value equal to the entered value.

Gather the Joint Data

def getArray():
	tempArray=[]
	for el in joint.jointlist:
		tempArray.append(el.getVal())
	return tempArray

Gather the slider values into a list.

Populating the GUI

urdf = rospy.get_param("/robot_description")
nameList = re.findall("<joint name="\"(.+?)\"">+?",urdf)
filteredNameList=[]
for el in nameList:
	if re.findall("type=\"fixed+?",el) == []:
		filteredNameList.append(el.split("\"",1)[0])
lowBoundList = re.findall(" lower=\"(.+?)\"+?",urdf)
highBoundList = re.findall(" upper=\"(.+?)\"+?",urdf)

index=0
while (index < len(filteredNameList)):
	fInd=0
	outFrame = Frame(master)
	while fInd < width and index < len(filteredNameList):
		Joint(filteredNameList[index],float(lowBoundList[index]),float(highBoundList[index]),outFrame)
		fInd+=1
		index+=1
	outFrame.pack()

getJointOrder = rospy.ServiceProxy(namespace + "/diagnostics/getJointIndices", get_parameters)
jointOrder = str(getJointOrder()) + "\n"
jointOrderList = re.findall(" value: (.+?)\n+?",jointOrder)
sortedJointList=[]
for el in jointOrderList:
	sortedJointList.append(Joint.jointList[Joint.nameList.index(el)])

The width parameter determines how many joint objects wide the GUI will be. This is significant because a 2X16 GUI is both more functional and more aesthetic than a single column of 32 joint objects.
For the robotcontrolit models, the URDF can be found on the ROS parameter server under "/robot_description". The URDF contains the information used to create the robot model. Using re.findall on the URDF specification, we find the names (along with the joint type) and limits of each joint in the robot. The joint name list is filtered by taking out all entries for immobile or "fixed" joints, then removing the joint type from the name.
Note that the joints may be further limited by limb collision. For each joint name found, a Joint object is created using that name and the corresponding angle limits
getJointOrder returns a string containing the names of the joints in order of publication (whereas the URDF is not ordered). This string contains extraneous information so we sort out the names into a list using re.findall. To sort the joint objects in proper order for publication, we append the joints in matching order of their names in jointOrder. nameList is used to convert joint name into index location through index()

Format, Launch, and Publish

initArray = getArray()

#create the MultiArray to be published
dim = MultiArrayDimension()
dim.size = len(initArray)
dim.label = "posMsg"
dim.stride = len(initArray)

goalArray = Float64MultiArray()
goalArray.data = initArray
goalArray.layout.dim.append(dim)
goalArray.layout.data_offset = 0

def updateGoal():
	goalArray.data = getArray()
	goalPublisher.publish(goalArray)
	master.after(50,updateGoal)

master.after(50,updateGoal) # update the goal every 50 ms (20 Hz)
mainloop()

Create the Float64MultiArray that will be published using the data retrieved from the sliders, define the method for updating the data in the Float64MultiArray message and finally launch the GUI.

Usage

Let's test our GUI out on stickbot_lowerleg_3dof. Launch the model as described here.

Once you have started the simulation, run the script:

$ rosrun controlit_goal_generators JointPositionControlGUI.py

If you input 1 for the desired width, you should see something like this:

You should be able to move the sliders around to change the joint angles of stickbot_lowerleg_3dof. You can also type into the entry box and press ENTER to change the slider position.