0
$\begingroup$

Rosanswers logo

Hi to all, hi @tom-moore

I finally solved all the problems about my system configuration and now I'm able to integrate my GPS data in ekf_localization_node by using the navsat_transform_node. I'm using two ekf_localization_node instances: the first which includes only continuous data (/imu/data, /husky_velocity_controller/odom) and the second which includes also the gps data (/imu/data, /husky_velocity_controller/odom, /fix).

The /odometry/gps topic publishes non-zero values, so I think it is working fine. The problem is that I obtain strange results in RVIZ and I can't understand the reason. It seems that the localization is not working properly, but I can't understand the reason.

In this test, the robot is moving forward, but RVIZ is showing random behavior.

This is my OLD recorded bag file, it contains only the related topics.


**EDIT 4 - May 04 2017 **

rosrun xsens_driver mtnode.py _frame_id:="base_imu"
[WARN] [WallTime: 1493887014.830545] Cannot find value for parameter: ~device, assigning default: auto
[WARN] [WallTime: 1493887014.832072] Cannot find value for parameter: ~baudrate, assigning default: 0
[WARN] [WallTime: 1493887014.833512] Cannot find value for parameter: ~timeout, assigning default: 0.002
[INFO] [WallTime: 1493887014.867422] Detected MT device on port /dev/ttyUSB0 @ 115200 bps
[INFO] [WallTime: 1493887014.867611] MT node interface: /dev/ttyUSB0 at 115200 bd.
[INFO] [WallTime: 1493887014.877234] Found parameter: ~frame_id, value: base_imu
[WARN] [WallTime: 1493887014.878095] Cannot find value for parameter: ~frame_local, assigning default: ENU

I used the compass on my Android device in order to align the IMU so the readings are not accurate at 100%, but they should give the idea about the output I get:

    **NORTH**: 
    X:  0.00150562881026,
    Y: 0.00796461943537, 
    Z: -0.605336606503, 
    W: 0.795928359032
    Degree: -74 degrees 

 

**EST**: 
X:  -0.00401881383732
Y: 0.00927101075649
Z: -0.929714620113
W: -0.368142217398
Degree: +136 degrees

**SOUTH**:
X:  0.00913885235786
Y: -0.0002562062582
Z: -0.989742994308
W: -0.142566725612
Degree: +163 degrees

**WEST**:
X:  0.00891534984112
Y: -0.00214970414527
Z: -0.877696692944
W: 0.479128926992
Degree: -122 degrees


----------

**NORTH**: 
X:  0.00407045986503
Y: 0.0111895436421
Z: 0.44247585535
W: 0.896631300449
Degree: +52 degrees  


**WEST**: 
X:  -0.00214476278052
Y: 0.0111895436421
Z: -0.225669115782
W: 0.974137365818
Degree: -26 degrees


**SOUTH**:
X: 0.0114099625498
Y: 0.00554236397147
Z:  -0.679173827171
W: 0.733830630779
Degree: -85 degrees  


**EST**: 
X: 0.0114099625498
Y: 0.00554236397147
Z:  0.0801176950336
W: 0.996705293655
Degree: +9 degrees  

Output by using the IMU software:

North: -108 degrees
WEST: -40 degrees
SOUTH: +38 degrees
EST: +135 degrees

END EDIT 4


EDIT3 - May 01 2017

This screenshot has been made by using this bag file and this launch file.

During this test, the robot was commanded to follow a straight path.

I expect to see the odometry/gps and the /husky_velocity_controller/odom to be parallel (or coincident) to the /odometry/filtered/global and /odometry/filtered/local output.

Fixed frame: map

Pink: /husky_velocity_controller/odom

Red: /odometry/gps

Green: /odometry/filtered/global

Blue: /odometry/filtered/local

Figure 1: results with odom and map frame changed like in this launch file.

image description

Figure 2: results with the original configuration for odom and map frame like in this launch file.

image description

The output of rostopic echo /imu/data for the orientation when it faces EAST is:

X:0, Y:0, Z:-0.63, W:0.77

for this reason, I added: <param name="yaw_offset" value="1.570796327"/>

in the navsat_transform_node in order to make the IMU conform to the standard.


This is the launch file (I added to change the world_frame from odom to map to make it works):

<!-- Test launch file for two EKF instances and one navsat_transform instance -->

<launch>
 
 <rosparam command="load" file="$(find husky_control)/config/control.yaml" />

  <node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="husky_joint_publisher husky_velocity_controller --shutdown-timeout 3"/>



 <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="--clock /home/rocco/Desktop/gps/gps1.bag -d 3"/>
   
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 base_link base_imu" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0.4 0 0 0 base_link gps" /> 
 
    <!-- Local (odom) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
      <param name="frequency" value="10"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="map"/>

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

      <param name="odom0" value="/husky_velocity_controller/odom"/>
      <param name="imu0" value="/imu/data"/>

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

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

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <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="odom0_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/local"/>
    </node>

    <!-- Global (map) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true">
      <param name="frequency" value="10"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

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

      <param name="odom0" value="/husky_velocity_controller/odom"/>
      <param name="odom1" value="/odometry/gps"/>
      <param name="imu0" value="/imu/data"/>

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

      <rosparam param="odom1_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,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="odom1_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="odom1_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="odom0_queue_size" value="10"/>
      <param name="odom1_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
   </node>

   <!-- navsat_transform -->
   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
      <param name="frequency" value="10"/>
      <param name="delay" value="3"/>
      <param name="magnetic_declination_radians" value="0.0640"/>
      <param name="yaw_offset" value="1.570796327"/>
      <param name="zero_altitude" value="true"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="false"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
      <remap from="/gps/fix" to="/fix"/>
      <remap from="/imu/data" to="/imu/data"/>
    </node>

 <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/>

  <node pkg="twist_mux" type="twist_mux" name="twist_mux">
    <rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
    <remap from="cmd_vel_out" to="husky_velocity_controller/cmd_vel"/>
  </node>

 </launch>

END OF EDIT3


EDIT2:

I obtained more accurate GPS data (double precision), so now there should be no jumps on GPS coordinates. I tried to draw them and they look very accurate.

I moved the robot on a straight line and I recorded a -- NEW -- new bag file.

The problem is that I'm still having the same problem as showed in this YouTube video: the odometry data are not aligned. @Tom-moore: can the problem be related to my IMU topic (/imu/data)? The launch file i'm using is this one.

I hope you can help me with my problem, I really do not know how can i solve it! :(

This is a photo which shows how the sensors are placed on the robot's frame. image description

END OF EDIT2



EDIT1:

This is the related video.

The robot should move straight forward for few meters as in the real test, but in RVIZ the /odometry/filtered seems to drift in a random way without any reason. Moreover, the odometry from encoders is totally wrong (is it normal?).

/odometry/gps (red arrows in RVIZ) is not overlapped on /odometry/local, is it normal?

END OF EDIT1


This is the screenshot from RVIZ:

image description

This is my launch file:

 <!-- Test launch file for two EKF instances and one navsat_transform instance -->

<launch>
 
 <rosparam command="load" file="$(find husky_control)/config/control.yaml" />

  <node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="husky_joint_publisher husky_velocity_controller --shutdown-timeout 3"/>



 <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="--clock /home/rocco/Desktop/gps/gps1.bag -d 3"/>
   
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 base_link base_imu" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0.4 0 0 0 base_link gps" /> 
 
    <!-- Local (odom) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
      <param name="frequency" value="10"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

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

      <param name="odom0" value="/husky_velocity_controller/odom"/>
      <param name="imu0" value="/imu/data"/>

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

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

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <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="odom0_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/local"/>
    </node>

    <!-- Global (map) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true">
      <param name="frequency" value="10"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="map"/>

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

      <param name="odom0" value="/husky_velocity_controller/odom"/>
      <param name="odom1" value="/odometry/gps"/>
      <param name="imu0" value="/imu/data"/>

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

      <rosparam param="odom1_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,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="odom1_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="odom1_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="odom0_queue_size" value="10"/>
      <param name="odom1_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
   </node>

   <!-- navsat_transform -->
   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
      <param name="frequency" value="10"/>
      <param name="delay" value="3"/>
      <param name="magnetic_declination_radians" value="0.0640"/>
      <param name="yaw_offset" value="1.570796327"/>
      <param name="zero_altitude" value="true"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="false"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
      <remap from="/gps/fix" to="/fix"/>
      <remap from="/imu/data" to="/imu/data"/>
    </node>

 <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/>

  <node pkg="twist_mux" type="twist_mux" name="twist_mux">
    <rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
    <remap from="cmd_vel_out" to="husky_velocity_controller/cmd_vel"/>
  </node>

 </launch>

This is my /imu/data topic:

  header: 
  seq: 39152
  stamp: 
    secs: 1491907066
    nsecs: 340787887
  frame_id: base_imu
orientation: 
  x: 0.000862320477609
  y: 0.00599063839763
  z: -0.841121971607
  w: 0.540811538696
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.00405800947919
  y: -0.0405332967639
  z: 0.0163467507809
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: -0.0645809024572
  y: -0.27205106616
  z: 9.68260002136
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---

This is my /fix topic:

header: 
  seq: 196
  stamp: 
    secs: 1489494103
    nsecs: 299462080
  frame_id: gps
status: 
  status: 2
  service: 1
latitude: 40.0600583333
longitude: 18.346335
altitude: 121.65
position_covariance: [1.2100000000000002, 0.0, 0.0, 0.0, 1.2100000000000002, 0.0, 0.0, 0.0, 4.840000000000001]
position_covariance_type: 1

EDIT1:

This is my /husky_velocity_controller/odom topic:

`header: 
  seq: 662
  stamp: 
    secs: 1490100346
    nsecs: 403310059
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: -0.620593001088
      y: 18.9989047922
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.782032919542
      w: 0.623237124017
  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, 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, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03]
twist: 
  twist: 
    linear: 
      x: 1.16217799273e-15
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -2.14411821631e-16
  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, 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, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03]`

Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2017-03-21

Post score: 1


Original comments

Comment by Tom Moore on 2017-04-11:
Can you please be more descriptive about what exactly is not right about the behavior? Maybe post a video, and say what you are expecting to see vs. what you are seeing? Apologies, it's just that viewing bag files can be very time-consuming.

Comment by Marcus Barnet on 2017-04-11:
Thanks for your help. I upload a video on Youtube. In the real test, the robot was moving straight forward for few meters while on RVIZ I see that it is drifting randomly like if there was an error on the odometry calculation. I can't explain why.

Comment by Tom Moore on 2017-05-01:
Pleases define "not working". Produce an rviz image (not video) like I did below. Tell me what each color represents, and please tell me what the "Fixed Frame" is in rviz.

Comment by Marcus Barnet on 2017-05-01:
I added the details as a new answer in order to make it readable.

Comment by Tom Moore on 2017-05-01:
Much clearer, and I have answers for you, but can you first make the view orthographic for me (TopDownOrtho setting under Views)? It will help a bit.

Comment by Tom Moore on 2017-05-01:
I also note that there are two sets of green arrows: one aligns precisely with the blue arrows (they are "mixed" together), and the other is slightly offset.

Comment by Marcus Barnet on 2017-05-01:
I edited the answer in order to add the TopDownOrtho view. /odometry/filtered/global is affected by some jumps during the execution in RVIZ and this results in the green arrows with the offset close to the green/blue mixed arrows.

Comment by Marcus Barnet on 2017-05-01:
If you watch this video you can see the jumps of the /odometry/filtered/global with the green arrows.

Comment by Marcus Barnet on 2017-05-01:
I deleted my answer and I added it in EDIT3 inside my first message. I added the time of the EDIT to make the first topic more readable.

Comment by Tom Moore on 2017-05-01:
Oh, one thing: put your map and odom frames back. The map frame EKF should have the GPS data.

Comment by Marcus Barnet on 2017-05-01:
Thanks for your answer! Now it's more clear to me. Just a question: with this configuration, if I use move_base to set some GPS waypoints, will the robot be able to exactly move over the GPS coodinates or will they have some offsets as in RVIZ?

Comment by Marcus Barnet on 2017-05-01:
I added figure 2 to my main message in order to show you that if I put back the odom and map frames, the result is not correct and it do not reflect the real path.

Comment by Tom Moore on 2017-05-04:
Then there's still an issue with your IMU. The GPS data should be fused in the map-frame EKF.

Comment by Marcus Barnet on 2017-05-04:
I'm really desperate to hear this. Can you suggest me how can I definitely check if the IMU is OK, please? Is there any test I can do? If I put world_frame = map_frame in the global EKF, it gives me incorrect results for sure.

Comment by Tom Moore on 2017-05-04:
You didn't acceidentally swap Z and W for those readings, right? You can see for yourself that they are not correct. Most ENU IMUs would read approximately 0 at north, +90 at west, +180 or -180 at south, and +270 or -90 at east.

Comment by Tom Moore on 2017-05-04:
In any case, you can see that your IMU data is wrong. North and east are separated by either 210 degrees or 150 degrees, and it should only be 90. West is showing as more negative than your north, which is the incorrect sign. Whatever the reason, you need to fix your IMU data.

Comment by Marcus Barnet on 2017-05-04:
I added the values for both clockwise and counter-clockwise rotations. For sure, there is something wrong with my IMU but I can't understand what it is. I'm going to take the same measurements with the official IMU software and I will compare them with the output of /imu/data topic.

Comment by Marcus Barnet on 2017-05-04:
I added the values from the IMU software, they are totally different from what I get from the /imu/data topic. If we suppose that the data from the software are correct, I can't see any sense..

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

image description

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

image description

The second image is in the map frame:

image description

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

EDIT in response to updates:

I generated the images above based on the last bag and launch file you produced. Note that the odom and map EKF instances are consistent. The only thing that is misbehaving is the location of the GPS odometry points. This is because your IMU itself or the settings you used in navsat_transform_node were wrong/not conformant. In other words, everything about that launch file was absolutely fine. The only thing wrong was the IMU data. You don't need to modify the launch configurations or tweak anything else from the launch file that I used. You just need to get your IMU to conform.

EDIT2 in response to updates:

That all looks correct to me. Explanations:

Why the husky wheel odometry doesn't match This is because you are integrating velocities in the EKF. The EKF starts at (0, 0) and integrates the Husky's velocity data. The Husky's wheel odometry will not have started at (0, 0), and isn't using an IMU, so the heading won't agree either. As long as they have the same general "shape," it's fine. In other words, it's completely normal for the wheel odometry to not agree with the EKF output when you are only fusing velocities.

Why the GPS odometry doesn't line up perfectly First, note that the direction and path shape do match for the GPS odometry data, so I think you've got the heading sorted out nicely. I think it looks pretty good; that static offset could just be error from the GPS itself. I would plot the raw lat/longs using Google Earth and the GPS Visualizer website.

Note that the GPS poses have no heading, so they are all facing a heading of 0.

EDIT3

Your IMU data is still not correct. Here is a plot of your GPS tracks:

image description

The yellow pin is your start location. You clearly moved in a north-west trajectory, yet here is a sample of your IMU data:

header: 
  seq: 38723
  stamp: 
    secs: 1491907055
    nsecs: 615744113
  frame_id: base_imu
orientation: 
  x: -0.00451018242165
  y: 0.00442147208378
  z: -0.858403027058
  w: 0.512937366962
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.00197708723135
  y: -0.0037388484925
  z: -0.000131436216179
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 0.0753428861499
  y: -0.147220656276
  z: 9.80377960205
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

That orientation corresponds to a yaw of -114.061 degrees. That is clearly not correct. If your IMU reported 0 facing east, then I would expect its heading to be closer to **+**114 degrees in that image. If it reported 0 facing north, it would report something like **+**25 degrees. Your configuration has a yaw offset of pi/2, so that means your IMU should report 0 facing north. Are you certain that the IMU is reporting data in ENU, and not NED?

Your IMU heading should increase as you rotate counter-clockwise. I would do the following:

  1. Make your IMU face north (i.e., the X-axis on the IMU drawing should face north). Take a reading using the ROS driver output (the /imu/data message). Convert it to Euler angles here. Write down the yaw.
  2. Make your IMU face west. Again, convert the quaternion to Euler angles and write down the yaw.
  3. Make your IMU face south. Convert and write down.
  4. Make your IMU face east. Convert and write down.

Originally posted by Tom Moore with karma: 13689 on 2017-04-11

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by Marcus Barnet on 2017-04-12:
Thanks for your help, Tom! I fixed my GPS problem, so now there are no jumps. I added EDIT2 on my first message with a new video. I did another test, but I'm still having the same results even if the GPS is more accurate and I also changed the IMU with another one and I placed it better.

Comment by Marcus Barnet on 2017-04-12:
I performed this new test with another IMU (XSENS mti-300) which is better than the previous one and now the new IMU topic is /imu/data. This is my launch file, when I run it I have no errors about tf or other stuff, but I still have the odom problem.

Comment by Marcus Barnet on 2017-04-18:
I added a photo which shows how the sensors (GPS and IMU) are placed on the robot. Can this be a problem related to the tf? The second edit provides accurate GPS data, but the problem is still there.

Comment by Marcus Barnet on 2017-04-23:
If I remove the GPS, then the robot seems to move correctly in RVIZ even if the result of /husky_velocity_controller/odom is not aligned with the local and global results. I can't understand why this happens.

Comment by Marcus Barnet on 2017-04-30:
I'm still struggling to find a solution to my problem. I hope you can help me, Tom! :) I can't obtain a good result even if the GPS is accurate now.

Comment by Marcus Barnet on 2017-04-30:
I compared the X and Y coordinates between /odometry/filtered/local, /odometry/filtered/global and /odometry/gps and they are all different! is it a normal behavior?

Comment by Tom Moore on 2017-05-01:
Yes, it's normal. The first is in the odom frame and the second is in the map frame. When you display them (the first two) in rviz, they ought to align, however, because rviz is doing the transform. /odometry/gps ought to be near /odometry/filtered/global, but not exactly the same.

Comment by Tom Moore on 2017-05-01:
If /odometry/gps moves in a very different direction to /odometry/filtered/global, then you have something wrong with your IMU setup. Make sure it adheres to the standards in the wiki.

Comment by Marcus Barnet on 2017-05-01:
Thank you, Tom, for your support! I checked the standard, the pitch angle is positive when the IMU rotate on the left from "UP" to "DOWN", roll is negative when IMU rotate on the left from "UP" to "DOWN". The X axis of the IMU point towards the front bumper of the robot.

Comment by Marcus Barnet on 2017-05-01:
When the IMU points to EAST, the quaternion is: X:0, Y:0, Z:-0.63, W:0.77 So the YAW offset should be 1.570796327 and my IMU should adheres to the standard, is it correct?

Comment by Marcus Barnet on 2017-05-01:
Do you think it can be a problem related to the map and odom frames? I changed their relation in this new launch file, and now in this RVIZ video /odometry/gps moves in the same direction of /odometry/filtered/global even if it is always orthogonal to it.

Comment by Marcus Barnet on 2017-05-01:
Tom, I've read your updates, thank you. Did you checked my previous messages abou the IMU? From the quaternion, I think that it is correct to use the yaw_offset correction as I'm doing and it seems that the IMU adheres to the standard as requested. Then, why is it not working?

Comment by Marcus Barnet on 2017-05-01:
Cont'd: please keep in mind that the last launch file contains a different odom-map reference. What do you suggest in order to be sure that the IMU is working correctly? I'm pretty sure it reports 0° when it faces EAST with the offset correction.

Comment by Marcus Barnet on 2017-05-01:
If i do not change the odom and map frame, the results are not correct. Please see figure 2 in my main message. The path is totally wrong.

Comment by Marcus Barnet on 2017-05-09:
@Tom-moore: thank you for your support! I fixed my IMU as you suggested and now it seems to work fine! If you can, have a look at my bag file and my launch file. It seems OK, now!

Comment by Marcus Barnet on 2017-05-09:
If it is OK for you, I will mark the question as solved.

Comment by Marcus Barnet on 2017-05-11:
Tom, can i ask you what kind of tool do you use to display GPS patch starting the the /fix topic? Now, I have to manually copy&paste the GPs coords from the /fix output to a text file in order to use it on GPS Visualizer. Is there any faster way?

$\endgroup$

Your Answer

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