이타인클럽

Kobuki 모델에 Hokuyo 센서 모델 추가하기

Kobuki에 Hokuyo 센서를 달아보아요
토픽 이타인클럽 > ROS > Multi-robot Simulation

토픽 결과물: ROS Kinetic용의 Kobuki에 Laser 센서가 추가돼요.

 

현 시점에서 Indigo에서 지원되던 turtlebot gazebo가 Kinetic에서는 지원이 원활하지 않아서 Kinetic 버전의 코스를 추가합니다. 이번 토픽은 ROS Indigo에서 했던 내용을 Kinectic 버전으로 수정합니다. 특히나, multi-robot을 위해 수정합니다.

준비 사항

(현재) 아래의 kobuki 패키지는 ROS Kinetic에 포함되었으므로 apt-get으로 설치할 수 있습니다.

$ sudo apt-get install ros-kinetic-kobuki*
 

(과거) 이번 강좌를 따라하기 위해서는 Kinetic용 kobuki package들이 설치되어 있어야 합니다. 이곳에서 kobuki_desktop을 clone한 후 branch를 kinetic으로 변경합니다.

$ cd ~/catkin_ws/src
$ git clone https://github.com/rohbotics/kobuki_desktop.git
$ git checkout kinetic

 

이번 강좌를 위해서 package를 하나 만들고, 아래와 같이 디렉터리들을 만듭니다.

$ cd ~/cakin_ws/src
$ catkin_create_pkg kobuki_hokuyo
$ cd kobuki_hokuyo
$ mkdir -p urdf/meshes
$ mkdir worlds
$ mkdir -p launch/includes

Kobuki URDF 준비

Kobuki의 URDF를 변경할 것이므로, 변경할 URDF 파일을 위에서 생성한 package로 복사합니다. 먼저 kobuki_description 패키지에 있는 urdf 파일들을 복사합니다.

$ roscd kobuki_description/urdf
$ cp * ~/catkin_ws/src/kobuki_hokuyo/urdf

kobuki_description에서 kobuki meshes 파일 및 images 파일을 복사합니다.

$ roscd kobuki_description
$ cp -r meshes/ ~/catkin_ws/src/kobuki_hokuyo/urdf/meshes/

hokuyo 센서 모델의 mesh파일을 이곳에서 복사하여 urdf/meshes 디렉터리 밑에 저장합니다.

현재 위 링크가 죽었네요.

 

Hokuyo 모델 생성

생성에 이곳를 참고하였습니다. 아 이곳도 링크가 죽었네요.

Hokuyo URDF 모델을 hokuyo.urdf.xacro이라는 이름으로 아래와 같이 생성합니다.

<?xml version="1.0"?>
<robot name="sensor_hokuyo" xmlns:xacro="http://ros.org/wiki/xacro"
                            xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
                            xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">

  <xacro:property name="hokuyo_link" value="0.1" /> <!-- Size of square 'hokuyo' box -->
  <xacro:macro name="sensor_hokuyo" params="name parent *origin">
    <joint name="${name}_joint" type="fixed">
      <axis xyz="0 1 0"/>
      <insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>

    <link name="${name}_link">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <box size="${hokuyo_link} ${hokuyo_link} ${hokuyo_link}"/>
        </geometry>
      </collision>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://kobuki_hokuyo/urdf/meshes/hokuyo.dae"/>
        </geometry>
      </visual>

      <inertial>
        <mass value="0.0001"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>
    </link>

  <!-- hokuyo -->
  <gazebo reference="${name}_link">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.570796</min_angle>
            <max_angle>1.570796</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.20</min>
          <max>6.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>${name}_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  </xacro:macro>
</robot>

kobuki.urdf.xacro를 열어서 아래와 같은 세 곳에서 meshes 파일의 위치만 변경하고 저장합니다.

<mesh filename="package://kobuki_hokuyo/urdf/meshes/main_body.dae" />
<mesh filename="package://kobuki_hokuyo/urdf/meshes/wheel.dae"/>
<mesh filename="package://kobuki_hokuyo/urdf/meshes/wheel.dae"/>

kobuki_standalone.urdf.xacro를 복사하여 kobuki_hokuyo.urdf.xacro을 만들고 아래와 같이 수정하여 위에서 생성한 Hokuyo 모델을 include합니다. 이 때 사용되는 urdf 파일들은 새로 생성한 패키지의 파일들입니다.

<!-- kobuki_hokuyo.urdf.xacro -->
<?xml version="1.0"?>
<robot name="kobuki_hokuyo"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:xacro="http://ros.org/wiki/xacro">

  <!-- Defines the kobuki component tag. -->
  <xacro:include filename="$(find kobuki_hokuyo)/urdf/kobuki.urdf.xacro" />
  <!-- include hokuyo model and set its pose -->
  <xacro:include filename="$(find kobuki_hokuyo)/urdf/hokuyo.urdf.xacro"/>
		<xacro:sensor_hokuyo name="laser" parent="base_link">
        <origin xyz="0 0 0.12" rpy="0 0 0"/>
    </xacro:sensor_hokuyo>
  <kobuki/>
</robot>

gazebo world 파일 생성

$ gazebo

gazebo를 실행시켜서 Edit -> Building Editor를 클릭하여 world 모델을 만듭니다. 여기서 world 파일명은 simple.world로 하였습니다. 아래와 같이 간단한 모델을 만듭니다. 만들 때 한가지 팁은 wall의 시작 위치들을 직접 입력하여 모델의 위치를 잡는 것입니다. 나중에 kobuki 초기 위치 설정등에 유용하게 쓸 수 있습니다. 참고로 본 토픽에서는 world의 시작점을 (0,0)으로 설정하였습니다. 

gazebo 파일 준비

kobuki_gazebo 패키지에서 아래와 같이 robot.launch.xml 파일을 복사합니다.

$ roscd kobuki_hokuyo/launch/includes
$ roscp kobuki_gazebo robot.launch.xml .

robot.launch.xml 파일을 열어서 아래와 같이 새로 생성한 패키지를 사용하고 kobuki_standalone.urdf.xacro를 위에서 생성한 kobuki_hokuyo.urdf.xacro로 변경합니다. 또한, kobuki의 시작점을 설정합니다. 여기서는 1.5, 1.5, 0.0 으로 하였습니다.

<!--
  Spawns Kobuki inside a Gazebo simulation
  -->
<launch>
  <arg name="robot_name" value="mobile_base"/>
  <param name="robot_description"
        command="$(find xacro)/xacro.py '$(find kobuki_hokuyo)/urdf/kobuki_hokuyo.urdf.xacro'"/>
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_$(arg robot_name)" 
        args="-x 1.5 -y 1.5 -z 0 -unpause -urdf -param robot_description -model $(arg robot_name)" respawn="false">
  </node>
  
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>
  
  <node pkg="nodelet" type="nodelet" name="$(arg robot_name)_nodelet_manager" args="manager"/>
</launch>

마지막으로, 위에서 생성한 gazebo world로 hokuyo가 부착된 kobuki 모델을 gazebo에서 spawn하는 launch 파일을 만듭니다.

$ roscd kobuki_hokuyo/launch
$ touch kobuki_hokuyo.launch

그 다음 kobuki_hokuyo.launch 파일에 다음과 같이 내용을 입력합니다.

<!-- Launches Kobuki Gazebo simulation in an empty world -->
<launch>
  <!-- start Gazebo with an empty world -->
  <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 kobuki_hokuyo)/worlds/simple.world"/>
  </include>
  
  <!-- spawn the robot -->
  <include file="$(find kobuki_hokuyo)/launch/includes/robot.launch.xml"/>
</launch>

위 launch 파일을 실행합니다.

$ roslaunch kobuki_hokuyo kobuki_hokuyo.launch

그러면 아래와 같이 kobuki base에 hokuyo 센서 모델이 추가된 것을 확인할 수 있습니다. 파란색 rays는 visualize 옵션을 false하면 사라집니다. 

댓글

댓글 본문