この記事が私が担当しているロボットプログラミングⅡの講義用です。
今回は前回作成した車輪型ロボットモデルをプログラムで動かしましょう。
なお、本記事は以下のGAZEBO.orgのチュートリアルを改変しています。
http://wiki.gazebosim.org/wiki/Tutorials/1.2/control_robot/mobile_base
コードが古くgazebo4.0に対応していない部分を変更し、コメントを日本語化しています。それ以外のソースコードはほとんど同じものです。本ソースコードはgazebo4.0用なので、それ以前のバージョンでは動かないかもしれません。
1. 作業ディレクトリの作成
端末を開き、以下のコマンドを実行する（$は打ち込まない）。ロボットプログラミングの演習では~/prog2ディレクトリ（フォルダ）の中にいろいろなプログラムを作っていく。
$ mkdir -p ~/prog2/4wheel
$ cd ~/prog2/4wheel
$ gedit wheel.cc
2. ソースコードの作成
プラグインとなるC++言語のソースコード(拡張子cc)を作成する。
以下のプログラムを「1.作業ディレクトリの作成」で開いたgedit（エディター）に次のソースコードをコピペしてwheel.ccを保存する。
注：コピペするときは、下プログラムの右上にあるプリンタアイコンの左にあるView Sourceアイコンをクリックして、ソースを表示して、それをコピペすること。そうしないと、コンパイルと時のエラーになやまされることになる。
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.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))
{
// 更新イベントを聞く。更新イベントは各シミュレーションループ時にブロードキャストされる。
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&MobileBasePlugin::OnUpdate, this));
}
}
public:
// model.sdfファイルからパラメータをロードする。
// "l
bool LoadParams(sdf::ElementPtr _sdf)
{
if (this->FindJointByParam(_sdf, this->leftWheelJoint,
"left_wheel_hinge") &&
this->FindJointByParam(_sdf, this->rightWheelJoint,
"right_wheel_hinge"))
return true;
else
return false;
}
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()
{
this->leftWheelJoint->SetForce(0, 0.2);
this->rightWheelJoint->SetForce(0, 0.2);
}
// モデルへのポインタ
private:
physics::ModelPtr model;
// 更新イベントへ接続するためのポインタ
private:
event::ConnectionPtr updateConnection;
// 車輪ジョイントへのポインタ
private:
physics::JointPtr leftWheelJoint;
private:
physics::JointPtr rightWheelJoint;
};
// シミュレータにこのプラグインを登録
GZ_REGISTER_MODEL_PLUGIN(MobileBasePlugin)
}
コピペしたらgeditの「保存」ボタンをクリックして~/prog2/4hello/の中に保存し、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(wheel SHARED wheel.cc)
target_link_libraries(wheel ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
4．コンパイル
以下のコマンドを実行し、ソースコードをメイクする。
$ cd ~/prog2/4wheel
$ mkdir build
$ cd build
$ cmake ../
$ make
5. モデルファイルのコピー
(1) 以下のコマンドを実行し、モデルファイルをコピーする。
$ cd ~/.gazebo/models
$ cp -r wheel_robot1 wheel_robot2
(2) model.configファイルの変更
model.configの３行目
<name>Wheel Robot 1 </name>
を
<name>Wheel Robot 2 </name>
に変更する。
(3) model.sdfファイルへのプラグイン追加
model.sdfの下から2行目にある＜／ｍｏｄｅｌ＞の上に以下のプラグインに関するコードを追加する。追加しないとロボットが動きません。
<plugin name='wheel' filename='libwheel.so'> <left_wheel_hinge>left_wheel_hinge</left_wheel_hinge> <right_wheel_hinge>right_wheel_hinge</right_wheel_hinge> </plugin>
6. 実行
以下のコマンドを実行し、gazeboを立ち上げよう！
画面左側のinsert(挿入)タブから制作したwheel robot 2を選択して、ロボットが動き出したら成功。今回はここまで。
$ export GAZEBO_PLUGIN_PATH=~/prog2/4wheel/build:$GAZEBO_PLUGIN_PATH
$ gazebo
○ 演習
前回の演習で制作した家政婦ロボットを動かしてみましょう。ただ、直進させるだけでは面白くないので、半径２ｍ程度の円を描くようなプログラムを作ろう！
コメント