Controller Manager Problem for Multiple Robots in Simulation

edit

Hi, I edited my question.

I want to simulate multiple robots in gazebo and started with namespace settings. I'm following this tutorial with separated launch files for each robot.

but Controller Spawner couldn't find controller_manager ROS interface:

[INFO] [WallTime: 1462190862.012295] [0.000000] Controller Spawner: Waiting for service controller_manager/load_controller

and;

[WARN] [WallTime: 1462189886.211852] [209.507000] Controller Spawner couldn't find the expected controller_manager ROS interface.

~$ rosservice list | grep controller_manager:

/robot1/controller_manager/list_controller_types
/robot1/controller_manager/list_controllers
/robot1/controller_manager/load_controller
/robot1/controller_manager/reload_controller_libraries
/robot1/controller_manager/switch_controller
/robot1/controller_manager/unload_controller

~$ rosnode list:

/gazebo
/robot1/controller_spawner ### also quits after error above
/robot1/joy_teleop
/robot1/joystick
/robot1/joystick_relay
/robot1/robot_state_publisher
/robot1/twist_marker
/robot1/twist_marker_server
/robot1/twist_mux
/rosout

robot1.launch:

<launch>
    <group ns="robot1">
        <param name="tf_prefix" value="robot1_tf" />
        <include file="$(find mrp2_gazebo)/launch/include/single_mrp2.launch" >
            <arg name="init_pose" value="-x 3.0 -y 0.0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0" />
            <arg name="robot_name"  value="robot1" />
        </include>

        <include file="$(find mrp2_control)/launch/controls.launch" />
        <rosparam command="load" file="$(find mrp2_gazebo)/config/controls.yaml" />

        <!--include file="$(find mrp2_control)/launch/teleop.launch" /-->
    </group>
</launch>

controls.launch(working without namespace variables for one robot):

<launch>
    <group ns="robot1">
        <rosparam command="load" file="$(find mrp2_control)/config/controls.yaml" />

        <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
                args="mrp2_joint_publisher mobile_base_controller --shutdown-timeout 1  --namespace robot1" />

        <!--node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" /-->
    </group>
</launch>

gazebo.urdf.xacro's plugin lines:

<plugin filename="libgazebo_ros_control.so" name="ros_control">
    <robotNamespace>robot1</robotNamespace>
    <robotSimType>mrp2_hardware_gazebo/Mrp2HardwareGazebo</robotSimType>
    <controlPeriod>0.001</controlPeriod>
</plugin>

single_mrp2.launch:

<launch>
    <arg name="robot_name"/>
    <arg name="init_pose"/>
   <arg name="robot"  default="full"/>
    <include file="$(find mrp2_gazebo)/launch/include/mrp2.launch">
        <arg name="robot_name" default="$(arg robot_name)"/>
        <arg name="robot"  value="$(arg robot)"/>
        <arg name="gzpose" value="$(arg init_pose)"/>
    </include>
</launch>

mrp2.launch:

<launch>
  <arg name="robot_name"/>
  <arg name="init_pose" default="-x 0.0 -y 0.0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0"/>
  <arg name="robot"  default="full"/>
  <arg name="model"  default="mrp2"/>
  <arg name="gzpose" default="$(arg init_pose)"/>

  <!-- Robot model -->
  <include file="$(find mrp2_description)/launch/description.launch" >
    <arg name="robot" value="$(arg robot)"/>
  </include>

  <!-- Spawn robot in Gazebo -->
  <node pkg="gazebo_ros" type="spawn_model" name="$(anon spawn_model)"
    args="-urdf -param robot_description $(arg gzpose) -model $(arg model)" />
</launch>

description.launch:

<launch>
  <arg name="config" default="base" />
  <arg name="robot" default="full" />

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find mrp2_description)/robots/mrp2_$(arg robot).urdf.xacro'" />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="use_tf_static" value="true"/>
  </node>
</launch>

Any suggestions will be appreciated. Thanks!