0
$\begingroup$

Rosanswers logo

Hi to all, Hi Tom,

I'm using robot_localization and the navsat_transform_node to obtain the 3D position of my mobile robot. I'm using XSens IMU Mti-300 with magnetometer, a SICK laser, Husky A200 and C94-MP8 U-blox GPS with RTK correction.

The node runs fine and I receive no errors, but when I try to run the bag in RVIZ, I see that there are lot of jumps and that even the /odometry/filtered/local (blue arrows) is not accurate even if the robot is moving on a straight line.

In this bag file [16MB], the robot is moving on a straight line and as you can see the path is not accurate.

(The steering maneuver at the end is correct)

image description


image description

I think that the jumps on /odometry/gps (red arrows) and /odometry/filtered/global (green arrows) are caused by the GPS (it is in FIXED mode, but may be sometimes it loses the signal with the base station), but I can't explain why the /odometry/filtered/local is not accurate.

Can you give me some suggestions, please?

it is possible to set a priority on /odometry/filtered/local to obtain more accurate results with the GPS signal is not good?

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/vine.bag -k -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" /> 
  <node pkg="tf2_ros" type="static_transform_publisher" name="bl_laser" args="0 0 0.4 0 0 0 base_link laser" /> 
  <node pkg="tf2_ros" type="static_transform_publisher"  name="base_to_realsense" args="0 0 0 -1.5707963 0 -1.5707963 base_link realsense_frame" />
 
    <!-- 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="/ublox_gps/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>

EDIT 1 on 26-01-2018


/imu/data:

header: 
  seq: 37640
  stamp: 
    secs: 1514030312
    nsecs: 207535028
  frame_id: base_imu
orientation: 
  x: -0.0144539698958
  y: 0.00340300658718
  z: -0.259908914566
  w: -0.965518832207
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.0856303721666
  y: 0.0190725494176
  z: -0.00679668551311
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.515246808529
  y: -0.0915262028575
  z: 9.87729644775
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---
header: 
  seq: 37641
  stamp: 
    secs: 1514030312
    nsecs: 232536077
  frame_id: base_imu
orientation: 
  x: -0.0136555787176
  y: 0.00352110108361
  z: -0.260078012943
  w: -0.965484678745
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.0674997344613
  y: -0.00309981871396
  z: 0.0163492392749
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.577718853951
  y: 0.138521924615
  z: 10.4242248535
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

/ublox_gps/fix:

header: 
  seq: 510
  stamp: 
    secs: 1514030378
    nsecs: 4294860033
  frame_id: gps
status: 
  status: 2
  service: 3
latitude: 40.0601173
longitude: 18.3455565
altitude: 125.17
position_covariance: [0.00019600000000000002, 0.0, 0.0, 0.0, 0.00019600000000000002, 0.0, 0.0, 0.0, 0.00012099999999999999]
position_covariance_type: 2
---
header: 
  seq: 511
  stamp: 
    secs: 1514030379
    nsecs: 4294859926
  frame_id: gps
status: 
  status: 2
  service: 3
latitude: 40.0601214
longitude: 18.3455549
altitude: 125.154
position_covariance: [0.00019600000000000002, 0.0, 0.0, 0.0, 0.00019600000000000002, 0.0, 0.0, 0.0, 0.0001]
position_covariance_type: 2
---
header: 
  seq: 512
  stamp: 
    secs: 1514030380
    nsecs: 4294859818
  frame_id: gps
status: 
  status: 2
  service: 3
latitude: 40.0601255
longitude: 18.3455539
altitude: 125.158
position_covariance: [0.00019600000000000002, 0.0, 0.0, 0.0, 0.00019600000000000002, 0.0, 0.0, 0.0, 0.00012099999999999999]
position_covariance_type: 2
---

/husky_velocity_controller/odom:

header: 
  seq: 9706
  stamp: 
    secs: 1514030323
    nsecs: 276060293
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 12.6372744471
      y: 2.13529366866
      z: 0.0
    orientation: 
      x: -0.0
      y: 0.0
      z: 0.121363672306
      w: -0.9926081095
  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: 0.503785172839
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.000933553219127
  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]
---
header: 
  seq: 9707
  stamp: 
    secs: 1514030323
    nsecs: 374537970
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 12.6765768223
      y: 2.12551751486
      z: 0.0
    orientation: 
      x: -0.0
      y: 0.0
      z: 0.121827386262
      w: -0.992551302431
  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: 0.48587331925
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.00095820465276
  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]
---

/husky_velocity_controller/cmd_vel:

linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0
---
linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0
---
linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0
---
linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0
---
linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0
---

EDIT2 on JAN 29:


If I change the sign in the magnetic declination parameter in this way:

<param name="magnetic_declination_radians" value="-0.0684"/>

according to the declination value for my area: +3° 55' Declination is POSITIVE (EAST)

I obtain this path in RVIZ:

image description

which seems to be more accurate and it only includes a jumps of about 1.19 meters probably due to the GPS. Does it make any sense for you?


Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2017-12-23

Post score: 1


Original comments

Comment by Marcus Barnet on 2017-12-23:
UPDATE: I tried to play the bag without the /ublox_gps/fix topic and the path is more accurate even if there are still some strange jumps mostly during steering!

Comment by Tom Moore on 2018-01-26:
Please post sample input messages from every sensor.

Comment by Marcus Barnet on 2018-01-26:
Thank you, Tom! I edited my main question by adding the input from the IMU, the GPS and the odometry topic. The samples come from this bag file [16MB].

Comment by Tom Moore on 2018-01-26:
In what frame are you visualizing data in rviz? Remember that the "local" odom data is going to get transformed to the map frame by rviz before it gets displayed. This accounts for the jumpiness, but it doesn't account for the fact that they are not perfectly overlaid on each other.

Comment by Marcus Barnet on 2018-01-26:
I'm using the "map frame" as "Fixed Frame" in RVIZ.

Comment by Marcus Barnet on 2018-01-26:
I uploaded a short video about the first seconds of the test in RVIZ. It's very strange that the points are not aligned since the robot is only moving straight forward.

Comment by Tom Moore on 2018-01-26:
Can you add one sample output message for each? Also, try visualizing with the fixed_frame as odom. You should at least see the odom data no jump around. You're not manually publishing the map->odom transform somewhere, are you?

Comment by Tom Moore on 2018-01-26:
Oh! You are visualizing the wrong thing for the "local" data. You are visualizing the Husky raw odometry. Visualize the /odometry/filtered/local data.

Comment by Marcus Barnet on 2018-01-26:
If I set the Odom as fixed frame, I can see the correct path without jumps. I'm not publishing any transform manually, they are all included in the launch file. What do you mean with the sample output message for each?

Comment by Marcus Barnet on 2018-01-26:
Do you mean a single frame for each sensor at the same timestamp?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You are visualizing the wrong topic for the "local" data. Here's what I see when I visualize /odometry/filtered/local (green), /odometry/filtered/global (red), and /odometry/gps (blue):

rviz fixed_frame == map:

image description

rviz fixed_frame == odom:

image description

Both of those look completely correct to me. Your GPS is a bit noisy, but that's normal.

EDIT in response to comments:

From the wiki:

image description

Visualization of raw GPS tracks:

image description

EDIT 2 after more analysis:

I figured out why it's so much more jittery. You need to get rid of the 0.4 value for Z in your bl_gps static transform:

 <node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0.0 0 0 0 base_link gps" />

image description

That's a bug; I'll file a ticket. Thanks for the report!

Even with that fix, though, the direction of travel doesn't quite match the GPS. The robot has a tiny bit of lateral motion, which suggests something is not right with your IMU heading or the declination parameter.

EDIT 3 to answer more questions:

So you're saying the path is more accurate/in line with the GPS readings when you use the negative declination value (-0.0684)? If so, yes, that makes sense, and it's what I suggested in the comments. Your deviation from "true north" is 0.0684 radians clockwise:

image description

Clockwise rotation in an ENU frame is negative, hence the -0.0684 declination value. I realize r_l uses different standards than geographic services, but it's just so that the nodes handle rotations consistently.


Originally posted by Tom Moore with karma: 13689 on 2018-01-26

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by Marcus Barnet on 2018-01-26:
Thank you for your answer! I can't understand why the data are correct. I drive the robot along a straight line and so I expect to see a straight line like it happens when you set the odom as fixed frame. When I set the map as the fixed frame, I can't understand why the points drift on the left

Comment by Marcus Barnet on 2018-01-26:
Both local and global coordinates are coincident, but they drift and jump randomly. Why do you consider these data as correct? Shouldn't I see a straight line like it happens in the odom frame?

Comment by Tom Moore on 2018-01-26:
No, they are correct. The blue arrows are your GPS readings, and the filter is just updating itself accordingly. If you plot your raw GPS data using a tool like http://www.gpsvisualizer.com/ or the mapviz package, you will see that the raw GPS data jumps around in the exact manner you see.

Comment by Tom Moore on 2018-01-26:
Having said that, the navsat_transform_node output is a lot noisier than I'd expect. I'll look into it.

Comment by Marcus Barnet on 2018-01-26:
Yes, the GPS is not very accurate, but it does not have all the jumps as showed in RVIZ by the navsat_transform_node. I can't explain why since I was expecting to see only few jumps when the GPS module loses the fixed position.

Comment by Marcus Barnet on 2018-01-26:
How was you able to draw the GPS path so fast? When I use http://www.gpsvisualizer.com/, I always need to manually copy only few GPS data in a TXT file and then upload it on the website. Is there any faster way to do that?

Comment by Tom Moore on 2018-01-26:
rostopic echo gps_topic -p > out.txt, then edit the column headers for latitude and longitude to get rid of the field. bit, then upload. Really, we should use something like mapviz instead.

Comment by Tom Moore on 2018-01-26:
But I concede that the output of navsat_transform_node is not very good. I'll look into it.

Comment by Marcus Barnet on 2018-01-26:
I tried to use mapviz, but it doesn't work on my ROS Indigo. Honestly, I didn't find any usable documentation on it. I thought it needed only the gps/fix topic as input, but it seems to be more complicated than this.

Comment by Marcus Barnet on 2018-01-26:
I tried to update my navsat_transform_node and robot_localization, but I had no success. I still have the same behavior.

Comment by Tom Moore on 2018-01-26:
I think the sign may be wrong on your magnetic declination.

Comment by Marcus Barnet on 2018-01-26:
I checked again my magnetic declination and it says it is +3° 55' Declination is POSITIVE (EAST) which in radians should be 0.0684 and so it should be correct since in my code I put: <param name="magnetic_declination_radians" value="0.0640"/>

Comment by Tom Moore on 2018-01-29:
The node treats west declination (counter-clockwise) as positive, in keeping with the node being right-handed and in ENU frame.

Comment by Marcus Barnet on 2018-01-29:
Thank you, Tom, for your support. I tried to change the sign in my magnetic declination just to do a test and with this modification it seems that the path is more accurate. I included a screenshot in my first port. Does it make any sense for you?

Comment by Marcus Barnet on 2018-01-29:
Thank you a lot for all your support, Tom!

Comment by Tom Moore on 2018-01-29:
You're welcome!

Comment by mbudris on 2018-12-08:
This is the only place I was able to find that mentions that magnetic declination sign must be inverted. For a layman like me magnetic declination is what one can find on the internet or chart. It would be way more understandable if the parameter would be named magnetic declination correction.

Comment by mbudris on 2018-12-08:
And the documentation states it rather straight forward: Enter the magnetic declination for your location.

Comment by Tom Moore on 2019-01-09:
Agreed. Feel free to submit a PR to fix the documentation.

Comment by tseco on 2020-05-04:
Hi, I have some issues (https://answers.ros.org/question/347395/problem-with-gps-and-robot_localization-map-frame/) with the integration of a GPS-RTK data using two robot_localization instances one local and the other global and using navsat_trasnform_node in a similar way that you did. My question about your implementation is why your navsat_transform_node is subscrived to the odometry resultant from the global robot localization instead of the local robot localization. It should be better that I open a new question? Thanks in advance!

$\endgroup$

Your Answer

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