0
$\begingroup$

Rosanswers logo

Hello everyone I am a beginner with ROS, and want to test things with the ekf_localization_node (I use ROS Jade and Ubuntu 14.04)

So, I simulated a 2-wheeled robot in a 2D environment in Matlab. Typically, a robot that goes away from its charging station to reach a target. Now, I want to test the ekf_localization node to implement it soon on hardware(mobile robot with wheel encoders and gyro output signals), after I know it is at least possible

I extracted log files from my simulation (equivalent to odometry): x velocity, y velocity and yaw velocity.

image:![simulated path](/C:/Users/Karim Kouki/Desktop/traj1.png "indoor trajectory of the robot")

--> it was converted into a geometry_msgs/TwistWithCovarianceStamped to test it with the ekf_localization_node. Here is my launchfile

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

 
  <param name="frequency" value="20"/>

 
  <param name="sensor_timeout" value="0.06"/>


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

 
  
  <!-- Defaults to "odom" if unspecified -->
  <param name="odom_frame" value="odom"/>
  <!-- Defaults to "base_link" if unspecified -->
  <param name="base_link_frame" value="base_link"/>
  <!-- Defaults to the value of "odom_frame" if unspecified -->
  <param name="world_frame" value="odom"/>

 <param name="twist0" value="/twist_input1"/> 
  <param name="transform_time_offset" value="0.0"/>

  
     
  <rosparam param="twist0_config">[false, false, false,
                                  false, false, false,
                                  true,  true, false,
                                  false, false, true,
                                  false, false, false]</rosparam>


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

  <param name="twist0_relative" value="true"/>
 
  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->

  <param name="twist0_queue_size" value="2"/>
  
  <param name="debug"           value="false"/>
  <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  
  <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 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.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

  
       <rosparam param="initial_estimate_covariance">[1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,     1e-9,  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,     1e-9, 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,    1e-9]</rosparam>




</node>

here you will find one example of a TwistWithCovarianceStamped message published over the topic /twist_input1 as input for the ekf_localization_node

  header: 
  seq: 534
  stamp: 
    secs: 1438178264
    nsecs: 22708520
  frame_id: odom
twist: 
  twist: 
    linear: 
      x: 1.641866
      y: 1.472035
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.2307692
  covariance: [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.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, 0.0, 0.0, 0.0, 0.0, 0.0]

My problem is that I have no output of ekf_localization_node (even if I rostopic echo /odometry/filtered) after I rostopic echo /diagnostics,I am warned that yaw is not measured which could roughly "lead to unbounded error".

Actually, the tutorial suggested that with odometry input, x velocity, y velocity and yaw velocity should be the input data... I am quite confused now.

One other thing: I have no idea what to do for the covariance input matrix in the TwistWithCovarianceStamped message. Is it even useful?

Any idea what I did wrong? Could you please give me some pieces of advice?

I am really looking forward to your answers

EDIT: while removing the commentsof the launchfile I also removed the line param name="twist0" value="/twist_input1"(for the ros answers post), It was added in the edit


Originally posted by Cookie32 on ROS Answers with karma: 3 on 2015-07-29

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Your primary culprit is that you never tell the EKF what twist topic to look for. You tell it the configuration for twist0, but it needs to know what topic to listen on. You need this line in there:

<param name="twist0" value="/name/of/your/twist/topic"/>

Just replace the value with the name of whatever topic you're trying to use.

I need to fix that diagnostic message. It's really only a problem if you have infrequent absolute position measurements and are only measuring orientation differentially. You should be able to disregard it.

Also, the differential and relative parameters are meaningless for twist messages. They only apply to pose data.

EDIT in response to twist message post:

Is your twist message producing body-frame velocities (so, for example, if you drive forward, it's always +X), or is it producing world (odom) frame velocities (such that if you drive forward, you get +X, but then if you turn left and drive forward again, you get +Y velocity)? The issue is that your frame_id is set to odom, whereas velocities should be reported in base_link. All velocities that are fed into the EKF are transformed into base_link before being fused. Since the EKF itself is responsible for producing the odom->base_link transform, and it hasn't produced any output yet, no transform is available to work with your message.

Long story short: make sure your twist message is reported in the base_link (body) frame. See REP-105 for information about coordinate frames in ROS. Then change the frame_id in the header to base_link and try again.


Originally posted by Tom Moore with karma: 13689 on 2015-07-29

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Cookie32 on 2015-07-29:
Thanks for your answer. I edited the question (sorry for my mistake) and set the differential and relative parameters to false. It still doesn't work. Any other idea?

Comment by Tom Moore on 2015-07-29:
Do rostopic echo /twist_input1 and post one message in your question.

Oh, and you can just remove the differential and relative parameters for the twist.

Comment by Cookie32 on 2015-08-04:
Thanks, now it does work, I have outuput to my Kalman filter with a TwistWithCovarianceStamped message as input. Now, I'll consider a more complex model for two wheeled robot navigation thanks to this ekf_localization_node.

$\endgroup$

Your Answer

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