Help us understand the problem. What is going on with this article?

ROS抜きでGazeboのモデルを動かす

More than 1 year has passed since last update.

ROSアプリの検証によく使われる3DシミュレーターGazeboであるが、Gazebo自体はROSとは独立しているため単体でも動作は可能である。

手っ取り早くはGazeboモデルがsubscribeする/gazebo/default/<モデル名>/joint_cmdに対してコマンドを投げるgz jointコマンドが使用可能であるが、この記事ではいろいろ細かい制御が可能となるプラグインを作成して動作させてみることにする。

環境はGazebo9を想定。

リポジトリはこちら
https://github.com/maueki/example_gazebo_standalone_plugin

流れ

作るものはメッセージ、モデル、プラグインの3つ。
このプラグインが

  1. Gazeboメッセージ(≠ROSメッセージ)を受信し
  2. シミュレーターのアップデートごとにコールバックを仕掛けて
  3. コールバック内で受信したモーター値をモデルに設定

することで動作をおこなう。

準備

$ mkdir standalone_plugin

以下このディレクトリで作業をおこなう

プラグイン作成

Gazeboプラグインを作ったこと無いという人は以下を読んでおくと良い。
http://gazebosim.org/tutorials/?tut=plugins_hello_world

まずはシミュレーター更新毎にコールバックを呼び出すだけのプラグインを作ってみる。

standalone_plugin/plugin.hh
#pragma once

#include <gazebo/common/Plugin.hh>
#include <gazebo/common/UpdateInfo.hh>

namespace gazebo
{

class GAZEBO_VISIBLE StandalonePlugin : public ModelPlugin
{
    event::ConnectionPtr update_conn_;
public:
    StandalonePlugin();
    ~StandalonePlugin();

    void Load(physics::ModelPtr model, sdf::ElementPtr _sdf) override;
    void Reset() override;

    void OnUpdate();
};

}
standalone_plugin/plugin.cc
#include "plugin.hh"
#include <gazebo/common/Events.hh>

namespace gazebo {

GZ_REGISTER_MODEL_PLUGIN(StandalonePlugin)

StandalonePlugin::StandalonePlugin(){}
StandalonePlugin::~StandalonePlugin(){}

void StandalonePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
    gzmsg << "Load StandalonePlugin\n";

    update_conn_ = event::Events::ConnectWorldUpdateBegin(std::bind(&StandalonePlugin::OnUpdate, this));
}

void StandalonePlugin::OnUpdate() {
    gzmsg << "call OnUpdate\n";
}

void StandalonePlugin::Reset() {
}

}

プラグインのロード時に呼ばれるLoad()でコールバックの設定をおこなっている。
残念ながらevent::Events::ConnectWorldUpdateBegin()にラムダ式を渡すことはできないようだ。

standalone_plugin/CMakeLists.txt
# 2.8.8 required to use PROTOBUF_IMPORT_DIRS
cmake_minimum_required(VERSION 2.8.8 FATAL_ERROR)
set (CMAKE_CXX_FLAGS "-g -Wall -std=gnu++17")

include (FindPkgConfig)
pkg_check_modules(GAZEBO gazebo)
pkg_check_modules(SDF sdformat)

include_directories(
  ${GAZEBO_INCLUDE_DIRS}
  )

add_library(StandalonePlugin SHARED plugin.cc )

ここではC++17を指定しているがC++11でも十分動くはずだ。

試しにビルドしてみる

$ mkdir build
$ cd build
$ cmake ..
$ make

問題なければlibStandalonePlugin.soができているはずだ。

モデル作成

作業ディレクトリ直下で以下のコマンドでGazeboを起動する。

GAZEBO_PLUGIN_PATH=build gazebo --verbose

プラグインが見えるようにGAZEBO_PLUGIN_PATHを指定。
--verbosegzmsgの出力を確認したいため。

モデルはこんな感じ。

Screenshot from 2019-03-11 14-40-44.png

  • 四輪
  • 今回は実装しないが操舵も見据えてジョイント種はUniversal
  • Axis1: Z, Axis2: -Y
  • ジョイント名はそれぞれ、front_right, front_left, rear_right, rear_left

そして「Addボタン」から先程作成したプラグインを指定する。

select_plugin.png

モデルを保存してGazeboを再起動し、作成したモデルを置いてやると以下のように出力されプラグインが動作していることがわかる。

[Msg] Load StandalonePlugin
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
[Msg] call OnUpdate
...

 メッセージ

メッセージについてのチュートリアルは以下、一読しておくと良い
http://gazebosim.org/tutorials?tut=custom_messages

今回はとりあえず速度([rad/s])をint32で送ることにする

standalone_plugin/msgs/velocity.proto
syntax = "proto2";
package standalone_plugin.msgs;

message Velocity
{
  required int32 vel = 1;
}

GazeboのメッセージはROSのメッセージとは異なりProtobufフォーマットで定義するようだ。

CMakeLists.txtは以下の通り

standalone_plugin/msgs/CMakeLists.txt
cmake_minimum_required(VERSION 3.0)
find_package(Protobuf REQUIRED)

include (FindPkgConfig)
pkg_check_modules(GAZEBO gazebo)

set (msgs
  velocity.proto
)

PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs})

add_library(msgs SHARED ${PROTO_SRCS})
target_link_libraries(msgs ${PROTOBUF_LIBRARY})

作業ディレクトリ直下のCMakeLists.txtにadd_subdirectory(msgs)を追加して

standalone_plugin/CMakeLists.txt
@@ -10,4 +10,6 @@ include_directories(
   ${GAZEBO_INCLUDE_DIRS}
   )

+add_subdirectory(msgs)
+
 add_library(StandalonePlugin SHARED plugin.cc )

ビルドできることを確認する。ビルドが成功すればbuild/msgs/libmsgs.soができているはず。

Gazeboメッセージ受信と速度指定

コードは以下
https://github.com/maueki/example_gazebo_standalone_plugin/blob/master/plugin.hh
https://github.com/maueki/example_gazebo_standalone_plugin/blob/master/plugin.cc

StandalonePlugin::Load()

定義したメッセージを受け取るため、subscribeをおこなう。
ここでは/velocityというパスでメッセージが送られてくることを仮定。

standalone_plugin/plugin.cc
    node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
    node_->Init();

    sub_ = node_->Subscribe("/velocity", &StandalonePlugin::CbMsgVelocity, this);

StandalonePlugin::CbMsgVelocity()

Gazeboメッセージで送られてくる速度を受信しメンバに格納する。

standalone_plugin/plugin.hh
using MsgVelocityPtr = const boost::shared_ptr<const standalone_plugin::msgs::Velocity>;
standalone_plugin/plugin.cc
void StandalonePlugin::CbMsgVelocity(MsgVelocityPtr& recv_vel) {
    vel_ = recv_vel->vel();
}

StandalonePlugin::OnUpdate()と同一スレッドで動作するか不明だったためvel_std::atomic<int32_t>型にしてある。

StandalonePlugin::OnUpdate()

速度値をジョイントに設定する。

standalone_plugin/plugin.cc
void StandalonePlugin::OnUpdate() {
    model_->GetJoint("front_right")->SetVelocity(0, vel_);
    model_->GetJoint("front_left")->SetVelocity(0, vel_);
    model_->GetJoint("rear_right")->SetVelocity(0, vel_);
    model_->GetJoint("rear_left")->SetVelocity(0, vel_);
}

SetVelocity()の第一引数は軸のインデックス。

CMakeLists.txt

特筆すべきものはないが、target_link_librariesは忘れないように。

standalone_plugin/CMakeLists.txt
# 2.8.8 required to use PROTOBUF_IMPORT_DIRS
cmake_minimum_required(VERSION 2.8.8 FATAL_ERROR)
set (CMAKE_CXX_FLAGS "-g -Wall -std=gnu++17")

include (FindPkgConfig)
pkg_check_modules(GAZEBO gazebo)
pkg_check_modules(SDF sdformat)

include_directories(
  ${GAZEBO_INCLUDE_DIRS}
  ${CMAKE_CURRENT_BINARY_DIR}/msgs
  )
link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs)

add_subdirectory(msgs)

add_library(StandalonePlugin SHARED plugin.cc )
target_link_libraries(StandalonePlugin ${GAZEBO_LIBRARIES} msgs)

実行

さて準備ができたので動作させたいわけだが、ROSと異なりGazeboメッセージはターミナルからpublishする手段がどうやら無いらしい。

仕方ないので、publishするコードを書いてみた。

standalone_plugin/publish_vel.cc
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo_client.hh>

#include "velocity.pb.h"

int main(int _argc, char **_argv)
{
  // Load gazebo
  gazebo::client::setup(_argc, _argv);

  // Create our node for communication
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  auto adv = node->Advertise<standalone_plugin::msgs::Velocity>("/velocity");

  gazebo::common::Time::MSleep(1000);
  standalone_plugin::msgs::Velocity vel;
  vel.set_vel(1);
  adv->Publish(vel);

  // Make sure to shut everything down.
  gazebo::client::shutdown();
}

CMakeLists.txtに追加してビルド

standalone_plugin/CMakeLists.txt
@@ -17,3 +17,5 @@ add_subdirectory(msgs)
 add_library(StandalonePlugin SHARED plugin.cc )
 target_link_libraries(StandalonePlugin ${GAZEBO_LIBRARIES} msgs)

+add_executable(publish_vel publish_vel.cc msgs)
+target_link_libraries(publish_vel ${GAZEBO_LIBRARIES} msgs)

gazebo起動してもでるを配置した後実行する。

$ ./publish_vel

動いた。

standalone.gif

itage
ITAGEは「IT」のAGENCYになることを夢、目標として進化、変化していきます。「It’s It Agency」
http://www.itage.co.jp
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした