0
$\begingroup$

Rosanswers logo

Hello,

I would like to initialize my robot on a map through GPS for a navigation task. How can I convert GPS coordinates to map Coordinates?

Thanks

UPDATE 1

I am using the following launch files:

<launch>

    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">

      <param name="magnetic_declination_radians" value="0"/>

      <param name="roll_offset" value="0"/>

      <param name="pitch_offset" value="0"/>

      <param name="yaw_offset" value="0"/>

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

      <remap from="/imu/data" to="/robot/imu" />
      <remap from="/gps/fix" to="/robot/gps" />

      <remap from="/odometry/gps" to="/initialpose" />

    </node>

</launch>

Launch file for ekf_localization_node

<!-- This node will take in measurements from odometry, IMU, stamped pose, and stamped twist messages. It tracks
     the state of the robot, with the state vector being defined as X position, Y position, Z position,
     roll, pitch, yaw, and the respective velocites for those quantities. Accelerations are not used (yet).
     Units for all measurements are assumed to be in meters and radians. By default, the node outputs an
     Odometry message with the topic name odometry/filtered -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <!-- Set initial pose of the robot through code or rviz -->
  <remap from="set_pose" to="initialpose"/>

  <!-- The frequency, in Hz, at which the filter will output a position estimate. Note that
       the filter will not begin computation until it receives at least one message from
       one of the inputs. It will then run continuously at the frequency specified here,
       regardless of whether it receives more measurements. --> 
  <param name="frequency" value="30"/>  

  <!-- The period, in seconds, after which we consider a sensor to have timed out. In this event, we
       carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
       as the minimum frequency with which the filter will generate output. -->
  <param name="sensor_timeout" value="0.1"/>  

  <!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
       TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
       e.g., odom0, odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. -->
  <param name="odom0" value="robot/odom"/>
  <param name="imu0" value="robot/imu"/> 

  <!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
       which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
       but only want to use its Z position value, then set the entire vector to false, except for the third entry.
       The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw. Note that not some message
       types lack certain variables. For example, a TwistWithCovarianceStamped message has no pose information, so the first
       six values would be meaningless in that case. -->
  <rosparam param="odom0_config">[false, false, true, true, true, true, true, true, true, true, true, true]</rosparam>
  <rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, true, true, true]</rosparam>

  <!-- The best practice for including new sensors in ekf_localization_node is to pass in velocity measurements and let
       ekf_localization_node integrate them. However, this isn't always feasible, and so ekf_localization (and the other
       nodes in robot_localization) support fusion of absolute measurements. If you have more than one sensor providing
       absolute measurements, however, you will quickly run into problems, as the sensors will inevitably diverge from
       one another, causing the filter to jump back and forth rapidly. To combat this situation, you can set the sensor's
       differential parameter to true. This will carry out differential integration of the sensor data, i.e., it will 
       repeatedly integrate the difference between a given measurement and the previous measurement from that sensor. 
       NOTE: this only applies to sensors that provide absolute measurements, so Twist messages are not supported. -->
  <param name="odom0_differential" value="true"/>
  <param name="imu0_differential" value="true"/>

  <!-- Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file
       specified by debug_out_file. I hope you like matrices! -->
  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <!-- For the tf tree, this specifies the name of the local (odom) and vehicle (base_link) frames. There's 
       a good chance you can leave these alone, as the default values adhere to the ROS spec. -->
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="robot/base_footprint"/>

  <!-- The process noise covariance matrix can be difficult to tune, and can vary for each application, so it
       is exposed as a configuration parameter. PLEASE NOTE that every value in this matrix *must* be specified
       as real-valued, i.e., it must have a decimal point. Integers don't load correctly. The values are ordered
       as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw. -->
  <rosparam param="process_noise_covariance">[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.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.4, 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.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004]</rosparam>

</node>
When I am running the move_base.launch (navigation stack), I get the following error: 
[ERROR] [1416949561.233882834]: Client [/ekf_localization] wants topic /initialpose to have datatype/md5sum [geometry_msgs/PoseWithCovarianceStamped/953b798c0f514ff060a53a3498ce6246], but our version has [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]. Dropping connection.
 [ERROR] [1416949561.248454412]: Client [/fake_localization] wants topic /initialpose to have datatype/md5sum [geometry_msgs/PoseWithCovarianceStamped/953b798c0f514ff060a53a3498ce6246], but our version has [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]. Dropping connection.

Originally posted by ROSCMBOT on ROS Answers with karma: 651 on 2014-11-24

Post score: 2

Original comments

Comment by dornhege on 2014-11-25:
What are map coordinates for you? What are GPS coordinates for you? What do you want to do? Might a TF link be a better choice?

Comment by ROSCMBOT on 2014-11-25:
I'm using the navigation stack, so I believe map is in the UTM frame and GPS is of type sensor_msgs/NavSatFix message (Not sure about the coordinate frame)

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

You can navsat_transform_node a shot:

http://wiki.ros.org/robot_localization#navsat_transform_node

It requires that you have a sensor_msgs/NavSatFix message with your GPS data in it, a sensor_msgs/Imu message with a global heading (e.g., from a magnetometer), and a nav_msgs/Odometry message with your vehicle's current position (as output by a state estimation node in robot_localization, or whatever you want to use). It will automatically transform all GPS data into your robot's world frame_id. The frame_id of the output nav_msgs/Odometry message will match the input nav_msgs/Odometry message frame_id. If you use the version that's currently in the repo, it will also publish the transform from your world frame_id to the UTM grid.

EDIT for update 1:

OK, for ekf_localization_node, I don't have your launch file, but your issue is almost certainly that you have this:

<param name="pose0" value="/initialpose"/>

...when you want this:

<param name="odom0" value="/initialpose"/>

If you specify a "poseN" parameter for ekf_localization_node, it assumes the type is geometry_msgs/PoseWithCovarianceStamped. Since navsat_transform_node is outputting a nav_msgs/Odometry message, it's complaining.

You're having a similar issue with amcl, but I don't know if you can get amcl to listen to a nav_msgs/Odometry message instead of a geometry_msgs/PoseWithCovarianceStamped message. You could always modify the navsat_transform_node source to output a geometry_msgs/PoseWithCovarianceStamped message. It would be pretty straightforward.

Two more things:

  1. Does your IMU read 0 at true north? You navsat_transform_node is configured that way right now.
  2. Why are you feeding the GPS location to both amcl and ekf_localization_node? Again, I'm not very familiar with amcl.

It might be a good idea to post all of your launch files (ekf_localization_node and amcl as well).

Edit after launch file posts:

OK, I think there are quite a few things going on here.

First, you appear to be running ekf_localization_node, amcl, and fake_localization. As I understand it, both amcl and fake_localization provide the same information, so I'm thinking you really only need amcl (assuming you have a LIDAR and a map).

Second, let's consider your transforms. As you have it configured now, ekf_localization_node will provide a transform from odom->robot/base_footprint. If you do have a LIDAR and a map, then amcl will provide the map->odom transform, and you'll be in good shape. If you do not have a LIDAR and a map, then you have two options:

  1. Use fake_localization to generate a map->odom transform.
  2. Use a second instance of ekf_localization_node to generate the map->odom transform. Fuse the GPS data with that, and do not fuse it with the first instance that is providing the odom->robot/base_link transform.

Third - and this is critical - if you need to fuse the GPS data using ekf_localization_node, do not use the set_pose topic/service for feeding in the GPS data to ekf_localization_node. That is reserved for manually resetting the entire pose of the filter, and it's really designed to interact with things like rviz. It lets users reset out the state estimate without having to shut the node down and bring it back up. Instead, do this:

<param name="odom1" value="/initialpose"/>
<rosparam param="odom1">[true, true, false, false, false, false, false, false, false, false, false, false]</rosparam>
<param name="odom1_differential" value="false"/>

I'm not sure what amcl expects, but I have a feeling you might want to double-check to make sure that you're using it correctly there as well.

Finally, your ekf_localization_node launch file is a bit out of date. If you're using the latest version (x.1.6 as of this writing), then see this for an example. The most important changes are that the state now includes linear acceleration, so your boolean vectors need an extra 3 values (it's backwards-compatible, though). Also, you're using amcl, which I believe is 2D, so you can set the two_d_mode parameter to true.

EDIT for comments:

Sorry, I may have mistakenly said that the set_pose topic/service wants an Odometry message. It doesn't. It wants a PoseWithCovarianceStamped message, hence the incompatibility (the output of navsat_transform_node is an Odometry message).

However, I'm still not sure exactly what you mean by "I only want to initialize the robot on the map using the GPS." When the robot first starts, the map and odom frames should be (or can be) completely overlapping, i.e., with the same origin and orientation. In both coordinate frames, your robot should believe it's at position (0, 0) when it first starts. As the robot moves, its position in both frames will change. If you use navsat_transform_node and configure it correctly, the Odometry message will also start at (0, 0) and should be very close to your fused odometry data (without the GPS). They will eventually drift from one another as the odometry data accumulates error.

In any case, if you try to use the set_pose topic and tie it to the output of navsat_transform_node, it's literally going to continuously move the robot to the exact location of the GPS odometry message. You may as well not even use a filter.

I'm happy to continue this conversation here, but it's starting to get messy. :) I know this goes against the guidelines for this site, but maybe we can get you squared away offline (via e-mail, see my comment or the Github repo for my address), and then we'll update/clean this question.

Originally posted by Tom Moore with karma: 13689 on 2014-11-25

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by ROSCMBOT on 2014-11-25:
Thanks a lot Tom, This is what I needed. I'm using the launch file from here to run the node. The only issue is when I'm remapping /odometry/gps to /initialpose(Although they are of the same type nav_msgs/Odometry),

Comment by ROSCMBOT on 2014-11-25:
I get the error: [ERROR] [1416945303.777949700]: Client [/ekf_localization] wants topic /initialpose to have datatype/md5sum [geometry_msgs/PoseWithCovarianceStamped/953b798c0f514ff060a53a3498ce6246], but our version has [nav_msgs/Odometry/cd5e73d190d741a2f92e81eda573aca7]. Dropping connection.

Comment by Tom Moore on 2014-11-25:
Sorry, what is ekf_localization? Are you using ekf_localization_node (robot_localization) or robot_pose_ekf? If it's ekf_localization_node, you've got an issue in that launch file.

Comment by Tom Moore on 2014-11-25:
...and that issue is likely that you have "initialpose" included as "pose0" intead of "odom0." Pose messages assume the PoseWithCovarianceStamped type. Odometry messages assume the Odometry type.

Comment by ROSCMBOT on 2014-11-25:
I am using ekf_localization_node (robot_localization), updated my question

Comment by ROSCMBOT on 2014-11-25:
In fact I am not setting any pose0 for my ekf_localization_node

Comment by Tom Moore on 2014-11-25:
I noticed. Answer forthcoming...

Comment by ROSCMBOT on 2014-11-25:
Sorry, that amcl was a mistake. I'm not using amcl. I'm only using fake_localization. fake_localization generates the map->odom transform and ekf_lozalization generates the odom->robot/base_footprint transform. I updated the error messages (amcl is not complainig anymore)

Comment by ROSCMBOT on 2014-11-25:
Regarding your third point, I didn't intend to fuse the GPS data along with odometry and imu using ekf_localization_node, and as it is shown in my ekf_template.launch file I haven't included the GPS. I only want to initialize the robot on the map using the GPS

Comment by ROSCMBOT on 2014-11-25:
@Tom Moore, I couldn't figure out how to private message you on here

Comment by ROSCMBOT on 2014-11-27:
@Tom Moore, Tom can you please have a look at here ?

Comment by Tom Moore on 2014-11-27:
Use email: tmoore at cra dot com

Improve this answer
$\endgroup$

Your Answer

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