search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
970 stars 557 forks source link

ICP odometry does not publish TF #568

Open stevemartinov opened 3 years ago

stevemartinov commented 3 years ago

I want to use ICP odometry with having a guess from the current odom frame but I am keep getting this:

[ WARN] [1618071637.466157178, 1617868751.621934913]: odometry: Could not get transform from base_footprint to lidar_front (stamp=1617868751.524755) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error="Lookup would require extrapolation 0.016184853s into the past.  Requested time 1617868751.524755001 but the earliest data is at time 1617868751.540939808, when looking up transform from frame [lidar_front] to frame [base_footprint]. canTransform returned after 0.100534 timeout was 0.1."
[ERROR] [1618071637.466209035, 1617868751.621934913]: TF of received laser scan topic at time 1617868751.424164s is not set, aborting odometry update.

This is strange because I do have such frame and this is the TF tree:

frames.pdf

And this is my launc file:

<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
        <remap from="scan"      to="/scan_front_filtered"/>
        <remap from="odom"      to="/scanmatch_odom"/>
        <remap from="odom_info"      to="/rtabmap/odom_info"/>

        <param name="frame_id"        type="string" value="base_footprint"/>

        <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
        <param if="$(arg odom_guess)" name="guess_frame_id"  type="string" value="odom"/>

        <param name="publish_tf" value="true" />

        <param name="Icp/VoxelSize"     type="string" value="0.05"/>
        <param name="Icp/RangeMax"      type="string" value="15"/>
        <param name="Icp/Epsilon"       type="string" value="0.001"/>
        <param unless="$(arg odom_guess)" name="Icp/MaxTranslation" type="string" value="0"/> <!-- can be set to reject large ICP jumps -->
        <param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="true"/>
        <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="5"/>
        <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
        <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
        <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
        <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
        <param name="Icp/PM"             type="string" value="$(arg pm)"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
        <param name="Icp/PMOutlierRatio" type="string" value="0.85"/>
        <param name="Odom/Strategy"        type="string" value="0"/>
        <param name="Odom/GuessMotion"     type="string" value="true"/>
        <param name="Odom/ResetCountdown"  type="string" value="0"/>
        <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/>
    </node>

If I remove the guess_odom param, it works well but I want to supply guess

matlabbe commented 3 years ago

The map->odom tf may break the tree. You would have to change it to map -> icp_odom.

Is that warning appearing only one time? The delay in the past seems that the tf was not yet published when the topic has been published. Unless the warning is appearing often, I would ignore this issue.

The final resulting Tf tree would be map->icp_odom->odom->base_footprint->lidar_front