はじめに
今回は、手順6.についてです。
- 完成までの手順
- 好きなCADソフトを使ってSTLファイルを作る
- 好きなソフトを使ってCOLLADAファイルを作る
- URDFファイルを作る
- GazeboとROSの連携
- ros_controlについて ←今ココ
- 実際に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の制御の種類は速度制御である必要があります。これは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 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ファイルを読み込んでいます。
# 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上で動かしてみます!!
参考
- http://qiita.com/MoriKen/items/613635b90f3a98042dc5
- http://qiita.com/MoriKen/items/78b0ad8c1eae257646dd#_reference-161e7f66cc3aeb6ffdd3
- http://daikimaekawa.github.io/ros/2014/12/19/ros_control/
- http://cir-kit.github.io/blog/categories/gazebo/
- http://gazebosim.org/tutorials/?tut=ros_control
- http://gazebosim.org/tutorials
- https://github.com/ros-controls/ros_control/wiki
- http://wiki.ros.org//ros_control
- http://wiki.ros.org/diff_drive_controller
- https://github.com/ros-controls/ros_control/wiki/hardware_interface
- http://myenigma.hatenablog.com/entry/20100201/1265032395
- 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 |