Robotics StackExchange | Archived questions

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: Trajectory seen on a map.

If I plot the latitude and longitude from the NavsatFix data, I have the following:

GPS Data

(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:

gps/filtered

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:

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

Comments

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