Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hy new_forROS, I had the same error, you have two options to solve it.

1º using the robot_pose_ekf + utm(with changes) + ublox like this:

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_gps_ekf" output="screen">
  <remap from="imu_data" to="/imau/imu" />
  <remap from="odom" to="/gps_odom" />
  <param name="output_frame" value="gps_odom_pose"/>
  <param name="freq" value="30.0"/>
  <param name="sensor_timeout" value="1.0"/>
  <param name="odom_used" value="true"/> 
  <param name="imu_used" value="true"/>
  <param name="vo_used" value="false"/>
  <param name="debug" value="false"/>
  <param name="self_diagnose" value="false"/>
  <param name="publish_tf" value="false"/>

/gps_odom, must be given to robot_pose_ekf in nav_msgs/Odometry message type (using utm gps_common),and you must had set covariance matrix not only x y z but with too rxx(rotation on x axis),ryy and rzz. I initialy copy the convariance of imu to the gps, if you see with attention, utm set the rxx, ryy and rz, I re-code the source and did this:

<node name="utm_odometry_inipos_node" pkg="gps_common" type="utm_odometry_inipos_node">
    <remap from="fix" to="/gps/fix" /> <!--input-->
    <remap from="odom" to="gps_odom" /> <!--ouput-->
    <!--<param name="rot_covariance" value="1000000000000.0" />-->
    <param name="rot_covariance_x" value="1000000000000.0" />
    <param name="rot_covariance_y" value="1000000000000.0" />
    <param name="rot_covariance_z" value="0.001" />
    <param name="frame_id" value="world" />
    <param name="child_frame_id" value="base_footprint" />      

In this way I can control the values of rxx,ryy and rzz. But as you know this approach ins't the best because with one gps you can't obtain the rotation of robot, but work.

2º Option robot_pose_ekf(new version jade, work in indigo) + utm(with changes) + ublox(changes): With the new robot_pose_ekf they treat the gps values differently, they doesn't use rxx, ryy and rzz.

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_gps_pose_ekf" output="screen">
      <remap from="imu_data" to="/imu" />
      <remap from="gps" to="/gps_odom"/>
      <param name="freq" value="30.0"/>
      <param name="sensor_timeout" value="1.0"/>
      <param name="odom_used" value="false"/>
      <param name="imu_used" value="true"/>
      <param name="vo_used" value="false"/>
      <param name="gps_used" value="true"/>
      <param name="debug" value="false"/>
      <param name="self_diagnose" value="false"/>

The difference is here <remap from="gps" to="/gps_odom"/> <param name="gps_used" value="true"/>. Use the utm as the first option, and the big difference of 2º option is do some change at ublox. Open the node.cpp from ublox package, go to function void publishNavPosLLH(const ublox_msgs::NavPOSLLH& m) and add this, after fix.altitude = m.height*1e-3;:

fix.position_covariance[0] = m.hAcc*1e-3; // Horizontal Accuracy Estimate [mm]->[m]
  fix.position_covariance[4] = m.hAcc*1e-3; // Horizontal Accuracy Estimate [mm]->[m]
  fix.position_covariance[8] = m.vAcc*1e-3; // Vertical Accuracy Estimate [mm]->[m]

doing this you are setting the convariance x,y and z of gps. The utm will add the rxx,ryy and rzz by default but will be ignored by the new robot_pose_ekf. I think this approach is more correct then the other. It worked for me.

If you want some code of utm to help ask me.