Ardros – Dead Reckoning

Now that we have the ability to retrieve odometry information from the robot and send velocity commands we combine both to drive the robot around using dead reckoning. The setup is very similar to the teleop scenario that I briefly discussed in the blog entry ROS Introduction Class with Ardros. Instead of the teleop node we now use a new DeadReckoning node:

The other key difference is that we now leverage the odometry transformation information that the arduino node publishes (represented by the ‘tf’ lollipop in the block diagram above). The DeadReckoning.py node is modeled after the corresponding C++ node in Willow Garage PR2 stack ‘Using the base controller with odometry and transform information‘.

Note that the DeadReckoning.py node can be used with any ROS enabled robot that publishes odometry information and accepts velocity commands based on the contract defined by ROS. Obviously in our case we use it in conjunction with the arduino node. If you want to follow along I suggest you locally check out the code from the Google Subversion repository. This blog entry is based on revision 61. In order to check out the code to the local directory ‘drh-robotics-ros’ use this command:

svn checkout -r 61 http://drh-robotics-ros.googlecode.com/svn/trunk/ drh-robotics-ros

For ROS to see the new package its path needs to be added to the ROS_PACKAGE_PATH environment variable. This is accomplished by adding a line at the end of the .bashrc file in your home folder:

export ROS_PACKAGE_PATH=~/drh-robotics-ros/ros/ardros:$ROS_PACKAGE_PATH

The path to the ardros folder might need to be adjusted.

Now we are ready to spin up the arduino node.

1. Open a new terminal and enterroscoreThis will start the ros server.
2. In a new terminal typeroscd ardrosroslaunch ./launch/ardros_standalone.launch

The content of the launch file ardros_standalone.launch is:

<launch>
  <node name="arduino" pkg="ardros" type="arduino.py">
    <rosparam file="$(find ardros)/info/ardros.yaml" command="load" />
  </node>
</launch>

It starts the arduino.py node with the parameters specified in the referenced ardros.yaml file:

port: /dev/ttyUSB0
baudRate: 115200
batteryMonitorParams: {voltageTooLowlimit: 11.7}

# wheel diameter [m], trackwidth [m], ticks per revolution
driveGeometry: {wheelDiameter: 0.0763, trackWidth: 0.37, countsPerRevolution: 9750}

# The speed controller parameters inlcude the P and I gains for the three PI controllers and the command timeout for the speed controller in seconds.
# If no velocity command arrives for more than the specified timeout then the speed controller will stop the robot.
speedController: {velocityPParam: 0.1, velocityIParam: 0.1, turnPParam: 0.4, turnIParam: 0.5, commandTimeout: 1.0}

The arduino.py node is now running, publishing tf and odometry messages while waiting for velocity commands. Sending those commands is the task of the  DeadReckoning.py node. This node implements the Driver class (more about it later) which exposes two public functions:

• DriveX(self, distance, speed)
• Turn(self, angle, angularSpeed)

The node creates an instance of the Driver class and then uses the two public functions to command the robot to drive a given pattern. The checked in code commands the robot to drive forward 2m, turn 180°, drive back 2m and finally turn 180° again to end up in the start position:

if __name__ == '__main__':
	try:
		driver = Driver()
		driver.DriveX(distance = 2, speed = 0.1);
		driver.Turn(angle = math.pi, angularSpeed = 0.3);
		driver.DriveX(distance = 2, speed = 0.1);
		driver.Turn(angle = -math.pi, angularSpeed = 0.3)
		#driver.Turn(angle = 3 * math.pi, angularSpeed = 0.3);
	except rospy.ROSInterruptException:
		pass

Let’s look at the code of the Driver class starting with its constructor:

	def __init__(self):
		rospy.init_node('DeadReckoning')

		self._VelocityCommandPublisher = rospy.Publisher("cmd_vel", Twist)
		self._TransformListener = tf.TransformListener()

		# wait for the listener to get the first transform message
		self._TransformListener.waitForTransform("/odom", "/base_link", rospy.Time(), rospy.Duration(4.0))

In the constructor the instance is registered as a ROS node. Since the node will publish twist messages on the cmd_vel topic a corresponding publisher needs to be created. In order to get information about the transformation from the /odom frame to the robot’s /base_link frame we need a TransformListener. Finally we wait up to 4 seconds for the requested transformation to arrive. If we don’t get a message within this time the constructor fails.

Driving forward and backward is handled by the DriveX function:

	def DriveX(self, distance, speed):
		'''
		Drive in x direction a specified distance based on odometry information
		distance [m]: the distance to travel in the x direction (>0: forward, <0: backwards) 		speed [m/s]: the speed with which to travel; must be positive 		''' 		forward = (distance >= 0)

		# record the starting transform from the odom to the base_link frame
		# Note that here the 'from' frame precedes 'to' frame which is opposite to how they are
		# ordered in tf.TransformBroadcaster's sendTransform function.
		# startTranslation is a tuple holding the x,y,z components of the translation vector
		# startRotation is a tuple holding the four components of the quaternion
		(startTranslation, startRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0))

		done = False

		velocityCommand = Twist()
		if forward:
			velocityCommand.linear.x = speed # going forward m/s
		else:
			velocityCommand.linear.x = -speed # going forward m/s

		velocityCommand.angular.z = 0.0 # no angular velocity

		while True:
			try:
				(currentTranslation, currentRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0))

				dx = currentTranslation[0] - startTranslation[0]
				dy = currentTranslation[1] - startTranslation[1]

				distanceMoved = math.sqrt(dx * dx + dy * dy)
				print distanceMoved
				if (forward):
					arrived = distanceMoved >= distance
				else:
					arrived = distanceMoved >= -distance

				if (arrived):
					break
				else:
					# send the drive command
					print("sending vel command" + str(velocityCommand))
					self._VelocityCommandPublisher.publish(velocityCommand)

			except (tf.LookupException, tf.ConnectivityException):
				continue

			rospy.sleep(0.1)

		#stop
		velocityCommand.linear.x = 0.0
		velocityCommand.angular.z = 0.0
		self._VelocityCommandPublisher.publish(velocityCommand)

		return done

The function first retrieves the current transformation between the frames /odom and /base_link to know where the robot starts from. Then it sets up a twist message that commands the robot to drive forward (or backward) with the specified speed. The actual driving happens in the while loop. We get the current transformation and calculate how far the robot drove since the start of the function. If we traveled the requested distance the while loop exists and the function returns. Otherwise the twist message is published on the cmd_vel topic and we wait a little before checking again …

The Turn function is structured in the same way. Since angles are involved it is slightly more complex but hopefully the code is clear enough to understand.

	def Turn(self, angle, angularSpeed):
		'''
		Turn the robot based on odometry information
		angle [rad]: the angle to turn (positive angles mean clockwise rotation)
		angularSpeed [rad/s]: the speed with which to turn; must be positive
		'''

		ccw = (angle >= 0) # counter clockwise rotation

		# record the starting transform from the odom to the base frame
		# Note that here the 'from' frame precedes 'to' frame which is opposite to how they are
		# ordered in tf.TransformBroadcaster's sendTransform function.
		(startTranslation, startRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0))
		startAngle = 2 * math.atan2(startRotation[2], startRotation[3])

		print "start angle: " + str(startAngle)
		previousAngle = startAngle
		angleOffset = 0.0

		done = False

		velocityCommand = Twist()
		velocityCommand.linear.x = 0.0 # going forward m/s
		if ccw:
			velocityCommand.angular.z = angularSpeed
		else:
			velocityCommand.angular.z = -angularSpeed

		while not rospy.is_shutdown():
			try:
				(currentTranslation, currentRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0))
				currentAngle = 2 * math.atan2(currentRotation[2], currentRotation[3])
				print "currentAngle: " + str(currentAngle)

				# we need to handle roll over of the angle
				if (currentAngle * previousAngle < 0 and math.fabs(currentAngle - previousAngle) > math.pi / 2):
					if (currentAngle > previousAngle):
						print "subtracting"
						angleOffset = angleOffset - 2 * math.pi
					else:
						print "adding"
						angleOffset = angleOffset + 2 * math.pi

				angleTurned = currentAngle + angleOffset - startAngle
				previousAngle = currentAngle

				print "angleTurned: " + str(angleTurned)
				if (ccw):
					arrived = (angleTurned >= angle)
				else:
					arrived = (angleTurned <= angle)

				print arrived

				if (arrived):
					break
				else:
					# send the drive command
					print("sending vel command" + str(velocityCommand))
					self._VelocityCommandPublisher.publish(velocityCommand)

			except (tf.LookupException, tf.ConnectivityException):
				continue

			time.sleep(0.1)

		#stop
		velocityCommand.linear.x = 0.0
		velocityCommand.angular.z = 0.0
		self._VelocityCommandPublisher.publish(velocityCommand)

		return done

We can now startup the DeadReckoning.py node and see it control the robot’s movements. In a new terminal type

roscd ardros
rosrun ardros DeadReckoning.py

This covers dead reckoning.