git commit -m "first commit for v2"
This commit is contained in:
101
Localizations/Libraries/Ros/amcl/test/basic_localization.py
Normal file
101
Localizations/Libraries/Ros/amcl/test/basic_localization.py
Normal file
@@ -0,0 +1,101 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import sys
|
||||
import time
|
||||
from math import fmod, pi
|
||||
|
||||
import unittest
|
||||
import rospy
|
||||
import rostest
|
||||
|
||||
import tf2_py as tf2
|
||||
import tf2_ros
|
||||
import PyKDL
|
||||
from std_srvs.srv import Empty
|
||||
|
||||
|
||||
class TestBasicLocalization(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.tf = None
|
||||
self.target_x = None
|
||||
self.target_y = None
|
||||
self.target_a = None
|
||||
self.tfBuffer = None
|
||||
|
||||
def print_pose_diff(self):
|
||||
a_curr = self.compute_angle()
|
||||
a_diff = self.wrap_angle(a_curr - self.target_a)
|
||||
print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr))
|
||||
print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a))
|
||||
print('Diff:\t %16.6f %16.6f %16.6f' % (
|
||||
abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff))
|
||||
|
||||
def get_pose(self):
|
||||
try:
|
||||
tf_stamped = self.tfBuffer.lookup_transform("map", "base_link", rospy.Time())
|
||||
self.tf = tf_stamped.transform
|
||||
self.print_pose_diff()
|
||||
except (tf2.LookupException, tf2.ExtrapolationException):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def wrap_angle(angle):
|
||||
"""
|
||||
Wrap angle to [-pi, pi)
|
||||
:param angle: Angle to be wrapped
|
||||
:return: wrapped angle
|
||||
"""
|
||||
angle += pi
|
||||
while angle < 0:
|
||||
angle += 2*pi
|
||||
return fmod(angle, 2*pi) - pi
|
||||
|
||||
def compute_angle(self):
|
||||
rot = self.tf.rotation
|
||||
a_curr = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2]
|
||||
a_diff = self.wrap_angle(a_curr - self.target_a)
|
||||
return self.target_a + a_diff
|
||||
|
||||
def test_basic_localization(self):
|
||||
global_localization = int(sys.argv[1])
|
||||
self.target_x = float(sys.argv[2])
|
||||
self.target_y = float(sys.argv[3])
|
||||
self.target_a = self.wrap_angle(float(sys.argv[4]))
|
||||
tolerance_d = float(sys.argv[5])
|
||||
tolerance_a = float(sys.argv[6])
|
||||
target_time = float(sys.argv[7])
|
||||
|
||||
rospy.init_node('test', anonymous=True)
|
||||
while rospy.rostime.get_time() == 0.0:
|
||||
print('Waiting for initial time publication')
|
||||
time.sleep(0.1)
|
||||
|
||||
if global_localization == 1:
|
||||
print('Waiting for service global_localization')
|
||||
rospy.wait_for_service('global_localization')
|
||||
global_localization = rospy.ServiceProxy('global_localization', Empty)
|
||||
global_localization()
|
||||
|
||||
start_time = rospy.rostime.get_time()
|
||||
self.tfBuffer = tf2_ros.Buffer()
|
||||
listener = tf2_ros.TransformListener(self.tfBuffer)
|
||||
|
||||
while (rospy.rostime.get_time() - start_time) < target_time:
|
||||
print('Waiting for end time %.6f (current: %.6f)' % (target_time, (rospy.rostime.get_time() - start_time)))
|
||||
self.get_pose()
|
||||
time.sleep(0.1)
|
||||
|
||||
print("Final pose:")
|
||||
self.get_pose()
|
||||
a_curr = self.compute_angle()
|
||||
|
||||
self.assertNotEqual(self.tf, None)
|
||||
self.assertAlmostEqual(self.tf.translation.x, self.target_x, delta=tolerance_d)
|
||||
self.assertAlmostEqual(self.tf.translation.y, self.target_y, delta=tolerance_d)
|
||||
self.assertAlmostEqual(a_curr, self.target_a, delta=tolerance_a)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rostest.run('amcl', 'amcl_localization', TestBasicLocalization, sys.argv)
|
||||
@@ -0,0 +1,51 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- setting pose: 47.943 21.421 -0.503
|
||||
setting pose: 30.329 34.644 3.142
|
||||
117.5s -->
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="47.443"/>
|
||||
<param name="initial_pose_y" value="21.421"/>
|
||||
<param name="initial_pose_a" value="-1.003"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="basic_localization_stage" pkg="amcl"
|
||||
type="basic_localization.py" args="0 30.329 34.644 3.142 0.75 0.75 90.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,48 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- setting pose: 47.943 21.421 -0.503
|
||||
setting pose: 30.329 34.644 3.142
|
||||
117.5s -->
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/global_localization_stage_indexed.bag"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="global_localization_stage" pkg="amcl"
|
||||
type="basic_localization.py" args="1 10.00 9.67 6.21 0.25 0.25 28.0" />
|
||||
</launch>
|
||||
51
Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml
Normal file
51
Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml
Normal file
@@ -0,0 +1,51 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- Setting pose: 42.378 17.730 1.583
|
||||
Setting pose: 33.118 34.530 -0.519
|
||||
103.5s -->
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/rosie_localization_stage.bag"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="42.378"/>
|
||||
<param name="initial_pose_y" value="17.730"/>
|
||||
<param name="initial_pose_a" value="1.583"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="basic_localization_stage_rosie" pkg="amcl"
|
||||
type="basic_localization.py" args="0 33.12 34.53 -0.52 0.75 0.4 103.5"/>
|
||||
</launch>
|
||||
54
Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml
Normal file
54
Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml
Normal file
@@ -0,0 +1,54 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- setting pose: 47.943 21.421 -0.503
|
||||
setting pose: 30.329 34.644 3.142
|
||||
117.5s -->
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<!--
|
||||
<param name="initial_pose_x" value="47.443"/>
|
||||
<param name="initial_pose_y" value="21.421"/>
|
||||
<param name="initial_pose_a" value="-1.003"/>
|
||||
-->
|
||||
</node>
|
||||
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
|
||||
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,58 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- setting pose: 47.943 21.421 -0.503
|
||||
setting pose: 30.329 34.644 3.142
|
||||
117.5s -->
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
|
||||
<!-- The bagfile starts at 34.6s. We try to send the same initial pose as above with stamp 34.8 at 5 seconds
|
||||
into the bagfile (at 39.6s). AMCL should add the robot motion from 34.8 to 39.6 to that initial pose
|
||||
before using it, so there should be no mis-localization caused by this. -->
|
||||
<node name="delayed_pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003 34.8 39.6"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<!--
|
||||
<param name="initial_pose_x" value="47.443"/>
|
||||
<param name="initial_pose_y" value="21.421"/>
|
||||
<param name="initial_pose_a" value="-1.003"/>
|
||||
-->
|
||||
</node>
|
||||
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
|
||||
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
|
||||
</launch>
|
||||
46
Localizations/Libraries/Ros/amcl/test/set_pose.py
Normal file
46
Localizations/Libraries/Ros/amcl/test/set_pose.py
Normal file
@@ -0,0 +1,46 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
|
||||
import math
|
||||
import PyKDL
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped
|
||||
|
||||
|
||||
class PoseSetter(rospy.SubscribeListener):
|
||||
def __init__(self, pose, stamp, publish_time):
|
||||
self.pose = pose
|
||||
self.stamp = stamp
|
||||
self.publish_time = publish_time
|
||||
|
||||
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
|
||||
p = PoseWithCovarianceStamped()
|
||||
p.header.frame_id = "map"
|
||||
p.header.stamp = self.stamp
|
||||
p.pose.pose.position.x = self.pose[0]
|
||||
p.pose.pose.position.y = self.pose[1]
|
||||
(p.pose.pose.orientation.x,
|
||||
p.pose.pose.orientation.y,
|
||||
p.pose.pose.orientation.z,
|
||||
p.pose.pose.orientation.w) = PyKDL.Rotation.RPY(0, 0, self.pose[2]).GetQuaternion()
|
||||
p.pose.covariance[6*0+0] = 0.5 * 0.5
|
||||
p.pose.covariance[6*1+1] = 0.5 * 0.5
|
||||
p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
|
||||
# wait for the desired publish time
|
||||
while rospy.get_rostime() < self.publish_time:
|
||||
rospy.sleep(0.01)
|
||||
peer_publish(p)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pose = list(map(float, rospy.myargv()[1:4]))
|
||||
t_stamp = rospy.Time()
|
||||
t_publish = rospy.Time()
|
||||
if len(rospy.myargv()) > 4:
|
||||
t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4]))
|
||||
if len(rospy.myargv()) > 5:
|
||||
t_publish = rospy.Time.from_sec(float(rospy.myargv()[5]))
|
||||
rospy.init_node('pose_setter', anonymous=True)
|
||||
rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec()))
|
||||
pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1)
|
||||
rospy.spin()
|
||||
@@ -0,0 +1,51 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_crazy_driving_prg_indexed.bag"/>
|
||||
<node name="map_sever" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="14.016"/>
|
||||
<param name="initial_pose_y" value="25.7500"/>
|
||||
<param name="initial_pose_a" value="-3.0"/>
|
||||
<param name="initial_cov_xx" value="0.1"/>
|
||||
<param name="initial_cov_yy" value="0.1"/>
|
||||
<param name="initial_cov_aa" value="0.1"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="small_loop_crazy_driving_prg" pkg="amcl"
|
||||
type="basic_localization.py" args="0 14.05 28.91 4.28 0.75 0.75 90.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,51 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_crazy_driving_prg_indexed.bag"/>
|
||||
<node name="map_sever" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni-corrected"/>
|
||||
<param name="odom_alpha1" value="0.005"/>
|
||||
<param name="odom_alpha2" value="0.005"/>
|
||||
<param name="odom_alpha3" value="0.01"/>
|
||||
<param name="odom_alpha4" value="0.005"/>
|
||||
<param name="odom_alpha5" value="0.003"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="14.016"/>
|
||||
<param name="initial_pose_y" value="25.7500"/>
|
||||
<param name="initial_pose_a" value="-3.0"/>
|
||||
<param name="initial_cov_xx" value="0.1"/>
|
||||
<param name="initial_cov_yy" value="0.1"/>
|
||||
<param name="initial_cov_aa" value="0.1"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="small_loop_crazy_driving_prg" pkg="amcl"
|
||||
type="basic_localization.py" args="0 14.05 28.91 4.28 0.75 0.75 90.0"/>
|
||||
</launch>
|
||||
48
Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml
Normal file
48
Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml
Normal file
@@ -0,0 +1,48 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_prf_indexed.bag"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2" />
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="omni"/>
|
||||
<param name="odom_alpha1" value="0.2"/>
|
||||
<param name="odom_alpha2" value="0.2"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.2"/>
|
||||
<param name="odom_alpha5" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_short" value="0.05"/>
|
||||
<param name="laser_z_max" value="0.05"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.2"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_lambda_short" value="0.1"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="transform_tolerance" value="0.1"/>
|
||||
<param name="recovery_alpha_slow" value="0.0"/>
|
||||
<param name="recovery_alpha_fast" value="0.0"/>
|
||||
<param name="initial_pose_x" value="14.099"/>
|
||||
<param name="initial_pose_y" value="21.063"/>
|
||||
<param name="initial_pose_a" value="-0.006"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="basic_localization_small_loop_prf" pkg="amcl"
|
||||
type="basic_localization.py" args="0 13.94 23.02 4.72 0.75 0.75 66.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_greenroom_loop_indexed.bag"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2"/>
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="diff"/>
|
||||
<param name="odom_alpha1" value="0.1"/>
|
||||
<param name="odom_alpha2" value="0.3"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.25"/>
|
||||
<param name="laser_max_range" value="5.0"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="initial_pose_x" value="14.049"/>
|
||||
<param name="initial_pose_y" value="24.234"/>
|
||||
<param name="initial_pose_a" value="-1.517"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="texas_greenroom_loop" pkg="amcl"
|
||||
type="basic_localization.py" args="0 13.87 24.07 4.65 0.75 0.75 89.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_greenroom_loop_indexed.bag"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2"/>
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="diff"/>
|
||||
<param name="odom_alpha1" value="0.04"/>
|
||||
<param name="odom_alpha2" value="0.6"/>
|
||||
<param name="odom_alpha3" value="0.3"/>
|
||||
<param name="odom_alpha4" value="0.04"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.25"/>
|
||||
<param name="laser_max_range" value="5.0"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="initial_pose_x" value="14.049"/>
|
||||
<param name="initial_pose_y" value="24.234"/>
|
||||
<param name="initial_pose_a" value="-1.517"/>
|
||||
</node>
|
||||
<test time-limit="180" test-name="texas_greenroom_loop_corrected" pkg="amcl"
|
||||
type="basic_localization.py" args="0 13.87 24.07 4.65 0.75 0.75 89.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_willow_hallway_loop_indexed.bag"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2"/>
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="diff"/>
|
||||
<param name="odom_alpha1" value="0.1"/>
|
||||
<param name="odom_alpha2" value="0.3"/>
|
||||
<param name="odom_alpha3" value="0.8"/>
|
||||
<param name="odom_alpha4" value="0.1"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.25"/>
|
||||
<param name="laser_max_range" value="5.0"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="initial_pose_x" value="31.872"/>
|
||||
<param name="initial_pose_y" value="31.557"/>
|
||||
<param name="initial_pose_a" value="0.0"/>
|
||||
</node>
|
||||
<test time-limit="350" test-name="texas_willow_hallway_loop" pkg="amcl"
|
||||
type="basic_localization.py" args="0 32.36 31.44 6.18 0.75 0.75 250.0"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
|
||||
<node name="rosbag" pkg="rosbag" type="play"
|
||||
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_willow_hallway_loop_indexed.bag"/>
|
||||
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
|
||||
<remap from="scan" to="base_scan" />
|
||||
<param name="transform_tolerance" value="0.2"/>
|
||||
<param name="gui_publish_rate" value="10.0"/>
|
||||
<param name="save_pose_rate" value="0.5"/>
|
||||
<param name="laser_max_beams" value="30"/>
|
||||
<param name="min_particles" value="500"/>
|
||||
<param name="max_particles" value="5000"/>
|
||||
<param name="kld_err" value="0.05"/>
|
||||
<param name="kld_z" value="0.99"/>
|
||||
|
||||
<param name="odom_model_type" value="diff-corrected"/>
|
||||
<param name="odom_alpha1" value="0.04"/>
|
||||
<param name="odom_alpha2" value="0.6"/>
|
||||
<param name="odom_alpha3" value="0.3"/>
|
||||
<param name="odom_alpha4" value="0.04"/>
|
||||
|
||||
<param name="laser_z_hit" value="0.5"/>
|
||||
<param name="laser_z_rand" value="0.5"/>
|
||||
<param name="laser_sigma_hit" value="0.25"/>
|
||||
<param name="laser_max_range" value="5.0"/>
|
||||
<param name="laser_model_type" value="likelihood_field"/>
|
||||
<param name="laser_likelihood_max_dist" value="2.0"/>
|
||||
|
||||
<param name="update_min_d" value="0.2"/>
|
||||
<param name="update_min_a" value="0.5"/>
|
||||
<param name="odom_frame_id" value="odom_combined"/>
|
||||
<param name="resample_interval" value="1"/>
|
||||
<param name="initial_pose_x" value="31.872"/>
|
||||
<param name="initial_pose_y" value="31.557"/>
|
||||
<param name="initial_pose_a" value="0.0"/>
|
||||
</node>
|
||||
<test time-limit="350" test-name="texas_willow_hallway_loop_corrected" pkg="amcl"
|
||||
type="basic_localization.py" args="0 32.36 31.44 6.18 0.75 0.75 250.0"/>
|
||||
</launch>
|
||||
Reference in New Issue
Block a user