grzegorz.f-16's profile - activity

Hello there! I wan't to use ekf_localization_node to fuse data from odom and IMU in my (2,0) class robot, but I have problem to set proper parameters. At first I wanted to check how covariance matrices affect the result based only on odometry data. And I have problem with translation.

After starting all nodes, when I send linear velocity command it's fine. Also rotation works good. Problem occurs when I rotate robot from the starting pose and then say robot to move in the rotated direction. Frame base_link comming from ekf move in wrong direction. But the "raw" odometry from controler with encoders is good.

Could you explain me, what am I doing wrong? Its hard to explain so I attached the rosbag. Below is my code, and rosbag containing: /robot_0/odom, /robot_0/odom_filtered/ and /cmd_vel.

rosbag: https://drive.google.com/file/d/1CQanWBfs2-Z1P4hsT_xgxT77E0foy65c/view?usp=sharing

Here is video showing how it looks like: https://www.youtube.com/watch?v=NjQIzocDBrQ&feature=youtu.be

tf tree: https://imgur.com/Ir3Rhfe

My launch:

<launch>
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
      <rosparam command="load" file="$(find ekf_test)/params/ekf_rosbag_params.yaml" />

      <remap from="odometry/filtered" to="robot_0/odom_filtered"/>
    </node>

    <node name="controler_tf_publisher" type="pub_tf_from_odom_topic.py" pkg="ekf_test">
        <rosparam param="odom_topic">robot_0/odom</rosparam>
        <rosparam param="parent_frame_id">controler_odom</rosparam>
        <rosparam param="child_frame_id">controler_base_link</rosparam>
    </node>

    <node name="ekf_tf_publisher" type="pub_tf_from_odom_topic.py" pkg="ekf_test">
        <rosparam param="odom_topic">robot_0/odom_filtered</rosparam>
        <rosparam param="parent_frame_id">robot_0/odom</rosparam>
        <rosparam param="child_frame_id">robot_0/base_link</rosparam>
    </node>

    <node pkg="tf" type="static_transform_publisher" name="world_frame_publisher1" args="0 0 0 0 0 0 world robot_0/odom 100"/> 
    <node pkg="tf" type="static_transform_publisher" name="world_frame_publisher2" args="0 0 0 0 0 0 world controler_odom 100"/> 
</launch>

Example odom msg:

header: 
  seq: 761
  stamp: 
    secs: 1576944830
    nsecs: 664253217
  frame_id: "robot_0/odom"
child_frame_id: "robot_0/base_link"
pose: 
  pose: 
    position: 
      x: 0.971138656139
      y: -0.000453197862953
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.615555694251
      w: 0.788093387407
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
twist: 
  twist: 
    linear: 
      x: 0.000464193755761
      y: 0.00186030799523
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.00590989086777
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0 ...