Ask Your Question
3

Is it possible to use robot_localization for multiple robots?

asked 2016-02-10 19:01:20 -0600

nbeyers gravatar image

updated 2016-10-24 09:07:43 -0600

ngrennan gravatar image

Hello all,

I'm working on a small, multi-robot simulation using Husky robots. I've been using the base husky_simulator with some modifications to allow for multiple robots. The way I've accomplished this is through the use of namespaces. I call each "spawn_husky.launch" file in a separate namespace, which sets up all the plugins and publishes unique topics for each robot.

Everything seems to be working fine except for the the topics husky1/odometry/filtered and husky2/odometry/filtered. So I dug through the launch files and found that this topic is published in the control.launch file which uses a "robot_localization" node of the "ekf_localization_node" type.

The tf frame tree of a single robot simulation (without namespaces) looks like this: image description

But when launched from within the namespace, with multiple robots, looks like this:

image description

I know it's hard to see, but the "odom" frame no longer exists, so this seems the localization is not working properly. My initial guess is that the robot_localization limits the parameters to "map" or "odom" but since I'm using a namespace and tf_prefix, it becomes "husky1/odom" and this is not allowed. But does anyone have any ideas as to how I might be able to correct this or why it might be happening?

Edit 1: Some additional information, as requested by Tom.

Here are the frame_ids during a single robot simulation with no namespace or tf_prefix

RESULTS: for all Frames

Frames:
Frame: base_footprint published by unknown_publisher Average Delay: -0.000202703 Max Delay: 0
Frame: base_laser published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: base_link published by unknown_publisher Average Delay: -6.75676e-05 Max Delay: 0
Frame: front_bumper_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: front_left_wheel_link published by unknown_publisher Average Delay: 0 Max Delay: 0
Frame: front_right_wheel_link published by unknown_publisher Average Delay: 0 Max Delay: 0
Frame: imu_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: inertial_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: laser_mount_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: rear_bumper_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: rear_left_wheel_link published by unknown_publisher Average Delay: 0 Max Delay: 0
Frame: rear_right_wheel_link published by unknown_publisher Average Delay: 0 Max Delay: 0
Frame: topPlate published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: top_chassis_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: top_plate_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0
Frame: user_rail_link published by unknown_publisher Average Delay: -0.000135135 Max Delay: 0

All Broadcasters:
Node: unknown_publisher 150.338 Hz, Average Delay: -6.94586e-05 Max Delay: 0

And here are all the frame_ids shown by tf_monitor when running the multi robot simulation:

RESULTS: for all Frames

Frames:
Frame: husky1/base_footprint published by unknown_publisher Average Delay: 0.0182353 Max Delay: 0.04
Frame: husky1/base_laser published by unknown_publisher Average Delay: 0.0182353 Max Delay: 0.04
Frame: husky1/front_bumper_link published by unknown_publisher Average Delay: 0 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you post sample messages for each input? I want to see the frame_ids. Also, even though I know what's in it, can you post the .yaml settings for ekf_localization_node? You can leave out the covariance matrices.

Tom Moore gravatar imageTom Moore ( 2016-02-11 08:33:00 -0600 )edit

I just updated the post to include the frames and .yaml file. Let me know if anything else might be useful. Thanks!

nbeyers gravatar imagenbeyers ( 2016-02-11 11:12:34 -0600 )edit

image description I think ekf/localization can subscribe topic odom and imu. But I cannot get /odometry/filtered neither.

Devin gravatar imageDevin ( 2016-02-11 14:04:57 -0600 )edit

Can you post a sample input message for each input topic?

Tom Moore gravatar imageTom Moore ( 2016-02-11 16:00:15 -0600 )edit

What do you mean by input topics? Are you referring to the topics used by the ekf_localization_node such as husky1/imu/data or husky2/husky_velocity_controller/odom?

nbeyers gravatar imagenbeyers ( 2016-02-11 16:21:31 -0600 )edit

Yes. I need to see sample messages so I can verify frame_ids in the messages themselves.

Tom Moore gravatar imageTom Moore ( 2016-02-11 16:52:18 -0600 )edit

4 Answers

Sort by ยป oldest newest most voted
1

answered 2016-02-12 13:02:21 -0600

Tom Moore gravatar image

updated 2016-03-01 11:58:37 -0600

I'm assuming that the tf_prefix tag is active for each robot. r_l will append the tf_prefix to the frame_ids specified in its launch file, even if the parameter is specified outside its configuration (we use searchParam). For example, for husky1, it's going to be trying to transform velocities to husky1/base_link and pose data to husky1/odom. However, all the incoming messages lack any tf_prefix (look at your IMU message: the frame_id is just base_link), so r_l doesn't know how to transform from base_link to husky1/base_link. I don't know if Gazebo supports prefixing those frame_ids, so one thing you might try is setting the tf_prefix parameter inside your r_l configuration to the empty string.

EDIT 1 in response to comment

I get the three separate tf trees; what is publishing the fourth odom->base_link transform?

rviz is going to yell at you here, as it doesn't know how to transform from each world (huskyN/odom) frame to the other, so you won't be able to display more than one robot at a time. One way to get around this is to define a frame that is a parent to all N of the Husky frames, and then set that to the fixed frame in rviz. To do this, you can just add three static transforms (using static_transform_publisher in the tf or tf2 library), each with the same parent frame, and with the huskyN/odom frame as the child(ren).

edit flag offensive delete link more

Comments

Thank you! This worked, at least partially. Both husky#/odometry/filtered messages are now being published and have the correct data. However, there are now 3 tf trees: one for each robot and a third for odom->base_link. So rviz is throwing errors because it can't resolve between the three.

nbeyers gravatar imagenbeyers ( 2016-02-16 16:12:05 -0600 )edit

In response to your edit: The odom->base_link transform was being published by robot_localization (since I was setting the tf_prefix to an empty string). I've since solved the issue by setting the frame_id parameter for both the IMU and the velocity controller for each robot.

nbeyers gravatar imagenbeyers ( 2016-03-01 13:56:18 -0600 )edit

Great! So is your issue solved?

Tom Moore gravatar imageTom Moore ( 2016-03-03 07:58:39 -0600 )edit

nbeyers - Could you explain how you set the "frame_id" parameter for the velocity controller in gazebo? I have not been able to determine how/where the frame is set for the /husky_velocity_controller/odom topic.

BSBingham gravatar imageBSBingham ( 2016-03-06 17:41:27 -0600 )edit
2

answered 2017-04-21 12:28:13 -0600

proboscisjoe gravatar image

I solved this problem and posted a solution here on/around Feb 16, 2016. A few days later, I was forced to remove the solution by my research advisor. I am no longer working on the project that motivated my solution, so I'm reposting it now in case it helps someone still having issues.

Source code for the amcl, robot_localization, and diff_drive_controller ROS Indigo packages were modified to introduce namespacing. The "map" frame serves as the global frame that allows multiple instances of localization. Please see this github repo. Package dependencies are included in a shell script in the root of the repo. Also, there is an example ROS node, leader_follower, that operated on two huskies: the first is controlled using keyboard teleop, and the second one follows the first one around.

The only outstanding issue that I didn't get around to working on is that models in rviz launch in the center of the frame no mater where the husky vehicle is launched in Gazebo. This is very annoying and it would be great if someone could solve that problem.

I wish this could have been in everyone's hands 14 months ago, but such is life.

edit flag offensive delete link more
0

answered 2016-03-07 13:21:07 -0600

BSBingham gravatar image

Would it be possible to circulate a working example of multiple Huskies in Gazebo as a conclusion to this thread? I am trying to reproduce this example without any luck. I believe the crux of my issue is being able to set the frameId for the velocity controller in gazebo - as mentioned in one of the comments above. I am able to specify the frameId, with a different prefix for each robot, for the Gazebo-simulated IMU and GPS , but can't seem to determine how to do the same for the Gazebo-simulated odometry.

I have posted a new question to try and get at this...

http://answers.ros.org/question/22838...

but haven't had any responses.

Thank you. Brian

edit flag offensive delete link more
0

answered 2016-02-11 16:35:43 -0600

Devin gravatar image

updated 2016-02-11 16:36:21 -0600

Now, transform odom->base_link works. I can get data from odometry/filtered.But tf tree seems wrong. image description I think I have the same problem with nbeyers. sample input message:

rostopic echo -n 1  husky1/imu/data
header: 
  seq: 126441
  stamp: 
    secs: 2529
    nsecs: 140000000
  frame_id: base_link
orientation: 
  x: 8.24817187477e-05
  y: -1.54054925496e-05
  z: -0.000118581523633
  w: 0.999999989449
orientation_covariance: [2.6030820491461885e-07, 0.0, 0.0, 0.0, 2.6030820491461885e-07, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.000469267629483
  y: 0.00348299595008
  z: 0.00112975030799
angular_velocity_covariance: [2.5e-05, 0.0, 0.0, 0.0, 2.5e-05, 0.0, 0.0, 0.0, 2.5e-05]
linear_acceleration: 
  x: 0.00478306465545
  y: 0.00630121064874
  z: 9.80227133955
linear_acceleration_covariance: [2.5e-05, 0.0, 0.0, 0.0, 2.5e-05, 0.0, 0.0, 0.0, 2.5e-05]
---
edit flag offensive delete link more

Comments

After using Tom Moore's solution above, this is what my tf map looks like as well. There is no transform between odom and husky1/base_link. Were you able to solve this?

nbeyers gravatar imagenbeyers ( 2016-02-18 16:58:01 -0600 )edit

No, I cannot solve it. But I think it doesn't matter. Nothing seems wrong when I use robot_localization.

Devin gravatar imageDevin ( 2016-02-18 18:06:05 -0600 )edit

I just found a fix. In each of the husky#.gazebo.xacro files, set <frameid>husky2/base_link</frameid> for both the imu and gps controller. GPS already has the field, it just needs to be changed. For the IMU, it needs to be added. I now have 2 trees. Each with husky#/odom->husky#/base_link.

nbeyers gravatar imagenbeyers ( 2016-02-18 18:34:24 -0600 )edit

Thanks.^_^

Devin gravatar imageDevin ( 2016-02-18 18:57:40 -0600 )edit

Do you think you could provide a final launch file as a working example? I am having very similar problems with a multi-husky Gazebo setup.

BSBingham gravatar imageBSBingham ( 2016-03-05 18:29:58 -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

5 followers

Stats

Asked: 2016-02-10 19:01:20 -0600

Seen: 1,525 times

Last updated: Apr 21 '17