Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Xsens MTi-G 710 and robot_localization

Hey guys, I am trying to implement Xsens INS device with robot_localization package that i could make robot navigate by GPS coordinates. However, i cannot get any data from navsat_transform node neither from ekf_node in the robot_localization package. I did find few suggestions what to do but it seems other people are more advanced than me so i got stuck at this point.

<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value="0.36878808"/>
  <param name="zero_altitude" value="false"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="broadcast_utm_transform" value="true"/>
  <param name="wait_for_datum" value="false"/>

  <remap from="/gps/fix" to="/fix"/>
</node>
<node pkg="robot_localization" type="ekf_localization_node"  name="ekf_localization_node" clear_params="true">

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

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="2"/>
  <param name="two_d_mode" value="true"/>

  <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"/>

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

  <param name="imu0_differential" value="false"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>

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


                        false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

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

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


    </node>
    </launch>

I have to mention that i do not have odometry atm, as robot has no wheel encoders. Also i attach the configuration of the Xsens INS sensor.

rosrun xsens_driver mtdevice.py --configure=pl400fe,pa400fe,oq400fe,aa400fe,wr400fe,gd4fe --output-settings=tqMAG

For this sensor i use ethzasl_xsens_driver package as it seems to be properly working with the sensor. What I am trying to achieve this moment is to get data from robot_localization package topics but nothing is being subscribed even though they subscribe to Xsens published topics /fix and /imu/data. Please let me know what I am doing wrong and what extra info i should attach.

Xsens MTi-G 710 and robot_localization

Hey guys, I am trying to implement Xsens INS device with robot_localization package that i could make robot navigate by GPS coordinates. However, i cannot get any data from navsat_transform node neither from ekf_node in the robot_localization package. I did find few suggestions what to do but it seems other people are more advanced than me so i got stuck at this point.

<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value="0.36878808"/>
  <param name="zero_altitude" value="false"/>
  <param name="publish_filtered_gps" value="true"/>
  <param name="broadcast_utm_transform" value="true"/>
  <param name="wait_for_datum" value="false"/>

  <remap from="/gps/fix" to="/fix"/>
</node>
<node pkg="robot_localization" type="ekf_localization_node"  name="ekf_localization_node" clear_params="true">

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

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="2"/>
  <param name="two_d_mode" value="true"/>

  <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"/>

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

  <param name="imu0_differential" value="false"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>

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


                        false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

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

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


    </node>
    </launch>

I have to mention that i do not have odometry atm, as robot has no wheel encoders. Also i attach the configuration of the Xsens INS sensor.

rosrun xsens_driver mtdevice.py --configure=pl400fe,pa400fe,oq400fe,aa400fe,wr400fe,gd4fe --output-settings=tqMAG

For this sensor i use ethzasl_xsens_driver package as it seems to be properly working with the sensor. What I am trying to achieve this moment is to get data from robot_localization package topics but nothing is being subscribed even though they subscribe to Xsens published topics /fix and /imu/data. Please let me know what I am doing wrong and what extra info i should attach.

EDIT

This is the following data that i receive when echo'ing. From topic /fix

header: 
  seq: 10809
  stamp: 
    secs: 1488376624
    nsecs: 581118106
  frame_id: /imu
status: 
  status: 0
  service: 0
latitude: 57.0143623352
longitude: 9.98601722717
altitude: 62.4811706543
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---

From topic /imu/data

header: 
  seq: 1592301
  stamp: 
    secs: 1488380578
    nsecs: 149617910
  frame_id: /imu
orientation: 
  x: -0.0012584940996
  y: -0.0031715943478
  z: -0.214321956038
  w: -0.976757109165
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.000515580235515
  y: 0.00211000442505
  z: 0.000423193065217
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.035540368408
  y: -0.0115282256156
  z: 9.81090068817
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---

And when launching navsat_transform.launch i only get the INFO status

 INFO] [1488380635.870630401]: Datum (latitude, longitude, altitude) is (57.014286, 9.985973, 78.685829)
[ INFO] [1488380635.870718003]: Datum UTM coordinate is (559871.633204, 6319408.306693)
[ INFO] [1488380635.970500574]: Datum (latitude, longitude, altitude) is (57.014286, 9.985973, 78.685822)
[ INFO] [1488380635.970558908]: Datum UTM coordinate is (559871.633204, 6319408.306693)

However, when i echo /odometry/filtered , /odometry/gps or /gps/filtered i receive no data stream at all.

Xsens MTi-G 710 and robot_localization

EDIT I am restructurizing the question because i made some progress but still stuck at one problem.

Hey guys, I am trying to implement Xsens INS device with robot_localization package that i could make robot navigate by GPS coordinates. However, i cannot get any data from navsat_transform node neither from ekf_node in the robot_localization package. I did find few suggestions what to do but it seems other people are more advanced than me so i got stuck at this point.

 <launch>
<!-- Parameters setting of the node: ekf_localization_node -->
  <node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link base_imu 20"/>
  <node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link imu 20"/>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
      <param name="output_frame" value="odom"/>
      <param name="frequency" value="20"/>
      <param name="odom_used" value="true"/>
      <param name="imu_used" value="true"/>
      <param name="vo_used" value="false"/>  
      <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="odom0" value="odom"/>
      <param name="imu0"  value="/imu/data"/> 

      <rosparam param="odom0_config">[true, true, true,
                                      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="imu0_differential"  value="false"/>

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

      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative"  value="false"/>

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

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

      <param name="odom0_queue_size" value="2"/>
      <param name="imu0_queue_size" value="10"/>

      <rosparam param="process_noise_covariance">[0.05, 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.05, 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.06, 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.03, 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.03, 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.06, 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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.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>

  <!-- Parameters setting of the node: navsat_transform_node -->
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
respawn="true">
      <param name="magnetic_declination_radians" value="0.36878808"/>
value="0.0036651914"/>
      <param name="yaw_offset" value="0.0"/>
      <param name="zero_altitude" value="false"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
   <param name="broadcast_utm_transform" value="true"/>
name="use_odometry_yaw" value="false"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/imu/data" to="/imu/data" />
      <remap from="/gps/fix" to="/fix"/>
to="/fix" />
      <!--remap from="/odometry/filtered" to="/odometry/filtered" /-->
    </node>
<node pkg="robot_localization" type="ekf_localization_node"  name="ekf_localization_node" clear_params="true">

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

  <param name="frequency" value="30"/>
  <param name="sensor_timeout" value="2"/>
  <param name="two_d_mode" value="true"/>

  <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"/>

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

  <param name="imu0_differential" value="false"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>

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


                        false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

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

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


    </node>
     </launch>

The odometry source is from Hector slam from Hokuyo laser sensor. Seems that odometry/filtered topic publishes the data, not sure yet what is the actual value of it but before i had no data flow at all. However, I still have problem with /odometry/gps or /gps/filtered as none of them publish anything when subscribed. I did check if the navsat_transform_node subscribes to mention ekf_localization node and it does do that indeed. Also, the navsat_transform_node also subscribes to both /imu/data and /fix so i do provide GPS flow. Following is my data flow from different topics. Note that in launch file odometry/gps was not have odometry atm, as robot has subsribed in odom0, but i changed in to odom in order to get some data. Fact is that when i echo the topic odometry/gps i get no wheel encoders. data flow, same is with gps/filtered - in other words any topic published by navsat_transform_node does not provide data flow.

Also i attach the configuration of the Xsens INS sensor.

rosrun xsens_driver mtdevice.py --configure=pl400fe,pa400fe,oq400fe,aa400fe,wr400fe,gd4fe --output-settings=tqMAG

For this sensor i use ethzasl_xsens_driver package as it seems to be properly working with the sensor. What I am trying to achieve this moment is to get data from robot_localization package topics but nothing is being subscribed even though they subscribe to Xsens published topics /fix and /imu/data. Please let me know what I am doing wrong and what extra info i should attach.

EDIT

This is the following data that i receive when echo'ing. From topic /fix

header: 
  seq: 10809
  stamp: 
    secs: 1488376624
    nsecs: 581118106
  frame_id: /imu
status: 
  status: 0
  service: 0
latitude: 57.0143623352
longitude: 9.98601722717
altitude: 62.4811706543
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---

From topic /imu/data

header: 
  seq: 1592301
  stamp: 
    secs: 1488380578
    nsecs: 149617910
  frame_id: /imu
orientation: 
  x: -0.0012584940996
  y: -0.0031715943478
  z: -0.214321956038
  w: -0.976757109165
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.000515580235515
  y: 0.00211000442505
  z: 0.000423193065217
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.035540368408
  y: -0.0115282256156
  z: 9.81090068817
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---

And when launching navsat_transform.launch i only get the INFO statusFrom /odometry/filtered:

 INFO] [1488380635.870630401]: Datum (latitude, longitude, altitude) is (57.014286, 9.985973, 78.685829)
[ INFO] [1488380635.870718003]: Datum UTM coordinate is (559871.633204, 6319408.306693)
[ INFO] [1488380635.970500574]: Datum (latitude, longitude, altitude) is (57.014286, 9.985973, 78.685822)
[ INFO] [1488380635.970558908]: Datum UTM coordinate is (559871.633204, 6319408.306693)
header: 
  seq: 160
  stamp: 
    secs: 1488812771
    nsecs: 471517563
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: -3.2737282894e-07
      y: -2.40200328791e-06
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 3.24346089091e-11
      w: 1.0
  covariance: [0.6998459214051511, -0.09306646898637555, 0.0, 0.0, 0.0, 1.448969985883608e-05, -0.0930664689863756, 0.4099058675760928, 0.0, 0.0, 0.0, -1.1834283019316645e-06, 0.0, 0.0, 9.999993476136396e-10, -5.6646719919275456e-15, -4.613325278206316e-15, 0.0, 0.0, 0.0, -5.6646719919275716e-15, 1.9474625016874716, 8.515942117782655e-11, 0.0, 0.0, 0.0, -4.613325278206308e-15, 8.515942117782666e-11, 1.9474625028938, 0.0, 1.4489699858836067e-05, -1.183428301931665e-06, 0.0, 0.0, 0.0, 3.8949249401146293]
twist: 
  twist: 
    linear: 
      x: -2.09464334202e-07
      y: -4.27962175671e-06
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 8.7039657586e-12
  covariance: [0.30525458618611934, -0.02126161143694827, 0.0, 0.0, 0.0, -1.692420648337544e-07, -0.021261611436948238, 0.23771918626862049, 0.0, 0.0, 0.0, 4.442784255567807e-08, 0.0, 0.0, 0.07805270605408478, 1.2696522846092139e-06, -1.0790358776083711e-07, 0.0, 0.0, 0.0, 1.269652284609214e-06, 0.08016313173816005, 1.4901440328568602e-12, 0.0, 0.0, 0.0, -1.0790358776083711e-07, 1.4901440328568598e-12, 0.08016313176004526, 0.0, -1.6924206483375303e-07, 4.4427842555677875e-08, 0.0, 0.0, 0.0, 0.16032626251201518]

However, when i echo /odometry/filtered , /odometry/gps or /gps/filtered i receive no data stream at all.