Ask Your Question
1

GPS Coordinates to Map Coordinates

asked 2014-11-24 22:07:43 -0500

ROSCMBOT gravatar image

updated 2014-11-25 16:53:02 -0500

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

<launch>

<!-- 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 ...
(more)
edit retag flag offensive close merge delete

Comments

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?

dornhege gravatar imagedornhege ( 2014-11-25 04:25:56 -0500 )edit

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)

ROSCMBOT gravatar imageROSCMBOT ( 2014-11-25 13:00:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-11-25 07:56:19 -0500

Tom Moore gravatar image

updated 2014-11-27 13:22:20 -0500

You can navsat_transform_node a shot:

http://wiki.ros.org/robot_localizatio...

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 ...

(more)
edit flag offensive delete link more

Comments

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),

ROSCMBOT gravatar imageROSCMBOT ( 2014-11-25 14:02:39 -0500 )edit

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.

ROSCMBOT gravatar imageROSCMBOT ( 2014-11-25 14:03:47 -0500 )edit

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.

Tom Moore gravatar imageTom Moore ( 2014-11-25 14:33:32 -0500 )edit

...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.

Tom Moore gravatar imageTom Moore ( 2014-11-25 14:37:36 -0500 )edit

I am using ekf_localization_node (robot_localization), updated my question

ROSCMBOT gravatar imageROSCMBOT ( 2014-11-25 15:02:01 -0500 )edit

In fact I am not setting any pose0 for my ekf_localization_node

ROSCMBOT gravatar imageROSCMBOT ( 2014-11-25 15:54:19 -0500 )edit

I noticed. Answer forthcoming...

Tom Moore gravatar imageTom Moore ( 2014-11-25 15:56:33 -0500 )edit

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)

ROSCMBOT gravatar imageROSCMBOT ( 2014-11-25 17:07:04 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2014-11-24 22:07:43 -0500

Seen: 4,513 times

Last updated: Nov 27 '14