Simulation of multiple Husky robots

edit

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 ...