0
$\begingroup$

I'm trying to configure an ekf_filter_node. I started by adding just the odom topic from a ros2_control DiffDriveController in gazebo.

For some reason, the robot starts sliding sideways in odom (in the y direction from the robot's perspective) when I drive forward after I rotate it some. I thought it would use the fact that the y velocity is zero from the robot odometry and not move much in that direction in the estimation.

Is this sliding expected behaviour with only odom as input, or am I doing something wrong?

Sliding To The Side

ekf_filter_node_odom:
  ros__parameters:
    permit_corrected_publication: true
    frequency: 50.0
    sensor_timeout: 0.02

    two_d_mode: true
    transform_time_offset: 0.1
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /velocity_controller/odom 
    odom0_config: [false, false, false, # X, Y, Z
                   false, false, false, # roll, pitch, yaw
                   true,  true, false,  # X', Y', Z'
                   false, false, true,  # roll', pitch', yaw'
                   false, false, false] # X'', Y'', Z''
                   
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    use_control: false
    process_noise_covariance: [1.0,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    1.0,    0.0,    0.0,    0.0,    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-3,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.3,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.3,    0.0,    0.0,     0.0,     0.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.5,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.5,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.3,    0.0,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.3,    0.0,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.3,    0.0,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.3,    0.0,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.3,    0.0,
                          0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.3]

initial_estimate_covariance: [1.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    1.0,    0.0,    0.0,    0.0,    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-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    1.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    1.0,    0.0,    0.0,    0.0,    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-9,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1.0,     0.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1.0,     0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1.0]

(edit: Added top level URDF in case the issue is there)

<?xml version="1.0"?>
<robot name="bot" xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:property name="PI" value="3.14159265359" />

<xacro:property name="chassis_length" value="0.696" />
<xacro:property name="chassis_width" value="0.826" />
<xacro:property name="chassis_height" value="0.054" />

<xacro:property name="wheelbase" value="0.5" />
<xacro:property name="track" value="0.87" />
<xacro:property name="wheel_vertical_offset" value="0.027" />

<xacro:property name="wheel_radius" value="0.098" />
<xacro:property name="wheel_width" value="0.040" />

<xacro:property name="lidar_min_range" value="0" />
<xacro:property name="lidar_max_range" value="25.0" />

<material name="dark_grey">
    <color rgba="0.2 0.2 0.2 1.0" />
</material>
<material name="light_grey">
    <color rgba="0.4 0.4 0.4 1.0" />
</material>
<material name="yellow">
    <color rgba="0.8 0.8 0.0 1.0" />
</material>
<material name="black">
    <color rgba="0.15 0.15 0.15 1.0" />
</material>


<link name="base_link"></link>

<link name="gps_link">
    <visual>
    <geometry>
        <cylinder radius="0.03" length="0.02"/>
    </geometry>
    </visual>
</link>
<joint name="gps_joint" type="fixed">
    <parent link="base_link"/>
    <child link="gps_link"/>
    <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
</joint>

<link name="imu_link"></link>
<joint name="imu_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
</joint>

<link name="lidar_parent_link"></link>
<joint name="lidar_parent_joint" type="fixed">
    <parent link="base_link" />
    <child link="lidar_parent_link" />
    <origin xyz="0.0 0.0 0.3" rpy="0 0 0"/>
</joint>
<xacro:include filename="$(find bot_description)/urdf/accessories/lidar.xacro" />
<xacro:lidar frame="lidar" topic="/scan" parent_link="lidar_parent_link" min_angle="${-PI}" 
            max_angle="${PI}" min_range="${lidar_min_range}" max_range="${lidar_max_range}">
  <origin xyz="0 0 0.05" rpy="0 0 0" />
</xacro:lidar>

<link name="camera_frame"></link>
<joint name="camera_joint" type="fixed">
    <parent link="base_link" />
    <child link="camera_frame" />
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
</joint>

<link name="chassis_link">
    <visual>
        <origin xyz="0 0 ${chassis_height/2}"/>
        <geometry>
            <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
        </geometry>
        <material name="dark_grey" />
    </visual>
    <collision>
        <origin xyz="0 0 ${chassis_height/2}"/>
        <geometry>
            <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
        </geometry>
    </collision>
    <inertial>
        <!-- Center of mass -->
        <origin xyz="0.0  0.0 0.0" rpy="0 0 0"/>
        <mass value="16.523"/>
        <!-- Moments of inertia ( chassis without wheels ) -->
        <inertia ixx="0.3136" ixy="-0.0008" ixz="0.0164" iyy="0.3922" iyz="-0.0009" izz="0.4485"/>
    </inertial>
</link>

<joint name="chassis_joint" type="fixed">
    <parent link="base_link" />
    <child link="chassis_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/> 
</joint>

<xacro:macro name="wheel" params="prefix *joint_pose">
    <link name="${prefix}_wheel_link">
    <visual>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="black" />
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="0.477"/>
        <inertia ixx="0.0013" ixy="0" ixz="0" iyy="0.0024" iyz="0" izz="0.0013"/>
    </inertial>
    </link>

    <gazebo reference="${prefix}_wheel_link">
        <material>Gazebo/DarkGrey</material>
        <selfCollide>false</selfCollide>
        <mu1 value="1.0"/>
        <mu2 value="0.25"/>
        <kp value="10000000.0" />
        <kd value="1" />
        <fdir1 value="1 0 0"/>
    </gazebo>

    <joint name="${prefix}_wheel_joint" type="continuous">
        <parent link="chassis_link"/>
        <child link="${prefix}_wheel_link" />
        <xacro:insert_block name="joint_pose" />
        <axis xyz="0 1 0" />
    </joint>

</xacro:macro>

<xacro:wheel prefix="front_left">
    <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
</xacro:wheel>
<xacro:wheel prefix="front_right">
    <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
</xacro:wheel>
<xacro:wheel prefix="rear_left">
    <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
</xacro:wheel>
<xacro:wheel prefix="rear_right">
    <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
</xacro:wheel>

<!-- Import ros2_control description -->
<xacro:include filename="$(find bot_description)/ros2_control/bot_hardware.ros2_control.xacro" />
<xacro:bot_hardware prefix="" />

<!-- Bring in the simulation settings for Gazebo -->
<xacro:include filename="$(find bot_description)/urdf/bot.gazebo.xacro" />

</robot>
$\endgroup$
3
  • $\begingroup$ Look at your URDF, I'd guess the issue is there. $\endgroup$ Nov 17, 2023 at 17:15
  • $\begingroup$ You first should make sure the odometry published by the controller is fairly accurate. After movement stops, make sure that robot pose (translation and rotation) in gazebo matches what is being published on the odom topic - if error in either accumulates rapidly, you need to fix that. Then verify that the odom twist values are reasonable for constant linear velocity, and again for constant yaw velocity. $\endgroup$
    – Mike973
    Nov 19, 2023 at 16:45
  • $\begingroup$ Hi Steve and Mike! I could not find any obvious issue with the URDF. Added it to the question. If I rotate the robot 90 degrees in Gazebo, it rotates more than 180 degrees in Rviz. So that is pretty bad I guess... The robot sometimes moves slightly in Rviz after stopping even after the twist values in odom have gone to 0. The odom twist values look reasonable when compared to cmd_vel. Pretty spot-on for linear and slightly low for angular (1.0 -> 0.9). $\endgroup$ Nov 20, 2023 at 9:58

1 Answer 1

1
$\begingroup$

This is the expected behavior.

I thought it would use the fact that the y velocity is zero from the robot odometry

I think your problem comes from that you're making a confusion here. The y velocity may not be 0. The x, y and z coordinates of an odometry message are expressed in the odom frame, not the robot frame. For example, if your robot moves along the Y axis of the odom frame, y will increase and x will remain constant. It's the same for X', Y' and Z' velocities.

Back to your question, it's not possible to know the absolute position/orientation of the robot because you provide only relatives ones (velocities). The ekf filter node has no choice but to default to 0 values (x=0, y=0, yaw=0), and when it receives new velocities from the odom messages, it adds up the variations of x, y and yaw to the current pose. You can interpret what you see in rviz as a variation of the pose of the robot, not as it's absolute pose. That's why the movements you see are correct, but the robot orientation is offset.

Note that if you use an IMU message as the single input of the ekf, then the pose of the robot would give the correct orientation. This is because the imu X, Y and Z coordinates in an IMU message are expressed in the robot frame: here Y' = 0 for a diff-drive robot.

Hope this helps! Sorry if I made mistakes whiting this answer, english is not my language :)

$\endgroup$
2
  • $\begingroup$ Hi arusso, thank you for your answer :) You are right that X, Y and Z are expressed in the odom frame, but my understanding is that the velocities are in the child frame, that is base_link in my case. From navigation.ros.org/setup_guides/odom/setup_odom.html: "The pose message provides the position and orientation of the robot relative to the frame specified in header.frame_id. The twist message gives the linear and angular velocity relative to the frame defined in child_frame_id." $\endgroup$ Nov 20, 2023 at 9:19
  • $\begingroup$ @SamuelLindgren if you update your question to show a sample /velocity_controller/odom when the robot is sliding sideways, that will clear things up. My immediate suspicion is as arusso suggested. If you've found that arusso's answer solves your problem, can you please accept the answer? $\endgroup$
    – automatom
    Feb 16 at 10:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.