Ask Your Question
1

robot_localization use case with landmarks

asked 2015-08-19 06:40:27 -0600

Mehdi. gravatar image

updated 2015-08-19 08:15:25 -0600

I have a non holonomic mobile base that I want to localize. When I start it I get its odometry published to /odom but with covariances of 0 for both twist and pose. I also fixed a camera on it and can observe and identify landmarks at known global positions and then can estimate the pose of the robot in the world. If I understood well, I set the world frame in my launch file to the map frame as explaned here

If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:

a- Set your world_frame to your map_frame value

b- MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

Here Is my current launch file:

<launch>
    <arg name="use_sim" default="true"/>
    <param name="/use_sim_time" type="bool" value="$(arg use_sim)" />

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="1"/>
      <param name="two_d_mode" value="true"/>

      <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="/odom"/>
      <param name="pose0" value="/my_pose"/>

      <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  true, false,
                                      false, false, true,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="pose0_relative" value="false"/>

      <param name="print_diagnostics" value="true"/>

      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <param name="pose0_rejection_threshold" value="2"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

    </node>

</launch>

I play a bag providing /odom and the odom->base_link transform and then launch robot_localization. The topic odometry_filtered is there but nothing gets published on it. At this stage there is nothing published to /my_pose as no landmarks are observed.

What is missing?

My odom message looks like this:

header: 
  seq: 4701
  stamp: 
    secs: 1439389120
    nsecs: 950957354
  frame_id: /odom
child_frame_id: /base_footprint
pose: 
  pose: 
    position: 
      x: 17.6170005798
      y: 0.433000028133
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.00999983404194
      w: 0.99995000041
  covariance: [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.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: -0.286000013351
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-08-19 07:54:48 -0600

Tom Moore gravatar image

updated 2015-08-19 13:24:26 -0600

I'd have to see a sample odometry message, but just based on your description and launch file, my best guess is that you're having a transform issue. ekf_localization_node needs at least one message from one input source before it starts producing a state estimate. In your case, it will produce (a) an odometry message as /odometry/filtered, and (b) a transform from map->odom. Again, it will not produce either until it receives at least one message from one of its input sources. The problem is that your /odom message that you are using as input odom0 probably has a frame_id of odom. In order to use that message, the filter first needs to transform it into the world_frame specified in the launch file, which in this case is map. Since ekf_localization_node itself is responsible for producing the map->odom transform, and it hasn't produced a state estimate yet, the transform doesn't exist. It's a chicken-or-egg problem. The easiest way to get around it is to start producing messages for your other input source, /my_pose.

EDIT: actually, after looking again at your odom0 configuration, it appears that you're only fusing velocity, which should work. Can you post a sample /odom message?

EDIT 2 in response to comment: If you are using amcl to do global localization, then what do you gain from running ekf_localization_node? Regardless, you can't have two nodes publishing the same transform, so you'll have to stop one of them from publishing it. If amcl supports turning off the map->odom transform, then you can do that, and instead feed its output to ekf_localization_node as an additional input. I've not tried this before, but as long as the message type is correct, it may work.

Also, your frame_ids in the message you posted start with a forward slash, e.g., "/odom" and "/base_link." You should remove the forward slashes.

edit flag offensive delete link more

Comments

Actually my bag was taken during an amcl localization, so the transform between odom and map exists also.

Mehdi. gravatar imageMehdi. ( 2015-08-19 08:05:34 -0600 )edit

Now it is working ! I tested it directly on the robot after I changed the odom publisher to set frame_id's without a slash. I think it was the problem. Thanks

Mehdi. gravatar imageMehdi. ( 2015-08-20 06:26:57 -0600 )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

2 followers

Stats

Asked: 2015-08-19 06:40:27 -0600

Seen: 668 times

Last updated: Aug 19 '15