Skip to content
This repository was archived by the owner on Jun 5, 2025. It is now read-only.

Commit de7637e

Browse files
MatthijsBurghMatthijsBurgh
authored
(AMCL)(Noetic) use robot pose in tests (#1087)
1 parent 6471b7b commit de7637e

14 files changed

+166
-129
lines changed

‎amcl/CMakeLists.txt

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -163,17 +163,10 @@ if(CATKIN_ENABLE_TESTING)
163163
add_rostest(test/set_initial_pose.xml)
164164
add_rostest(test/set_initial_pose_delayed.xml)
165165
add_rostest(test/basic_localization_stage.xml)
166+
add_rostest(test/global_localization_stage.xml)
166167
add_rostest(test/small_loop_prf.xml)
167168
add_rostest(test/small_loop_crazy_driving_prg.xml)
168169
add_rostest(test/texas_greenroom_loop.xml)
169170
add_rostest(test/rosie_multilaser.xml)
170171
add_rostest(test/texas_willow_hallway_loop.xml)
171-
172-
# Not sure when or if this actually passed.
173-
#
174-
# The point of this is that you start with an even probability
175-
# distribution over the whole map and the robot localizes itself after
176-
# some number of iterations of sensing and motion.
177-
#
178-
# add_rostest(test/global_localization_stage.xml)
179172
endif()

‎amcl/test/basic_localization.py

Lines changed: 58 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
#!/usr/bin/env python
22

3+
from __future__ import print_function
4+
35
import sys
46
import time
57
from math import fmod, pi
@@ -8,7 +10,8 @@
810
import rospy
911
import rostest
1012

11-
from tf2_msgs.msg import TFMessage
13+
import tf2_py as tf2
14+
import tf2_ros
1215
import PyKDL
1316
from std_srvs.srv import Empty
1417

@@ -19,61 +22,80 @@ def setUp(self):
1922
self.target_x = None
2023
self.target_y = None
2124
self.target_a = None
25+
self.tfBuffer = None
2226

23-
def tf_cb(self, msg):
24-
for t in msg.transforms:
25-
if t.header.frame_id == 'map':
26-
self.tf = t.transform
27-
(a_curr, a_diff) = self.compute_angle_diff()
28-
print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr))
29-
print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a))
30-
print('Diff:\t %16.6f %16.6f %16.6f' % (
31-
abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff))
32-
33-
def compute_angle_diff(self):
34-
rot = self.tf.rotation
35-
a = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2]
36-
d_a = self.target_a
27+
def print_pose_diff(self):
28+
a_curr = self.compute_angle()
29+
a_diff = self.wrap_angle(a_curr - self.target_a)
30+
print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr))
31+
print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a))
32+
print('Diff:\t %16.6f %16.6f %16.6f' % (
33+
abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff))
34+
35+
def get_pose(self):
36+
try:
37+
tf_stamped = self.tfBuffer.lookup_transform("map", "base_link", rospy.Time())
38+
self.tf = tf_stamped.transform
39+
self.print_pose_diff()
40+
except (tf2.LookupException, tf2.ExtrapolationException):
41+
pass
3742

38-
return (a, abs(fmod(a - d_a + 5*pi, 2*pi) - pi))
43+
@staticmethod
44+
def wrap_angle(angle):
45+
"""
46+
Wrap angle to [-pi, pi)
47+
:param angle: Angle to be wrapped
48+
:return: wrapped angle
49+
"""
50+
angle += pi
51+
while angle < 0:
52+
angle += 2*pi
53+
return fmod(angle, 2*pi) - pi
54+
55+
def compute_angle(self):
56+
rot = self.tf.rotation
57+
a_curr = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2]
58+
a_diff = self.wrap_angle(a_curr - self.target_a)
59+
return self.target_a + a_diff
3960

4061
def test_basic_localization(self):
4162
global_localization = int(sys.argv[1])
4263
self.target_x = float(sys.argv[2])
4364
self.target_y = float(sys.argv[3])
44-
self.target_a = float(sys.argv[4])
65+
self.target_a = self.wrap_angle(float(sys.argv[4]))
4566
tolerance_d = float(sys.argv[5])
4667
tolerance_a = float(sys.argv[6])
4768
target_time = float(sys.argv[7])
4869

70+
rospy.init_node('test', anonymous=True)
71+
while rospy.rostime.get_time() == 0.0:
72+
print('Waiting for initial time publication')
73+
time.sleep(0.1)
74+
4975
if global_localization == 1:
50-
#print 'Waiting for service global_localization'
76+
print('Waiting for service global_localization')
5177
rospy.wait_for_service('global_localization')
5278
global_localization = rospy.ServiceProxy('global_localization', Empty)
5379
global_localization()
5480

55-
rospy.init_node('test', anonymous=True)
56-
while(rospy.rostime.get_time() == 0.0):
57-
#print 'Waiting for initial time publication'
58-
time.sleep(0.1)
5981
start_time = rospy.rostime.get_time()
60-
# TODO: This should be replace by a pytf listener
61-
rospy.Subscriber('/tf', TFMessage, self.tf_cb)
82+
self.tfBuffer = tf2_ros.Buffer()
83+
listener = tf2_ros.TransformListener(self.tfBuffer)
6284

6385
while (rospy.rostime.get_time() - start_time) < target_time:
64-
#print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
86+
print('Waiting for end time %.6f (current: %.6f)' % (target_time, (rospy.rostime.get_time() - start_time)))
87+
self.get_pose()
6588
time.sleep(0.1)
6689

67-
(a_curr, a_diff) = self.compute_angle_diff()
68-
print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr))
69-
print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a))
70-
print('Diff:\t %16.6f %16.6f %16.6f' % (
71-
abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff))
72-
self.assertNotEquals(self.tf, None)
73-
self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d)
74-
self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d)
75-
self.assertTrue(a_diff <= tolerance_a)
90+
print("Final pose:")
91+
self.get_pose()
92+
a_curr = self.compute_angle()
93+
94+
self.assertNotEqual(self.tf, None)
95+
self.assertAlmostEqual(self.tf.translation.x, self.target_x, delta=tolerance_d)
96+
self.assertAlmostEqual(self.tf.translation.y, self.target_y, delta=tolerance_d)
97+
self.assertAlmostEqual(a_curr, self.target_a, delta=tolerance_a)
98+
7699

77100
if __name__ == '__main__':
78-
rostest.run('amcl', 'amcl_localization',
79-
TestBasicLocalization, sys.argv)
101+
rostest.run('amcl', 'amcl_localization', TestBasicLocalization, sys.argv)

‎amcl/test/basic_localization_stage.xml

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
1+
<?xml version="1.0" ?>
12
<!-- setting pose: 47.943 21.421 -0.503
2-
setting pose: 30.329 34.644 3.142
3+
setting pose: 30.329 34.644 3.142
34
117.5s -->
45
<launch>
56
<param name="/use_sim_time" value="true"/>
6-
<node name="rosbag" pkg="rosbag" type="play"
7-
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
7+
<node name="rosbag" pkg="rosbag" type="play"
8+
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
89
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
910
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
1011
<remap from="scan" to="base_scan" />
@@ -16,13 +17,14 @@ setting pose: 30.329 34.644 3.142
1617
<param name="max_particles" value="5000"/>
1718
<param name="kld_err" value="0.05"/>
1819
<param name="kld_z" value="0.99"/>
20+
1921
<param name="odom_model_type" value="omni"/>
2022
<param name="odom_alpha1" value="0.2"/>
2123
<param name="odom_alpha2" value="0.2"/>
22-
<!-- translation std dev, m -->
2324
<param name="odom_alpha3" value="0.8"/>
2425
<param name="odom_alpha4" value="0.2"/>
2526
<param name="odom_alpha5" value="0.1"/>
27+
2628
<param name="laser_z_hit" value="0.5"/>
2729
<param name="laser_z_short" value="0.05"/>
2830
<param name="laser_z_max" value="0.05"/>
@@ -31,8 +33,8 @@ setting pose: 30.329 34.644 3.142
3133
<param name="laser_lambda_short" value="0.1"/>
3234
<param name="laser_lambda_short" value="0.1"/>
3335
<param name="laser_model_type" value="likelihood_field"/>
34-
<!-- <param name="laser_model_type" value="beam"/> -->
3536
<param name="laser_likelihood_max_dist" value="2.0"/>
37+
3638
<param name="update_min_d" value="0.2"/>
3739
<param name="update_min_a" value="0.5"/>
3840
<param name="odom_frame_id" value="odom"/>
@@ -44,6 +46,6 @@ setting pose: 30.329 34.644 3.142
4446
<param name="initial_pose_y" value="21.421"/>
4547
<param name="initial_pose_a" value="-1.003"/>
4648
</node>
47-
<test time-limit="180" test-name="basic_localization_stage" pkg="amcl"
48-
type="basic_localization.py" args="0 47.060 21.603 -1.053 0.75 0.75 90.0"/>
49+
<test time-limit="180" test-name="basic_localization_stage" pkg="amcl"
50+
type="basic_localization.py" args="0 30.329 34.644 3.142 0.75 0.75 90.0"/>
4951
</launch>

‎amcl/test/global_localization_stage.xml

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
1+
<?xml version="1.0" ?>
12
<!-- setting pose: 47.943 21.421 -0.503
2-
setting pose: 30.329 34.644 3.142
3+
setting pose: 30.329 34.644 3.142
34
117.5s -->
45
<launch>
56
<param name="/use_sim_time" value="true"/>
6-
<node name="rosbag" pkg="rosbag" type="play"
7-
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/global_localization_stage_indexed.bag"/>
7+
<node name="rosbag" pkg="rosbag" type="play"
8+
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/global_localization_stage_indexed.bag"/>
89
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
910
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
1011
<remap from="scan" to="base_scan" />
@@ -16,13 +17,14 @@ setting pose: 30.329 34.644 3.142
1617
<param name="max_particles" value="5000"/>
1718
<param name="kld_err" value="0.05"/>
1819
<param name="kld_z" value="0.99"/>
20+
1921
<param name="odom_model_type" value="omni"/>
2022
<param name="odom_alpha1" value="0.2"/>
2123
<param name="odom_alpha2" value="0.2"/>
22-
<!-- translation std dev, m -->
2324
<param name="odom_alpha3" value="0.8"/>
2425
<param name="odom_alpha4" value="0.2"/>
2526
<param name="odom_alpha5" value="0.1"/>
27+
2628
<param name="laser_z_hit" value="0.5"/>
2729
<param name="laser_z_short" value="0.05"/>
2830
<param name="laser_z_max" value="0.05"/>
@@ -31,19 +33,16 @@ setting pose: 30.329 34.644 3.142
3133
<param name="laser_lambda_short" value="0.1"/>
3234
<param name="laser_lambda_short" value="0.1"/>
3335
<param name="laser_model_type" value="likelihood_field"/>
34-
<!-- <param name="laser_model_type" value="beam"/> -->
3536
<param name="laser_likelihood_max_dist" value="2.0"/>
37+
3638
<param name="update_min_d" value="0.2"/>
3739
<param name="update_min_a" value="0.5"/>
3840
<param name="odom_frame_id" value="odom"/>
3941
<param name="resample_interval" value="1"/>
4042
<param name="transform_tolerance" value="0.1"/>
4143
<param name="recovery_alpha_slow" value="0.0"/>
4244
<param name="recovery_alpha_fast" value="0.0"/>
43-
<param name="robot_x_start" value="0.0"/>
44-
<param name="robot_y_start" value="0.0"/>
45-
<param name="robot_th_start" value="0.0"/>
4645
</node>
47-
<test time-limit="180" test-name="global_localization_stage" pkg="amcl"
48-
type="basic_localization.py" args="1 46.519 21.384 3.099 0.25 0.25 28.0"/>
46+
<test time-limit="180" test-name="global_localization_stage" pkg="amcl"
47+
type="basic_localization.py" args="1 10.00 9.67 6.21 0.25 0.25 28.0" />
4948
</launch>

‎amcl/test/rosie_multilaser.xml

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
1+
<?xml version="1.0" ?>
12
<!-- Setting pose: 42.378 17.730 1.583
23
Setting pose: 33.118 34.530 -0.519
34
103.5s -->
45
<launch>
56
<param name="/use_sim_time" value="true"/>
6-
<node name="rosbag" pkg="rosbag" type="play"
7-
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/rosie_localization_stage.bag"/>
7+
<node name="rosbag" pkg="rosbag" type="play"
8+
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/rosie_localization_stage.bag"/>
89
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
910
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
1011
<remap from="scan" to="base_scan" />
@@ -16,13 +17,14 @@ Setting pose: 33.118 34.530 -0.519
1617
<param name="max_particles" value="5000"/>
1718
<param name="kld_err" value="0.05"/>
1819
<param name="kld_z" value="0.99"/>
20+
1921
<param name="odom_model_type" value="omni"/>
2022
<param name="odom_alpha1" value="0.2"/>
2123
<param name="odom_alpha2" value="0.2"/>
22-
<!-- translation std dev, m -->
2324
<param name="odom_alpha3" value="0.8"/>
2425
<param name="odom_alpha4" value="0.2"/>
2526
<param name="odom_alpha5" value="0.1"/>
27+
2628
<param name="laser_z_hit" value="0.5"/>
2729
<param name="laser_z_short" value="0.05"/>
2830
<param name="laser_z_max" value="0.05"/>
@@ -31,8 +33,8 @@ Setting pose: 33.118 34.530 -0.519
3133
<param name="laser_lambda_short" value="0.1"/>
3234
<param name="laser_lambda_short" value="0.1"/>
3335
<param name="laser_model_type" value="likelihood_field"/>
34-
<!-- <param name="laser_model_type" value="beam"/> -->
3536
<param name="laser_likelihood_max_dist" value="2.0"/>
37+
3638
<param name="update_min_d" value="0.2"/>
3739
<param name="update_min_a" value="0.5"/>
3840
<param name="odom_frame_id" value="odom"/>
@@ -44,6 +46,6 @@ Setting pose: 33.118 34.530 -0.519
4446
<param name="initial_pose_y" value="17.730"/>
4547
<param name="initial_pose_a" value="1.583"/>
4648
</node>
47-
<test time-limit="180" test-name="basic_localization_stage_rosie" pkg="amcl"
48-
type="basic_localization.py" args="0 42.08 16.43 1.56 0.75 0.4 103.5"/>
49+
<test time-limit="180" test-name="basic_localization_stage_rosie" pkg="amcl"
50+
type="basic_localization.py" args="0 33.12 34.53 -0.52 0.75 0.4 103.5"/>
4951
</launch>

‎amcl/test/set_initial_pose.xml

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
1+
<?xml version="1.0" ?>
12
<!-- setting pose: 47.943 21.421 -0.503
2-
setting pose: 30.329 34.644 3.142
3+
setting pose: 30.329 34.644 3.142
34
117.5s -->
45
<launch>
56
<param name="/use_sim_time" value="true"/>
6-
<node name="rosbag" pkg="rosbag" type="play"
7-
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
7+
<node name="rosbag" pkg="rosbag" type="play"
8+
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
89
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
910
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
1011
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
@@ -17,13 +18,14 @@ setting pose: 30.329 34.644 3.142
1718
<param name="max_particles" value="5000"/>
1819
<param name="kld_err" value="0.05"/>
1920
<param name="kld_z" value="0.99"/>
21+
2022
<param name="odom_model_type" value="omni"/>
2123
<param name="odom_alpha1" value="0.2"/>
2224
<param name="odom_alpha2" value="0.2"/>
23-
<!-- translation std dev, m -->
2425
<param name="odom_alpha3" value="0.8"/>
2526
<param name="odom_alpha4" value="0.2"/>
2627
<param name="odom_alpha5" value="0.1"/>
28+
2729
<param name="laser_z_hit" value="0.5"/>
2830
<param name="laser_z_short" value="0.05"/>
2931
<param name="laser_z_max" value="0.05"/>
@@ -32,19 +34,21 @@ setting pose: 30.329 34.644 3.142
3234
<param name="laser_lambda_short" value="0.1"/>
3335
<param name="laser_lambda_short" value="0.1"/>
3436
<param name="laser_model_type" value="likelihood_field"/>
35-
<!-- <param name="laser_model_type" value="beam"/> -->
3637
<param name="laser_likelihood_max_dist" value="2.0"/>
38+
3739
<param name="update_min_d" value="0.2"/>
3840
<param name="update_min_a" value="0.5"/>
3941
<param name="odom_frame_id" value="odom"/>
4042
<param name="resample_interval" value="1"/>
4143
<param name="transform_tolerance" value="0.1"/>
4244
<param name="recovery_alpha_slow" value="0.0"/>
4345
<param name="recovery_alpha_fast" value="0.0"/>
44-
<!--param name="initial_pose_x" value="47.443"/>
46+
<!--
47+
<param name="initial_pose_x" value="47.443"/>
4548
<param name="initial_pose_y" value="21.421"/>
46-
<param name="initial_pose_a" value="-1.003"/-->
49+
<param name="initial_pose_a" value="-1.003"/>
50+
-->
4751
</node>
48-
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
49-
type="basic_localization.py" args="0 47.060 21.603 -1.074 0.75 0.75 90.0"/>
52+
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
53+
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
5054
</launch>

0 commit comments

Comments
 (0)