ROS入門 (40) - レーザースキャナー

Gazeboでレーザースキャナーを利用する方法をまとめました。

・Noetic

前回

1. レーザースキャナー

「レーザースキャナー」は、照射されたレーザーによって、対象物の空間位置情報を取得する計測するセンサーです。短時間に大量かつ「面」的な「3次元空間の点群座標」を取得することができます。

2. URDFへのレーザースキャナーの追加

URDFへのレーザースキャナーの追加手順は、次のとおりです。

(1) レーザースキャナーの「link」と「joint」の追加。
レーザースキャナーの見た目と配置を定義しています。

・toio_style.urdfの一部

  <!--レーザースキャナー-->
  <link name="hokuyo_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.1 0.1" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.1 0.1" />
      </geometry>
    </collision>
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>
  <joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0 0 0.2" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="hokuyo_link" />
  </joint>

(2) レーザースキャナーのGazeboプラグインの追加。
レーザースキャナーの機能をGazeboでシミュレートする定義になります。更新レートは1Hzにしています。

・toio_style.urdfの一部

  <!--laserプラグイン-->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="hokuyo">
      <pose>0 0 0 0 0 0 </pose>
      <visualize>false</visualize>
      <update_rate>1</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.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name="laser" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

URDFの全コードは、次のとおりです。

・toio_style.urdf

<?xml version="1.0"?>
<robot name="toio">

  <!--ボディ-->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.3 0.3 0.23" />
      </geometry>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.3 0.3 0.23" />
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0" />
      <inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0" />
    </inertial>
  </link>

  <!--前輪-->
  <link name="front_wheel">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
      <material name="white" />
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1" />
      <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0" />
    </inertial>
  </link>
  <joint name="front_wheel_joint" type="continuous">
    <axis xyz="0 0 1" />
    <parent link="base_link" />
    <child link="front_wheel" />
    <origin rpy="-1.5708 0 0" xyz="0.1 0 -0.1" />
  </joint>

  <!--後輪-->
  <link name="back_wheel">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
      <material name="white" />
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1" />
      <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0" />
    </inertial>
  </link>
  <joint name="back_wheel_joint" type="continuous">
    <axis xyz="0 0 1" />
    <parent link="base_link" />
    <child link="back_wheel" />
    <origin rpy="-1.5708 0 0" xyz="-0.1 0 -0.1" />
  </joint>

  <!--右車輪-->
  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
      <material name="gray">
        <color rgba="0.2 0.2 0.2 1" />
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1" />
      <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0" />
    </inertial>
  </link>
  <joint name="right_wheel_joint" type="continuous">
    <axis xyz="0 0 1" />
    <parent link="base_link" />
    <child link="right_wheel" />
    <origin rpy="-1.5708 0 0" xyz="0.0 -0.125 -.1" />
  </joint>

  <!--左車輪-->
  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
      <material name="gray" />
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.035" />
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1" />
      <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0" />
    </inertial>
  </link>
  <joint name="left_wheel_joint" type="continuous">
    <axis xyz="0 0 1" />
    <parent link="base_link" />
    <child link="left_wheel" />
    <origin rpy="-1.5708 0 0" xyz="0.0 0.125 -.1" />
  </joint>

  <!--レーザースキャナー-->
  <link name="hokuyo_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.1 0.1" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.1 0.1" />
      </geometry>
    </collision>
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>
  <joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0 0 0.2" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="hokuyo_link" />
  </joint>

  <!--Gazeboプラグイン-->
  <gazebo>
    <!--differential_drive_controllerプラグイン-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <leftJoint>left_wheel_joint</leftJoint>
      <rightJoint>right_wheel_joint</rightJoint>
      <robotBaseFrame>base_link</robotBaseFrame>
      <wheelSeparation>0.25</wheelSeparation>
      <wheelDiameter>0.07</wheelDiameter>
      <updateRate>1</updateRate>
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>

    <!--joint_state_publisherプラグイン-->
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <jointName>front_wheel_joint, back_wheel_joint</jointName>
    </plugin>
  </gazebo>

  <!--laserプラグイン-->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="hokuyo">
      <pose>0 0 0 0 0 0 </pose>
      <visualize>false</visualize>
      <update_rate>1</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.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name="laser" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
</robot>

3. 動作確認

(1) Gazeboとrvizの起動。

$ roslaunch hello toio_style.launch

$ rviz

(2) rvizのDisplaysのAddボタンで「LaserScan」を追加し、Topicに「/scan」を指定。

(3) Gazeboの画面上端の円柱ボタンで、ロボットの前に円柱を配置。
rvizにレーザースキャナーでスキャンした情報が赤く表示されます。

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門