GPS integration in robot_localization
Hello all,
I am trying to integrate UBlox Lea6h gps module with robotlocalization, this gps is to be mounted on my quadcopter. I am using ros indigo in ubuntu 14.04. By using NavSatFix of rosserial I am feeding lat and long data with a rostopic /fix to the navsattransform_node. My rostopic echo /fix output :
header:
seq: 84
stamp:
secs: 1464760028
nsecs: 759104988
frame_id: /fix
status:
status: 0
service: 0
latitude: 19.1345443726
longitude: 72.906288147
altitude: 0.0
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
I don't know how to find out and where to write the position covariance. My ekflocalizationnode:
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<!-- ======== STANDARD PARAMETERS ======== -->
<param name="frequency" value="30"/>
<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="map"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/odometry/gps"/>
<param name="odom0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="print_diagnostics" value="true"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="odom0_pose_rejection_threshold" value="5"/>
<param name="odom0_twist_rejection_threshold" value="1"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 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.06, 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.03, 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.025, 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.04, 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.01, 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.01, 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.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>
<!-- Placeholder for output topic remapping
<remap from="odometry/filtered" to=""/>
-->
</node>
My navsat_transform node :
<launch>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="frequency" value="30"/>
<param name="delay" value="3"/>
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<param name="zero_altitude" value="false"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="publish_filtered_gps" value="true"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="false"/>
<rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>
<!-- Placeholders for input remapping. Set your topic names as the "to" values.
<remap from="/imu/data" to=""/> -->
<remap from="/odometry/filtered" to="/odometry/filtered"/>
<!-- Placeholders for output remapping.-->
<remap from="/gps/filtered" to="/gps/filtered"/>
<remap from="/odometry/gps" to="/odometry/gps"/>
</node>
The problem I am facing is my rqtgraph shows that /odometry/gps going from navsattransformnode to ekflocalizationnode but I cant see that by rostopic echo , /odometry/filter goes from ekflocalizationnode to navsattransformnode but I cant see that also. The only data I can see is the UTM transform of gps data in navsattransformnode. I have gone through the wiki page of robotlocalization for gps integration and many answers in this forum, but I am still confused about how to configure ekf_localization node and navsat transform node.
I couldn't find the meaning of <remap from="" to=""/> in navsat transform node and what should be the parameter values of param name map, odom ,base_link and world.
Asked by pankaj patel on 2016-06-01 01:32:06 UTC
Answers
There are actually quite a lot of cheesy things in your configuration files.
In ekf_localization_node:
<param name="map_frame" value="/odometry/gps"/>
<param name="world_frame" value="/odometry/gps"/>
map_frame and world_frame should be the world reference frame (typically "map"). If you have a map and are fusing GPS, both should be the same. /odometry/gps doesn't look like a reference frame, but a topic.
Also, when using GPS, I think
<param name="odom0_differential" value="true"/>
should be false.
Start by fixing these issues and check back.
EDIT:
Another issue I see is that you are using only one EKF node, and using in the navsat as odometry input the output from this EKF. That is not correct, since the EKF uses as input the output from the navsat, and as both need the output from the other, no one is going to publish anything.
I think you should at least use another source of odometry for the EKF in order for it to start. It can be a fake one with a very high covariance.
Also, I'm not sure if something is providing the odom-->base_link transformation, which would also be necessary.
Finally, altough you see that nodes are linked in the graph, that doesn't mean that messages are being published. Only that they are subscribed/publishing the correct topics.
Asked by IvanV on 2016-06-01 03:31:49 UTC
Comments
Thanks for the reply IvanV, Do you mean in parameter values there should be only frame names like map, odom, base_link as given in default ? I changed those things you mentioned earlier, still not getting anything. Can you please tell more about problems in my configurations.
Asked by pankaj patel on 2016-06-01 04:08:16 UTC
One more thing that I want to ask is, robot localization will work with only gps or it needs IMU sensor also ? By saying Transformation from odom-> base_link do you mean integration of one more sensor ?
Asked by pankaj patel on 2016-06-01 05:34:41 UTC
Please check REP 105 for the standard reference frames and their relationship. Your EKF is publishing the transformation map->odom, but requires odom->base_link. If you want to avoid it, maybe you can set odom_frame to "map" so it publishes map->base_link.
Asked by IvanV on 2016-06-01 06:16:53 UTC
Comments