Gazebo with multiple odometery frames

asked 2016-03-06 20:19:21 -0600

BSBingham
108 ●4 ●8 ●13

I have been trying to put together a multiple (n=2) Husky robot simulation for testing purposes. I am struggling with getting the namespaces and tf_prefixes setup in a consistent way. Following previous discussions here which builds on the single Husky tutorials, I have been able to get almost there. At this point I believe the gist of my issue is how to tell Gazebo to use a tf_prefix for the "odom" frame.

My understanding is that robot_localization (ekf_localization) takes the imu and odom data generated from Gazebo to generate the odometry messages on the odometry/filtered topic. For the imu I am able to edit the robot_description parameters in the xacro file to have a frameId for each robot. For example...

<gazebo>
      <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
        <robotNamespace>/r1</robotNamespace>
        <frameId>r1_tf/base_link</frameId>
       ...
      </plugin>
  </gazebo>

This works as expected and the ekf_localization node is able to work with the imu observations.

What I don't know how to do is to prescribe the same frame prefix (e.g., r1_tf) for the odometry generated by Gazebo. Is there a way to tell Gazebo what tf frame to use for the odometry messages?

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2016-03-15 22:57:19 -0600

BSBingham
108 ●4 ●8 ●13

updated 2016-03-15 22:59:05 -0600

Thank you nbeyers. That was the hint I needed to get everything working. it was not clear to me, and is still not completely clear, how the controller_manager loads these controllers into gazebo, but for now I have a working example.

I was able to avoid having multiple control.yaml files for each robot by setting the base_frame_id parameter directly, overwriting the value form the control.yaml file.

 <rosparam command="load" file="$(find husky_control)/config/control.yaml" />
 <rosparam param="husky_velocity_controller/base_frame_id" subst_value="True">$(arg tfpre)/base_link</rosparam>
  <node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="husky_joint_publisher husky_velocity_controller --shutdown-timeout 3"/>

This lead to a somewhat satisfying result where I could spawn multiple Huskies succinctly by only changing one launch file. Something like this...

<group ns="h1">
  <include file="$(find nre_simmultihusky)/launch/husky.launch">
    <arg name="namespace" value="h1" />
    <arg name="initX" value="2" />
    <arg name="initY" value="2" />
    <arg name="initYaw" value="0" />
   </include>
 </group>

<group ns="h2">
  <include file="$(find nre_simmultihusky)/launch/husky.launch">
    <arg name="namespace" value="h2" />
    <arg name="initX" value="-3" />
    <arg name="initY" value="-3" />
    <arg name="initYaw" value="3.14" />
  </include>
</group>

I wish I could find a way to cleanly only have to enter the namespace (e.g., 'h1') once instead of having to repeat it in group and arg tags.

I've posted my solution here https://github.com/bsb808/nre_simmult...

I have to say, I was pretty surprised by how much effort it took to change the namespace and tf_prefix in all the appropriate places to get this to run correctly. I'm new to ROS, but I expected the use of the group tag with a namespace to percolate through components better. I also didn't appreciate the headache that the propagating the tf_prefix appropriately would be - to a new person it seems redundant with the namespace.

Thanks again - glad to get it up and running.

edit flag offensive delete link

add a comment

answered 2016-03-13 12:18:13 -0600

nbeyers
161 ●5 ●9 ●15

Based on my previous experience and the question that you linked to, I've found a few places where I needed to change the frame_id parameter. The first is in the initial launch file where you configure the namespaces. It'll look something like this:

  <group ns="husky1">
    <param name="tf_prefix" value="husky1" /> 
    <include file="$(find husky_gazebo)/launch/multi_spawn_husky.launch">
      <arg name="laser_enabled" value="$(arg laser_enabled)"/>
      <arg name="ur5_enabled" value="$(arg ur5_enabled)"/>
      <arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
      <arg name="x" value="-7.0"/>
      <arg name="y" value="0.0"/>
      <arg name="namespace" value="husky1"/>
    </include>
  </group>

I'm assuming you've done this. This covers all the transforms within the model for the robot, but not odometry. The odometry transform is broadcast by the robot_localization node and this gets data from a couple of other topics, specifically imu/data and husky_controller_velocity/odom. You'll have to configure both of these as well. For the IMU, what you did is correct; set the frameId tag in the huskyN.gazebo.xacro file for each robot. (As a side note, if you plan on using GPS, you'll also want to add the tag for that as well. It's not included by default, but the frameId tag can be used in the same way as with the IMU).

Finally, and I think this is where you're problems arise, you'll have to configure the husky_velocity_controller. This is started in the control.launch file in the husky_control package, which loads a set of parameters in control.yaml. I've edited mine to look like this:

husky_joint_publisher:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

husky_velocity_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: ['front_left_wheel', 'rear_left_wheel']
  right_wheel: ['front_right_wheel', 'rear_right_wheel']
  publish_rate: 50
  pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  cmd_vel_timeout: 0.25

  # Base frame_id
  base_frame_id: husky1/base_link
...

Notice the last line that I showed. That's where to set the frame ID for the velocity controller odometry. For me, I wasn't sure how to pass arguments to a .yaml file, so I just made a separate file for each robot and called the correct one using an argument in control.launch just like I did for the .xacro files. Now your IMU and velocity-based odometry should have a frame recognized by robot_localization and huskyN/odometry/filtered topic for each robot should work as it did with the single husky simulator.

edit flag offensive delete link

add a comment

Gazebo with multiple odometery frames

2 Answers

Question Tools

Stats

Related questions

Gazebo with multiple odometery frames edit

2 Answers

Question Tools

Stats

Related questions

Gazebo with multiple odometery frames