Robot localization and Landmarks
I am using the robot_localization package on a turtlebot running ROS Indigo. I would like the turtlebot to autonomously navigate a space and update it's position based on AprilTag landmarks. It seems that the way to do this is to take advantage of a map frame. However, when I publish landmark data, the map frame does not update in the discrete jumps I am hoping for. Perhaps I am doing something wrong with the launch file or with the way I am publishing tag data? How do you make sure robot_localization pays attention to the tags? Should I be using the set_pose service instead, or is that only meant to set initial position?
EDIT: Ideally, I would like the map frame to be updated discontinuously when it encounters a landmark, much as the GPS package in robot_localization allows the map frame to be discretely updated. The data published on the apriltags topic is relative to the map frame. There is not currently a transform between the apriltags frame and odom (I am a bit uncertain as to how to approach this). Will publishing a transform as well give discrete updates to position?
FINAL EDIT: Looks like figuring out the tf transformation is the key. I added a tf.TransformBroadcaster to go along with the apriltags publisher, and there were discrete updates; however, it doesn't seem to do what I want with non-zero data. If I publish a zeroed out message and a zeroed out transform, it zeros out the turtlebot. But if I do anything else, the map doesn't update.
My launch file looks like this:
<launch>
<!-- Set up the odom frame (local, continuous navigation) -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_imu" respawn="true" output="screen">
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="odom"/>
<param name="two_d_mode" value="true"/>
<param name="imu0" value="imu_data"/>
<rosparam param="imu0_config">
[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]
</rosparam>
<param name="imu0_differential" value="false"/>
<param name="odom0" value="odom"/>
<rosparam param="odom0_config">
[false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
</rosparam>
<param name="odom0_differential" value="true"/>
</node>
<!-- Set up the map frame (global, includes jumps) -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_map" respawn="true" output="screen">
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="map"/>
<param name="two_d_mode" value="true"/>
<param name="imu0" value="imu_data"/>
<rosparam param="imu0_config">
[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, false, false]
</rosparam>
<param name="imu0_differential" value="false"/>
<param name="odom0" value="odom"/>
<rosparam param="odom0_config">
[false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
</rosparam>
<param name="odom0_differential" value="true"/>
<param name="pose0" value = "apriltags"/>
<rosparam param="pose0_config">
[true, true, false,
false, false, false,
false, false, false,
false, false, false]
</rosparam>
<param name="pose0_differential" value ...
Are you certain that the apriltags data is in the map frame, and not relative to the robot? How do you define the global pose of every apriltag? Is there some database that says what their poses in the map frame are?
I'm manually publishing the topic, so I know that I intend for it to be in the global frame. The raw tag data is relative to the robot base, but I've been converting it manually for the time being. I need the tag data to tell me where the robot is, so there's an offset from the tag to the robot base
As to the position of the april tag in the map frame, yes, there is a database containing that information.
Can you post either a bag file or the code you're using to generate the
apriltags
->map
transform?Would you like to say the name of the package the you have used ?