Go to Qiita Advent Calendar 2024 Top
LoginSignup
6
6

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@srs(project_srs)

ROS講座106 GazeboのModelPluginを作る

Last updated at Posted at 2019-11-24

環境

この記事は以下の環境で動いています。

項目 値
CPU Core i5-8250U
Ubuntu 20.04
ROS Noetic
Gazebo 11.9.0

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

今までロボットを動かすためにDiffDrivePlugin等のすでに用意されていたプラグインを使っていましたが、このようなプラグインを自分で記述することが出来ます。
特に駆動側のプラグインは自作したくなることがあります。その方法を説明します。

Gazeboプラグインとは

Gazebo世界に作用したり、情報を取得する追加プログラムのpluginとして作成できます。主にロボットのアクチュエーター・センサーのシミュレーション用に用いられます。SDFファイルのどのタグに挿入するかで次の6種類があります。

  • Model: モデルの設定が出来ます。jointを駆動することもできます。
  • Sensor: センサー（カメラ、lidar等）のシミュレーションをできます。
  • Visual: モデルの表示の変更が出来ます。
  • World: シミュレーションの物理演算の設定や、複数のモデルにまたがる設定ができます。
  • System: gzclientの表示の設定を扱えます。
  • GUI: gzclientにGUI（ボタン等）を追加することが出来ます。

プラグインの記述

pluginファイル

gazebo2_lecture/src/turret_plugin.hh
#pragma once

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/UpdateInfo.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/physics.hh>

namespace gazebo
{
class GAZEBO_VISIBLE TurretPlugin : public ModelPlugin{
    event::ConnectionPtr update_conn_;
public:
    TurretPlugin();
    ~TurretPlugin();

    void Load(physics::ModelPtr model, sdf::ElementPtr _sdf) override;
    void Reset() override;
    void OnUpdate();
    bool GetParam(sdf::ElementPtr sdf);
    void PointCallback(const geometry_msgs::Point& point_msg);

private: 
    physics::ModelPtr model_;
    physics::WorldPtr world_;
    common::Time last_time_;
    physics::LinkPtr link_;

    ros::NodeHandle nh_;
    ros::Subscriber point_sub_;

    std::string topic_name_;
    std::string yaw_joint_name_;
    std::string pitch_joint_name_;
    float yaw_p_;
    float pitch_p_;
    float yaw_target_;
    float pitch_target_;
};
}
gazebo2_lecture/src/turret_plugin.cc
#include "turret_plugin.hh"
#include <gazebo/common/Events.hh>
#include <cmath>

namespace gazebo
{
GZ_REGISTER_MODEL_PLUGIN(TurretPlugin)

TurretPlugin::TurretPlugin() : nh_()
{
}
TurretPlugin::~TurretPlugin()
{
}

void TurretPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  gzmsg << "Load TurretPlugin\n";
  GetParam(_sdf);
  model_ = _model;
  world_ = _model->GetWorld();
  link_ = _model->GetLink("link");
  update_conn_ = event::Events::ConnectWorldUpdateBegin(std::bind(&TurretPlugin::OnUpdate, this));
  point_sub_ = nh_.subscribe(topic_name_, 10, &TurretPlugin::PointCallback, this);
}

void TurretPlugin::OnUpdate()
{
#if GAZEBO_MAJOR_VERSION >= 8
  common::Time current_time = world_->SimTime();
#else
  common::Time current_time = world_->GetSimTime();
#endif
  if ((current_time - last_time_).Double() > (1.0 / 2.0))
  {
    last_time_ = current_time;
#if GAZEBO_MAJOR_VERSION >= 8
    ignition::math::Pose3d pose = this->link_->WorldPose();
#else
    ignition::math::Pose3d pose = this->link_->GetWorldPose().Ign();
#endif
    printf("pos: %f %f %f\n", pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
    printf("rot: %f %f %f\n", pose.Rot().Roll(), pose.Rot().Pitch(), pose.Rot().Yaw());
  }

  auto yaw = model_->GetJoint(yaw_joint_name_);
  auto pitch = model_->GetJoint(pitch_joint_name_);
#if GAZEBO_MAJOR_VERSION >= 8
  yaw->SetVelocity(0, -yaw_p_ * (yaw->Position(0) - yaw_target_));
  pitch->SetVelocity(0, -pitch_p_ * (pitch->Position(0) - pitch_target_));
#else
  yaw->SetVelocity(0, -yaw_p_ * (yaw->GetAngle(0).Radian() - yaw_target_));
  pitch->SetVelocity(0, -pitch_p_ * (pitch->GetAngle(0).Radian() - pitch_target_));
#endif
}

void TurretPlugin::Reset()
{
}

void TurretPlugin::PointCallback(const geometry_msgs::Point& point_msg)
{
  float hlen = sqrt(point_msg.x * point_msg.x + point_msg.y * point_msg.y);
  if (0.1 <= hlen)
  {
    yaw_target_ = atan2(point_msg.y, point_msg.x);
    pitch_target_ = -atan2(point_msg.z, hlen);
  }
}

bool TurretPlugin::GetParam(sdf::ElementPtr sdf)
{
  if (!sdf->HasElement("topic_name"))
  {
    gzmsg << "topic_name not set\n";
    return false;
  }
  else
  {
    topic_name_ = sdf->GetElement("topic_name")->Get<std::string>();
  }

  if (!sdf->HasElement("yaw_joint_name"))
  {
    gzmsg << "yaw_joint_name not set\n";
    return false;
  }
  else
  {
    yaw_joint_name_ = sdf->GetElement("yaw_joint_name")->Get<std::string>();
  }
  if (!sdf->HasElement("yaw_p"))
  {
    gzmsg << "yaw_p not set\n";
    return false;
  }
  else
    yaw_p_ = sdf->GetElement("yaw_p")->Get<float>();

  if (!sdf->HasElement("pitch_joint_name"))
  {
    gzmsg << "pitch_joint_name not set\n";
    return false;
  }
  else
  {
    pitch_joint_name_ = sdf->GetElement("pitch_joint_name")->Get<std::string>();
  }
  if (!sdf->HasElement("pitch_p"))
  {
    gzmsg << "pitch_p not set\n";
    return false;
  }
  else
    pitch_p_ = sdf->GetElement("pitch_p")->Get<float>();

  return true;
}
}  // end namespace gazebo
  • 必須なのがLoad()関数です。pluginがロードされたときに呼ばれます。ここで初期化をします。
  • Load()関数の中でコールバック関数(OnUpdate())を登録します。コールバック関数はgazeboのシミュレーションの更新のたび(初期設定だと1000回/秒)に呼ばれます。
  • printf系は1000回/秒も呼ばれては大変なので上記のプログラムのように間引きます。
  • Load関数の引数でsdfのパラメーターへアクセスするポインタとgazeboモデルへのアクセスをするポインタを取得できます。
  • sdfパラメーターの操作
    • sdfでのパラメーターが存在するかは`sdf->HasElement("pitch_joint_name")‘のようにしてboolで判定できます。
    • 実際の値はsdf->GetElement("pitch_joint_name")->Get<std::string>()のようにして値を取得できます。
  • gazeboモデルへの操作
    • model_->GetJoint(joint_name)でモデル中のjointへのアクセスするポインタを取得できます。ジョイントの名前が間違っているとNullが返ってきてそれを使おうとするとexceptionが起きるので注意しましょう。
    • joint_ptr->GetAngle(0).Radian()でjointの角度を取得できます。GetAngle(0)の0はjointの中の軸のindexです。2軸以上に曲がるジョイント(universalとかballとか)はここのindexでどの方向化を指定します。
    • joint_ptr->SetVelocity(value)でjointの回転の速度を指定できます。SetForce()など力の次元でも指定できます。

CmakeListsの例

gazeboプラグインをビルドには以下の要素が必須です。

cmakeの一部
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_plugin)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  gazebo_plugins
)
find_package(gazebo REQUIRED)

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${GAZEBO_INCLUDE_DIRS}
)
add_library(TurretPlugin SHARED 
  src/turret_plugin.cc
)
target_link_libraries(MyPlugin 
  ${catkin_LIBRARIES}
  ${GAZEBO_LIBRARIES}
)
  • add_compile_options(-std=c++11)が必須です。
  • gazebo関連のリンクが必要です。
    • find_package(gazebo REQUIRED)でcmakeパッケージを読み込みます。
    • include_directories(${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})でincludeディレクトリを追加します
    • add_library(TurretPlugin SHARED src/turret_plugin.cc)でライブラリのビルドを追加します。pluginlibと違い1つのsoファイルには1つのpluginのみを入れます。
    • target_link_libraries(MyPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})でライブラリをリンクします。

ビルド

cd ~/catkin_ws
catkin build

シミュレーションの起動

ソースコード

model.sdf

modelプラグインは以下のようにmodelタグのの中で設定します。filenameのところにはlib<ライブラリ名>.soと入れます。ライブラリ名はCmakeListsの中で定義したものです。
pluginタグの中で記述した要素がpluginのにsdf構造体へのポインタとして渡されます。

gazebo2_lecture/models/robot2/model.sdfの一部
<?xml version='1.0'?>
<sdf version="1.4">
  <model name="robot2">

    <!-- 省略 -->

    <plugin name="turret_controller" filename="libTurretPlugin.so">
      <topic_name>cmd_pos</topic_name>
      <yaw_joint_name>yaw_joint</yaw_joint_name>
      <yaw_p>1.0</yaw_p>
      <pitch_joint_name>pitch_joint</pitch_joint_name>
      <pitch_p>1.0</pitch_p>
    </plugin>
  </model>
</sdf>

実行

実行
roslaunch gazebo_ros empty_world.launch world_name:=turret_robot.world.world verbose:=true

/cmd_posにtopicを投げると動きます。

rostopicの送信
ostopic pub -1 /cmd_pos geometry_msgs/Point {2.0,1.0,1.0}

sim3_robot2.gif

参考

plugin tutorial
SDF tutorial
gazebo plugin tutorial
model plugin tutorial
joint class referrence

目次ページへのリンク

ROS講座の目次へのリンク

6
6
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
6
6

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?