LoginSignup
16

More than 5 years have passed since last update.

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

Last updated at Posted at 2016-12-19

はじめに

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

  1. 完成までの手順
  2. 好きなCADソフトを使ってSTLファイルを作る
  3. 好きなソフトを使ってCOLLADAファイルを作る
  4. URDFファイルを作る
  5. GazeboとROSの連携
  6. ros_controlについて ←今ココ
  7. 実際にlaunchしてみる

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

また、本記事で対象にしている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
ros_controlの概念の説明は
http://qiita.com/MoriKen/items/78b0ad8c1eae257646dd#_reference-161e7f66cc3aeb6ffdd3
を読んでください。神記事です。これ以上うまく説明できません。
参照元の記事からは

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

が理解できてれば良いと思います。
ということで、これらの機能が*diff_drive_controllerならどのように実装されているのか見てみます。

HardwareIntarface

diff_drive_controllerはJointの制御の種類は速度制御である必要があります。これはDiffDriveControllercontroller_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に指定しています。

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>

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)
// 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)
   // 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ファイルを読み込んでいます。

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

実機に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

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
16