Gazebo上での移動ロボットモデルの構築

１．移動ロボットのモデル構築

移動ロボットはいろいろな人が使っているので，便利なxacroの定義が公開されています．

例えば下記．

ROS:Tutorials/Simulate Your Robot in Gazebo

今回は，このページで紹介されているxacroのmacroを使って移動ロボットを構築します．

まず，いろいろな定義を記載するファイルを集めておくフォルダを適当に作り，その中に車輪のマクロファイル(wheel.urdf.xacro)を作ります．

まずは，move-baseが入っていなければ下記でインストールしておく

$ sudo apt install ros-melodic-move-base*

$ cd ~/catkin_ws/src

$ catkin_create_pkg robot_pkg urdf xacro

$ cd robot_pkg

$ mkdir urdf

$ cd urdf

$ gedit wheel.urdf.xacro

車輪のマクロファイルの内容は下記の通り．

urdfの形状モデルだけでなく，gazeboでの車輪の摩擦や質量，慣性，トランスミッションなどの設定もgazeboのタグで記載してある．

ちなみに，参考ページのものをコピーするとエラーが出るので若干修正してある．

wheel.urdf.xacro

<?xml version="1.0"?>

<xacro:property name="wheel_radius" value="0.2" />

<xacro:property name="wheel_height" value="0.1" />

<xacro:property name="wheel_mass" value="2.5" />

<xacro:property name="base_x_origin_to_wheel_origin" value="0.25" />

<xacro:property name="base_y_origin_to_wheel_origin" value="0.3" />

<xacro:property name="base_z_origin_to_wheel_origin" value="0.0" />

<xacro:macro name="cylinder_inertia" params="m r h">

<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"

iyy="${m*(3*r*r+h*h)/12}" iyz = "0"

izz="${m*r*r/2}" />

</macro>

<xacro:macro name="wheel" params="fb lr parent translateX translateY flipY">

</geometry>

</visual>

</geometry>

</collision>

<xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_height}" />

</inertial>

</link>

<material>Gazebo/Grey</material>

<turnGravityOff>false</turnGravityOff>

</gazebo>

<joint_properties damping="0.0" friction="0.0"/>

</joint>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</xacro:macro>

</robot>

次に，便利なので参考ページにあるモデルの色データをまとめて同じディレクトリに置いておく．

$ cd ~/catkin_ws/src/robot_pkg/urdf

$ gedit materials.urdf.xacro

materials.urdf.xacro

<?xml version="1.0"?>

<robot>

</material>

</material>

</material>

</material>

</material>

</material>

</material>

</material>

</robot>

次に，移動ロボットのマクロファイルを作成する．

$ cd ~/catkin_ws/src/robot_pkg/urdf

$ gedit wheel_robot_base.urdf.xacro

移動ロボットのマクロファイルの内容は下記の通り．

wheel_robot_base.urdf.xacro

<?xml version="1.0"?>

<xacro:include filename="$(find robot_pkg)/urdf/materials.urdf.xacro" />

<xacro:include filename="$(find robot_pkg)/urdf/wheel.urdf.xacro" />

<xacro:property name="M_PI" value="3.1415926535897931" />

<xacro:property name="base_x_size" value="1.0" />

<xacro:property name="base_y_size" value="0.5" />

<xacro:property name="base_z_size" value="0.25" />

<xacro:property name="base_mass" value="35" />

<xacro:macro name="box_inertia" params="m x y z">

<inertia ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"

iyy="${m*(x*x+z*z)/12}" iyz = "0"

izz="${m*(x*x+z*z)/12}" />

</macro>

<inertia ixx="0.0001" ixy="0.0" ixz="0.0"

iyy="0.0001" iyz="0.0"

izz="0.0001" />

</inertial>

</geometry>

</visual>

</link>

<turnGravityOff>false</turnGravityOff>

</gazebo>

</joint>

<box_inertia m="${base_mass}" x="${base_x_size}" y="${base_y_size}" z="${base_z_size}"/>

</inertial>

</geometry>

</visual>

</geometry>

</collision>

</link>

<material>Gazebo/Yellow</material>

<turnGravityOff>false</turnGravityOff>

</gazebo>

<leftFrontJoint>front_left_wheel_joint</leftFrontJoint>

<rightFrontJoint>front_right_wheel_joint</rightFrontJoint>

<leftRearJoint>back_left_wheel_joint</leftRearJoint>

<rightRearJoint>back_right_wheel_joint</rightRearJoint>

<wheelDiameter>${2*wheel_radius}</wheelDiameter>

<robotBaseFrame>base_footprint</robotBaseFrame>

</plugin>

</gazebo>

</geometry>

</visual>

</link>

<!--

-->

</joint>

<material>Gazebo/Blue</material>

<turnGravityOff>false</turnGravityOff>

<pose>${hokuyo_size/2} 0 0 0 0 0</pose>

<visualize>false</visualize>

<update_rate>40</update_rate>

<ray>

<scan>

<min_angle>-1.570796</min_angle>

<max_angle>1.570796</max_angle>

</horizontal>

</scan>

<range>

</range>

</ray>

<frameName>hokuyo_link</frameName>

</plugin>

</sensor>

</gazebo>

</robot>

次に，上記の３つのファイルを使って移動ロボットのモデルを生成するパッケージ(wheel_robot)を作成する．

$ cd ~/catkin_ws/src

$ catkin_create_pkg wheel_robot gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control robot_pkg

$ cd wheel_robot

$ mkdir launch

$ cd launch

$ gedit wheel_robot.launch

移動ロボットを表示するためのlaunchファイルは下記の通り．

wheel_robot.launch

<!-- We resume the logic in empty_world.launch, changing only the name of

the world to be launched -->

</include>

<node name="wheel_robot_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"

args="-urdf -param robot_description -model wheel_robot_base" />

</launch>

早速上記のファイルを使ってロボットを表示させてみる．

$ cd ~/catkin_ws/

$ catkin_make

$ roslaunch wheel_robot wheel_robot.launch

上記を実行すると，下記のようにgazeboの画面が立ち上がり，４輪の移動ロボットが表示される．

rvizを立ち上げて，robot modelとTFを追加して確認すると下記のような表示が確認できるはず．

rvizを閉じる時に，今の表示をまた使うことになるので状態を保存しておく．

今回はの"wheel_robot.rviz"名前で下記のディレクトリに保存するものとする．

~/catkin_ws/src/wheel_robot/launch/wheel_robot.rviz

次にロボットを動かすための，車輪のコントローラを記載する．

$ cd ~/catkin_ws/src

$ catkin_create_pkg wheel_robot_control controller_manager geometry_msgs joy sensor_msgs std_msgs

$ cd wheel_robot_control && mkdir config

参考ページにはros_control ros_controllerの依存が書いてあるが，削除しておく．ジョイスティックを使わない人は joyも消して良い．

ジョイスティックを使いたいので下記のコマンドでjoyをインストールしておく．

$ sudo apt-get install ros-melodic-joy

kineticの人は下記．

$ sudo apt-get install ros-kinetic-joy

作成したパッケージの中に移動して，configディレクトリを作成し，中にcontrollers.yamlを作成する．

$ cd ~/catkin_ws/src/wheel_robot_control/config

$ gedit joint_position_control.yaml

joint_position_control.yamlの内容は，下記の通り．

joint_position_control.yaml

# Publish all joint states -----------------------------------

joint_state_controller:

type: joint_state_controller/JointStateController

publish_rate: 50

下記のようにlaunchファイルを書いて，ロボットが動く状態で立ち上がるようにする．

$ cd ~/catkin_ws/src/wheel_robot/launch

$ gedit wheel_robot_control.launch

wheel_robot_control.launch

<!-- We resume the logic in empty_world.launch, changing only the name of

the world to be launched -->

</include>

<node name="labrob_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"

args="-urdf -param robot_description -model wheel_robot" />

<node name="wheel_robot_controller" pkg="controller_manager" type="spawner" output="screen"

args="joint_state_controller" />

</node>

</group>

</launch>

上記のlaunchファイルは，少し前に保存したrvizの状態を読み込んでいるので注意．

早速上記のlaunchファイルでロボットをrvizとgazeboに表示させてみる．

$ roslaunch wheel_robot wheel_robot_control.launch

上記コマンドを実行すると下記のようにrvizとgazeboの画面が立ち上がる．

立ち上がっただけで，コントロールする方法がないと思われがちだが，rqtのツールを全部入れておくと「Robot Tools」に「Robot Steering」という便利なツールがあるのでこれを使う．

rqtをインストールしていない人は下記のコマンドで全部入れておく．

$ sudo apt-get install ros-melodic-rqt*

rqtを全部入れたら，下記のコマンドでrqtを立ち上げる．

$ rqt

rqtを立ち上げたら下図の通り「Plugins」→「Robot Tools」→ 「Robot Steering」を選んでツール立ち上げ．

縦軸は前後の速度指令，横軸は旋回角速度の指令．

ツールバーを動かしてあげると下記の図の通り，rvizとgazeboの両方で移動ロボットが動き出すことが確認できる．

以上で移動ロボットのモデル作成と速度指令の方法説明は終わり．

次は，レーザを載せて自律移動を確認予定．

以下，追記：（２０１６年7月7日）

ジョイスティックを使ったコントロール

ジョイスティックを使いたい時は，すでに記載していたが，下記のようにjoyをインストールしておく．

$ sudo apt-get install ros-melodic-joy

大抵は全部入りを入れている人が多いと思うので，わざわざ入れなおす必要はないと思います．

ros-melodic-joyについてはここ（ROSwiki英語）で確認できます．

さらに，全部入りを入れている人は，勝手に下記のコマンドでインストールすることのできるジョイスティックドライバーなるものも入っています！

$ sudo apt-get install ros-melodic-joystic-drivers

これを入れておくと，USBにジョイスティックを刺しただけで，/dev/inputの中に「js0」なるデバイスとして認識してくれます．

「js0」になるかどうかは，環境依存なので，下記のコマンドで本当に自分のパソコンに挿したジョイスティックが

js0なのかどうかを確かめましょう．

$ sudo jstest /dev/input/js0

上記のコマンドの結果は下記のような画面．

ちなみに，今回接続したジョイスティックはXBOXコントローラです．

どのボタンやキーが割り当てられて，どのような数字が出るのか確認しておきましょう．

管理者（sudo）じゃなくても動く場合があります．

私のパソコンの場合は，sudoがいらない設定に初めからなっていました．

管理者権限じゃないと動かない時は，下記のコマンドでアクセス権限をユーザにも拡張しましょう．

$ sudo chmod a+rw /dve/input/js0

「js0」で動かなかった人は，jsX（Xは任意の数字）としてリストアップされているファイルを片っ端から試してください．

ジョイスティックの情報は，joyノードが勝手に取得してくれるようになったのですが，

（簡単な時代になりました．．．）

それでは移動ロボットに指令を送ることはまだできません．

そこで，joyノードからpublishされているデータをsubscribeして，

移動ロボットを動かすための情報「/cmd_vel」に変換してpublishするノードを作成する必要があります．

と書いておいて，ノードの作成は後にして，まずもっと簡単な方法として

pythonで呼び出す方法を紹介します．

rospyを使ったジョイスティックコマンドの変更と移動ロボットへの指令

まず，下記のコマンドで，上記までの説明で作っていたwheel_robot_controlのディレクトリ内に

scriptディレクトリを作成して，pythonのファイルjoy_teleop.pyを作成します．（本当はどこでもいいけど．．．）

$ cd ~/catkin_ws/src/wheel_robot_control

$ mkdir scripts

$ cd scripts

$ gedit joy_teleop.py

joy_teleop.pyのファイルの中身を下記のように記載して保存してください．

joy_teleop.py

#!/usr/bin/env

pythonimport rospy from geometry_msgs.msg

import Twist from sensor_msgs.msg

import Joy

def callback(data):

twist = Twist()

twist.linear.x = 4*data.axes[1]

twist.angular.z = 4*data.axes[0]

pub.publish(twist)

def joy_teleop():

rospy.init_node('Joy_teleop')

rospy.Subscriber("joy", Joy, callback)

global pub

pub = rospy.Publisher('/cmd_vel', Twist)

r = rospy.Rate(10) # 10hz

while not rospy.is_shutdown():

r.sleep()

# spin() simply keeps python from exiting until this node is stopped

rospy.spin()

if __name__ == '__main__':

上記の編集が終わったら，下記のコマンドで，pythonのファイルを実行可能形式にしておきます．

$ chmod +x joy_teleop.py

これで，ジョイスティックで操作する準備が整いました．

ジョイスティックで動かすためには，まず，joyノードを立ち上げてから，上記のpythonスクリプトを実行します．

$ rosrun joy joy_node

$ rosrun wheel_robot_control joy_teleop.py　（上記コマンドを打ったウィンドウと違うウィンドウで実行）

上記のコマンドを実行するだけで，ジョイスティックでシミュレータ上のロボットを動かすことができることが確認できると思います．

なお，今回の例ではXBOXコントローラの左アナログスティックだけを使う設定になっています．

アナログスティックを上に倒すと前進，下に倒すと後退，右に倒すと右旋回，左に倒すと左旋回です．

ノード立ち上げにいちいち２つウィンドウを使うのが面倒なので，一回で済ませたい人は下記のlaunchファイルjoy_teleop.launchを書いてください．

$ cd ~/catkin_ws/src/wheel_robot_control

$ mkdir launch

$ cd launch

$ gedit joy_teleop.launch

joy_teleop.launchの内容は下記の通り．

joy_teleop.launch

</launch>

上記のlaunchファイルができれば下記のコマンド一発でジョイスティックのノードを立ち上げることができます．

$ roslaunch wheel_robot_control joy_teleop.launch

C++でジョイスティックコマンド変換ノードの作成

上記では，rospy(python)を使ったジョイスティックコマンドの変更と移動ロボットへの指令を行った．

ただ，pythonが気にくわないとか，C++で統一したいとかいう人がいると思うので，

C++でノードを普通に作る方法も説明する．

まず，移動ロボット用のジョイスティックノードを下記のコマンドで作成する．

$ cd ~/catkin_ws/src

$ catkin_create_pkg wheel_robot_joyteleop roscpp geometry_msgs joy sensor_msgs std_msgs

$ cd wheel_robot_joyteleop/src

$ gedit wheel_robot_joyteleop.cpp

wheel_robot_joyteleop.cpp(長い名前なので適当に変えてください．．．）の内容は下記のようにする．

wheel_robot_joyteleop.cpp

#include <ros/ros.h>

#include <sensor_msgs/Joy.h> //ジョイスティックからのデータ取得のためsensor_msgsのjoy.hの定義を入れておく

#include <geometry_msgs/Twist.h> //移動ロボットの速度指令のためにsgeometry_msgsのTwist.hの定義を入れておく

class wheel_robot_joyteleop{

private :

ros::Subscriber sub;

ros::Publisher pub;

geometry_msgs::Twist twist;

public :

wheel_robot_joyteleop();

void joyop_loop();

private :

void joyCallback(const sensor_msgs::Joy &joy_msg);

};

wheel_robot_joyteleop::wheel_robot_joyteleop(){

ros::NodeHandle n;

sub = n.subscribe("joy", 1, &wheel_robot_joyteleop::joyCallback, this); //joyをsubscribeしてjoy_nodeからのデータ取得

pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1); //cmd_velにpublishして移動ロボットに速度指令

}

void wheel_robot_joyteleop::joyop_loop() {

ros::Rate loop_rate(20); //20Hzのループ

while(ros::ok()) {

ros::spinOnce();

loop_rate.sleep();

}

return;

}

void wheel_robot_joyteleop::joyCallback(const sensor_msgs::Joy &joy_msg) {

twist.linear.x = 4.0 * joy_msg.axes[1];

twist.angular.z = 4.0 * joy_msg.axes[0];

pub.publish(twist);

//ROS_INFO("linear : %lf angular : %lf\n published.", twist.linear.x, twist.angular.z);

return;

}

int main(int argc, char **argv) {

ros::init(argc, argv, "wheel_robot_joyteleop");

wheel_robot_joyteleop joyop;

joyop.joyop_loop();

return (0);

}

makeするために，CMakeLists.txtを下記のように修正しましょう．

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(wheel_robot_joyteleop)

find_package(catkin REQUIRED COMPONENTS

geometry_msgs

joy

sensor_msgs

std_msgs

)

catkin_package(

LIBRARIES wheel_robot_joyteleop

CATKIN_DEPENDS geometry_msgs joy sensor_msgs std_msgs

DEPENDS system_lib

)

include_directories(

${catkin_INCLUDE_DIRS}

)

add_executable(wheel_robot_joyteleop src/wheel_robot_joyteleop.cpp)

target_link_libraries(wheel_robot_joyteleop ${catkin_LIBRARIES} )

次に，launchファイルを作って簡単に起動できるようにしましょう．

$ cd ~/catkin_ws/src/wheel_robot_joyteleop

$ mkdir launch

$ cd launch && gedit wheel_robot_joyteleop.launch

wheel_robot_joyteleop.launchの内容は下記の通り．

wheel_robot_joyteleop.launch

</launch>

コンパイル後に，上記のlaunchファイルを実行して動作を確認しましょう．

（移動ロボットのシミュレータは別のlaunchファイルで立ち上げておくこと）

$ cd ~/catkin_ws && catkin_make

$ roslaunch wheel_robot_joyteleop wheel_robot_joyteleop.launch

Pythonでキーボードを使った操作

ついでにキーボードもメモしておく，rospy(python)を使ったキーボードコマンドの変更と移動ロボットへの指令を行う．

ジョイスティックのスクリプトと同じ場所にキーボード用のスクリプトを記載します．

$ cd ~/catkin_ws/src/wheel_robot_control/scripts

$ gedit key_teleop.py

key_teleop.pyの内容は下記の通り．

key_teleop.py

詳細説明は省略．．．

上記の編集が終わったら，下記のコマンドで，pythonのファイルを実行可能形式にしておきます．

$ chmod +x key_teleop.py

シミュレータ立ち上げ後にキーボード操作用の画面で下記のコマンドを実行．

$ rosrun wheel_robot_control key_teleop.py　

画面に出てきている通りキーをどのように押すとどのように動くかわかるようになっています．

"w"を押すと前進，"x"を押すと後退，"s"を押すと停止，"a"を押すと左旋回，"d"を押すと右旋回です．

"g"を押すと終了します．ctl+cを押しても終了しないので注意．

参考

１．gazeboでの移動ロボット構築のチュートリアル（本家：英語）

２．ROS:Tutorials/Simulate Your Robot in Gazebo

３．Configuring and Using a Linux-Supported Joystick with ROS