0
$\begingroup$

Rosanswers logo

Hi, I am working with robot localization in ubuntu 16.04. I am using bags files with IMU and two GPS. However the IMU data don't give me info about the orientation, so I use mine /get_orientation node to obtain the orientation with the two GPS, getting the robot orientation.

The ekf outputs are working, i am not sure if fine but at least is giving some output. However the problem is the navsat_transform_node wich its output is all zero. So I am not sure where is the problem in that node.

I add the files:

fix_back(gps):

 header:    
   seq: 2733   
   stamp: 
        secs: 1455210306
        nsecs: 652417898   
   frame_id: "gps" 
status:    
   status: 1   
   service: 1 
latitude: 42.1819562283 
longitude: -8.73968366 
altitude: 384.671 
position_covariance: [1000000000000.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 16000000000000.0]
position_covariance_type: 0

/imu/data:

header: 
  seq: 145690
  stamp: 
    secs: 1455210324
    nsecs: 134030103
  frame_id: "base_imu"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
angular_velocity: 
  x: -0.0156264220278
  y: 0.0181547511189
  z: 0.0154273617187
angular_velocity_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
linear_acceleration: 
  x: 0.0415387526155
  y: -0.0178170949221
  z: 0.0885146409273
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

/get/orientation(topic with orientation wich is subscribed navsat node):

header: 
  seq: 1644
  stamp: 
    secs: 1455210302
    nsecs: 894764900
  frame_id: "base_imu"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.778295047494
  w: 0.627898733114
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

/odometry/gps: I think that maybe the problem is in de frame id

header: 
  seq: 2691
  stamp: 
    secs: 1455210292
    nsecs: 652157068
  frame_id: "odom"
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 16000000000000.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]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  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]

/odometry/filtered:

header: 
  seq: 51683
  stamp: 
    secs: 1577439407
    nsecs: 801655531
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 156563.13445
      y: -43035.340716
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.687894745035
      w: 0.725810457181
  covariance: [4.999999991019117e-10, 1.8213410266344878e-19, 0.0, 0.0, 0.0, 2.7155029258938172e-18, 1.8213410266344009e-19, 4.999999999627129e-10, 0.0, 0.0, 0.0, -4.734606596713057e-19, 0.0, 0.0, 4.99999999319995e-07, -1.4343813184271704e-13, -5.537703876135997e-14, 0.0, 0.0, 0.0, -1.4343813184271712e-13, 4.999532936965985e-07, -1.1681062864961878e-11, 0.0, 0.0, 0.0, -5.537703876135998e-14, -1.168106286496188e-11, 4.999790403984765e-07, 0.0, 2.7155029258938396e-18, -4.734606596713306e-19, 0.0, 0.0, 0.0, 4.999999685207372e-10]
twist: 
  twist: 
    linear: 
      x: 376.337918185
      y: -1026.94096739
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.160520859566
  covariance: [3.6023855883949554, 1.204648214979867, 0.0, 0.0, 0.0, 0.0017902513769453227, 1.2046482149798612, 0.7431464414456158, 0.0, 0.0, 0.0, 0.000144190040518795, 0.0, 0.0, 4.999876641741591e-07, -2.8393491075641632e-15, -1.1267194434469995e-15, 0.0, 0.0, 0.0, -2.839349107564164e-15, 4.999493611311244e-07, -1.188802486830638e-13, 0.0, 0.0, 0.0, -1.126719443447e-15, -1.1888024868306388e-13, 4.9994961353661e-07, 0.0, 0.001790251376945311, 0.00014419004051858424, 0.0, 0.0, 0.0, 0.03574059011903692]

Now configuration files:

ekf_template.yaml:

frequency: 30

sensor_timeout: 0.1

two_d_mode: true #false   zona plana

transform_time_offset: 0.0

transform_timeout: 0.0

print_diagnostics: true

debug: false

debug_out_file: /path/to/debug/file.txt

publish_tf: true

publish_acceleration: false 

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom           # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom          # Defaults to the value of odom_frame if unspecified

odom0: /get_orientation/odometry    #position + orientation
odom0_config: [true,  true,  true,
               true,  true,  true,
               false,  false,  false,
               false,  false,  false,
               false,  false,  false]
odom0_differential: false
odom0_relative: true 
odom0_nodelay: false
odom0_queue_size: 5


odom1: /odometry/gps                #gps in UTM
odom1_config: [true,  true,  true,
               false,  false,  true,
               false,  false,  false,
               false,  false,  false,
               false,  false,  false]
odom1_nodelay: false
odom1_differential: false
odom1_relative: true
odom1_queue_size: 5


pose0: /get_orientation/pose_covariance       #position + orientation
pose0_config: [true,  true,  true,
               true,  true,  true,    
              false,  false,  false,
              false,  false,  false,
              false,  false,  false]
pose0_nodelay: false
pose0_differential: false
pose0_relative: true
pose0_queue_size: 5


imu0: /imu/data                      #imu data
imu0_config: [false,  false,  false,
              true,  true,  true,
              false,  false,  false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5

imu1: /get_orientation/imu_orientation  #orientation + imu data
imu1_config: [false,  false,  false,
              true,  true,  true,
              false,  false,  false,
              false,  false,  false,
              false,  false,  false]
imu1_nodelay: false
imu1_differential: false
imu1_relative: true
imu1_queue_size: 5

imu0_remove_gravitational_acceleration: false

use_control: false
stamped_control: true #false
control_timeout: 0.5
control_config: [false, false, false, false, false, false]
acceleration_limits: [100.0, 0.0, 0.0, 0.0, 0.0, 1.0]
deceleration_limits: [100.0, 0.0, 0.0, 0.0, 0.0, 1.0]
acceleration_gains: [0.2, 0.0, 0.0, 0.0, 0.0, 0.5]

deceleration_gains: [0.2, 0.0, 0.0, 0.0, 0.0, 0.5]

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]

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]

navsat_transform_template.yaml:

frequency: 30

delay: 3.0

magnetic_declination_radians: 0

yaw_offset: 0.0

zero_altitude: false

broadcast_utm_transform: false

broadcast_utm_transform_as_parent_frame: false

publish_filtered_gps: true #false

use_odometry_yaw: false

wait_for_datum: false

datum: [55.944904, -3.186693, 0.0, map, base_link]

The launch of both nodes:

ekf_tamplate.launch:

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

  </node>

  <!-- TF'S -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="back_broadcaster"
    args="-40.0 0.0 0.0 0.0 0.0 0.0 1.0 base_link gps_back" respawn="true" />

  <node pkg="tf2_ros" type="static_transform_publisher" name="imu_broadcaster"
    args="30.0 0.0 0.0 0.0 0.0 0.0 1.0 base_link base_imu" respawn="true" />

  <node pkg="tf2_ros" type="static_transform_publisher" name="front_broadcaster"
    args="40.0 0.0 0.0 0.0 0.0 0.0 1.0 base_link gps_front" respawn="true" />

</launch>

navsat_transform_template.launch

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

  </node>
</launch>

Here I uploaded the picture of rqt_graph wich maybe give some info to help.

https://www.dropbox.com/s/8yyi8ahkrumyd5x/rosgraph.png?dl=0

I think that this is the most important info to show you, if somebody need some additional info, ask me for them. Thank you to everybody.


Originally posted by sergiootero on ROS Answers with karma: 26 on 2019-12-27

Post score: 0


Original comments

Comment by Tom Moore on 2020-01-06:
Where are you telling navsat_transform_node to listen to your new orientation topic? I don't see a remap.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hi Tom, I think i found the problem and it was that I had to change a frame_id I wasn´t see. Thank you for your response, I'll continue trying


Originally posted by sergiootero with karma: 26 on 2020-01-13

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Briant on 2021-01-18:
Hi,Sergiootero,I just met the same problem,how did you solve this,appreciate it if you could reply when you see it

$\endgroup$

Your Answer

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