#はじめに
ROSが素晴らしいです。豊富なライブラリ、盛んなコミュニティ、割と返答率が高い質問版などロボットエンジニアとして得られるものが多々あります。
ただ、ROSを勉強しているとGazeboやRvizなどのシュミレーションで終わることが多くて、いまいちROSを使った実機のロボットのつくり方がわかりづらいと思っていました。
ROS対応の製品を買えば解決する話なのですが、そうではなくて、ROSを使って、その他の部品も自分で用意して、1から実機のロボットを作りたいと思っている人もいるはずです。そこで備忘録も兼ねて、そんな野生のロボットエンジニア向けに記事を書こうと思いました。
具体的には、mbedマイコンを実機(RobotHW)として扱える様にしていきます。(そのmbedマイコンをお好みのセンサやアクチュエータに繋いであげれば、本当の実機のロボットができるのですが、この記事ではそこまではしません。)
#環境および事前準備
ROSのバージョンはkineticを使っています。LINUXのバージョンは16.04LTSです。
また、この記事ではROSのURDF、Xacro、Rviz、Gazebo、ros_control、ros_serialなどを使います。
なので、それらのパッケージは事前にインストールしておいて下さい。
#目次
上の図のGazeboとRealityの切り替えを同じロボットモデルで行い、実機のロボットを制御しようというのが今記事の目標です。
(1)ロボットモデル作成
(2)シミュレーション(RobotHWSim)によるロボット制御
(3)実機(RObotHW)によるロボット制御
の3つに分けて進めていきます。
#(1)ロボットモデル作成
###URDFによるロボットモデルの作成
まず、ロボットのモデルを作成します。今回のモデルはurdfで書きます。
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from trobo.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<!-- Revolute-Revolute Manipulator -->
<robot name="trobo" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Space btw top of beam and the each joint -->
<!-- Used for fixing robot to Gazebo 'base_link' -->
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="link1"/>
</joint>
<!-- Base Link -->
<link name="link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
<!--<material name="orange"/> -->
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin rpy="0 0 0" xyz="0 0.1 1.95"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<!-- Middle Link -->
<link name="link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<!--<material name="black"/>-->
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo>
<plugin nameta="gazebo_ros_control" filename="libgazebo_ros_control.so">
<!--<robotNamespace>/trobo</robotNamespace>-->
<!--<controlPeriod>0.001</controlPeriod>-->
</plugin>
</gazebo>
</robot>
こんな感じです。urdfの詳細については詳しく触れません。
シミュレーションのロボットモデルについては最後のgazeboの記述を忘れないで下さい。
シミュレーション(RobotHWSim)と実機(RObotHW)にのロボットモデルの違いはこの部分になります。
モデルを確認してみます。
$ roslaunch urdf_tutorial display.launch model:=trobo_sim.urdf
立派なロボット(!?)が現れました。
最初、うまく表示されないと思いますが、「Fixed Frame」の欄を「world」とかにすれば図の様に表示されると思います。
#(2)シミュレーション(RobotHWSim)によるロボット制御
###Gazeboでのロボット制御
simhw_control.launch、simhw_world.launchの2つのlaunchファイルを使います。
simhw_control.launchではros_controlの設定を、simhw_world.launchではロボットモデルを読み込んだ上でsimhw_control.launch、Gazebo、Rvizの呼び出しを行っています。
まずは、simhw_control.launchから見ていきます。
<launch>
<rosparam>
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 100
# Position Controllers ---------------------------------------
joint1_position_controller:
type: position_controllers/JointPositionController
joint: joint1
<!-- pid: {p: 1.0, i: 0.0, d: 0.0}-->
</rosparam>
<node name="controller_spawner" pkg="controller_manager"
type="spawner" respawn="false"
output="screen"
args="joint_state_controller joint1_position_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" respawn="false" output="screen">
</node>
</launch>
ros_controlのコントローラーとしてjoint_state_controller、joint1_position_controllerの２つを使っています。
joint_state_controllerはロボットの関節の状態をros_controlから読みこんで表示してくれます。
joint1_position_controllerは関節のPID制御を行ってくれます。(今回はコントローラーの仕様上行っていませんが...)
これらコントローラーの設定はlaunchファイルで書いても良いですし、yamlファイルを読みこんで設定しても良いです。
次に、simhw_world.launchでロボット制御を行います。
<launch>
<!--<param name="robot_description"
command="$(find xacro)/xacro '$(find trobo_description)/urdf/trobo_sim.xacro'" />-->
<param name="robot_description" textfile="$(find trobo_description)/urdf/trobo_sim.urdf"/>
<include file="$(find simhw_ros_control)/launch/simhw_control.launch" />
<include file="$(find gazebo_ros)/launch/empty_world.launch"/>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model mobilerobot"/>
<node name ="rviz" pkg="rviz" type="rviz" required="true"/>
</launch>
ROSの参考書などに良く載っている内容だと思います。ロボットモデルの読み込みは今回はurdfで行っていますが、コメントアウト部分を使えばxacroでも読み込めます。
$ roslaunch simhw_ros_control simhw_world.launch
ロボットモデルがうまく表示されないと思いますが、「Fixed Frame」の欄を「world」にするのと、「add」で「robotmodel」を追加すれば図の様に表示されると思います。
折角なので動かしてみましょう。
$ rostopic pub /joint1_position_controller/command s_msgs/Float64 "0.5"
ロボットが0.5[rad]動きます。シャキッと動きます。参考資料のMorikenさんの記事(https://qiita.com/MoriKen/items/78b0ad8c1eae257646dd)を読むとわかるようにコントローラーposition_controllers/JointPositionControllerはPID制御をしておらず、シュミレーション上のロボットは位置指令通りに動きます。ここらへんはコントローラーの種類、PIDの調整により変わってきます。
これでシミュレーション(RobotHWSim)によるロボット制御は完了です。
ここまでできれば、moveitなどを使ってシミュレーション上でロボットをある程度自由に動かすことができます。
#(3)実機(RObotHW)によるロボット制御
いよいよこのロボットモデル、およびコントローラーを実機(RobotHW)に繋げていきます。
###実機用のロボットモデルを作成
シミュレーションのロボットモデルから実機用のロボットモデルを作成します。
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from trobo.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<!-- Revolute-Revolute Manipulator -->
<robot name="trobo" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Space btw top of beam and the each joint -->
<!-- Used for fixing robot to Gazebo 'base_link' -->
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="link1"/>
</joint>
<!-- Base Link -->
<link name="link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
<!--<material name="orange"/> -->
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin rpy="0 0 0" xyz="0 0.1 1.95"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<!-- Middle Link -->
<link name="link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<!--<material name="black"/>-->
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</robot>
(2)のtrobo_sim.urdfと比べてください。gazeboの部分の記述があるかないかだけになっています。これだけでシミュレーションから実機のロボット用のロボットモデルの変更は完了です。
###実機でのロボット制御(ROS側)の準備
実機でのロボット制御には実際の制御を行うROS側とセンサーから情報を取得したりアクチュエータを動かすmbed(マイコン)側の準備が必要になります。まずはROS側の準備です。
realhw_control.launchとrealhw_world.launchの2つのlaunchファイルを使います。
realhw_control.launchではros_controlの設定を、realhw_world.launchではロボットモデルを読み込んだ上でrealhw_control.launch、Rvizの呼び出しを行っています。
realhw_control.launchから見ていきます。
<launch>
<rosparam>
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 100
# Position Controllers ---------------------------------------
joint1_position_controller:
type: position_controllers/JointPositionController
joint: joint1
<!--pid: {p: 1.0, i: 0.0, d: 0.0}-->
</rosparam>
<!-- rosparam file="$(find trobo_control)/config/trobo_control.yaml" command="load"/ -->
<node name="realhw_control" pkg="realhw_control" type="realhw_control"
output="screen"/>
<node name="controller_spawner" pkg="controller_manager"
type="spawner" respawn="false"
output="screen"
args="joint_state_controller joint1_position_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" respawn="false" output="screen">
</node>
</launch>
simhw_control.launchとほとんど同じです。ただ、ノードにrealhw_controlが新たに加わっています。これが最後に説明するrealhw_control.cppのことでros_controlと実機のロボットを繋げるノードになります。
次に、realhw_world.launchでロボット制御を行います。
<launch>
<!-- Load the URDF into the ROS Parameter Server -->
<!--<param name="robot_description"
command="$(find xacro)/xacro '$(find trobo_description)/urdf/trobo.xacro'" />-->
<param name="robot_description" textfile="$(find trobo_description)/urdf/trobo.urdf"/>
<!-- ros_control rrbot launch file -->
<include file="$(find realhw_control)/launch/realhw_control.launch" />
<!-- bring up rviz -->
<!--<node name ="rviz" pkg="rviz" type="rviz" args="-d $(find minimum_ros_control)/trobo_description/urdf" required="true" /> -->
<node name ="rviz" pkg="rviz" type="rviz" required="true"/>
</launch>
simhw_world.launchとほとんど同じです。実機なのでGazeboを呼び出す部分がなくなっています。
$ roslaunch realhw_ros_control realhw_world.launch
実機と繋がっていないので何も起きません。
###実機でのロボット制御(mbed側)の準備
次にmbed(マイコン)側の準備をしていきます。今回、マイコンはmbedを使いますが、ros_serialが使えるマイコンであれば、Arduinoでも何でも構いません。
mbed側のソースは以下です。使用の際はros_lib_kineticのライブラリをインポートしておいて下さい。
/*
* return robot_state
* send joint_pos+0.1 to ros
*/
#include "mbed.h"
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32MultiArray.h>
//#include <std_msgs/MultiArrayDimension.h>
//#include <std_msgs/MultiArrayLayout.h>
ros::NodeHandle nh;
std_msgs::Float32 msg;
ros::Publisher responser("response", &msg);
void messageCb(const std_msgs::Float32& msg_sub)
{
msg.data =msg_sub.data;
responser.publish( &msg );
}
ros::Subscriber<std_msgs::Float32> sub("request", &messageCb);
int main()
{
nh.getHardware()->setBaud(921600);
nh.initNode();
nh.subscribe(sub);
nh.advertise(responser);
while (1)
{
nh.spinOnce();
//wait_ms(100);
}
}
上記、ソースはros_controlの結果を受け取ったら、コールバック関数を呼び出して、結果をそのまま返すという何とも地味なソースになっています。((2)のロボットのposition_controllers/JointPositionControllerの動きを真似してみました)
しかし、これが、ros_serialによる実機(RObotHW)とROSの繋ぎ方です。
このコールバック関数の部分に実機のロボットのセンサー情報やアクチュエータへの指令を入れてあげれば、実際のロボットとros_controlは繋がることになります。
では、実際にmbedに繋いでみます。
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=921600
実行権限エラーが出た場合は、
$ sudo chmod a+rw /dev/ttyACM0
で実行権限を変更して下さい(ttyACM0はmbedのポート番号。自分の環境に合わせて変更する)。
ちなみにros_coreが立ち上がってないと、rosrunできないので、realhw_ros_control realhw_world.launchを先に立ち上げといて下さい。
mbedが点滅します。一見、シミュレーションと変わりませんが、ROSがmbedと連動しています。
折角なので動かしてみましょう。
$ rostopic pub /joint1_position_controller/command s_msgs/Float64 "0.5"
ロボットが0.5[rad]動きます。シャキッと動きます。やっていることがシミュレーションと同じなので同じ様に動きます。
これで実機(RobotHW)よるロボット制御は完了です。
ここまでくればmoveitなどのライブラリを使って実機のロボットを動かすことができます。
###ros_controlと実機のロボットを繋ぐソース(realhw_control.cpp)
最後に後回しにしていたros_controlと実機のロボットを繋ぐソースを軽く説明します。
これらは、ヘッダーファイルのrealhw.hと実装ファイルのrealhw.cpp、実行ファイルのrealhw_control.cppの3つのファイルで作られています。
まずはヘッダーファイルのrealhw.hから見ていきます。
#include <ros/ros.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <map>
#include <string>
#include <vector>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32MultiArray.h>
class TRobo : public hardware_interface::RobotHW
{
public:
TRobo();
ros::NodeHandle nh;
ros::Time getTime() const { return ros::Time::now(); }
ros::Duration getPeriod() const { return ros::Duration(0.01); } //0.01sで
void timeCallback(const std_msgs::Float32::ConstPtr& msg_sub);
ros::Publisher pub;
ros::Subscriber sub;
void read(ros::Time, ros::Duration);
void write(ros::Time, ros::Duration);
//void write(ros::Time, ros::Duration,std_msgs::Float32 msg_pub);
protected:
hardware_interface::JointStateInterface jnt_state_interface;
hardware_interface::PositionJointInterface jnt_pos_interface;
double cmd_[1];
double pos_[1];
double vel_[1];
double eff_[1];
};
read()が実機のロボットのデータをros_controlに渡す関数、writeがros_controlからの指令を実機のロボットに渡す関数であること、ros_controlで使いたいhardware_interfaceを宣言しておく必要があるという事が重要です。
次に実装ファイルのrealhw.cppを見ていきます。
#include <ros/package.h>
#include <angles/angles.h>
#include <realhw_control/realhw.h>
#include <iostream> // for debug
#include <math.h>
TRobo::TRobo()
{
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]);
jnt_state_interface.registerHandle(state_handle_1);
registerInterface(&jnt_state_interface);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_1(jnt_state_interface.getHandle("joint1"), &cmd_[0]);
jnt_pos_interface.registerHandle(pos_handle_1);
registerInterface(&jnt_pos_interface);
}
void TRobo::timeCallback(const std_msgs::Float32::ConstPtr& msg_sub)
{
pos_[0]=msg_sub->data; //set pos_[0] as real_hw date
}
void TRobo::read(ros::Time time, ros::Duration period)
{
sub = nh.subscribe("response" ,100000,&TRobo::timeCallback, this); //to use subscribe in class, use "this"
}
void TRobo::write(ros::Time time, ros::Duration period)
//void TRobo::write(ros::Time time, ros::Duration period,std_msgs::Float32 msg_pub)
{
//msg_pub.data=0.01;
std_msgs::Float32 msg_pub;
msg_pub.data=cmd_[0];
pub = nh.advertise<std_msgs::Float32>("request",100000,true);
pub.publish(msg_pub);
}
hardware_interface、read()、write()関数を実装します。コールバック関数は位置制御か速度制御か力制御かでros_controlに渡すものが変わります。今回は位置制御なのでpos情報だけを渡しています。
最後に実行ファイルです。
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <realhw_control/realhw.h>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "trobo");
TRobo trobo;
controller_manager::ControllerManager cm(&trobo, trobo.nh);
ros::Rate rate(1 / trobo.getPeriod().toSec());
ros::AsyncSpinner spinner(1);
spinner.start();
while(ros::ok())
{
ros::Time now = trobo.getTime();
ros::Duration dt = trobo.getPeriod();
trobo.read(now, dt);
cm.update(now, dt);
trobo.write(now, dt);
rate.sleep();
}
spinner.stop();
return 0;
}
ros::Rate rateで情報の受け渡し周期を決めます。今回は100[hz]です。ros_control自身の制御周期はrealhw_control.launchで決める必要があると思います。
この実行ファイルをrealhw_world.launchで呼び出すことでros_controlと実機のロボットが繋がります。
ここらへんの書き方はros公式のチュートリアルを参考になります。https://github.com/ros-controls/ros_control/wiki/hardware_interface
#さいごに
今記事のソースはhttps://github.com/takijo0116/realhw_ros_controlからダウンロードできます。(mbedのソースは入ってません。)
また、これらのソースはyonekenさんのhttps://github.com/yoneken/minimum_ros_controlにソースを足して作られています。
また、ros_controlのRobotHWSimおよびRobotHWに関しては
が大変参考になります。
是非、ROSを使ってロボットをつくりましょう。