Gazeboでkobukiを二台出す

Xubuntu12.04上のHydro上でkobukiを二台だす．

前提として
http://wiki.ros.org/kobuki_gazebo
でkobukiが一台出せているとします．

同じ感じでlaunchファイルを作ります．
ワークスペースで

catkin_create_pkg hoge

roscd kobuki_gazebo

してkobuki_gazeboに移動，以下をhogeへコピーします
/launch/includes/*.xml
/launch/turtlebot_empty_world.launch
/worlds/empty.world

package.xmlのrun_dependをコピーします．

これで

roslaunch hoge turtlebot_empty_world.launch

すると一台のkobukiが出るはず．
まぁ，この時点ではincludes以下のxmlは参照してないのですが．

/launch/turtlebot_empty_world.launch
を以下のように編集します.
findの参照先に注意してください．

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="world_name" value="$(find hoge)/worlds/empty.world"/>
  </include>

  <arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba -->
  <arg name="battery"   value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 --> 
  <arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
  <arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro --> 

  <group ns="r1">
    <include file="$(find hoge)/launch/includes/$(arg base).launch.xml">
      <arg name="base" value="$(arg base)"/>
      <arg name="stacks" value="$(arg stacks)"/>
      <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
      <arg name="robot_name" value="r1"/>
      <arg name="init_pose" value="-x 1 -y 1 -z 0"/>
    </include>
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
      <param name="publish_frequency" type="double" value="30.0" />
    </node>
  </group>
  
  <group ns="r2">
    <include file="$(find hoge)/launch/includes/$(arg base).launch.xml">
      <arg name="base" value="$(arg base)"/>
      <arg name="stacks" value="$(arg stacks)"/>
      <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
      <arg name="robot_name" value="r2"/>
      <arg name="init_pose" value="-x 1 -y 4 -z 0"/>
    </include>
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
      <param name="publish_frequency" type="double" value="30.0" />
    </node>
  </group>
</launch>

/launch/includes/kobuki.launch.xml
を以下のように編集します

<launch>
  <arg name="base"/>
  <arg name="stacks"/>
  <arg name="3d_sensor"/>
  <arg name="robot_name"/>
  <arg name="init_pose"/>

  <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'" />
  <param name="robot_description" command="$(arg urdf_file)" />

  <!-- Gazebo model spawner -->
  <node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
        args="$(arg init_pose) -unpause -urdf -param robot_description -model $(arg robot_name)"/>

  <!-- Velocity muxer -->
  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
  </node>

  <!-- Bumper/cliff to pointcloud (not working, as it needs sensors/core messages) -->
  <include file="$(find turtlebot_bringup)/launch/includes/kobuki/bumper2pc.launch.xml"/>
</launch>

これで

roslaunch hoge turtlebot_empty_world.launch

すると二台のkobukiが出るはず．
要はそれぞれのモデルに名前をつけてくださいという，チュートリアルにもあったことです．

動かすには以下の様な感じでscriptsを作ってrosrunすればいいです．
dependencyにrospyとgeometry_msgs追加してください．

import rospy
from geometry_msgs.msg import Twist

if __name__=="__main__":
        rospy.init_node('it')

        p=rospy.Publisher('r1/commands/velocity',Twist)
        twist = Twist()
        twist.linear.x = -0.5;
        twist.linear.y = 0.0;
        twist.linear.z = 0.0;

        twist.angular.x = 0.0;
        twist.angular.y = 0.0;
        twist.angular.z = 0.0;
        for i in range(10):
                p.publish(twist);
                rospy.sleep(0.5)

フォーマッタとか色々設定してないので汚いのは容赦してほしい．
仮想マシン上のそういう設定全くやる気が起きないんだよなぁ…．