GAZEBO: レーザ式測域センサを使おう！

この記事が私が担当しているロボットプログラミングⅡの講義用です。

今回は前回作成した車輪型ロボットモデルに北陽電機のLIDAR（レーザ式測域センサ）モデルを搭載して動かしましょう。
　なお、本記事は以下のGAZEBO.orgのチュートリアルを改変しています。
http://wiki.gazebosim.org/wiki/Tutorials/1.2/control_robot/mobile_base_laser
コードが古くgazebo4.0に対応していない部分を変更し、ワールド座標系での位置、姿勢の取得に関するコードを追加し、コメントを日本語化しています。それ以外のソースコードはほとんど同じものです。本ソースコードはgazebo4.0用なので、それ以前のバージョンでは動かないかもしれません。

1. 作業ディレクトリの作成
端末を開き、以下のコマンドを実行する（$は打ち込まない）。ロボットプログラミングの演習では~/prog2ディレクトリ（フォルダ）の中にいろいろなプログラムを作っていく。

$ mkdir -p ~/prog2/5sensor
$ cd ~/prog2/5sensor
$ gedit sensor.cc

2. ソースコードの作成
プラグインとなるC++言語のソースコード(拡張子cc)を作成する。

以下のプログラムを「1.作業ディレクトリの作成」で開いたgedit（エディター）に次のソースコードをコピペしてwheel.ccを保存する。
　注：コピペするときは、下プログラムの右上にあるプリンタアイコンの左にあるView Sourceアイコンをクリックして、ソースを表示して、それをコピペすること。そうしないと、コンパイルと時のエラーになやまされることになる。

//http://wiki.gazebosim.org/wiki/Tutorials/1.2/control_robot/mobile_base_laserを改変
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <stdio.h>

namespace gazebo
{
class MobileBasePlugin : public ModelPlugin
{
public:
    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        // modelへポインタを格納
        this->model = _parent;

        // このプラグイン用のパラメータをロード
        if (this->LoadParams(_sdf))
        {
            // エラーチェック
            gzerr << this->leftWheelJoint->GetAngle(0) << "\n";
            gzerr << this->rightWheelJoint->GetAngle(0) << "\n";
            // アップテートイベントを聞く。シミュレーションの繰り返し時に
            // このイベントはブロードキャストされる。
            this->updateConnection
            = event::Events::ConnectWorldUpdateBegin(
                  boost::bind(&MobileBasePlugin::OnUpdate, this));
        }
    }

public:
    bool LoadParams(sdf::ElementPtr _sdf)
    {
        // 制御用のgainパラメータを見つける
        if (!_sdf->HasElement("gain"))
        {
            gzerr << "param [gain] not found\n";
            return false;
        }
        else
        {
            // gainの値を取得
            this->gain = _sdf->Get<double>("gain");
        }

        // プラグインパラメータからセンサ名を見つける
        if (!_sdf->HasElement("ray_sensor"))
        {
            gzerr << "param [ray_sensor] not found\n";
            return false;
        }
        else
        {
            // センサ名の取得
            std::string sensorName
            = _sdf->Get<std::string>("ray_sensor");

            // SensorMangaerを使い、センサのポインタを取得
            sensors::SensorPtr sensor
            = sensors::SensorManager::Instance()->GetSensor(sensorName);

            if (!sensor)
            {
                gzerr<< "sensor by name [" << sensorName
                << "] not found in model\n";
                return false;
            }

            this->laser = boost::shared_dynamic_cast<sensors::RaySensor>(sensor);
            if (!this->laser)
            {
                gzerr << "laser by name ["
                << sensorName
                << "] not found in model\n";
                return false;
            }
        }

        // プラグインパラメーターからジョイントをロード
        if (!this->FindJointByParam(_sdf, this->leftWheelJoint,
                                    "left_wheel_hinge") ||
                !this->FindJointByParam(_sdf, this->rightWheelJoint,
                                        "right_wheel_hinge"))
            return false;

        // 成功時
        return true;
    }

public:
    bool FindJointByParam(sdf::ElementPtr _sdf,
                          physics::JointPtr &_joint,std::string _param)
    {
        if (!_sdf->HasElement(_param))
        {
            gzerr << "param [" << _param << "] not found\n";
            return false;
        }
        else
        {
            _joint
            = this->model->GetJoint(_sdf->Get<std::string>(_param));

            if (!_joint)
            {
                gzerr << "joint by name ["
                << _sdf->Get<std::string>(_param)
                << "] not found in model\n";
                return false;
            }
        }
        return true;
    }

    // ワールド更新開始イベントから呼び出される
public:
    void OnUpdate()
    {
        // ワールド座標系での位置と姿勢を取得
        math::Pose pose = this->model->GetWorldPose();
        printf("x=%.1f y=%.1f z=%.1f\n", pose.pos.x,pose.pos.y,pose.pos.z);
        printf("roll=%f pitch=%f yaw=%f\n",pose.rot.GetRoll(),pose.rot.GetPitch(),pose.rot.GetYaw());

        unsigned int n = this->laser->GetRangeCount();
        double min_dist = 1e6;
        for (unsigned int i = 0; i < n; ++i)
        {
            if (this->laser->GetRange(i) < min_dist)
                min_dist = this->laser->GetRange(i);
        }

        double target_dist = 2.0;
        if (min_dist < this->laser->GetRangeMax())
        {
            // 比例制御
            double torque = this->gain*(min_dist - target_dist );
            this->leftWheelJoint->SetForce(0,  torque);
            this->rightWheelJoint->SetForce(0, torque);
        }
        else
        {
            this->leftWheelJoint->SetForce(0, 0);
            this->rightWheelJoint->SetForce(0, 0);
        }
    }

    // モデルへのポインタ
private:
    physics::ModelPtr model;
private:
    physics::WorldPtr world;

    // ワールド状態のサブスクライブ(講読)
private:
    transport::NodePtr node;
private:
    transport::SubscriberPtr statsSub;
private:
    common::Time simTime;

    // 更新イベントコネクションへのポインタ
private:
    event::ConnectionPtr updateConnection;
private:
    physics::JointPtr leftWheelJoint;
private:
    physics::JointPtr rightWheelJoint;
private:
    sensors::RaySensorPtr laser;
private:
    double gain;
};

// シミュレータへのプラグイン登録
GZ_REGISTER_MODEL_PLUGIN(MobileBasePlugin)
}

コピペしたらgeditの「保存」ボタンをクリックして~/prog2/5hello/の中に保存し、geditメニューバーの「ファイル(F)」→「終了(Q)」でgeditを終了する。

3. コンパイル設定ファイルの作成
gazeboではソースコードをコンパイル(ビルド）するのにcmakeというシステムを使う。先程から使っている端末で以下のコマンドを実行しgeditを起動する。

$ gedit CMakeLists.txt

CMakeLists.txtはcmakeの設定ファイル。次のソースコード（２番の注参照）をgeditにコピペし、CMakeLists.txtを保存する。保存場所は今までと同じ~/prog2/4wheelディレクトリ。保存したらgeditを終了する。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})

include (FindPkgConfig)
if (PKG_CONFIG_FOUND)
  pkg_check_modules(GAZEBO gazebo)
endif()
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})

add_library(sensor SHARED sensor.cc)
target_link_libraries(sensor ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})

4．コンパイル
以下のコマンドを実行し、ソースコードをメイクする。

$ cd ~/prog2/5sensor
$ mkdir build
$ cd build
$ cmake ../
$ make

5. モデルファイルのコピー
(1) 以下のコマンドを実行し、モデルファイルをコピーする。

$ cd ~/.gazebo/models
$ cp -r wheel_robot1 wheel_robot3

(2) model.configファイルの変更
model.configの３行目

<name>Wheel Robot 1 </name>

を

<name>Wheel Robot 3 </name>

に変更する。

(3) model.sdfファイルへのプラグイン追加
model.sdfの下から2行目にある＜／ｍｏｄｅｌ＞の上に以下のLIDARセンサとプラグインに関するコードを追加する。追加しないとロボットが動きません。

    <include>
      <uri>model://hokuyo</uri>
      <pose>0.175 0 0.3 0 0 0</pose>
    </include>
    <joint name="hokuyo_joint" type="revolute">
      <child>hokuyo::link</child>
      <parent>chassis</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint> 

    <plugin name="sensor" filename="libsensor.so" >
      <left_wheel_hinge>left_wheel_hinge</left_wheel_hinge>
      <right_wheel_hinge>right_wheel_hinge</right_wheel_hinge>
      <gain>0.1</gain>
      <ray_sensor>laser</ray_sensor>
    </plugin>

6. 実行
以下のコマンドを実行し、gazeboを立ち上げよう！ ここでは、gazeboを起動するのに–verboseのオプションをつけます。これは、標準出力に位置や姿勢を表示するためです。

gazeboが起動したら画面左側のinsert(挿入)タブから制作したwheel robot 3を選択する。ロボットの前に直方体と置くとロボットが動き出したら成功。今回はここまで。

$ export GAZEBO_PLUGIN_PATH=~/prog2/5sensor/build:$GAZEBO_PLUGIN_PATH
$ gazebo --verbose

○　演習
LIDARを搭載した家政婦ロボットを動かしてみましょう。今回はロボットの後ろ5mに球を置き、それを見つけ、ドリブル（押し続ける）するプログラムを作ろう！ロボットサッカーのもっとも基本的なスキルです。

gazebo lidar

コメント

タイトルとURLをコピーしました