Simulation of multiple Husky robots

Question

Hello all,

I'm relatively new to ROS and I'm working on simulation of multiple Clearpath Huskies running some simple obstacle avoidance code that I've written. I've installed and used the husky_gazebo package to successfully simulate a single robot with my code, but I'm running into difficulties when I try to add more. The way I'm hoping to accomplish this is through the use of namespaces, but I'm afraid I don't understand them very well. Here is the launch file where I'm attempting to do this:

<launch>

  <arg name="world_name" default="worlds/empty.world"/>

  <arg name="laser_enabled" default="true"/>
  <arg name="ur5_enabled" default="false"/>
  <arg name="kinect_enabled" default="false"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(arg world_name)"/> 
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find husky_gazebo)/urdf/description.gazebo.xacro'
    laser_enabled:=$(arg laser_enabled)
    ur5_enabled:=$(arg ur5_enabled)
    kinect_enabled:=$(arg kinect_enabled)
    " />

  <group ns="husky1">
    <param name="tf_prefix" value="husky1_tf" /> 
    <include file="$(find husky_gazebo)/launch/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="1.0"/>
      <arg name="y" value="-1.0"/>
    </include>
  </group>

  <group ns="husky2">
    <param name="tf_prefix" value="husky2_tf" /> 
    <include file="$(find husky_gazebo)/launch/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="-3.0"/>
      <arg name="y" value="-1.0"/>
    </include>
  </group>

</launch>

This calls another launch file to actually spawn the vehicle:

<launch>

 

     <arg name="laser_enabled" default="true"/>
      <arg name="ur5_enabled" default="false"/>
      <arg name="kinect_enabled" default="false"/>
    
        <!-- Robot pose -->

  <arg name="x" default="0"/>
  <arg name="y" default="0"/>
  <arg name="z" default="0"/>
  <arg name="roll" default="0"/>
  <arg name="pitch" default="0"/>
  <arg name="yaw" default="0"/>

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find mtu_husky_gazebo)/urdf/mtu_description.gazebo.xacro'
    laser_enabled:=$(arg laser_enabled)
    ur5_enabled:=$(arg ur5_enabled)
    kinect_enabled:=$(arg kinect_enabled)
    " />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

  <!-- Load Husky control information -->
  <include file="$(find mtu_husky_control)/launch/control.launch"/>

  <!-- Include ros_control configuration for ur5, only used in simulation -->
  <group if="$(arg ur5_enabled)">

    <!-- Load UR5 controllers -->
    <rosparam command="load" file="$(find mtu_husky_control)/config/control_ur5.yaml" />
    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller --shutdown-timeout 3"/>

    <!-- Fake Calibration -->
    <node pkg="rostopic" type="rostopic" name="fake_joint_calibration" args="pub calibrated std_msgs/Bool true" />

    <!-- Stow the arm -->
    <node pkg="husky_control" type="stow_ur5" name="stow_ur5"/>

  </group>

  <group if="$(arg kinect_enabled)">

    <!-- Include poincloud_to_laserscan if simulated Kinect is attached -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan" output="screen">

        <remap from="cloud_in" to="camera/depth/points"/>
        <remap from="scan" to="camera/scan"/>
        <rosparam>
            target_frame: base_link # Leave empty to output scan in the pointcloud frame
            tolerance: 1.0
            min_height: 0.05
            max_height: 1.0

            angle_min: -0.52 # -30.0*M_PI/180.0
            angle_max: 0.52 # 30.0*M_PI/180.0
            angle_increment: 0.005 # M_PI/360.0
            scan_time: 0.3333
            range_min: 0.45
            range_max: 4.0
            use_inf: true

            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
            # 0 : Detect number of cores
            # 1 : Single threaded
            # 2->inf : Parallelism level
            concurrency_level: 0
        </rosparam>
    </node>

  </group>

  <!-- Spawn robot in gazebo -->
  <node name="spawn_husky_model" pkg="gazebo_ros" type="spawn_model"
    args="-x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) 
            -unpause -urdf -param robot_description -model mobile_base"/>

</launch>

But when I actually run the code, several errors occur. The first seems to be keeping me from spawning a second model of the same robot. [ERROR] [1454450853.949772124, 1298.480000000]: SpawnModel: Failure - model name mobile_base already exist.

And then there are two of these errors: [WARN] [WallTime: 1454450882.509616] [1326.730000] Controller Spawner couldn't find the expected controller_manager ROS interface. Which seem to be a problem that the namespace is causing with the robot controller.

I'm just looking for anyone who might be doing similar work who would be willing to share or possibly someone who understands these errors well enough to tell me what they mean. I'll update this question as I figure out more.

Thanks!

---

Originally posted by nbeyers on ROS Answers with karma: 161 on 2016-02-02

Post score: 3

Answer 1

Each controller is running in a single namespace (see here), so you will need to adjust that. You will also need to configure the namespace for the imu and for the gps.

You need to also modify the -model parameter in spawn_husky_model or both robots will get an error.

here's a screenshot of the two huskies moving independently after applying the modifications

---

Originally posted by Gary Servin with karma: 962 on 2016-02-02

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by nbeyers on 2016-02-03:
Thank you! This is just what I was looking for! But is there an easy way to pass the namespace from the parent launch file to all the children. What I mean is how could I set in the xacro files based on the namespaces set in the husky_empty_world launch file?

Comment by Gary Servin on 2016-02-05:
Yeah, that's the tricky part. You can pass arguments to a xacro file and use the arg to set the robotNamespace tag. The problem is that you can't pass an arg from a xacro file to an <xacro:include as stated here.

Comment by Gary Servin on 2016-02-05:
What I did was create two different husky.gazebo.xacro files and hard-code the robotNamespace for each robot there. And on the description.gazebo.xacro included each of those files depending on a namespace arg like: <xacro:include filename="/home/gary/$(arg namespace).gazebo.xacro"/>

Comment by nbeyers on 2016-02-08:
This is how I've set mine up as well, so I can confirm that it works! Thanks again! Although a can see it might be cumbersome when working with a large number of robots...

Comment by nbeyers on 2016-02-08:
Another question, since you've helped so much this far: do you use rviz or the odometry topic with your simulations? I'm not seeing anything published to "/husky1/odometry/filtered" and errors in the RobotModel like "No transform from [base_footprint] to [husky1_tf/base_link]"

Comment by nbeyers on 2016-02-09:
I've narrowed down the problem. Rviz needed me to set the TF Prefix for the robot model, so that part is solved. However, it seems that the "Odom" frame is not being published since I started using namespaces. Any idea why that might be the case?

Comment by proboscisjoe on 2016-02-20:
I have explored the odom transform issue slightly further, discovering that the husky_velocity_controller (diff_drive_controller) is hard-coded to transform from "odom" to (a parameterizable) base link. After editing the controller source to parameterize odom as well, I still cannot transform.

Comment by nbeyers on 2016-02-24:
I was able to get it partially working. The solution involved also setting the FrameID parameter in the xacro file as described here. I now have 2 full trees, each with odom but no tf between them