Robot Localization Package: Transform was unavailable for the time requested. Using latest instead.
Hello, I am using an ArduPilot board with an internal IMU connected to an external GPS+Compass module.
I wanted to localize a frame base_link
in relation to odom_combined
. This base link is connected to other frames, so I have created a simple URDF file:
<link name="base_link">
<origin xyz="0 0 0"/>
</link>
<link name="laser_base">
</link>
<joint name="laser_down" type="fixed">
<origin xyz="0 0 1" rpy="0 0 1.57079632679"/>
<parent link="base_link"/>
<child link="laser_base"/>
</joint>
So, I went outside and with the sensors, moved around making a rectangle (moving forwards + turn right + forwards ...).
This the trajectory I did:
If I plot the latitude and longitude from the NavsatFix data, I have the following:
(I recorded the data from two GPS modules, but the External Module is the one I am talking about).
Everything seems fine, but if I try to use the robot_localization_package
with the navsat_transform_node
to estimate IMU which will then be the input to the ekflocalizationnode, it shows the error:
[ WARN] Transform from base_link to odom_combined was unavailable for the time requested. Using latest instead.
And the frame in RViz does not seems fine at all, with very aggressive discrete jumps. Based on another question I did, I set the predict_to_current_time
parameter to true, but it did not solve the problem.
Also, the gps/filtered
output (shown below) from navsat_transform_node
is very different from the original gps data:
Yet, the gps output from navsat_transform_node
that will be the input from ekf_localization_node
is correct (makes a perfect rectangle). This means that the problem is related to the IMU, right?
I really do not know how I can solve this problem because, the information from the sensors seems correct. The IMU frequency is around 2Hz. I know that it is too low, but I can not figure why (the mavros launch is commented below).
Also, here is the result of $ rosrun tf view_frames
:
Here is the bag recorded in the parking lot (containing the IMU and NavSat/fix data). Also, I have created a another bag which, not only contains the IMU+Navsat information, but also, the topics associated with the robot_localization_package
, when running navsat_transform_node
and ekf_localization_node
.
<launch>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
</node>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
<param name="robot_description" textfile="$(find obstacle_detection)/description/tripod.urdf" />
<node name="modelvisualization" pkg="rviz" type="rviz" output="screen"/>
<!-- VR Brain Parameters -->
<!-- <arg name="fcu_url" default="/dev/ttyUSB0:57600" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<arg name="fcu_protocol" default="v2.0" />
<arg name="respawn_mavros" default="false" /> -->
<!-- Launch VR Brain -->
<!-- <include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find obstacle_detection)/params/apm_pluginlists.yaml" /> -->
<!-- <arg name="config_yaml" value="$(find obstacle_detection)/params/apm_config.yaml" /> -->
<!-- <arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
<arg name="respawn_mavros" default="$(arg respawn_mavros)" />
</include> -->
<!-- EKF -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
<param name="frequency" value="15"/>
<param name="sensor_timeout" value="0.1"/>
<param name="predict_to_current_time" value="true"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom_combined"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom_combined"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/odometry/gps"/>
<param name="imu0" value="/mavros/imu/data"/>
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true, <!-- roll, pitch, yaw -->
false, false, false,
true, true, true, <!-- droll, dpitch, dyaw -->
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/> <!-- this was false -->
<param name="imu0_differential" value="false"/>
<!-- Fused w/ t0 (if starts always at zero pos yaw pitch ... -->
<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="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="frequency" value="30"/>
<!-- <param name="delay" value="3"/> -->
<param name="magnetic_declination_radians" value="0"/>
<!-- <param name="yaw_offset" value="1.570796327"/> -->
<param name="zero_altitude" value="false"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="publish_filtered_gps" value="true"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="false"/>
<!-- <param name="predict_to_current_time" value="true"/> -->
<remap from="/gps/fix" to="/mavros/global_position/raw/fix"/>
<remap from="/imu/data" to="/mavros/imu/data"/>
</node>
</launch>
In the launch file above, I have commented the fusion of Z on the odom because when I plot the z, it does not seems correct. Also, I do not know why, but I only have the IMU publishing at 2Hz.
Also, here is a sample of sensor output: IMU:
header:
seq: 480
stamp:
secs: 1536196132
nsecs: 675659339
frame_id: "base_link"
orientation:
x: -0.01038560877
y: 0.0207573504383
z: 0.354425421472
w: -0.934796176794
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity:
x: 0.0015384554863
y: -0.000201269984245
z: 0.0021977359429
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration:
x: 0.00980665
y: 0.1176798
z: 9.6693569
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
And from the GPS:
header:
seq: 1336
stamp:
secs: 1536196628
nsecs: 257958525
frame_id: "base_link"
status:
status: 0
service: 1
latitude: 38.6603919
longitude: -9.2061606
altitude: 143.787508098
position_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
Any help is appreciated. Thank you.
Asked by jpde.lopes on 2018-09-09 19:35:59 UTC
Answers
Thanks for including your full configuration and sample messages. That's very helpful. Also, apologies for the delay in responding.
Anyway, I note in your EKF configuration that you (obviously) are not using two_d_mode
, but yet you don't have a measurement/reference for Z
. You are measuring every other pose variable in 3D, but unless you have a reference for at least Z velocity, I would expect your EKF state estimate to be unstable, due to the fact that (a) Z isn't measured, and (b) Z is correlated with other state variables in the kinematic model.
If you have an altitude sensor, add that input to the EKF. If you don't, maybe consider faking a fixed altitude estimate?
Also, my experience has been that the IMU/GPS combo alone doesn't perform especially well. If you have any kind of velocity reference (camera-based optical flow data, for example), that would be helpful.
Asked by Tom Moore on 2018-10-22 10:43:12 UTC
Comments
Thank you very much for your reply. Instead of IMU and GPS, would adding an extra IMU help? Or is the optical flow preferable? Also, I am now using RTK, which I think will help with the xyz estimate.
Asked by jpde.lopes on 2018-10-22 11:10:40 UTC
I am currently not with the IMU and GPS setup to test, but on Wednesday, I will perform new tests having your answer in consideration and I will post the updates.
Asked by jpde.lopes on 2018-10-22 11:12:15 UTC
The reason why I have removed the Z measurement was because it was really unstable, going up and down, and I didnt know why. Even if I add the z measurement, the lack of z velocity would result in instabilities?, because I dont have that variable measured.
Asked by jpde.lopes on 2018-10-22 11:15:31 UTC
Every pose variable (x, y, z, roll, pitch, and yaw) needs to either be measured directly, or as a velocity. If you have any pose variable that isn't, the filter won't behave.
If you want a smoother filter output, I'd recommend fusing in some optical flow 3D velocity data.
Asked by Tom Moore on 2018-10-24 09:57:42 UTC
Hello,jpde.lopes,I encountered the same problem as you, did you solve it?
Asked by FeynH on 2020-03-30 01:12:53 UTC
in ekf_se_odom: transform_time_offset: 0.1 in ekf_se_map: transform_time_offset: 0.1
Asked by Debanik Roy on 2021-06-07 13:30:49 UTC
Comments