Robot Localization Package: Transform from base_link to odom was unavailable for the time requested. Using latest instead. (IMU+GPS)
Hello, I want to record data from a tripod, so I started by creating a urdf file, where I say that laser_base is 1m above of base_link, like this:
<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>
After this, I want to move the tripod around, so I need to keep track of its XYZ position, as well as orientation, so that the data I record is properly located. For that, I am using an ArduPilot board for retrieving IMU and GPS information, which I am fusing with the robotlocalizationpackage (Wiki) (ekf and navsattransformnode).
The launch file for this is as follows:
<launch>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
</node>
<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/apm2.launch">
<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="30 "/>
<param name="sensor_timeout" value="2"/>
<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, true,
false, false, false,
false, false, false,
false, false, false,
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_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="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="10"/>
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<param name="zero_altitude" value="false"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="publish_filtered_gps" value="false"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="false"/>
<remap from="/gps/fix" to="/mavros/global_position/raw/fix"/>
<remap from="/imu/data" to="/mavros/imu/data"/>
</node>
So, I walked around with the ArduPilot board and recorded a bag file with the IMU and GPS topics (/mavros/imu/data and /mavros/global_position/raw/fix). I have also created another bag file where I am running the launch file above and recorded all topics.
So, I have two problems:
1ST: When I do rostopic hz
with the 2 topics above, it shows a rate of approximately 1Hz. That is very slow, isn't it? I have tried this, but it didn't work, showing the message:
RLException: unused args [config_yaml, pluginlists_yaml] for include of [/opt/ros/kinetic/share/mavros/launch/apm2.launch]
2ND: When I use the launch file above, it starts by showing:
[ INFO] [1534032142.676696240]: Initial odometry pose is Origin: (0 0 0)
Rotation (RPY): (0.061743952333927182297, -0.017582930624485095666, -0.89866225322219372984)
[ INFO] [1534032142.776689397]: Initial odometry pose is Origin: (0 0 0)
Rotation (RPY): (0.061743952333927182297, -0.017582930624485095666, -0.89866225322219372984)
... And, after a couple of this messages, it shows:
[ INFO] [1534032142.878153405]: World frame->utm transform is Origin: (-475554.63263590750284 -4278148.5965311238542 -133980.14933940110495)
Rotation (RPY): (0.024644147360516073519, -0.059266088787775804414, -0.00012297369933011960061)
[ WARN] [1534032143.376845932]: Transform from base_link to odom_combined was unavailable for the time requested. Using latest instead.
And the latter line starts repeating. Once this happens, the Output on RViz also gets strange, Where the transform between odom and base_link starts changing randomly.
Any idea of what the problem with the transforms might be? Thank you.
EDIT:
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]
---
header:
seq: 481
stamp:
secs: 1536196133
nsecs: 194888951
frame_id: "base_link"
orientation:
x: -0.0104354376393
y: 0.0205274442474
z: 0.354258848487
w: -0.934863837114
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity:
x: 0.00285236537457
y: 0.00103874504566
z: 0.00217121653259
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.67916355
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0,
8.999999999999999e-08]
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
---
header:
seq: 1337
stamp:
secs: 1536196628
nsecs: 777257559
frame_id: "base_link"
status:
status: 0
service: 1
latitude: 38.6603921
longitude: -9.2061619
altitude: 143.967507108
position_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
Asked by jpde.lopes on 2018-08-11 19:41:23 UTC
Answers
Please update the question by posting a sample message from each sensor input.
So, I have two problems: 1ST: When I do rostopic hz with the 2 topics above, it shows a rate of approximately 1Hz. That is very slow, isn't it?
For a GPS, that makes sense. For an IMU, that doesn't seem right.
2ND: When I use the launch file above, it starts by showing:
If you are getting transform warnings, it means that the transforms coming out of the EKF are older than the timestamps on the input messages going into navsat_transform_node
. This is because the EKF only predicts up to the point of its most recent input message. Since the IMU and GPS data probably (?) have the same time stamps, navsat_transform_node
receives a GPS message at the same time that the EKF receives its latest IMU data. That means the EKF will have to compute and publish the transform before navsat_transform_node
can use it, but navsat_transform_node
needs that transform at the time it receives the GPS message (which is at the time the EKF just receives the IMU message, so it won't be available).
You have two options:
- Change your
transform_time_offset
EKF parameter to some small positive value. This will future-date its transform it's generating. - Set
predict_to_current_time
to true for the EKF. That will force the EKF to make a prediction to the current ROS time before publishing, rather than just publishing the state estimate at the time of the most recent measurement.
Asked by Tom Moore on 2018-08-27 04:44:34 UTC
Comments
Thank you for your answer. I have updated my question with a sample of the sensors.
Asked by jpde.lopes on 2018-09-05 20:24:34 UTC
I have created another question w/ more significant data.
Asked by jpde.lopes on 2018-09-09 19:38:55 UTC
predict_to_current_time is not documented in the wiki.
Asked by andrestoga on 2019-08-04 20:54:00 UTC
@andrestoga If you have time, then I'd welcome a PR to fix that. :)
Asked by Tom Moore on 2019-08-22 06:20:03 UTC
Comments
Tom would be the authoritative input on this, but in the meantime - for your 1st problem, what frequency are you expecting? GPS usually outputs at 1Hz so no discrepancy there; IMUs generally report many times a second... but again, what are your expectations? Did you run hz on the original topics?
Asked by autonomy on 2018-08-13 13:07:14 UTC
For 2, did you try running view_frames to see how old your static transform is? (http://wiki.ros.org/tf2/Tutorials/Introduction%20to%20tf2#Using_view_frames) From the source code: // The issue might be that the transforms that are available are not close // enough temporally to be used.
Asked by autonomy on 2018-08-13 13:10:37 UTC
Thank you for your answer @autonomy. Yes, I tried to run hz on the original topics. Both the IMU and GPS are around 2Hz. I noticed some discrete jumps in RViz. Also, on almost every example of navsat transform node implementation I've seen 30Hz being used.
Asked by jpde.lopes on 2018-08-13 13:24:58 UTC
On my case, I've used 10Hz, but if I reduce it to 3Hz, for example, I see much less of those TF warnings.
Asked by jpde.lopes on 2018-08-13 13:25:58 UTC
I'll check that (view_frames) and will give feedback.
Asked by jpde.lopes on 2018-08-13 13:28:25 UTC
Everything seems fine with the frames. Sometimes I notice that the frequency drops from approximately 2Hz to 0.002Hz or something like that. It is momentaneous, but it may be enough to cause that warning, right?
Asked by jpde.lopes on 2018-08-13 15:54:28 UTC
2Hz seems low for an IMU. I worked with IMUs running anywhere between 30 to 100Hz. You are requesting robot_localization to run at 30Hz but your sensor readings, which are driving it, are only publishing at 2Hz. Increase IMU rate or try running everything at 2Hz and see if the warning goes away
Asked by autonomy on 2018-08-13 16:17:30 UTC
Take a look here, maybe that can help increase the sensor frequency. Also, take care to set your magnetic_declination_radians correctly unless it really is 0
Asked by autonomy on 2018-08-13 16:25:57 UTC
That is what I thought as well (that is very slow). Thank you for the presented solution. I will try it.
Asked by jpde.lopes on 2018-08-15 12:47:31 UTC
Hi, I have tried it. The rate remains the same.
Asked by jpde.lopes on 2018-09-05 20:22:51 UTC
I have created another question w/ more significant data.
Asked by jpde.lopes on 2018-09-09 19:39:04 UTC