Controller と HardwareInterface との間の処理の仕組み（2. RobotHWSimのプラグインについて）

目次

1. ロボットモデルの定義と登録
   1. URDFでロボットモデルを定義する
   2. HardwareInterfaceの存在意義
   3. URDFでGazeboの設定をする
   4. ロボットモデルをパラメータサーバに登録する
2. RobotHWSimのプラグインについて　←　イマココ
   1. プラグインの定義
   2. プラグインで定義された処理が実行される場所
   3. RobotHWSinとRobotHWとの対比
   4. プラグインとして使えるようにする
   5. DefaultRobotHWSimについて
3. Controllerについて
   1. GazeboにロボットモデルをSpawnする
   2. Controllerを起動する
   3. ControllerManagerとHardwareInterfaceとのマッピング

お題

• 書籍
  • Learning ROS for Robotics Programming - Second Edition Chapter 10: Manipulation with MoveIt!
    
    
    
• リポジトリ
  • github: AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition

はじめに

「1. ロボットモデルの定義と登録」の説明において，gazebo.urdf.xacroでrosbook_arm_hardware_gazebo/ROSBookArmHardwareGazeboというカスタムプラグインを指定した箇所がありました．この正体を棚上げにしていたので，ここで理解を深めることにします．

本家サイトの図において，下記ピンクで囲った箇所です．

本記事用のまとめの図においては，下記の緑で囲った箇所です．

プラグインの定義

このカスタムプラグインの実体は，rosbook_arm_hardware_gazeboパッケージの中にあり，C++で実装されています．大事なところだけ覗いてみます．

rosbook_arm_hardware_gazebo/include/rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h

  // 省略
#include <hardware_interface/robot_hw.h>
  // 省略
#include <gazebo_ros_control/robot_hw_sim.h>
  // 省略
namespace rosbook_arm_hardware_gazebo
{

class ROSBookArmHardwareGazebo : public gazebo_ros_control::RobotHWSim
{
public:

  ROSBookArmHardwareGazebo();

  bool initSim(const std::string& robot_namespace,
      ros::NodeHandle model_nh,
      gazebo::physics::ModelPtr parent_model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions);

  void readSim(ros::Time time, ros::Duration period);

  void writeSim(ros::Time time, ros::Duration period);
  // 省略
private:
  // 省略
  std::vector<double> jnt_pos_;
  std::vector<double> jnt_vel_;
  std::vector<double> jnt_eff_;

  std::vector<double> jnt_pos_cmd_;

  std::vector<gazebo::physics::JointPtr> sim_joints_;

  // Hardware interface: joints
  hardware_interface::JointStateInterface    jnt_state_interface_;
  hardware_interface::PositionJointInterface jnt_pos_cmd_interface_;
  // 省略
};

} // namespace rosbook_arm_hardware_gazebo

#endif // ROSBOOK_ARM_HARDWARE_GAZEBO_H

initSim(), readSim(), writeSim() メソッドが重要です．HardwareInterfaceとControllerとのやりとりや，HardwareInterfaceとGazeboとのやりとりを実現する部分です．これらのメソッドは，gazebo_ros_control::RobotHWSimクラスで純粋仮想メソッドとして宣言されているので，カスタマイズしたメソッドでオーバーライドしてあげます．定義については，下記のコードで見てみましょう．

rosbook_arm_hardware_gazebo/src/rosbook_arm_hardware_gazebo.cpp

  // 省略
#include <rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h>

namespace rosbook_arm_hardware_gazebo
{
  using namespace hardware_interface;
  // 省略
  bool ROSBookArmHardwareGazebo::initSim(const std::string& robot_namespace,
      ros::NodeHandle nh,
      gazebo::physics::ModelPtr model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions)
  {
    using gazebo::physics::JointPtr;

    // Cleanup
    sim_joints_.clear();
    jnt_pos_.clear();
    jnt_vel_.clear();
    jnt_eff_.clear();
    jnt_pos_cmd_.clear();

    // Simulation joints
    sim_joints_ = model->GetJoints();
    n_dof_ = sim_joints_.size();

    std::vector<std::string> jnt_names;
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_names.push_back(sim_joints_[i]->GetName());
    }

    // Raw data
    jnt_pos_.resize(n_dof_);
    jnt_vel_.resize(n_dof_);
    jnt_eff_.resize(n_dof_);
    jnt_pos_cmd_.resize(n_dof_);

    // Hardware interfaces
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_state_interface_.registerHandle(
          JointStateHandle(jnt_names[i], &jnt_pos_[i], &jnt_vel_[i], &jnt_eff_[i]));

      jnt_pos_cmd_interface_.registerHandle(
          JointHandle(jnt_state_interface_.getHandle(jnt_names[i]), &jnt_pos_cmd_[i]));

      ROS_DEBUG_STREAM("Registered joint '" << jnt_names[i] << "' in the PositionJointInterface.");
    }

    registerInterface(&jnt_state_interface_);
    registerInterface(&jnt_pos_cmd_interface_);

  // 省略
    }
  // 省略
    return true;
  }

  void ROSBookArmHardwareGazebo::readSim(ros::Time time, ros::Duration period)
  {
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_pos_[i] += angles::shortest_angular_distance
          (jnt_pos_[i], sim_joints_[i]->GetAngle(0u).Radian());
      jnt_vel_[i] = sim_joints_[i]->GetVelocity(0u);
      jnt_eff_[i] = sim_joints_[i]->GetForce(0u);
    }
  }

  void ROSBookArmHardwareGazebo::writeSim(ros::Time time, ros::Duration period)
  {
    // Enforce joint limits
    jnt_limits_interface_.enforceLimits(period);

    // Compute and send commands
    for (size_t i = 0; i < n_dof_; ++i)
    {
      const double error = jnt_pos_cmd_[i] - jnt_pos_[i];
      const double effort = pids_[i].computeCommand(error, period);

      sim_joints_[i]->SetForce(0u, effort);
    }
  }

} // namespace rosbook_hardware_gazebo

PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)

重要な三つのメソッドがオーバーライドされていることが確認できます．概要は下記のとおりです．

• initSim()
  • パラメータサーバに登録されたロボットモデルから，joint 名を取得します．
  • 取得したjoint 名でJointStateのハンドルを作成し，Hardware Resource Interfaceをインターフェースとして登録します．状態の読み出しに使うのがhardware_interface::JointStateInterface jnt_state_interface_で，指令に書き込みに使うのがhardware_interface::PositionJointInterface jnt_pos_cmd_interface_です．（これらはいずれもhardware_interface::HardwareResourceManagerクラスのサブクラスのインスタンスです．）
  • 後々ControllerがHardwareInterfaceを介してやりとりできるように，JointStateInterfaceやPositionJointInterfaceのハンドル作成時に，joint にまつわるメンバ変数（jnt_pos_, jnt_vel_, jnt_eff_, jnt_pos_cmd_）のポインタを登録しています．
• readSim()
  • 現在のGazeboにおけるロボットモデルのjoint の状態（位置，速度，トルク）を取得して，先ほどinitSim()で渡したメンバ変数を介してHardwareInterfaceに取得した情報を教えてあげます．
• writeSim()
  • Gazeboにおけるロボットモデルのjoint に指令値を送ります．今回のサンプルの場合，hardware_interface::PositionJointInterface jnt_pos_cmd_interface_で取得した位置指令から力指令を計算し，Gazeboに送ります．

上述のイメージを図で表すと，下図のようになります．

ここまで見れば，この3つのメソッドが，プラグインの動作を決定づける重要項目であることが理解できます．

プラグインで定義された処理が実行される場所

では，これらのメソッドはいつ呼ばれるのでしょうか？

ROSとGazeboを連携させると，GazeboRosControlPluginという，GazeboのプラグインとROS間のインターフェースを制御するためのクラスが働いてくれます．ロボットモデルをSpawnする際に，robot_descriptionの情報が渡されます．使用するプラグインの情報がその中に組み込まれているので，GazeboRosControlPluginはどのプラグインを使えば良いのかを知ることができます．

では，上記プラグインのメソッドが実際に呼ばれる場所がどこになるのかが気になります．調べると，下記のリポジトリにありました．
github: ros-controls/gazebo_ros_control

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp

  // 省略
// Called by the world update start event
void GazeboRosControlPlugin::Update()
{
  // Get the simulation time and period
  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
  ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
  ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;

  robot_hw_sim_->eStopActive(e_stop_active_);

  // Check if we should update the controllers
  if(sim_period >= control_period_) {
    // Store this simulation time
    last_update_sim_time_ros_ = sim_time_ros;

    // Update the robot simulation with the state of the gazebo model
    robot_hw_sim_->readSim(sim_time_ros, sim_period);

    // Compute the controller commands
  // 省略
    controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
  }

  // Update the gazebo model with the result of the controller
  // computation
  robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
  last_write_sim_time_ros_ = sim_time_ros;
}
  // 省略

本Update()メソッドはシミュレーション周期毎にコールバックされます．本コールバック関数の登録は同クラスのメソッドであるLoad()で行われています．

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp

// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  // 省略
    // Listen to the update event. This event is broadcast every simulation iteration.
    update_connection_ =
      gazebo::event::Events::ConnectWorldUpdateBegin
      (boost::bind(&GazeboRosControlPlugin::Update, this));
  // 省略
}

Load()は，プラグインのエントリポイントであり，本メソッド内でGazeboとROSを連携するための様々な初期化が行われています．

• 追記
  • GazeboからどのようにこのLoad()を呼んでいるかについては，Gazebo から ROS のプラグインを呼ぶ処理の仕組み（2. ロボットモデル登録時のModelPluginの読込み）にて扱いました．

さて，話を戻して，肝心のUpdate()メソッドの大きな流れは次の通りです．

1. シミュレーション周期と制御周期を比較します
   • 制御周期分時間が進んでいたら，Controllerの状態を更新するため，以降の処理に移ります．
2. Gazeboモデルの状態を取得し，Controllerで使用するロボットモデルの状態を更新します．
   • ここで，RobotHWSim::readSim()の登場です．プラグインのコードでオーバライドしたメソッドがここで走ります．
   • 具体的には，前述した通り，jnt_pos_, jnt_vel_, jnt_eff_を更新します．
3. 更新したロボットモデルの情報を元に，Controllerによる制御則の計算を実行します．
   • controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs); の部分です．controller_manager_は，controller_manager::ControllerManagerクラスのインスタンスです．
   • 本インスタンスは自分が管理すべきControllerを全て知っているので，ControllerManager::update()メソッド一つで全コントローラを更新できます．
   • 例えば，ControllerがJointTrajectoryControllerであれば，前述したjnt_pos_cmd_が更新されるのがここです．
4. Controllerで算出した結果を，Gazeboモデル側に反映させます．
   • ここで，RobotHWSim::writeSim()が登場します．
   • 今回の例では，位置指令であるjnt_pos_cmd_を元に力指令を計算して，Gazebo側に書き込んでいます．

RobotHWSinとRobotHWとの対比

参考として，Gazebo利用時に使われるRobotHWSimを，実機適用時に使われるRobotHWと対比させます．RobotHWのWikiに記載されたmain()の記述を見れば，シミュレータ側との違いを比較でき，理解が深まると思います．

declaration_of_MyRobot

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
 // 省略
};

hardware_interface::RobotHWを継承して，MyRobotというオリジナルロボット用のサブクラスを作成しています．
これをどう使うのか．main()を見てみます．

main_routine

main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}

read()とwrite()は，前述したreadSim()とwriteSim()と対応します．ただし，実際のロボットのハードウェアの仕様（read()ならエンコーダ等，write()ならアクチュエータ等）に合わせて，自分でドライバ部分を実装する必要があります．
cm.update()は，まさに前述のcontroller_manager_->update()と同じ役割です．cmの宣言の部分を見ると，controller_manager::ControllerManagerのコンストラクタの引数にMyRobot robotのポインタを渡しています．cmは，robotの情報を全部知っていることになります．

実は，ここが重要です．詳細な挙動は後述しますが，ControllerManager側は，RobotHWまたはRobotHWSimの情報を知らないと，にっちもさっちもいかないことになるのです．実際，先ほどのgazebo_ros_control_plugin.cppの中でも，ControllerManagerの初期化はLoad()メソッド内で下記のように行われています．

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp

// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
 // 省略
    // Create the controller manager
    ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
    controller_manager_.reset
      (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
 // 省略
}

RobotHWとRobotHWSimでは，下記のような違いが有ります．

• RobotHWを使った先ほどのサンプルでは，ControllerManagerの初期化時にコンストラクタでRobotHWのインスタンスのポインタ渡していました．
• GazeboRosControlPluginでは，reset()というメソッドでRobotHWSimの情報をControllerManagerに渡しています．
  • コンストラクタの段階で，どんなロボットがやってくるのか，GazeboRosControlPluginでは知ったこっちゃありません．
  • 入れ物だけ作って，後でロボットの情報を上書きしてあげるイメージです．

いずれにしろ，ControllerManagerにロボットモデルの情報を渡していることは，共通しているようです．

以上が，プラグインの動作の理解を深めるために解析した内容です．

ただ，上述した説明ではRobotHWの具体的な実装は省略されています．そこで，本節の最後にRobotHWの良いサンプルについてご紹介します．

• minimum_ros_control(Github: yoneken/minimum_ros_control)
  • RobotHWのサンプルとしては，yonekenさんによるminimum_ros_controlというリポジトリが有用ですので，ここでご紹介させていただきます．
  • このサンプルでは，RobotHWを使う上で必要となる最低限の要素で構成された，非常にシンプルなロボットが扱われている，極めて見通しの良い素晴らしいサンプルです．RobotHWの理解を深める上で，ぜひ一読することをオススメします．
  • 実機を組むときにはこのminimum_ros_controlをベースにカスタマイズしていくと，効率的に開発ができると思います．

ここで，gazebo_ros_pkgでGazebo内のロボットを制御する場合と，minimum_ros_controlで実機を制御する場合のソフトウェア構成図を比較してみます．

---

モジュールとして異なる部分は，上記図中において緑の矢印で示した部分ですが，本質的には全く同一の構成を取っていることが分かります．
使用するコントローラが，JointTrajectoryControllerとJointPositionControllerで異なっていますが，これは設定というかロボットの制御の仕方の違いというレベルで，GazeboだろうがReal Robotだろうが共通で入れ替えできる部分です．ただ，周期やゲインの設定がある場合には，シミュレーションか実機かで設定値を切り替えるのが実用的でしょう．この辺りの話は，
次のエントリでも触れます．

プラグインとして使えるようにする

ここまで，プラグインの定義方法とその仕組みについて述べました．しかし，実はこれまでの作業だけではまだ使える状態にはありません．ROSシステム側に対して，プラグインを有効にするための情報を教えてあげる必要があります．
そこで，pluginlibの仕組みを使って，上記コードを正式にプラグイン化します．

• プラグインとして出力するための設定
  • プラグイン用のファイルとして出力されるように，コード側に追記が必要です．
  • コード内のどこでもよいので，PLUGINLIB_EXPORT_CLASS(登録対象のクラス名, 登録対象のクラスの親クラス名)のマクロを書きます．
  • 先ほどの例では，最終行に書いてありました．これが一般的なやり方とのことです．

rosbook_arm_hardware_gazebo/src/rosbook_arm_hardware_gazebo.cpp

// ぜーんぶ省略して，最終行のみ
PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)

• プラグイン説明ファイルの登録
  • 上記マクロでは書ききれない情報を登録します．プラグインの仕様とかはこちらです．
  • ROSシステムがこのプラグインを自動で検出・読込できるようにするために必要とのことです．
  • rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazeboの例では下記のとおりです．

rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo_plugins.xml

<library path="lib/librosbook_arm_hardware_gazebo">
  <class
    name="rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo"
    type="rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo"
    base_class_type="gazebo_ros_control::RobotHWSim">
    <description>
      ROS Book arm simulation interface.
    </description>
  </class>
</library>

• ROSパッケージシステムへのプラグインの登録
  • ROSシステム全体でこのプラグインを使えるようにするための設定をします．
  • package.xmlに<export>タグを追加します．
  • rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazeboの例では下記のとおりです．

rosbook_arm_hardware_gazebo/package.xml

  <export>
    <gazebo_ros_control plugin="${prefix}/rosbook_arm_hardware_gazebo_plugins.xml"/>
  </export>

ファイルとしての設定は以上です．以降の処理はpluginlibを参考に進めればOKです．

DefaultRobotHWSimについて

作成したROSBookArmHardwareGazeboでは，JointStateInterfaceやPositionJointInterfaceを自分で宣言しましたが，DefaultRobotHWSimではどうしているのでしょうか？デフォルトで用意されたプラグインを使う場合，私達が編集をする余地はありません．
ちょっとコードを覗いてみます．

gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h

// 省略
namespace gazebo_ros_control
{
class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
// 省略
{
// 省略
  hardware_interface::JointStateInterface    js_interface_;
  hardware_interface::EffortJointInterface   ej_interface_;
  hardware_interface::PositionJointInterface pj_interface_;
  hardware_interface::VelocityJointInterface vj_interface_;
// 省略
}
// 省略
}

JointStateInterface，EffortJointInterface，PositionJointInterfaceｍ，VelocityJointInterfaceのインスタンスの宣言がオンパレードです．
ユーザ定義の設定ファイルからどんなインターフェースが降ってくるかなど，DefaultRobotHWSimからしたら分からないことなので，代表的なインターフェースについては予め土台を作っておくようです．これをどう扱うのか，initSim()を見てみます．

gazebo_ros_control/src/default_robot_hw_sim.cpp

// 省略
namespace gazebo_ros_control
{
// 省略
bool DefaultRobotHWSim::initSim(
  const std::string& robot_namespace,
  ros::NodeHandle model_nh,
  gazebo::physics::ModelPtr parent_model,
  const urdf::Model *const urdf_model,
  std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// 省略
  n_dof_ = transmissions.size(); 
// 省略
  // Initialize values 
  for(unsigned int j=0; j < n_dof_; j++) 
  { 
// 省略
    std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
// 省略
    const std::string& hardware_interface = joint_interfaces.front();
// 省略
    // Add data from transmission
    joint_names_[j] = transmissions[j].joints_[0].name_;
// 省略
    if(hardware_interface == "EffortJointInterface")
    {
      // Create effort joint interface
      joint_control_methods_[j] = EFFORT;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_effort_command_[j]);
      ej_interface_.registerHandle(joint_handle);
    }
    else if(hardware_interface == "PositionJointInterface")
    {
      // Create position joint interface
      joint_control_methods_[j] = POSITION;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_position_command_[j]);
      pj_interface_.registerHandle(joint_handle);
    }
    else if(hardware_interface == "VelocityJointInterface")
    {
      // Create velocity joint interface
      joint_control_methods_[j] = VELOCITY;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_velocity_command_[j]);
      vj_interface_.registerHandle(joint_handle);
    }
// 省略
  }
// 省略
}
// 省略
}

URDFの<transmission>タグ内の<hardwareInterface>タグで指定したインターフェース種別を読みだして，その種別に応じて登録対象のインターフェースを切り替えています．だから，Gazeboを使うときには，<transmission>の設定でHardwareInterfaceの種別を入力する必要があったのです．

おわりに

今回は，RobotHWSimの動作にフォーカスを当てて解析を行いました．プラグインの作成方法と絡めながら，周辺のモジュールとどのように関わっているかも少し理解できるようになりました．

次回は，「3. Controllerについて」説明します．ここまで分かれば，GazeboとROSの連携に関する，一通りの流れがつかめるようになります．