Controller と HardwareInterface との間の処理の仕組み（3. Controllerについて）

目次

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. ロボットモデルの定義と登録」と「2. RobotHWSimのプラグインについて」で，下記図の青色で囲った部分のお話が終わりました．今回は，同図内のピンクで囲った部分のお話に移ります．

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

ただ，いきなりController云々の話から始めるスタイルは取りません．Gazeboを起動するlaunchファイルを起点に，どんなファイルが何のために読み込まれるのかを紐解きながら，徐々にControllerの中身を見ていくスタイルを取ります．

launchファイルの構造

最も単純な起動ファイルであるrosbook_arm_empty_world.launchを起点に，下記のようなファイル構成を取っています．

これを順番に見ていきます．

rosbook_arm_gazebo/launch/rosbook_arm_empty_world.launch

<launch>
  <include file="$(find rosbook_arm_gazebo)/launch/rosbook_arm_gazebo.launch">
    <arg name="world" default="empty"/>
  </include>
</launch>

empty_world でGazeboを起動するよう指定しています．次のrosbook_arm_gazebo.launchに行きます．

rosbook_arm_gazebo/launch/rosbook_arm_gazebo.launch

<launch>
  <arg name="world" default="empty"/>
  <arg name="robot" default="base"/>
  <arg name="gzpose" default="-x 0 -y 0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0"/>
  <!--省略-->
  <!-- Start up world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="$(find rosbook_arm_gazebo)/worlds/$(arg world).world"/>
    <arg name="gui" default="$(arg gui)"/>
    <arg name="debug" default="$(arg debug)"/>
  </include>

  <!-- Spawn robot in simulation -->
  <include file="$(find rosbook_arm_gazebo)/launch/rosbook_arm_spawn.launch">
    <arg name="robot"  value="$(arg robot)"/>
    <arg name="gzpose" value="$(arg gzpose)"/>
  </include>

  <!-- Bring up robot -->
  <include file="$(find rosbook_arm_bringup)/launch/rosbook_arm_bringup.launch"/>
</launch>

worldの起動、Gazebo上へのロボットモデルのSpawn(rosbook_arm_gazebo/launch/rosbook_arm_spawn.launch)、Controllerの起動(rosbook_arm_bringup/launch/rosbook_arm_bringup.launch)を行います．worldの起動はここでは重要ではないのでSpawnから行きます．
実はSpawn自体も，ここの主題を考える上ではあまり重要ではありません．しかし，そこに行き着く過程でControllerに必要なパラメータを読み込む箇所があります．実機とGazeboを切り替えるシステムを組む時のノウハウにもなるので，ここも追っておくべきと判断しました．

GazeboにロボットモデルをSpawn する

対象は下記図のピンク部分です．

では，対象範囲の頭であるrosbook_arm_spawn.launchから見てみます．

rosbook_arm_gazebo/launch/rosbook_arm_spawn.launch

<launch>
  <arg name="robot" default="base"/>
  <arg name="gzpose" default="-x 0 -y 0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0"/>

  <!-- PID gains -->
  <rosparam file="$(find rosbook_arm_controller_configuration_gazebo)/config/pids.yaml" command="load"/>

  <!-- Joint trajectory controllers -->
  <rosparam file="$(find rosbook_arm_controller_configuration_gazebo)/config/joint_trajectory_controllers.yaml" command="load"/>

  <!-- Robot model -->
  <include file="$(find rosbook_arm_description)/robots/upload_rosbook_arm.launch">
    <arg name="robot" default="$(arg robot)"/>
  </include>

  <!-- Spawn robot in Gazebo -->
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_model"
        args="-urdf -param robot_description $(arg gzpose) -model rosbook_arm"/>
</launch>

まず，いくつかyamlファイルを読み込んでいます．pids.yamlと，joint_trajectory_controllers.yamlです．いかにもController向けのパラメータが入っていそうな名前です．中を覗いてみます．例によって，shoulder_jointだけを対象に追っていきます．

rosbook_arm_controller_configuration_gazebo/config/pids.yaml

gains:
  shoulder_joint:  {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0}
  #省略

上記ファイルでは，joint の制御ゲインを設定します．

rosbook_arm_controller_configuration_gazebo/config/joint_trajectory_controllers.yaml

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - shoulder_joint
  #省略

gripper_controller:
  #省略

上記ファイルでは，使用するControllerの種類と，それを適用するjoint の対応付けを行います．
このファイルでは，arm_controllerという名前空間を指定し，position_controllers/JointTrajectoryControllerというControllerを使うことと，それに対してshoulder_jointを割り当てることが設定されていることが分かります．本当は，これ以外のjoint も割り当てているし，その他制約に関する設定も記述されておりますが，ここでは割愛します．

• 少し寄り道をします．
  • はじめ，上記yamlファイルをここで読み込むことに違和感がありました．現在対象としているrosbook_arm_spawn.launchでは，ロボットモデルをGazeboにSpawnすることが主題のはずなのに，ここで突然Controllerに必要なパラメータが出るのは何故だ？と思ったからです．
  • しかし，次のように考えれば，これに意味はありそうです．例えば，実機とGazeboのモデルが両方揃っており，切り替えて使える状況下では，それぞれの場合でパラメータを変えたいという要求があるはずです．シミュレータと実機の制御ゲインが一致するなんてことは，通常はありえないためです．

では，話を戻します．次に，upload_rosbook_arm.launchを見てみます．

rosbook_arm_description/robots/upload_rosbook_arm.launch

<launch>
  <arg name="robot" default="base"/>

  <!-- Load URDF into ROS param server -->
  <param name="robot_description" command="$(find xacro)/xacro
    '$(find rosbook_arm_description)/robots/rosbook_arm_$(arg robot).urdf.xacro'"/>

  <!-- Joint limits -->
  <rosparam file="$(find rosbook_arm_description)/config/joint_limits.yaml" command="load"/>
</launch>

以前のエントリで，demo.launchでrobot_descriptionにモデルを登録する記述がありましたが，あれはGazeboを抜きにした簡易確認用のファイルなので，ここでは不要です．Gazeboを使う場合は，upload_rosbook_arm.launchを起動すればロボットモデルがパラメータサーバに登録されます．

その後joint_limits.yamlを読み込んでいますが，関節速度や加速度のリミットを設定しています．ここは追わなくても主題に影響しないので，割愛します．

そして，いよいよモデルをSpawn します．
gazebo_rosパッケージにあるspawn_modelを起動しています．「ロボットを表示させろ」と，gazebo_rosにサービスメッセージを送っています．引数はもちろん，robot_descriptionです.これで，Gazeboにロボットが出現するはずです．
Gazeboのチュートリアルを見るとこの辺りが詳しく書いてあります．

Controllerを起動する

ようやく準備が終わり，本丸の話に入ります．対象は下記図のピンクの部分です．

ロボットの世界でもbringupという表現を使うのかと(子供が育つ，的な場面で使われるような気がする)，どうでもよいことに若干の関心を示しつつ，頭から見てみます．

rosbook_arm_bringup/launch/rosbook_arm_bringup.launch

<launch>
  <!-- Load default controllers -->
  <include file="$(find rosbook_arm_controller_configuration)/launch/default_controllers.launch"/>

  <!-- Robot state publisher -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" value="50.0"/>
    <param name="tf_prefix"         value=""/>
  </node>

  <!-- Motion planning -->
  <include file="$(find rosbook_arm_moveit_config)/launch/move_group.launch"/>

  <!--省略-->
</launch>

default_controllers.launchでControllerを起動します．
その後，robot_state_publisherとmove_groupを起動しています．ここは深入りしなくても良いのでざっくりと解釈すると，次のとおりです．

• robot_state_publihser
  • hardware_interface::JointStateInterfaceがpublishする/joint_statesトピックをsubscribeして，TFのフレームによろしく変換してくれる人
• move_group.launch
  • 後者はエンドイフェクタの経路計画とナビゲーションを司るMoveit!のノードを起動してくれる人

ここでミソなのはdefault_controllers.launchなので，ファイルの中身を見てみます．

rosbook_arm_controller_configuration/launch/default_controllers.launch

<launch>
  <!-- Joint state controller -->
  <rosparam file="$(find joint_state_controller)/joint_state_controller.yaml" command="load"/>

  <!-- Default controllers -->
  <node pkg="controller_manager" type="spawner" name="default_controllers_spawner"
        args="joint_state_controller
              arm_controller
              gripper_controller"/>
</launch>

joint_state_controller.yamlというファイルを読み込んでいます．これの位置づけは，次のとおりです．

• 先ほど，joint_trajectory_controllers.yamlというファイルを読み込んでposition_controllers/JointTrajectoryControllerを使うことを指定しました．
• joint_state_controller.yamlは，それのjoint_state_controller/JointStateController版です．

ros_controllers/joint_state_controller/joint_state_controller.yaml

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

これがjoint_state_controller.yamlの全文です．JointStateControllerでは全joint を扱うので，特定のjoint を指定する必要はないようです．

そして，ControllerをSpawn します．主役は，controller_managerパッケージのspawnerノードです．引数に，hoge_hoge_controller.yamlで設定した名前空間が入っています．この名前空間にひも付けされたtype等のパラメータを読み取って，Controllerを立ち上げるように，ControllerManagerにサービスメッセージを送ります．

このノードの正体は，ros_control/controller_manager/scripts/spawnerです．一応覗いてみます．

controller_manager/scripts/spawner

# 省略
def main():
    # find yaml files to load
# 省略
    controllers = []
    for name in args.controllers:
        if os.path.exists(name):
            # load yaml file onto the parameter server, using the namespace specified in the yaml file
            rosparam.set_param("",open(name))
            # list the controllers to be loaded
            name_yaml = yaml.load(open(name))
            for controller in name_yaml:
                controllers.append(controller)
        else:
            controllers.append(name)

    # load controllers
    for name in controllers:
        rospy.loginfo("Loading controller: "+name)
        resp = load_controller(name)
        if resp.ok != 0:
            loaded.append(name)
        else:
            time.sleep(1) # give error message a chance to get out
            rospy.logerr("Failed to load %s" % name)

    rospy.loginfo("Controller Spawner: Loaded controllers: %s" % ', '.join(loaded))

    if rospy.is_shutdown():
        return

    # start controllers is requested
    if autostart:
        resp = switch_controller(loaded, [], 2)
        if resp.ok != 0:
            rospy.loginfo("Started controllers: %s" % ', '.join(loaded))
        else:
            rospy.logerr("Failed to start controllers: %s" % ', '.join(loaded))

    rospy.spin()
# 省略

ノード起動時に引数として渡された名前空間名(joint_state_controller, arm_controller等)を元にパラメータを読み込み，Controllerの種類(position_controllers/JointTrajectoryController等)を読み出します．

load_controller()では，パラメータサーバから得た情報を基にControllerを起動するための準備をするよう，ControllerManagerにメッセージを送ります．ここでは，GazeboRosControlPlugin側でロボットモデルの情報を元にHardwareInterfaceとのマッピングをします．ControllerManagerはコンストラクタでRobotHWSimの情報を知っていることは，「2. RobotHWSimのプラグインについて」でも触れました．

そして，switch_controller()という関数にControllerの種類を渡して，ControllerManagerにControllerを起動させるメッセージを送信しています．

ControllerManagerとHardwareInterfaceとのマッピング

ここで，ControllerManagerとHardwareInterfaceとのマッピングをどう行っているのか，ちょっとだけ見てみます．

controller_manager/src/controller_manager.cpp

// 省略
namespace controller_manager{
// 省略
bool ControllerManager::loadController(const std::string& name)
{
// 省略
  boost::shared_ptr<controller_interface::ControllerBase> c;
// 省略
  // Initializes the controller
  try{
// 省略
    initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
  }
// 省略
}
// 省略
}

このメソッドは，コントローラ1つを読み込む処理を行います．
cには，Controllerのインスタンスが入ります．JointTrajectoryControllerやJointStateController等，その時指定されたControllerのサブクラスのインスタンスが入っています．そして，c->initRequest()というメソッドを実行しています．中を見てみます．

controller_interface/include/controller_interface/controller.h

namespace controller_interface
{
// 省略
  virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
                           ros::NodeHandle&             root_nh,
                           ros::NodeHandle&             controller_nh,
                           ClaimedResources&            claimed_resources)
  {
// 省略
    // get a pointer to the hardware interface
    T* hw = robot_hw->get<T>();
// 省略
    // return which resources are claimed by this controller
    hw->clearClaims();
    if (!init(hw, controller_nh) || !init(hw, root_nh, controller_nh))
    {
      ROS_ERROR("Failed to initialize the controller");
      return false;
    }
// 省略
  }
}

T* hw = robot_hw->get<T>();で，HardwareInterfaceが登場します．TはHardwareInterfaceの型が入ります．RobotHWがメンバとして持っているHardwareInterface型のインスタンスの中から，Tの型を持ったものへのポインタを取得しています．確かにこの情報があれば，RobotHWSimでいじくり回していたjnt_pos_やjnt_pos_cmd_等の値を，ControllerManger側で扱えそうな予感がします．

この先少し話がややこしいので，青写真的にイメージ図を載せておきます．白抜き矢印が継承，黒抜き矢印がデータの方向を表します．

JointStateControllerの場合

このTついて使用例を挙げます．JointStateControllerの場合は，下記のように宣言されているので，TにはJointStateInterfaceが当てはまります．

joint_state_controller/include/joint_state_controller/joint_state_controller.h

class JointStateController: public controller_interface::Controller<hardware_interface::JointStateInterface>

気になるのはinit()です．ここがミソなのは間違いなさそうです．構造が単純なJointStateControllerを例に，init()の例を見てみます．

joint_state_controller/src/joint_state_controller.cpp

// 省略
namespace joint_state_controller
{
// 省略
  bool JointStateController::init(hardware_interface::JointStateInterface* hw,
                                  ros::NodeHandle&                         root_nh,
                                  ros::NodeHandle&                         controller_nh)
  {
    // get all joint names from the hardware interface
    const std::vector<std::string>& joint_names = hw->getNames();
    num_hw_joints_ = joint_names.size();
    for (unsigned i=0; i<num_hw_joints_; i++)
      ROS_DEBUG("Got joint %s", joint_names[i].c_str());

    // get publishing period
    if (!controller_nh.getParam("publish_rate", publish_rate_)){
      ROS_ERROR("Parameter 'publish_rate' not set");
      return false;
    }

    // realtime publisher
    realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", 4));

    // get joints and allocate message
    for (unsigned i=0; i<num_hw_joints_; i++){
      joint_state_.push_back(hw->getHandle(joint_names[i]));
      realtime_pub_->msg_.name.push_back(joint_names[i]);
      realtime_pub_->msg_.position.push_back(0.0);
      realtime_pub_->msg_.velocity.push_back(0.0);
      realtime_pub_->msg_.effort.push_back(0.0);
    }
    addExtraJoints(controller_nh, realtime_pub_->msg_);

    return true;
  }
// 省略
}

const std::vector<std::string>& joint_names = hw->getNames();で，RobotHWSimが持っている全joint 名を取得します．
そのjoint 名からハンドルを取得し，addExtraJoints(controller_nh, realtime_pub_->msg_);で，Controller側のメンバであるjoint_state_と，HardwareInterface側で既に作成されているjoint のハンドルをマッピングしています．以降，JointStateControllerは自由にjoint の状態(RobotHWSimのjnt_pos_等の値)を取得できるようになりました．
これでJointStateControllerが，JointStateInterfaceを介して，RobotHWSimと繋がったわけです．納得です！

飛ばしましたが，realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", 4));でpublisherが初期化されているようです．ついでに，JointStateInterfaceのupdate()メソッドも見てみます．

joint_state_controller/src/joint_state_controller.cpp

// 省略
  void JointStateController::update(const ros::Time& time, const ros::Duration& /*period*/)
  {
    // limit rate of publishing
    if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){

      // try to publish
      if (realtime_pub_->trylock()){
        // we're actually publishing, so increment time
        last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_);

        // populate joint state message:
        // - fill only joints that are present in the JointStateInterface, i.e. indices [0, num_hw_joints_)
        // - leave unchanged extra joints, which have static values, i.e. indices from num_hw_joints_ onwards
        realtime_pub_->msg_.header.stamp = time;
        for (unsigned i=0; i<num_hw_joints_; i++){
          realtime_pub_->msg_.position[i] = joint_state_[i].getPosition();
          realtime_pub_->msg_.velocity[i] = joint_state_[i].getVelocity();
          realtime_pub_->msg_.effort[i] = joint_state_[i].getEffort();
        }
        realtime_pub_->unlockAndPublish();
      }
    }
  }
// 省略

/joint_statesトピックをpublishする処理が記述されているのが確認できます．

JointTrajectoryControllerの場合

position_controllers/JointTrajectoryControllerでも，大きな流れは同じようです．init()でjointのマッピングを行い，update()で指令値を更新しています．

ただ，JointTrajectoryController自体は，位置でも速度でも力でも対応しているようなので，どのインターフェースが来ても対応できるように，クラスの宣言時はベースクラスであるHardwareIntefaceをTとしているようです．ただし，position_controllers/JointTrajectoryControllerの場合は，位置指令で軌道コマンドを受け取る仕様となっており，対応するインターフェースはPositionJointInterfaceとなるようです．私の力不足により，ここの解析はしきれませんでした．
ひとまず先に進めます．

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

// 省略
bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInterface* hw,
                                                                     ros::NodeHandle&   root_nh,
                                                                     ros::NodeHandle&   controller_nh)
{
// 省略
  // List of controlled joints
  joint_names_ = getStrings(controller_nh_, "joints");
  if (joint_names_.empty()) {return false;}
  const unsigned int n_joints = joint_names_.size();

  // URDF joints
  boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
// 省略
  std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
// 省略
  for (unsigned int i = 0; i < n_joints; ++i)
  {
    // Joint handle
    try {joints_[i] = hw->getHandle(joint_names_[i]);}
// 省略
    // Whether a joint is continuous (ie. has angle wraparound)
    angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS;
// 省略
  }
// 省略
  // Hardware interface adapter
  hw_iface_adapter_.init(joints_, controller_nh_);
// 省略
}
// 省略

すべてのjoint 名を取得するのではなく，joint_trajectory_controllers.yaml内でjointsとして登録したjoint名だけを取得します．urdf_jointsを取ってきているのは，URDF内で定義したjointの種類(continuous等)によって挙動を変えるためのようです．
hw_iface_adapter_.init()で，Controllerとjoint のマッピングをしています．

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

// 省略
void JointTrajectoryController<SegmentImpl, HardwareInterface>::
update(const ros::Time& time, const ros::Duration& period)
{
// 省略
  // Update current state and state error
  for (unsigned int i = 0; i < joints_.size(); ++i)
  {
    current_state_.position[i] = joints_[i].getPosition();
    current_state_.velocity[i] = joints_[i].getVelocity();
    // There's no acceleration data available in a joint handle

    state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
    state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
    state_error_.acceleration[i] = 0.0;
  }
// 省略
  // Hardware interface adapter: Generate and send commands
  hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
                                  desired_state_, state_error_);

  // Publish state
  publishState(time_data.uptime);
}
// 省略

update()メソッドは，現在のjoint の状態を更新して目標値との差分を計算した後，hw_iface_adapter_.updateCommand()で指令値(jnt_pos_cmd_)を更新します．
JointTrajectoyControllerが，例えばMoveit!からメッセージを受け取れば，上記フローを経由してGazeboまで指令が送り届けられます．

イメージ図

以上で大体追えたと思います．

まとめ

冒頭に記載した，ここまでのまとめの図を再掲します．

最後にこれを振り返ると，全体が繋がっているんだなってことを確認できます．

終わりに

初めてros_controlのControllerとHardwareInterfaceの設定を見た時，その関係が掴めずあっけにとられていましたが，ようやくクリアになりました．詳細には眼をつむりましたが，流れを理解するには十分と判断しました．プラグインを自作するときにも，どこを触ればいいのか大体わかるようになりました．

個々のモジュールが独立していて，個々の要素は個別に交換が可能です．既存のプラグインでは物足りない，ちょっと変えたい，というところだけプラグイン化してしまえば，自分仕様のロボットモデルが出来上がります．今回の取組みで，ROSによる分散開発の良さについて，理解を深めることができました．ROSを学ぶことの良さの一つして，ロボット開発の巨人が培ったノウハウの集大成を惜しみなく見せてもらえることが挙げられるなぁ，と感じた次第です．

この設計思想は，ロボットに限らず一般のシステムでのインターフェースへも適用できるでしょう．今後ソフトウェア開発でなにか困ったことがあったら，ROSから学ばせて頂くことはないか，と思いを馳せてみよう思います．