## Lesson 7: Move control-calibrate linear

After above steps, robot can work but lacking of precise control. To fulfill ideal control precision, calibration is required, including linear velocity calibration for moving distance and angle velocity calibration for rotation angle.

### 1. Linear velocity calibration principle and code

Linear velocity calibration is fulfilled via checking the difference between robot move distance and control command required move distance. Considering of hardware precision, the error in allowable range can be accepted

Following code control robot moving forward 1m distance.

``````#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tf
class CalibrateLinear():
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_linear', anonymous=False)

# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)

# How fast will we check the odometry values?
self.rate = 10
r = rospy.Rate(self.rate)

# Set the distance to travel
self.test_distance = 1.0 # meters
self.speed = 1.0 # meters per second
self.tolerance = 0.01 # meters
self.odom_linear_scale_correction = 1.0
self.start_test = True

# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot

# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')

# Initialize the tf listener
self.tf_listener = tf.TransformListener()

# Give tf some time to fill its buffer
rospy.sleep(2)

# Make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))

rospy.loginfo("Bring up rqt_reconfigure to control the test.")

self.position = Point()

# Get the starting position from the tf transform between the odom and base frames
self.position = self.get_position()

x_start = self.position.x
y_start = self.position.y

move_cmd = Twist()

while not rospy.is_shutdown():
# Stop the robot by default
move_cmd = Twist()

if self.start_test:
# Get the current position from the tf transform between the odom and base frames
self.position = self.get_position()

# Compute the Euclidean distance from the target point
distance = sqrt(pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))

# Correct the estimated distance by the correction factor
distance *= self.odom_linear_scale_correction
# How close are we?
error =  distance - self.test_distance

# Are we close enough?
if not self.start_test or abs(error) <  self.tolerance:
self.start_test = False
params = False
else:
# If not, move in the appropriate direction
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y

self.cmd_vel.publish(move_cmd)
r.sleep()

# Stop the robot
self.cmd_vel.publish(Twist())

def get_position(self):
# Get the current transform between the odom and base frames
try:
(trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
return

return Point(*trans)

def shutdown(self):
# Always stop the robot when shutting down the node
self.cmd_vel.publish(Twist())
rospy.sleep(1)

if __name__ == '__main__':
try:
CalibrateLinear()
rospy.spin()
except:
``````

### 2. Code description

First importing necessary package

```import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tf
```

Initiating node and setting command sending frequency based on ROS standard initiation code

``````
class CalibrateLinear():
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_linear', anonymous=False)

# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)

# How fast will we check the odometry values?
self.rate = 10
r = rospy.Rate(self.rate)``````

Setting calibration parameter

test_distance：Moving forward distance

speed：Moving forward velocity

tolerance：precision range

odom_linear_scale_correction：odom data scale

start_test：test ON/OFF switch

``````       # Set the distance to travel
self.test_distance = 1.0 # meters
self.speed = 1.0 # meters per second
self.tolerance = 0.01 # meters
self.odom_linear_scale_correction = 1.0
self.start_test = True``````

Publishing Twist message to control robot move

``````        # Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)``````

Initiating base_frame, odom_frame, tf listener

``````        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot

# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')

# Initialize the tf listener
self.tf_listener = tf.TransformListener()

# Give tf some time to fill its buffer
rospy.sleep(2)

# Make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))

rospy.loginfo("Bring up rqt_reconfigure to control the test.")``````

Initiating position

``````        self.position = Point()

# Get the starting position from the tf transform between the odom and base frames
self.position = self.get_position()

x_start = self.position.x
y_start = self.position.y

move_cmd = Twist()``````

While loop is used to control robot move forward 1m as shown in following code

``````
while not rospy.is_shutdown():
# Stop the robot by default
move_cmd = Twist()

if self.start_test:
# Get the current position from the tf transform between the odom and base frames
self.position = self.get_position() #Getting current location information

#  Calculating the distance between current location and initial location
distance = sqrt(pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))

# Correct the estimated distance by the correction factor
distance *= self.odom_linear_scale_correction

# Calculating the distance to target location
error =  distance - self.test_distance

# Are we close enough?
if not self.start_test or abs(error) <  self.tolerance: #When arriving target location, stop robot moving
self.start_test = False
params = False
else:
# If still not arriving target location, keep moving forward; if exceeding target location, control motor reversion
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y

self.cmd_vel.publish(move_cmd)#Publishing Twist message
r.sleep()
``````

Getting current location based on tf data

``````    def get_position(self):
# Get the current transform between the odom and base frames
try:
(trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
return

return Point(*trans)``````

Stopping node

``````    def shutdown(self):
# Always stop the robot when shutting down the node
self.cmd_vel.publish(Twist())
rospy.sleep(1)``````

### 3.Calibration

With above calibrated node, we can calibrate robot linear velocity. First, finding a flat space for moving forward 1m, putting robot at starting point, then executing following command to check whether robot can move forward 1m

``rosrun diego_nav calibrate_linear.py ``

It’s ideal if robot exactly moves forward 1m, if not, double checking robot related parameters in my_arduino_params.yaml file, also, keeping in mind that the unit is meter in ROS.

``````# === Robot drivetrain parameters
wheel_diameter: 0.02900
wheel_track: 0.18
encoder_resolution: 2 # from Pololu for 131:1 motors
gear_reduction: 75.0
motors_reversed: True``````

If parameters of configuration file is the same as real data but robot cannot move forward as required precision, it may be related to motor performance, my_arduino_params.yaml parameter can be adjusted for precision compensation.

For high precision control, high precision hardware is required.

Scroll to top