Gazebo + ROS で自分だけのロボットをつくる 6. ros_controlについて

# はじめに
今回は、手順6.についてです。


0. [完成までの手順](http://qiita.com/RyodoTanaka/items/c3014fd6d0f06d12814f)
1. [好きなCADソフトを使ってSTLファイルを作る](http://qiita.com/RyodoTanaka/items/cad2cf100dd916d56717)
2. [好きなソフトを使ってCOLLADAファイルを作る](http://qiita.com/RyodoTanaka/items/b1ebd48da81669db7409)
3. [URDFファイルを作る](http://qiita.com/RyodoTanaka/items/174e82f06b10f9885265) 
4. [GazeboとROSの連携](http://qiita.com/RyodoTanaka/items/6fa7e45f98b55376a95b)
5. [ros_controlについて](http://qiita.com/RyodoTanaka/items/e7feb74d0e29a8598f48) **←今ココ**
6. [実際にlaunchしてみる](http://qiita.com/RyodoTanaka/items/af834ed767589a02e27a)

尚、本記事に出てくるロボットのソースコードは、GitHubにて公開しております。

- 独立二輪型ロボット：http://github.com/CIR-KIT/fourth_robot_pkg

また、本記事で対象にしているROSバージョンはIndigoです。

# ros_control
前回の投稿までに、ROSとGazeboの連携には**ros_control**が重要である事を述べました。
また、前回は**diff_drive_controller**を使った例をご紹介しました。
本記事では、その中身を見て行きたいと思います。
というのも、**ros_control**は、Gazeboと実機に共通したコントローラを作る事が目的であるため、
ソースコードを詳細に見ておくことは、非常に重要であるためです。

# ros_controlを使うと何が良いのか？
前回の投稿でも触れましたが、ros_controlの良さは、とにかく柔軟なインタフェースを使用できる点です。
大きくわけて2点の良い点があります。

- 力制御のロボットを位置制御でも速度制御でも動かせるようなインタフェース
- ソフトウェアとハードウェアの違いに関係なくコントローラを設計できる

つまり、ソフトウェアをハードウェアの仕様に左右されづらい仕様にできるといったような利点があります。

# diff_drive_controllerの詳細
ここでは、例題として使っている**diff_drive_controller**を見てみます。
[https://github.com/ros-controls/ros_controllers](https://github.com/ros-controls/ros_controllers)
ros_controlの概念の説明は
http://qiita.com/MoriKen/items/78b0ad8c1eae257646dd#_reference-161e7f66cc3aeb6ffdd3
を読んでください。神記事です。これ以上うまく説明できません。
参照元の記事からは

- HardwareInterfaceにはPosition, Velocity, Effortの３種類があること
- HardwareInterfaceと組み合わせることのできる指令値は決められていること
- GazeboとはRobotHWSimを介して、実機とはRobotHWを介してWrite()、Read()の処理を行っていること
- Write(),Read()の処理はHardwareInterfaceにより一般化されており、これを介することで柔軟なコントローラ設計ができるということ
- 自分のオリジナルロボットに適用するにはRobotHWを編集する必要があること
- オリジナルコントローラを作る際にはHardwareInterfaceを継承し、それにあわせて設計する必要があること

が理解できてれば良いと思います。
ということで、これらの機能が**diff_drive_controller*ならどのように実装されているのか見てみます。
### HardwareIntarface
**diff_drive_controller**はJointの制御の種類は速度制御である必要があります。これは`DiffDriveController`が`controller_interface::Controller<hardware_interface::VelocityJointInterface>`を継承していることからわかります。（下記リンク参照）
https://github.com/ros-controls/ros_controllers/blob/kinetic-devel/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h#L64
つまり、**diff_drive_controller**を使用するということは、`VelocityJointInterface`を介してロボットが動けるようにしておく必要があるということです。なので、例にしているロボットでは下記のように`VelocityJointInterface`をURDFに指定しています。

```xml:wheel.transmission.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:macro name="wheel_trans_v0" params="prefix">
	<transmission name="${prefix}_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${prefix}_wheel_joint">
		<hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
	  <actuator name="${prefix}_wheel_motor">
		<hardwareInterface>VelocityJointInterface</hardwareInterface>
		<mechanicalReduction>30</mechanicalReduction>
      </actuator>
	</transmission>
  </xacro:macro>
  
</robot>
```
https://github.com/CIR-KIT/fourth_robot_pkg/blob/master/fourth_robot_description/urdf/wheel/wheel.transmission.xacro#L8

### diff_drive_controller
ロボットのJointを`controller_interface::Controller<hardware_interface::VelocityJointInterface>`に設定できたので、**diff_drive_controller**がどのようにしてJointの指令値を決定しているのか見てみます。前述の通り、指令値の更新は`update()`関数で行っています。（下記リンク参照）
https://github.com/ros-controls/ros_controllers/blob/kinetic-devel/diff_drive_controller/src/diff_drive_controller.cpp#L250#L370
**diff_drive_controller**の場合には

-  ホイールオドメトリの計算 & Publish（L252〜L308）

```cpp
// COMPUTE AND PUBLISH ODOMETRY
    if (open_loop_)
    {
      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
    }
    else
    {
      double left_pos  = 0.0;
      double right_pos = 0.0;
      for (size_t i = 0; i < wheel_joints_size_; ++i)
      {
        const double lp = left_wheel_joints_[i].getPosition();
        const double rp = right_wheel_joints_[i].getPosition();
        if (std::isnan(lp) || std::isnan(rp))
          return;

        left_pos  += lp;
        right_pos += rp;
      }
      left_pos  /= wheel_joints_size_;
      right_pos /= wheel_joints_size_;

      // Estimate linear and angular velocity using joint information
      odometry_.update(left_pos, right_pos, time);
    }

    // Publish odometry message
    if (last_state_publish_time_ + publish_period_ < time)
    {
      last_state_publish_time_ += publish_period_;
      // Compute and store orientation info
      const geometry_msgs::Quaternion orientation(
            tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

      // Populate odom message and publish
      if (odom_pub_->trylock())
      {
        odom_pub_->msg_.header.stamp = time;
        odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
        odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
        odom_pub_->msg_.pose.pose.orientation = orientation;
        odom_pub_->msg_.twist.twist.linear.x  = odometry_.getLinear();
        odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
        odom_pub_->unlockAndPublish();
      }

      // Publish tf /odom frame
      if (enable_odom_tf_ && tf_odom_pub_->trylock())
      {
        geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
        odom_frame.header.stamp = time;
        odom_frame.transform.translation.x = odometry_.getX();
        odom_frame.transform.translation.y = odometry_.getY();
        odom_frame.transform.rotation = orientation;
        tf_odom_pub_->unlockAndPublish();
      }
}
```

- Jointへの速度値計算 & 更新（L310〜L344）

```cpp
   // MOVE ROBOT
    // Retreive current velocity command and time step:
    Commands curr_cmd = *(command_.readFromRT());
    const double dt = (time - curr_cmd.stamp).toSec();

    // Brake if cmd_vel has timeout:
    if (dt > cmd_vel_timeout_)
    {
      curr_cmd.lin = 0.0;
      curr_cmd.ang = 0.0;
    }

    // Limit velocities and accelerations:
    const double cmd_dt(period.toSec());

    limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
    limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);

    last1_cmd_ = last0_cmd_;
    last0_cmd_ = curr_cmd;

    // Apply multipliers:
    const double ws = wheel_separation_multiplier_ * wheel_separation_;
    const double wr = wheel_radius_multiplier_     * wheel_radius_;

    // Compute wheels velocities:
    const double vel_left  = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr;
    const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr;

    // Set wheels velocities:
    for (size_t i = 0; i < wheel_joints_size_; ++i)
    {
      left_wheel_joints_[i].setCommand(vel_left);
      right_wheel_joints_[i].setCommand(vel_right);
}
```

の手順で処理されています。
また、タイヤ経やホイールベースなどの情報は下記のようなYAMLファイルを読み込んでいます。

```yaml:controller.yaml
# fourth_robot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  diff_drive_controller:
    type        : "diff_drive_controller/DiffDriveController"
    left_wheel  : 'left_wheel_joint'
    right_wheel : 'right_wheel_joint'
    publish_rate: 50.0               # default: 50
    pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

    # Wheel separation and diameter. These are both optional.
    # diff_drive_controller will attempt to read either one or both from the
    # URDF if not specified as a parameter
    wheel_separation : 0.43515
    wheel_radius : 0.193125 #0.38625

    # Wheel separation and radius multipliers
    wheel_separation_multiplier: 1.0 # default: 1.0
    wheel_radius_multiplier    : 1.0 # default: 1.0

    # Velocity commands timeout [s], default 0.5
    cmd_vel_timeout: 1.0
    
    # Base frame_id
    base_frame_id: base_footprint #default: base_link
    
    # Velocity and acceleration limits
    # Whenever a min_* is unspecified, default to -max_*
    linear:
      x:
        has_velocity_limits    : true
        max_velocity           : 0.825  # m/s
        min_velocity           : -0.825 # m/s
        has_acceleration_limits: true
        max_acceleration       : 1.0  # m/s^2
        min_acceleration       : -1.0 # m/s^2
    angular:
      z:
        has_velocity_limits    : true
        max_velocity           : 3.14  # rad/s
        min_velocity           : -3.14
        has_acceleration_limits: true
        max_acceleration       : 1.0  # rad/s^2
min_acceleration       : -1.0
```
https://github.com/CIR-KIT/fourth_robot_pkg/blob/master/fourth_robot_control/config/controller.yaml

# 実機に**diff_drive_controller**を適用するには?
前述の通り、オリジナルの実機で`ros_control`を利用するには実機用の`RobotHW`を作成する必要があります。**diff_drive_controller**では`VelofityJointController`を利用するので、`controller_interface::Controller<hardware_interface::VelocityJointInterface>`を継承してWrite()、Read()関数を例えばマイコンへの書き込み操作とマイコンからの読み込み操作にすれば良い事になります。本記事ではGazeboでのモデルの動かし方にスポットを当てているので、実機への適用は別記事で触れたいと思います。

# まとめ
今回は、ROSとGazeboを連携させるときに不可欠な`ros_control`の役目を**diff_drive_controller**を例にして具体的に見てみました。あまり深くは説明していませんが、`ros_control`が何をしているのか？何ができるのか理解しておけばオリジナルロボットが作りやすくなると思ったので紹介しました。次回はいよいよオリジナルロボットをGazebo上で動かしてみます！！

# 参考
1. http://qiita.com/MoriKen/items/613635b90f3a98042dc5
2. http://qiita.com/MoriKen/items/78b0ad8c1eae257646dd#_reference-161e7f66cc3aeb6ffdd3
3. http://daikimaekawa.github.io/ros/2014/12/19/ros_control/
4. http://cir-kit.github.io/blog/categories/gazebo/
5. http://gazebosim.org/tutorials/?tut=ros_control
6. http://gazebosim.org/tutorials
7. https://github.com/ros-controls/ros_control/wiki
8. http://wiki.ros.org//ros_control
9. http://wiki.ros.org/diff_drive_controller
10. https://github.com/ros-controls/ros_control/wiki/hardware_interface
11. http://myenigma.hatenablog.com/entry/20100201/1265032395
12. http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/wheelrobot.html

# 動作環境
| | |
| :---: | :---: |
| **PC** | Lenovo ThinkPad X240 |
| **Prosessor** |  Intel Core i7-4600U (2.10GHz, 4MB, 1600MHz) |
| **RAM** | PC3-12800 DDR3L (8GB) |
| **OS** | Ubuntu 14.04 LTS 64bit |
| **Kernel** | 3.13.0-44-generic |
| **ROS** | Indigo |

| | |
| :---: | :---: |
| **PC** | Desktop |
| **Prosessor** |  Intel® Core™ i5-4460 CPU @ 3.20GHz × 4  |
| **RAM** | DDR3 (24GB) |
| **OS** | Ubuntu 14.04 LTS 64bit |
| **Kernel** | 3.13.0-83-generic |
| **ROS** | Indigo |