forked from pirobot/rbx1
/
calibrate_angular.py
executable file
·163 lines (120 loc) · 6.35 KB
/
calibrate_angular.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
#!/usr/bin/env python
""" calibrate_angular.py - Version 1.1 2013-12-20
Rotate the robot 360 degrees to check the odometry parameters of the base controller.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.5
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import rospy
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
from dynamic_reconfigure.server import Server
import dynamic_reconfigure.client
from rbx1_nav.cfg import CalibrateAngularConfig
import tf
from math import radians, copysign
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
class CalibrateAngular():
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_angular', 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 = rospy.get_param('~rate', 20)
r = rospy.Rate(self.rate)
# The test angle is 360 degrees
self.test_angle = radians(rospy.get_param('~test_angle', 360.0))
self.speed = rospy.get_param('~speed', 0.5) # radians per second
self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians
self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# Fire up the dynamic_reconfigure server
dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60)
# The base frame is usually base_link or base_footprint
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# 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.")
reverse = 1
while not rospy.is_shutdown():
if self.start_test:
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
last_angle = self.odom_angle
turn_angle = 0
self.test_angle *= reverse
error = self.test_angle - turn_angle
# Alternate directions between tests
reverse = -reverse
while abs(error) > self.tolerance and self.start_test:
if rospy.is_shutdown():
return
# Rotate the robot to reduce the error
move_cmd = Twist()
move_cmd.angular.z = copysign(self.speed, error)
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
# Compute how far we have gone since the last measurement
delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
# Add to our total angle so far
turn_angle += delta_angle
# Compute the new error
error = self.test_angle - turn_angle
# Store the current angle for the next comparison
last_angle = self.odom_angle
# Stop the robot
self.cmd_vel.publish(Twist())
# Update the status flag
self.start_test = False
params = {'start_test': False}
dyn_client.update_configuration(params)
rospy.sleep(0.5)
# Stop the robot
self.cmd_vel.publish(Twist())
def get_odom_angle(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):
rospy.loginfo("TF Exception")
return
# Convert the rotation from a quaternion to an Euler angle
return quat_to_angle(Quaternion(*rot))
def dynamic_reconfigure_callback(self, config, level):
self.test_angle = radians(config['test_angle'])
self.speed = config['speed']
self.tolerance = radians(config['tolerance'])
self.odom_angular_scale_correction = config['odom_angular_scale_correction']
self.start_test = config['start_test']
return config
def shutdown(self):
# Always stop the robot when shutting down the node
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
CalibrateAngular()
except:
rospy.loginfo("Calibration terminated.")