HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
Localizations/Packages/robot_localization/test/record_all_stats.sh Normal file
View File

@@ -0,0 +1,9 @@
#!/bin/bash
./stat_recorder.sh test_ekf_localization_node_bag1.test $1/ekf1.txt
./stat_recorder.sh test_ekf_localization_node_bag2.test $1/ekf2.txt
./stat_recorder.sh test_ekf_localization_node_bag3.test $1/ekf3.txt
./stat_recorder.sh test_ukf_localization_node_bag1.test $1/ukf1.txt
./stat_recorder.sh test_ukf_localization_node_bag2.test $1/ukf2.txt
./stat_recorder.sh test_ukf_localization_node_bag3.test $1/ukf3.txt

18
Localizations/Packages/robot_localization/test/stat_recorder.sh Normal file
View File

@@ -0,0 +1,18 @@
#!/bin/bash
rm $2
echo "x = [" > $2
i="0"
while [ $i -lt 30 ]
do
rostest robot_localization $1 output_final_position:=true output_location:=$2
i=$[$i+1]
sleep 3
done
echo "]" >> $2
echo "mean(x)" >> $2
echo "sqrt(sum((4 * std(x)).^2))" >> $2

BIN
Localizations/Packages/robot_localization/test/test1.bag Normal file
View File

Binary file not shown.

BIN
Localizations/Packages/robot_localization/test/test2.bag Normal file
View File

Binary file not shown.

BIN
Localizations/Packages/robot_localization/test/test3.bag Normal file
View File

Binary file not shown.

135
Localizations/Packages/robot_localization/test/test_ekf.cpp Normal file
View File

@@ -0,0 +1,135 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <gtest/gtest.h>
#include <limits>
#include <vector>
using RobotLocalization::Ekf;
using RobotLocalization::RosEkf;
using RobotLocalization::STATE_SIZE;
class RosEkfPassThrough : public RosEkf
{
public:
RosEkfPassThrough() : RosEkf(ros::NodeHandle(), ros::NodeHandle("~"))
{
}
Ekf &getFilter()
{
return filter_;
}
};
TEST(EkfTest, Measurements)
{
RosEkfPassThrough ekf;
Eigen::MatrixXd initialCovar(15, 15);
initialCovar.setIdentity();
initialCovar *= 0.5;
ekf.getFilter().setEstimateErrorCovariance(initialCovar);
Eigen::VectorXd measurement(STATE_SIZE);
measurement.setIdentity();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurement[i] = i * 0.01 * STATE_SIZE;
}
Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
measurementCovariance.setIdentity();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
std::vector<int> updateVector(STATE_SIZE, true);
// Ensure that measurements are being placed in the queue correctly
ros::Time time;
time.fromSec(1000);
ekf.enqueueMeasurement("odom0",
measurement,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ekf.integrateMeasurements(ros::Time(1001));
EXPECT_EQ(ekf.getFilter().getState(), measurement);
EXPECT_EQ(ekf.getFilter().getEstimateErrorCovariance(), measurementCovariance);
ekf.getFilter().setEstimateErrorCovariance(initialCovar);
// Now fuse another measurement and check the output.
// We know what the filter's state should be when
// this is complete, so we'll check the difference and
// make sure it's suitably small.
Eigen::VectorXd measurement2 = measurement;
measurement2 *= 2.0;
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
time.fromSec(1002);
ekf.enqueueMeasurement("odom0",
measurement2,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ekf.integrateMeasurements(ros::Time(1003));
measurement = measurement2.eval() - ekf.getFilter().getState();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
EXPECT_LT(::fabs(measurement[i]), 0.001);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ekf");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

7
Localizations/Packages/robot_localization/test/test_ekf.test Normal file
View File

@@ -0,0 +1,7 @@
<!-- Launch file for ekf_test -->
<launch>
<test test-name="test_ekf" pkg="robot_localization" type="test_ekf" clear_params="true" time-limit="100.0" />
</launch>

105
Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag1.test Normal file
View File

@@ -0,0 +1,105 @@
<!-- Launch file for test_ekf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 5" required="true"/>
<node name="test_ekf_localization_node_bag1_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag1_pose" pkg="robot_localization" type="test_ekf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-40.0454"/>
<param name="final_y" value="-76.9988"/>
<param name="final_z" value="-2.6974"/>
<param name="tolerance" value="1.0452"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

85
Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag2.test Normal file
View File

@@ -0,0 +1,85 @@
<!-- Launch file for test_ekf_localization_node_bag2 -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test2.bag --clock -d 5" required="true"/>
<node name="test_ekf_localization_node_bag2_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true" >
<param name="frequency" value="50"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/jackal_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="process_noise_covariance">[0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag2_pose" pkg="robot_localization" type="test_ekf_localization_node_bag2" clear_params="true" time-limit="1000.0">
<param name="final_x" value="1.0206"/>
<param name="final_y" value="3.4292"/>
<param name="final_z" value="0.7419"/>
<param name="tolerance" value="0.1383"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

69
Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag3.test Normal file
View File

@@ -0,0 +1,69 @@
<!-- Launch file for test_ekf_localization_node_bag3_ekf" -->
<launch>
<arg name="output_final_position" default="false"/>
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test3.bag --clock -d 5" required="true"/>
<node name="test_ekf_localization_node_bag3_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/ardrone/odometry/raw"/>
<param name="imu0" value="/ardrone/imu"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="odom0_config">[false, false, true,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag3_pose" pkg="robot_localization" type="test_ekf_localization_node_bag3" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-0.4859"/>
<param name="final_y" value="-0.2412"/>
<param name="final_z" value="2.9883"/>
<param name="tolerance" value="0.1290"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

1075
Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

68
Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.test Normal file
View File

@@ -0,0 +1,68 @@
<!-- Launch file for test_ekf_localization_node_interfaces-->
<launch>
<node name="test_ekf_localization_node_interfaces_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/odom_input0"/>
<param name="odom1" value="/odom_input1"/>
<param name="odom2" value="/odom_input2"/>
<param name="pose0" value="/pose_input0"/>
<param name="pose1" value="/pose_input1"/>
<param name="twist0" value="/twist_input0"/>
<param name="imu0" value="/imu_input0"/>
<param name="imu1" value="/imu_input1"/>
<param name="imu2" value="/imu_input2"/>
<param name="imu3" value="/imu_input3"/>
<rosparam param="odom0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom2_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="pose0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="pose1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, true, true, true, false, false, false]</rosparam>
<rosparam param="imu2_config">[false, false, false, false, false, false, false, false, false, false, false, false, true, true, true]</rosparam>
<rosparam param="imu3_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<param name="odom1_differential" value="true"/>
<param name="pose1_differential" value="true"/>
<param name="imu3_differential" value="true"/>
<param name="print_diagnostics" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ekf_localization_node_interfaces_int" pkg="robot_localization" type="test_ekf_localization_node_interfaces" clear_params="true" time-limit="1000.0" />
</launch>

104
Localizations/Packages/robot_localization/test/test_ekf_localization_nodelet_bag1.test Normal file
View File

@@ -0,0 +1,104 @@
<!-- Launch file for test_ekf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 25" required="true"/>
<node pkg="nodelet" type="nodelet" name="EKF_NODELET_MANAGER" args="manager"/>
<node pkg="nodelet" type="nodelet" name="test_ekf_localization_node_bag1_ekf" output="screen" args="load RobotLocalization/EkfNodelet EKF_NODELET_MANAGER" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<test test-name="test_ekf_localization_node_bag1_pose" pkg="robot_localization" type="test_ekf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-40.0454"/>
<param name="final_y" value="-76.9988"/>
<param name="final_z" value="-2.6974"/>
<param name="tolerance" value="1.0452"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

216
Localizations/Packages/robot_localization/test/test_filter_base.cpp Normal file
View File

@@ -0,0 +1,216 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/filter_base.h"
#include "robot_localization/filter_common.h"
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <iostream>
#include <queue>
#include <string>
using RobotLocalization::STATE_SIZE;
using RobotLocalization::Measurement;
namespace RobotLocalization
{
class FilterDerived : public FilterBase
{
public:
double val;
FilterDerived() : val(0) { }
void correct(const Measurement &measurement)
{
EXPECT_EQ(val, measurement.time_);
EXPECT_EQ(measurement.topicName_, "topic");
EXPECT_EQ(measurement.updateVector_.size(), 10u);
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
EXPECT_EQ(measurement.updateVector_[i], true);
}
}
void predict(const double refTime, const double delta)
{
val = delta;
}
};
class FilterDerived2 : public FilterBase
{
public:
FilterDerived2() { }
void correct(const Measurement &measurement)
{
}
void predict(const double refTime, const double delta)
{
}
void processMeasurement(const Measurement &measurement)
{
FilterBase::processMeasurement(measurement);
}
};
} // namespace RobotLocalization
TEST(FilterBaseTest, MeasurementStruct)
{
RobotLocalization::Measurement meas1;
RobotLocalization::Measurement meas2;
EXPECT_EQ(meas1.topicName_, std::string(""));
EXPECT_EQ(meas1.time_, 0);
EXPECT_EQ(meas2.time_, 0);
// Comparison test is true if the first
// argument is > the second, so should
// be false if they're equal.
EXPECT_EQ(meas1(meas1, meas2), false);
EXPECT_EQ(meas2(meas2, meas1), false);
meas1.time_ = 100;
meas2.time_ = 200;
EXPECT_EQ(meas1(meas1, meas2), false);
EXPECT_EQ(meas1(meas2, meas1), true);
EXPECT_EQ(meas2(meas1, meas2), false);
EXPECT_EQ(meas2(meas2, meas1), true);
}
TEST(FilterBaseTest, DerivedFilterGetSet)
{
using RobotLocalization::FilterDerived;
FilterDerived derived;
// With the ostream argument as NULL,
// the debug flag will remain false.
derived.setDebug(true);
EXPECT_FALSE(derived.getDebug());
// Now set the stream and do it again
std::stringstream os;
derived.setDebug(true, &os);
EXPECT_TRUE(derived.getDebug());
// Simple get/set checks
double timeout = 7.4;
derived.setSensorTimeout(timeout);
EXPECT_EQ(derived.getSensorTimeout(), timeout);
double lastMeasTime = 3.83;
derived.setLastMeasurementTime(lastMeasTime);
EXPECT_EQ(derived.getLastMeasurementTime(), lastMeasTime);
Eigen::MatrixXd pnCovar(STATE_SIZE, STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
for (size_t j = 0; j < STATE_SIZE; ++j)
{
pnCovar(i, j) = static_cast<double>(i * j);
}
}
derived.setProcessNoiseCovariance(pnCovar);
EXPECT_EQ(derived.getProcessNoiseCovariance(), pnCovar);
Eigen::VectorXd state(STATE_SIZE);
state.setZero();
derived.setState(state);
EXPECT_EQ(derived.getState(), state);
EXPECT_EQ(derived.getInitializedStatus(), false);
}
TEST(FilterBaseTest, MeasurementProcessing)
{
using RobotLocalization::FilterDerived2;
FilterDerived2 derived;
Measurement meas;
Eigen::VectorXd measurement(STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurement[i] = 0.1 * static_cast<double>(i);
}
Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
for (size_t j = 0; j < STATE_SIZE; ++j)
{
measurementCovariance(i, j) = 0.1 * static_cast<double>(i * j);
}
}
meas.topicName_ = "odomTest";
meas.measurement_ = measurement;
meas.covariance_ = measurementCovariance;
meas.updateVector_.resize(STATE_SIZE, true);
meas.time_ = 1000;
// The filter shouldn't be initializedyet
EXPECT_FALSE(derived.getInitializedStatus());
derived.processMeasurement(meas);
// Now it's initialized, and the entire filter state
// should be equal to the first state
EXPECT_TRUE(derived.getInitializedStatus());
EXPECT_EQ(derived.getState(), measurement);
// Process a measurement and make sure it updates the
// lastMeasurementTime variable
meas.time_ = 1002;
derived.processMeasurement(meas);
EXPECT_EQ(derived.getLastMeasurementTime(), meas.time_);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

434
Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.cpp Normal file
View File

@@ -0,0 +1,434 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <sensor_msgs/Imu.h>
#include "robot_localization/filter_base.h"
#include "robot_localization/filter_common.h"
#include "robot_localization/SetPose.h"
#include <diagnostic_msgs/DiagnosticArray.h>
#include <gtest/gtest.h>
#include <vector>
namespace RobotLocalization
{
/*
Convenience functions to get valid messages.
*/
geometry_msgs::PoseWithCovarianceStamped getValidPose()
{
geometry_msgs::PoseWithCovarianceStamped pose_msg;
pose_msg.header.frame_id = "base_link";
pose_msg.pose.pose.position.x = 1;
pose_msg.pose.pose.orientation.w = 1;
for (size_t i = 0; i < 6 ; i++)
{
pose_msg.pose.covariance[i*6 + i] = 1;
}
return pose_msg;
}
geometry_msgs::TwistWithCovarianceStamped getValidTwist()
{
geometry_msgs::TwistWithCovarianceStamped twist_msg;
twist_msg.header.frame_id = "base_link";
for (size_t i = 0; i < 6 ; i++)
{
twist_msg.twist.covariance[i*6 + i] = 1;
}
return twist_msg;
}
sensor_msgs::Imu getValidImu()
{
sensor_msgs::Imu imu_msg;
imu_msg.header.frame_id = "base_link";
imu_msg.orientation.w = 1;
for (size_t i = 0; i < 3 ; i++)
{
imu_msg.orientation_covariance[i * 3 + i] = 1;
imu_msg.angular_velocity_covariance[i * 3 + i] = 1;
imu_msg.linear_acceleration_covariance[i * 3 + i] = 1;
}
return imu_msg;
}
nav_msgs::Odometry getValidOdometry()
{
nav_msgs::Odometry odom_msg;
odom_msg.header.frame_id = "odom";
odom_msg.child_frame_id = "base_link";
odom_msg.pose = getValidPose().pose;
odom_msg.twist = getValidTwist().twist;
return odom_msg;
}
/*
Helper class to handle the setup and message publishing for the testcases.
It provides convenience to send valid messages with a specified timestamp.
All diagnostic messages are stored into the public diagnostics attribute.
*/
class DiagnosticsHelper
{
private:
ros::Publisher odom_pub_;
ros::Publisher pose_pub_;
ros::Publisher twist_pub_;
ros::Publisher imu_pub_;
geometry_msgs::PoseWithCovarianceStamped pose_msg_;
geometry_msgs::TwistWithCovarianceStamped twist_msg_;
nav_msgs::Odometry odom_msg_;
sensor_msgs::Imu imu_msg_;
ros::Subscriber diagnostic_sub_;
ros::ServiceClient set_pose_;
public:
std::vector< diagnostic_msgs::DiagnosticArray > diagnostics;
DiagnosticsHelper()
{
ros::NodeHandle nh;
ros::NodeHandle nhLocal("~");
// ready the valid messages.
pose_msg_ = getValidPose();
twist_msg_ = getValidTwist();
odom_msg_ = getValidOdometry();
imu_msg_ = getValidImu();
// subscribe to diagnostics and create publishers for the odometry messages.
odom_pub_ = nh.advertise<nav_msgs::Odometry>("example/odom", 10);
pose_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("example/pose", 10);
twist_pub_ = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("example/twist", 10);
imu_pub_ = nh.advertise<sensor_msgs::Imu>("example/imu/data", 10);
diagnostic_sub_ = nh.subscribe("/diagnostics", 10, &DiagnosticsHelper::diagnosticCallback, this);
set_pose_ = nh.serviceClient<robot_localization::SetPose>("/set_pose");
}
void diagnosticCallback(const diagnostic_msgs::DiagnosticArrayPtr &msg)
{
diagnostics.push_back(*msg);
}
void publishMessages(ros::Time t)
{
odom_msg_.header.stamp = t;
odom_msg_.header.seq++;
odom_pub_.publish(odom_msg_);
pose_msg_.header.stamp = t;
pose_msg_.header.seq++;
pose_pub_.publish(pose_msg_);
twist_msg_.header.stamp = t;
twist_msg_.header.seq++;
twist_pub_.publish(twist_msg_);
imu_msg_.header.stamp = t;
imu_msg_.header.seq++;
imu_pub_.publish(imu_msg_);
}
void setPose(ros::Time t)
{
robot_localization::SetPose pose_;
pose_.request.pose = getValidPose();
pose_.request.pose.header.stamp = t;
set_pose_.call(pose_);
}
};
} // namespace RobotLocalization
/*
First test; we run for a bit; then send messagse with an empty timestamp.
Then we check if the diagnostics showed a warning.
*/
TEST(FilterBaseDiagnosticsTest, EmptyTimestamps)
{
RobotLocalization::DiagnosticsHelper dh_;
// keep track of which diagnostic messages are detected.
bool received_warning_imu = false;
bool received_warning_odom = false;
bool received_warning_twist = false;
bool received_warning_pose = false;
// For about a second, send correct messages.
ros::Rate loopRate(10);
for (size_t i = 0; i < 10; ++i)
{
ros::spinOnce();
dh_.publishMessages(ros::Time::now());
loopRate.sleep();
}
ros::spinOnce();
// create an empty timestamp and send all messages with this empty timestamp.
ros::Time empty;
empty.fromSec(0);
dh_.publishMessages(empty);
ros::spinOnce();
// The filter runs and sends the diagnostics every second.
// Just run this for two seconds to ensure we get all the diagnostic message.
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
loopRate.sleep();
}
/*
Now the diagnostic messages have to be investigated to see whether they contain our warning.
*/
for (size_t i=0; i < dh_.diagnostics.size(); i++)
{
for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++)
{
for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++)
{
diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key];
// Now the keys can be checked to see whether we found our warning.
if (kv.key == "imu0_timestamp")
{
received_warning_imu = true;
}
if (kv.key == "odom0_timestamp")
{
received_warning_odom = true;
}
if (kv.key == "twist0_timestamp")
{
received_warning_twist = true;
}
if (kv.key == "pose0_timestamp")
{
received_warning_pose = true;
}
}
}
}
EXPECT_TRUE(received_warning_imu);
EXPECT_TRUE(received_warning_odom);
EXPECT_TRUE(received_warning_twist);
EXPECT_TRUE(received_warning_pose);
}
TEST(FilterBaseDiagnosticsTest, TimestampsBeforeSetPose)
{
RobotLocalization::DiagnosticsHelper dh_;
// keep track of which diagnostic messages are detected.
bool received_warning_imu = false;
bool received_warning_odom = false;
bool received_warning_twist = false;
bool received_warning_pose = false;
// For about a second, send correct messages.
ros::Rate loopRate(10);
for (size_t i = 0; i < 10; ++i)
{
ros::spinOnce();
dh_.publishMessages(ros::Time::now());
loopRate.sleep();
}
ros::spinOnce();
// Set the pose to the current timestamp.
dh_.setPose(ros::Time::now());
ros::spinOnce();
// send messages from one second before that pose reset.
dh_.publishMessages(ros::Time::now() - ros::Duration(1));
// The filter runs and sends the diagnostics every second.
// Just run this for two seconds to ensure we get all the diagnostic message.
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
loopRate.sleep();
}
/*
Now the diagnostic messages have to be investigated to see whether they contain our warning.
*/
for (size_t i=0; i < dh_.diagnostics.size(); i++)
{
for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++)
{
for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++)
{
diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key];
// Now the keys can be checked to see whether we found our warning.
if (kv.key == "imu0_timestamp")
{
received_warning_imu = true;
}
if (kv.key == "odom0_timestamp")
{
received_warning_odom = true;
}
if (kv.key == "twist0_timestamp")
{
received_warning_twist = true;
}
if (kv.key == "pose0_timestamp")
{
received_warning_pose = true;
}
}
}
}
EXPECT_TRUE(received_warning_imu);
EXPECT_TRUE(received_warning_odom);
EXPECT_TRUE(received_warning_twist);
EXPECT_TRUE(received_warning_pose);
}
TEST(FilterBaseDiagnosticsTest, TimestampsBeforePrevious)
{
RobotLocalization::DiagnosticsHelper dh_;
// keep track of which diagnostic messages are detected.
// we have more things to check here because the messages get split over
// various callbacks if they pass the check if they predate the set_pose time.
bool received_warning_imu_accel = false;
bool received_warning_imu_pose = false;
bool received_warning_imu_twist = false;
bool received_warning_odom_twist = false;
bool received_warning_twist = false;
bool received_warning_pose = false;
// For two seconds send correct messages.
ros::Rate loopRate(10);
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
dh_.publishMessages(ros::Time::now());
loopRate.sleep();
}
ros::spinOnce();
// Send message that is one second in the past.
dh_.publishMessages(ros::Time::now() - ros::Duration(1));
// The filter runs and sends the diagnostics every second.
// Just run this for two seconds to ensure we get all the diagnostic message.
for (size_t i = 0; i < 20; ++i)
{
ros::spinOnce();
loopRate.sleep();
}
/*
Now the diagnostic messages have to be investigated to see whether they contain our warning.
*/
for (size_t i=0; i < dh_.diagnostics.size(); i++)
{
for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++)
{
for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++)
{
diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key];
// Now the keys can be checked to see whether we found our warning.
if (kv.key == "imu0_acceleration_timestamp")
{
received_warning_imu_accel = true;
}
if (kv.key == "imu0_pose_timestamp")
{
received_warning_imu_pose = true;
}
if (kv.key == "imu0_twist_timestamp")
{
received_warning_imu_twist = true;
}
if (kv.key == "odom0_twist_timestamp")
{
received_warning_twist = true;
}
if (kv.key == "pose0_timestamp")
{
received_warning_pose = true;
}
if (kv.key == "twist0_timestamp")
{
received_warning_odom_twist = true;
}
}
}
}
EXPECT_TRUE(received_warning_imu_accel);
EXPECT_TRUE(received_warning_imu_pose);
EXPECT_TRUE(received_warning_imu_twist);
EXPECT_TRUE(received_warning_odom_twist);
EXPECT_TRUE(received_warning_pose);
EXPECT_TRUE(received_warning_twist);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "filter_base_diagnostics_timestamps-test-interfaces");
ros::Time::init();
// Give ekf_localization_node time to initialize
ros::Duration(2.0).sleep();
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

44
Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.test Normal file
View File

@@ -0,0 +1,44 @@
<!-- Launch file for test_filter_base_diagnostics -->
<launch>
<!--
Although we test the filter base we need a valid node running which sends the diagnostic messages.
-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<param name="odom0" value="example/odom"/>
<param name="pose0" value="example/pose"/>
<param name="twist0" value="example/twist"/>
<param name="imu0" value="example/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="pose0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false,
false, false, false,
true, true, true,
true, true, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="print_diagnostics" value="true"/>
</node>
<test test-name="test_filter_base_diagnostics" pkg="robot_localization" type="test_filter_base_diagnostics_timestamps" clear_params="true" time-limit="100.0" />
</launch>

111
Localizations/Packages/robot_localization/test/test_localization_node_bag_pose_tester.cpp Normal file
View File

@@ -0,0 +1,111 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <gtest/gtest.h>
#include <fstream>
#include <iostream>
#include <string>
nav_msgs::Odometry filtered_;
void filterCallback(const nav_msgs::OdometryConstPtr &msg)
{
filtered_ = *msg;
}
TEST(BagTest, PoseCheck)
{
ros::NodeHandle nh;
ros::NodeHandle nhLocal("~");
double finalX = 0;
double finalY = 0;
double finalZ = 0;
double tolerance = 0;
bool outputFinalPosition = false;
std::string finalPositionFile;
nhLocal.getParam("final_x", finalX);
nhLocal.getParam("final_y", finalY);
nhLocal.getParam("final_z", finalZ);
nhLocal.getParam("tolerance", tolerance);
nhLocal.param("output_final_position", outputFinalPosition, false);
nhLocal.param("output_location", finalPositionFile, std::string("test.txt"));
ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback);
while (ros::ok())
{
ros::spinOnce();
ros::Duration(0.0333333).sleep();
}
if (outputFinalPosition)
{
try
{
std::ofstream posOut;
posOut.open(finalPositionFile.c_str(), std::ofstream::app);
posOut << filtered_.pose.pose.position.x << " " << filtered_.pose.pose.position.y << " " <<
filtered_.pose.pose.position.z << std::endl;
posOut.close();
}
catch(...)
{
ROS_ERROR_STREAM("Unable to open output file.\n");
}
}
double xDiff = filtered_.pose.pose.position.x - finalX;
double yDiff = filtered_.pose.pose.position.y - finalY;
double zDiff = filtered_.pose.pose.position.z - finalZ;
EXPECT_LT(::sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff), tolerance);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "localization_node-bag-pose-tester");
ros::Time::init();
// Give ekf_localization_node time to initialize
ros::Duration(2.0).sleep();
return RUN_ALL_TESTS();
}

69
Localizations/Packages/robot_localization/test/test_navsat_conversions.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*
* Copyright (c) 2021, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_conversions.h"
#include <gtest/gtest.h>
#include <string>
void NavsatConversionsTest(const double lat, const double lon,
const double UTMNorthing, const double UTMEasting,
const std::string UTMZone, const double gamma)
{
double UTMNorthing_new;
double UTMEasting_new;
std::string UTMZone_new;
double gamma_new;
RobotLocalization::NavsatConversions::LLtoUTM(lat, lon, UTMNorthing_new, UTMEasting_new, UTMZone_new, gamma_new);
EXPECT_NEAR(UTMNorthing, UTMNorthing_new, 1e-2);
EXPECT_NEAR(UTMEasting, UTMEasting_new, 1e-2);
EXPECT_EQ(UTMZone, UTMZone_new);
EXPECT_NEAR(gamma, gamma_new, 1e-2);
double lat_new;
double lon_new;
RobotLocalization::NavsatConversions::UTMtoLL(UTMNorthing, UTMEasting, UTMZone, lat_new, lon_new);
EXPECT_NEAR(lat_new, lat, 1e-5);
EXPECT_NEAR(lon_new, lon, 1e-5);
}
TEST(NavsatConversionsTest, UtmTest)
{
NavsatConversionsTest(51.423964, 5.494271, 5699924.709, 673409.989, "31U", 1.950);
NavsatConversionsTest(-43.530955, 172.636645, 5178919.718, 632246.802, "59G", -1.127);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

91
Localizations/Packages/robot_localization/test/test_navsat_transform.cpp Normal file
View File

@@ -0,0 +1,91 @@
/*
* Copyright (c) 2021, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/navsat_transform.h"
#include <robot_localization/SetDatum.h>
#include <robot_localization/ToLL.h>
#include <robot_localization/FromLL.h>
#include <gtest/gtest.h>
#include <string>
TEST(NavSatTransformUTMJumpTest, UtmTest)
{
ros::NodeHandle nh;
ros::ServiceClient set_datum_client = nh.serviceClient<robot_localization::SetDatum>("/datum");
ros::ServiceClient from_ll_client = nh.serviceClient<robot_localization::FromLL>("/fromLL");
EXPECT_TRUE(set_datum_client.waitForExistence(ros::Duration(5)));
// Initialise the navsat_transform node to a UTM zone
robot_localization::SetDatum set_datum_srv;
set_datum_srv.request.geo_pose.position.latitude = 1;
set_datum_srv.request.geo_pose.position.longitude = 4;
set_datum_srv.request.geo_pose.orientation.w = 1;
EXPECT_TRUE(set_datum_client.call(set_datum_srv));
// Let the node figure out its transforms
ros::Duration(0.2).sleep();
// Request the GPS point of said point:
robot_localization::FromLL from_ll_srv;
from_ll_srv.request.ll_point.latitude = 10;
from_ll_srv.request.ll_point.longitude = 4.5;
EXPECT_TRUE(from_ll_client.call(from_ll_srv));
auto initial_response = from_ll_srv.response;
// Request GPS point also in that zone
from_ll_srv.request.ll_point.longitude = 5.5;
EXPECT_TRUE(from_ll_client.call(from_ll_srv));
auto same_zone_response = from_ll_srv.response;
// 1° of longitude is about 111 kilometers in length
EXPECT_NEAR(initial_response.map_point.x, same_zone_response.map_point.x, 120e3);
EXPECT_NEAR(initial_response.map_point.y, same_zone_response.map_point.y, 120e3);
// Request GPS point from neighboring zone (zone crossing is at 6 degrees)
from_ll_srv.request.ll_point.longitude = 6.5;
from_ll_client.call(from_ll_srv);
auto neighbour_zone_response = from_ll_srv.response;
EXPECT_NEAR(initial_response.map_point.x, neighbour_zone_response.map_point.x, 2*120e3);
EXPECT_NEAR(initial_response.map_point.y, neighbour_zone_response.map_point.y, 2*120e3);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_navsat_transform");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

9
Localizations/Packages/robot_localization/test/test_navsat_transform.test Normal file
View File

@@ -0,0 +1,9 @@
<!-- Launch file for navsat_transform test -->
<launch>
<node name="navsat_transform_node" pkg="robot_localization" type="navsat_transform_node" output="screen" />
<test test-name="test_navsat_transform" pkg="robot_localization" type="test_navsat_transform" />
</launch>

169
Localizations/Packages/robot_localization/test/test_robot_localization_estimator.cpp Normal file
View File

@@ -0,0 +1,169 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/robot_localization_estimator.h"
#include <vector>
#include <ros/ros.h>
#include <gtest/gtest.h>
TEST(RLETest, StateBuffer)
{
// Generate a few estimator states
std::vector<RobotLocalization::EstimatorState> states;
for ( int i = 0; i < 10; i++ )
{
/*
* t = i s;
* x = i m;
* vx = 1.0 m/s;
*/
RobotLocalization::EstimatorState state;
state.time_stamp = i;
state.state(RobotLocalization::StateMemberX) = i;
state.state(RobotLocalization::StateMemberY) = 0;
state.state(RobotLocalization::StateMemberZ) = 0;
state.state(RobotLocalization::StateMemberRoll) = 0;
state.state(RobotLocalization::StateMemberPitch) = 0;
state.state(RobotLocalization::StateMemberYaw) = 0;
state.state(RobotLocalization::StateMemberVx) = 1;
state.state(RobotLocalization::StateMemberVy) = 0;
state.state(RobotLocalization::StateMemberVz) = 0;
state.state(RobotLocalization::StateMemberVroll) = 0;
state.state(RobotLocalization::StateMemberVpitch) = 0;
state.state(RobotLocalization::StateMemberVyaw) = 0;
state.state(RobotLocalization::StateMemberAx) = 0;
state.state(RobotLocalization::StateMemberAy) = 0;
state.state(RobotLocalization::StateMemberAz) = 0;
states.push_back(state);
}
// Instantiate a robot localization estimator with a buffer capacity of 5
int buffer_capacity = 5;
Eigen::MatrixXd process_noise_covariance = Eigen::MatrixXd::Identity(RobotLocalization::STATE_SIZE,
RobotLocalization::STATE_SIZE);
RobotLocalization::RobotLocalizationEstimator estimator(buffer_capacity, RobotLocalization::FilterTypes::EKF,
process_noise_covariance);
RobotLocalization::EstimatorState state;
// Add the states in chronological order
for ( int i = 0; i < 6; i++ )
{
estimator.setState(states[i]);
// Check that the state is added correctly
estimator.getState(states[i].time_stamp, state);
EXPECT_EQ(state.time_stamp, states[i].time_stamp);
}
// We filled the buffer with more states that it can hold, so its size should now be equal to the capacity
EXPECT_EQ(static_cast<int>(estimator.getSize()), buffer_capacity);
// Clear the buffer and check if it's really empty afterwards
estimator.clearBuffer();
EXPECT_EQ(estimator.getSize(), 0u);
// Add states at time 1 through 3 inclusive to the buffer (buffer is not yet full)
for ( int i = 1; i < 4; i++ )
{
estimator.setState(states[i]);
}
// Now add a state at time 0, but let's change it a bit (set StateMemberY=12) so that we can inspect if it is
// correctly added to the buffer.
RobotLocalization::EstimatorState state_2 = states[0];
state_2.state(RobotLocalization::StateMemberY) = 12;
estimator.setState(state_2);
EXPECT_EQ(RobotLocalization::EstimatorResults::Exact,
estimator.getState(states[0].time_stamp, state));
// Check if the state is correctly added
EXPECT_EQ(state.state, state_2.state);
// Add some more states. State at t=0 should now be dropped, so we should get the prediction, which means y=0
for ( int i = 5; i < 8; i++ )
{
estimator.setState(states[i]);
}
EXPECT_EQ(RobotLocalization::EstimatorResults::ExtrapolationIntoPast,
estimator.getState(states[0].time_stamp, state));
EXPECT_EQ(states[0].state, state.state);
// Estimate a state that is not in the buffer, but can be determined by interpolation. The predicted state vector
// should be equal to the designed state at the requested time.
EXPECT_EQ(RobotLocalization::EstimatorResults::Interpolation,
estimator.getState(states[4].time_stamp, state));
EXPECT_EQ(states[4].state, state.state);
// Estimate a state that is not in the buffer, but can be determined by extrapolation into the future. The predicted
// state vector should be equal to the designed state at the requested time.
EXPECT_EQ(RobotLocalization::EstimatorResults::ExtrapolationIntoFuture,
estimator.getState(states[8].time_stamp, state));
EXPECT_EQ(states[8].state, state.state);
// Add missing state somewhere in the middle
estimator.setState(states[4]);
// Overwrite state at t=3 (oldest state now in the buffer) and check if it's correctly overwritten.
state_2 = states[3];
state_2.state(RobotLocalization::StateMemberVy) = -1.0;
estimator.setState(state_2);
EXPECT_EQ(RobotLocalization::EstimatorResults::Exact,
estimator.getState(states[3].time_stamp, state));
EXPECT_EQ(state_2.state, state.state);
// Add state that came too late
estimator.setState(states[0]);
// Check if getState needed to do extrapolation into the past
EXPECT_EQ(estimator.getState(states[0].time_stamp, state),
RobotLocalization::EstimatorResults::ExtrapolationIntoPast);
// Check state at t=0. This can only work correctly if the state at t=3 is
// overwritten and the state at zero is not in the buffer.
EXPECT_DOUBLE_EQ(3.0, state.state(RobotLocalization::StateMemberY));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_robot_localization_estimator");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

7
Localizations/Packages/robot_localization/test/test_robot_localization_estimator.test Normal file
View File

@@ -0,0 +1,7 @@
<!-- Launch file for ekf_test -->
<launch>
<test test-name="test_rle" pkg="robot_localization" type="test_robot_localization_estimator" clear_params="true" time-limit="100.0" />
</launch>

132
Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.cpp Normal file
View File

@@ -0,0 +1,132 @@
/*
* Copyright (c) 2016, TNO IVS, Helmond
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_robot_localization_listener.h"
#include "robot_localization/filter_common.h"
#include <string>
#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <gtest/gtest.h>
RobotLocalization::RosRobotLocalizationListener* g_listener;
TEST(LocalizationListenerTest, testGetStateOfBaseLink)
{
ros::spinOnce();
ros::Time time2(1001);
Eigen::VectorXd state(RobotLocalization::STATE_SIZE);
Eigen::MatrixXd covariance(RobotLocalization::STATE_SIZE, RobotLocalization::STATE_SIZE);
std::string base_frame("base_link");
g_listener->getState(time2, base_frame, state, covariance);
EXPECT_DOUBLE_EQ(1.0, state(RobotLocalization::StateMemberX));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberY));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberZ));
EXPECT_FLOAT_EQ(M_PI/4, state(RobotLocalization::StateMemberRoll));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberPitch));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberYaw));
EXPECT_DOUBLE_EQ(M_PI/4.0, state(RobotLocalization::StateMemberVroll));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberVpitch));
EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberVyaw));
}
TEST(LocalizationListenerTest, GetStateOfRelatedFrame)
{
ros::spinOnce();
Eigen::VectorXd state(RobotLocalization::STATE_SIZE);
Eigen::MatrixXd covariance(RobotLocalization::STATE_SIZE, RobotLocalization::STATE_SIZE);
ros::Time time1(1000);
ros::Time time2(1001);
std::string sensor_frame("sensor");
EXPECT_TRUE(g_listener->getState(time1, sensor_frame, state, covariance) );
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberX));
EXPECT_FLOAT_EQ(1.0, state(RobotLocalization::StateMemberY));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberZ));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberRoll));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberPitch));
EXPECT_FLOAT_EQ(M_PI/2, state(RobotLocalization::StateMemberYaw));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVx));
EXPECT_FLOAT_EQ(-1.0, state(RobotLocalization::StateMemberVy));
EXPECT_FLOAT_EQ(M_PI/4.0, state(RobotLocalization::StateMemberVz));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVroll));
EXPECT_FLOAT_EQ(-M_PI/4.0, state(RobotLocalization::StateMemberVpitch));
EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberVyaw));
EXPECT_TRUE(g_listener->getState(time2, sensor_frame, state, covariance));
EXPECT_FLOAT_EQ(1.0, state(RobotLocalization::StateMemberX));
EXPECT_FLOAT_EQ(sqrt(2)/2.0, state(RobotLocalization::StateMemberY));
EXPECT_FLOAT_EQ(sqrt(2)/2.0, state(RobotLocalization::StateMemberZ));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberRoll));
EXPECT_TRUE(1e-12 > fabs(-M_PI/4.0 - state(RobotLocalization::StateMemberPitch)));
EXPECT_FLOAT_EQ(M_PI/2, state(RobotLocalization::StateMemberYaw));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVx));
EXPECT_FLOAT_EQ(-1.0, state(RobotLocalization::StateMemberVy));
EXPECT_FLOAT_EQ(M_PI/4, state(RobotLocalization::StateMemberVz));
EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVroll));
EXPECT_FLOAT_EQ(-M_PI/4.0, state(RobotLocalization::StateMemberVpitch));
EXPECT_FLOAT_EQ(0, state(RobotLocalization::StateMemberVyaw));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_robot_localization_estimator");
g_listener = new RobotLocalization::RosRobotLocalizationListener();
testing::InitGoogleTest(&argc, argv);
int res = RUN_ALL_TESTS();
delete g_listener;
return res;
}

68
Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.test Normal file
View File

@@ -0,0 +1,68 @@
<!-- Launch file for ros_robot_localization_listener_test -->
<launch>
<node name="test_estimator" pkg="robot_localization" type="test_ros_robot_localization_listener_publisher" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/odom_input0"/>
<param name="odom1" value="/odom_input1"/>
<param name="odom2" value="/odom_input2"/>
<param name="pose0" value="/pose_input0"/>
<param name="pose1" value="/pose_input1"/>
<param name="twist0" value="/twist_input0"/>
<param name="imu0" value="/imu_input0"/>
<param name="imu1" value="/imu_input1"/>
<param name="imu2" value="/imu_input2"/>
<param name="imu3" value="/imu_input3"/>
<rosparam param="odom0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom2_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="pose0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="pose1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, true, true, true, false, false, false]</rosparam>
<rosparam param="imu2_config">[false, false, false, false, false, false, false, false, false, false, false, false, true, true, true]</rosparam>
<rosparam param="imu3_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<param name="odom1_differential" value="true"/>
<param name="pose1_differential" value="true"/>
<param name="imu3_differential" value="true"/>
<param name="print_diagnostics" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ros_robot_localization_listener" pkg="robot_localization" type="test_ros_robot_localization_listener" clear_params="true" time-limit="100.0">
<remap from="robot_localization" to="test_estimator" />
</test>
</launch>

117
Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener_publisher.cpp Normal file
View File

@@ -0,0 +1,117 @@
/*
* Copyright (c) 2016, TNO IVS, Helmond
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <string>
#include <ros/ros.h>
#include <tf2/utils.h>
#include <tf2_ros/static_transform_broadcaster.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_robot_localization_listener_publisher");
ros::NodeHandle nh;
ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odometry/filtered", 1);
ros::Publisher accel_pub = nh.advertise<geometry_msgs::AccelWithCovarianceStamped>("accel/filtered", 1);
tf2_ros::StaticTransformBroadcaster transform_broadcaster;
ros::Time end_time = ros::Time::now() + ros::Duration(10);
while (ros::ok() && ros::Time::now() < end_time)
{
ros::Time time1(1000);
double x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az;
x = y = z = roll = pitch = yaw = vy = vz = vroll = vpitch = vyaw = ax = ay = az = 0.0;
vx = 1.0;
vroll = M_PI/4.0;
tf2::Quaternion q;
q.setRPY(0, 0, 0);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = time1;
odom_msg.header.frame_id = "map";
odom_msg.child_frame_id = "base_link";
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.y = z;
odom_msg.pose.pose.orientation.x = q.x();
odom_msg.pose.pose.orientation.y = q.y();
odom_msg.pose.pose.orientation.z = q.z();
odom_msg.pose.pose.orientation.w = q.w();
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.linear.y = vy;
odom_msg.twist.twist.linear.z = vz;
odom_msg.twist.twist.angular.x = vroll;
odom_msg.twist.twist.angular.y = vpitch;
odom_msg.twist.twist.angular.z = vyaw;
geometry_msgs::AccelWithCovarianceStamped accel_msg;
accel_msg.header.stamp = time1;
accel_msg.header.frame_id = "base_link";
accel_msg.accel.accel.linear.x = ax;
accel_msg.accel.accel.linear.y = ay;
accel_msg.accel.accel.linear.z = az;
accel_msg.accel.accel.angular.x = 0;
accel_msg.accel.accel.angular.y = 0;
accel_msg.accel.accel.angular.z = 0;
odom_pub.publish(odom_msg);
accel_pub.publish(accel_msg);
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "base_link";
transformStamped.child_frame_id = "sensor";
transformStamped.transform.translation.x = 0.0;
transformStamped.transform.translation.y = 1.0;
transformStamped.transform.translation.z = 0.0;
{
tf2::Quaternion q;
q.setRPY(0, 0, M_PI/2);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
transform_broadcaster.sendTransform(transformStamped);
}
ros::Duration(0.1).sleep();
}
return 0;
}

141
Localizations/Packages/robot_localization/test/test_ukf.cpp Normal file
View File

@@ -0,0 +1,141 @@
/*
* Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "robot_localization/ros_filter_types.h"
#include <gtest/gtest.h>
#include <limits>
#include <vector>
using RobotLocalization::Ukf;
using RobotLocalization::RosUkf;
using RobotLocalization::STATE_SIZE;
class RosUkfPassThrough : public RosUkf
{
public:
explicit RosUkfPassThrough(std::vector<double> &args) : RosUkf(ros::NodeHandle(), ros::NodeHandle("~"), args)
{
}
Ukf &getFilter()
{
return filter_;
}
};
TEST(UkfTest, Measurements)
{
std::vector<double> args;
args.push_back(0.001);
args.push_back(0);
args.push_back(2);
RosUkfPassThrough ukf(args);
Eigen::MatrixXd initialCovar(15, 15);
initialCovar.setIdentity();
initialCovar *= 0.5;
ukf.getFilter().setEstimateErrorCovariance(initialCovar);
EXPECT_EQ(ukf.getFilter().getEstimateErrorCovariance(), initialCovar);
Eigen::VectorXd measurement(STATE_SIZE);
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurement[i] = i * 0.01 * STATE_SIZE;
}
Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
measurementCovariance.setIdentity();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
std::vector<int> updateVector(STATE_SIZE, true);
// Ensure that measurements are being placed in the queue correctly
ros::Time time;
time.fromSec(1000);
ukf.enqueueMeasurement("odom0",
measurement,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ukf.integrateMeasurements(ros::Time(1001));
EXPECT_EQ(ukf.getFilter().getState(), measurement);
EXPECT_EQ(ukf.getFilter().getEstimateErrorCovariance(), measurementCovariance);
ukf.getFilter().setEstimateErrorCovariance(initialCovar);
// Now fuse another measurement and check the output.
// We know what the filter's state should be when
// this is complete, so we'll check the difference and
// make sure it's suitably small.
Eigen::VectorXd measurement2 = measurement;
measurement2 *= 2.0;
for (size_t i = 0; i < STATE_SIZE; ++i)
{
measurementCovariance(i, i) = 1e-9;
}
time.fromSec(1002);
ukf.enqueueMeasurement("odom0",
measurement2,
measurementCovariance,
updateVector,
std::numeric_limits<double>::max(),
time);
ukf.integrateMeasurements(ros::Time(1003));
measurement = measurement2.eval() - ukf.getFilter().getState();
for (size_t i = 0; i < STATE_SIZE; ++i)
{
EXPECT_LT(::fabs(measurement[i]), 0.001);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ukf");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

7
Localizations/Packages/robot_localization/test/test_ukf.test Normal file
View File

@@ -0,0 +1,7 @@
<!-- Launch file for test_ukf -->
<launch>
<test test-name="test_ukf" pkg="robot_localization" type="test_ukf" clear_params="true" time-limit="100.0" />
</launch>

110
Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag1.test Normal file
View File

@@ -0,0 +1,110 @@
<!-- Launch file for test_ukf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. The IMU did not report 0
when facing east, however, but when facing north. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 5" required="true"/>
<node name="test_ukf_localization_node_bag1_ukf" pkg="robot_localization" type="ukf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ukf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
<param name="alpha" value="0.001"/>
<param name="kappa" value="0"/>
<param name="beta" value="2"/>
</node>
<test test-name="test_ukf_localization_node_bag1_pose" pkg="robot_localization" type="test_ukf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-39.7333"/>
<param name="final_y" value="-76.9820"/>
<param name="final_z" value="-2.4427"/>
<param name="tolerance" value="1.2910"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

91
Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag2.test Normal file
View File

@@ -0,0 +1,91 @@
<!-- Launch file for test_ukf_localization_node_bag2 -->
<launch>
<arg name="output_final_position" default="false"/>
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test2.bag --clock -d 5" required="true"/>
<node name="test_ukf_localization_node_bag2_ukf" pkg="robot_localization" type="ukf_localization_node" clear_params="true" >
<param name="frequency" value="50"/>
<param name="frequency" value="50"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/jackal_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="process_noise_covariance">[0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
<param name="alpha" value="0.001"/>
<param name="kappa" value="0"/>
<param name="beta" value="2"/>
</node>
<test test-name="test_ukf_localization_node_bag2_pose" pkg="robot_localization" type="test_ukf_localization_node_bag2" clear_params="true" time-limit="1000.0">
<param name="final_x" value="1.0438"/>
<param name="final_y" value="3.4940"/>
<param name="final_z" value="0.7260"/>
<param name="tolerance" value="0.2351"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

79
Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag3.test Normal file
View File

@@ -0,0 +1,79 @@
<!-- Launch file for test_ukf_localization_node_bag3 -->
<launch>
<arg name="output_final_position" default="false"/>
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test3.bag --clock -d 5" required="true"/>
<node name="test_ukf_localization_node_bag3_ukf" pkg="robot_localization" type="ukf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="odom0" value="/ardrone/odometry/raw"/>
<param name="imu0" value="/ardrone/imu"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<rosparam param="odom0_config">[false, false, true,
false, false, false,
true, true, false,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]</rosparam>
<!-- Differential setting is off for this test -->
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="alpha" value="0.001"/>
<param name="kappa" value="0"/>
<param name="beta" value="2"/>
<rosparam param="process_noise_covariance">[0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
<test test-name="test_ukf_localization_node_bag3_pose" pkg="robot_localization" type="test_ukf_localization_node_bag3" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-0.41148"/>
<param name="final_y" value="-0.2513"/>
<param name="final_z" value="2.990"/>
<param name="tolerance" value="0.1681"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>

1079
Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

73
Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.test Normal file
View File

@@ -0,0 +1,73 @@
<!-- Launch file for ukf_localization_node_test-interfaces -->
<launch>
<node name="test_ukf_localization_node_interfaces_ukf" pkg="robot_localization" type="ukf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="odom0" value="/odom_input0"/>
<param name="odom1" value="/odom_input1"/>
<param name="odom2" value="/odom_input2"/>
<param name="pose0" value="/pose_input0"/>
<param name="pose1" value="/pose_input1"/>
<param name="twist0" value="/twist_input0"/>
<param name="imu0" value="/imu_input0"/>
<param name="imu1" value="/imu_input1"/>
<param name="imu2" value="/imu_input2"/>
<param name="imu3" value="/imu_input3"/>
<rosparam param="odom0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="odom2_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="pose0_config">[true, false, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="pose1_config">[true, true, true, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, true, true, true, true, false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, true, true, true, false, false, false]</rosparam>
<rosparam param="imu2_config">[false, false, false, false, false, false, false, false, false, false, false, false, true, true, true]</rosparam>
<rosparam param="imu3_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
<!-- Differential setting is off for this test -->
<param name="odom1_differential" value="true"/>
<param name="pose1_differential" value="true"/>
<param name="imu3_differential" value="true"/>
<param name="print_diagnostics" value="false"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<rosparam param="process_noise_covariance">[1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
<param name="alpha" value="0.001"/>
<param name="kappa" value="0"/>
<param name="beta" value="2"/>
</node>
<test test-name="test_ukf_localization_node_interfaces_int" pkg="robot_localization" type="test_ukf_localization_node_interfaces" clear_params="true" time-limit="1000.0" />
</launch>

111
Localizations/Packages/robot_localization/test/test_ukf_localization_nodelet_bag1.test Normal file
View File

@@ -0,0 +1,111 @@
<!-- Launch file for test_ukf_localization_node_bag1 -->
<!-- In this run, we ran a Clearpath Husky with a slightly broken Microstrain 3DM-GX2 IMU
around a parking lot. The IMU was intentionally rotated +90 degrees about the X axis,
then rotated (extrinsically) -90 degrees about the Z axis. The IMU did not report 0
when facing east, however, but when facing north. -->
<launch>
<arg name="output_final_position" default="false" />
<arg name="output_location" default="test.txt" />
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 -0.3 0.52 -1.570796327 0 1.570796327 base_link imu_link" />
<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test/test1.bag --clock -d 5" required="true"/>
<node pkg="nodelet" type="nodelet" name="UKF_NODELET_MANAGER" args="manager"/>
<node pkg="nodelet" type="nodelet" name="test_ukf_localization_node_bag1_ukf" output="screen" args="load RobotLocalization/UkfNodelet UKF_NODELET_MANAGER" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ukf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
<param name="alpha" value="0.001"/>
<param name="kappa" value="0"/>
<param name="beta" value="2"/>
</node>
<test test-name="test_ukf_localization_node_bag1_pose" pkg="robot_localization" type="test_ukf_localization_node_bag1" clear_params="true" time-limit="1000.0">
<param name="final_x" value="-39.7333"/>
<param name="final_y" value="-76.9820"/>
<param name="final_z" value="-2.4427"/>
<param name="tolerance" value="1.2910"/>
<param name="output_final_position" value="$(arg output_final_position)"/>
<param name="output_location" value="$(arg output_location)"/>
</test>
</launch>