Revision history [back]

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.

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

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.

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

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

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, amcl, but I don't know if you can get amcl 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).

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

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

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.