シミュレータ上で動くロボットをつくってプログラムで操作しよう!
ロボットモデルを記述して,可視化ツールであるRViz2で表示,また,ロボットシミュレータであるGazebo上でロボットをプログラムで操作するまでを体験することができる.
実行環境は,Ubuntu 20.04, ROSのバージョンはROS2 foxyを想定.
完成品のソースコードは,以下のURLからダウンロードできる.
このチュートリアルでの作業の流れは,以下のようになる.
1. 準備
ROS2のインストール
ワークスペースを作成する
ROSパッケージを作成する
ROSパッケージの設定を記述する
2. URDFでロボットを作成する
部品を記述する
部品を組み立てる
ロボットモデルを可視化ツールで確認
3. ROSプログラミングでロボットを動かす
ロボットモデルをシミュレータで確認
GUIのコントローラで動作を確認
プログラムでロボットを動かす
1. 準備
1.1 ROS2のインストール
1. Ubuntuの動作環境を準備
MacやWindowsでUbuntuを利用する場合は,仮想環境を用意してUbuntuをインストールする.
仮想環境の構築方法は複数あるが参考として以下の方法を紹介.
Mac
Windows
2. ROSをインストール
追加のパッケージをインストール
$ sudo apt update
$ sudo apt install ros-foxy-joint-state-publisher ros-foxy-joint-state-publisher-gui ros-foxy-xacro ros-foxy-rqt-robot-steering
1.2 ワークスペースを作成する
$ mkdir -p ~/tutorial_ws/src
1.3 パッケージを作成する
$ cd ~/tutorial_ws/src
$ ros2 pkg create --build-type ament_python q_ros_tutorial
1.4 パッケージの設定を記述する
ディレクトリを追加
$ cd ~/tutorial_ws/src/q_ros_tutorial
$ mkdir urdf launch
パッケージに反映するように設定を修正
setup.pyを修正.コメントで「追加」と書かれている部分を追記する.
code:setup.py
from glob import glob ### 追加 ###
from setuptools import setup
package_name = 'ros_tutorial'
setup(
...
data_files=[
...
('share/' + package_name + '/launch', glob('launch/*.launch.xml')), ### 追加 ###
('share/' + package_name + '/urdf', glob('urdf/*.urdf.xacro')), ### 追加 ###
]
...
)
(オプション)VSCodeのインストール
VSCodeのインストール
Windowsの方でWSLを使っている場合は,以下からWindows版をダウンロードしてインストール
Ubuntuの方は以下のコマンドを利用してインストール(他にも方法あり)
$ sudo snap install --classic code
Visual Studio Code Extension for ROSをVSCodeにインストール
URDFのプレビュー機能が便利
URDFファイルを選択して,Ctrl+Shift+Pを押して,「ROS:Preview URDF」を選択
2. URDFでロボットを作成する
URDFの基本的な記述は,
1. ロボットの部品として「link」を作成
link内には,外観に関する要素「visual」と,衝突に関する要素「collision」,慣性に関する要素「inertial」を記述.
2. ロボットの部品である「link」を「joint」でつなげて,ロボットモデルを構成
URDFファイルは,urdfディレクトの下に保存する.
なお,本チュートリアルでは,変数や簡単な計算,マクロができるマクロ言語であるXacroを使ってURDFを記述する.
準備
複数のファイルで共有するプロパティなどを記述
なお,このファイルにロボットの部品を読み込みモデルを作成していく.VSCodeでURDFをプレビューするときは,このファイルを選択する.
simplebot.urdf.xacro を新規作成
code:xml
<?xml version="1.0"?>
<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />
<xacro:property name="leglen" value="0.6" />
<xacro:property name="footlen" value="0.4" />
<material name="blue">
<color rgba="0 0 0.8 1" />
</material>
<material name="white">
<color rgba="1 1 1 1" />
</material>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</robot>
2.1 部品を記述する
体を作成
body.urdf.xacro を新規作成
code:xml
<?xml version="1.0"?>
<xacro:macro name="body" params="width bodylen">
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${width}" length="${bodylen}" />
</geometry>
<material name="blue" />
</visual>
<collision>
<geometry>
<cylinder radius="${width}" length="${bodylen}" />
</geometry>
</collision>
<inertial>
<mass value="0.4" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
</xacro:macro>
</robot>
車輪を作成
wheel.urdf.xacro を新規作成
code:xml
<?xml version="1.0"?>
<xacro:macro name="wheel" params="prefix suffix wheeldiam">
<link name="${prefix}_${suffix}_wheel">
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="${wheeldiam/2}" length="0.1" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="${wheeldiam/2}" length="0.1" />
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
</xacro:macro>
</robot>
足を作成
leg.urdf.xacro を新規作成
code:xml
<?xml version="1.0"?>
<xacro:macro name="leg" params="prefix leglen footlen">
<!-- レッグの作成 -->
<link name="${prefix}_leg">
<visual>
<geometry>
<box size="${leglen} 0.1 0.2" />
</geometry>
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<box size="${leglen} 0.1 0.2" />
</geometry>
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0" />
</collision>
<inertial>
<mass value="10" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
<!-- フットの作成 -->
<link name="${prefix}_foot">
<visual>
<geometry>
<box size="${footlen} 0.1 0.1" />
</geometry>
<material name="white" />
</visual>
<collision>
<geometry>
<box size="${footlen} 0.1 0.1" />
</geometry>
</collision>
<inertial>
<mass value="10" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
</xacro:macro>
</robot>
頭を作成
head.urdf.xacro を新規作成
code:xml
<?xml version="1.0"?>
<xacro:macro name="head" params="width">
<link name="head">
<visual>
<geometry>
<sphere radius="${width}" />
</geometry>
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="${width}" />
</geometry>
</collision>
<inertial>
<mass value="2.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
<link name="box">
<visual>
<geometry>
<box size="0.08 0.08 0.08" />
</geometry>
<material name="blue" />
<origin xyz="-0.04 0 0" />
</visual>
<collision>
<geometry>
<box size="0.08 0.08 0.08" />
</geometry>
</collision>
<inertial>
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</link>
</xacro:macro>
</robot>
2.2 部品を組み立てる
足の組み立て1
leg.urdf.xacro を編集
レッグとフットの部品をつなげる
code:xml
<?xml version="1.0"?>
<xacro:macro name="leg" params="prefix leglen footlen">
<link name="${prefix}_leg">
・・・
</link>
<link name="${prefix}_foot">
・・・
</link>
<!-- レッグとフットをつなげる -->
<joint name="${prefix}_base_joint" type="fixed">
<parent link="${prefix}_leg" />
<child link="${prefix}_foot" />
<origin xyz="0 0 ${-leglen}" />
</joint>
・・・
足の組み立て2
leg.urdf.xacro を編集
車輪のファイル( wheel.urdf.xacro ) を読み込み,車輪を作成.車輪とフットをつなげる
code:xml
<?xml version="1.0"?>
<xacro:macro name="leg" params="prefix leglen footlen">
<!-- ホイールの読み込み -->
<xacro:include filename="wheel.urdf.xacro" />
<!-- ホイールの直径 -->
<xacro:property name="wheeldiam" value="0.07" />
・・・
<!-- 前輪,後輪のホイールを作成 -->
<xacro:wheel prefix="${prefix}" suffix="front" wheeldiam="${wheeldiam}" />
<xacro:wheel prefix="${prefix}" suffix="back" wheeldiam="${wheeldiam}" />
<!-- フットと前輪をつなげる -->
<joint name="${prefix}_front_wheel_joint" type="continuous">
<axis xyz="0 1 0" rpy="0 0 0" />
<parent link="${prefix}_foot" />
<child link="${prefix}_front_wheel" />
<origin xyz="${footlen/3} 0 -${wheeldiam/2+.05}" rpy="0 0 0" />
</joint>
<!-- フットと後輪をつなげる -->
<joint name="${prefix}_back_wheel_joint" type="continuous">
<axis xyz="0 1 0" rpy="0 0 0" />
<parent link="${prefix}_foot" />
<child link="${prefix}_back_wheel" />
<origin xyz="${-1*footlen/3} 0 -${wheeldiam/2+.05}" rpy="0 0 0" />
</joint>
</xacro:macro>
</robot>
全体の組み立て
simplebot.urdf.xacro を編集
部品のファイルを読み込み,頭と体,足をつなげる
code: xml
<?xml version="1.0"?>
<xacro:include filename="body.urdf.xacro" />
<xacro:include filename="leg.urdf.xacro" />
<xacro:include filename="head.urdf.xacro" />
・・・
<!-- 頭を作成 -->
<xacro:head width="${width}" />
<!-- ボディを作成 -->
<xacro:body width="${width}" bodylen="${bodylen}" />
<!-- 足を作成 -->
<xacro:leg prefix="right" leglen="${leglen}" footlen="${footlen}" />
<xacro:leg prefix="left" leglen="${leglen}" footlen="${footlen}" />
<!-- ボディと頭をつなげる -->
<joint name="head_swivel" type="continuous">
<parent link="base_link" />
<child link="head" />
<axis xyz="0 0 1" />
<origin xyz="0 0 ${bodylen/2}" />
</joint>
<!-- ボディと足をつなげる -->
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link" />
<child link="right_leg" />
<origin xyz="0 ${-1*(width+.02)} 0.25" />
</joint>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link" />
<child link="left_leg" />
<origin xyz="0 ${(width+.02)} 0.25" />
</joint>
・・・
2.3 ロボットモデルを可視化ツールで確認
URDFのロボットモデルを可視化ツールで表示させるには,「パラメータ(Parameter)」にロボットモデルを登録する必要がある.
パラメータは,すべてのROSパッケージで変数を共有する仕組み
モデルを登録するには,「robot_state_publisher」パッケージを利用
1. robot_state_publisherを起動させるlaunchファイルを記述する
launchディレクトリに urdf.launch.xml を新規作成
code:urdf.launch.xml
<launch>
<arg name="model" default="$(find-pkg-share q_ros_tutorial)/urdf/simplebot.urdf.xacro" />
<node pkg="joint_state_publisher" exec="joint_state_publisher" />
<node pkg="robot_state_publisher" exec="robot_state_publisher">
<param name='robot_description' value="$(command 'xacro $(var model)')" />
</node>
</launch>
joint_state_publisher を起動しないと,頭の位置がずれる...
2. パッケージをビルド
$ cd ~/tutorial_ws/
$ colcon build --symlink-install
$ . ./install/local_setup.bash
3. launchファイルを起動
$ ros2 launch q_ros_tutorial urdf.launch.xml
4. rviz2を立ち上げる
$ ros2 run rviz2 rviz2
5. rviz2でロボットモデルを表示させる
DisplaysパネルのGlobal OptionsのFixed Frameを「base_link」に変更
https://gyazo.com/6ae50ecbeab05be07d8dc30735003494
「Add」ボタンを押して,「RobotModel」を選択する.
https://gyazo.com/c64ec820e98c8a7712adae1316339693
RobotModelの「Description Topic」で「/robot_description」を選択する.
https://gyazo.com/b0697fd834f9c6a5653e18b85f39e017
Rviz2上にロボットモデルが表示される.
3. ROSプログラミングでロボットを動かす
3.1 ロボットモデルをシミュレータで確認
1. ロボットモデルをシミュレータで動かすための記述を追記
simplebot.urdf.xacro を編集
code:xml
<?xml version="1.0"?>
・・・
<gazebo>
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
<ros>
<namespace>/simplebot</namespace>
</ros>
<update_rate>30</update_rate>
<num_wheel_pairs>2</num_wheel_pairs>
<left_joint>left_front_wheel_joint</left_joint>
<right_joint>right_front_wheel_joint</right_joint>
<wheel_separation>0.2</wheel_separation>
<wheel_diameter>0.07</wheel_diameter>
<left_joint>left_back_wheel_joint</left_joint>
<right_joint>right_back_wheel_joint</right_joint>
<wheel_separation>0.2</wheel_separation>
<wheel_diameter>0.07</wheel_diameter>
<robot_base_frame>base_link</robot_base_frame>
</plugin>
</gazebo>
<gazebo reference="left_front_wheel_joint">
<mu>10.0</mu>
</gazebo>
<gazebo reference="right_front_wheel_joint">
<mu>10.0</mu>
</gazebo>
<gazebo reference="left_back_wheel_joint">
<mu>10.0</mu>
</gazebo>
<gazebo reference="right_back_wheel_joint">
<mu>10.0</mu>
</gazebo>
</robot>
2. ロボットシミュレータのGazeboを起動するため,launchファイルを編集
urdf.launch.xml を編集
code:urdf.launch.xml
<launch>
...
<!-- 以下を追記 -->
<include file="$(find-pkg-share gazebo_ros)/launch/gazebo.launch.py" />
<node pkg="gazebo_ros" exec="spawn_entity.py" args="-z 1.0 -unpause -topic robot_description -entity simplebot" output="screen" />
</launch>
3. パッケージを再ビルド
$ cd ~/tutorial_ws/
$ colcon build --symlink-install
$ . ./install/local_setup.bash
4. launchファイルを起動
$ ros2 launch q_ros_tutorial urdf.launch.xml
3.2 GUIのコントローラで動作を確認
rqt_robot_steeringパッケージを起動して,動作を確認する.
$ ros2 run rqt_robot_steering rqt_robot_steering
トピック名を「/simplebot/cmd_vel」に設定
https://gyazo.com/5a0fdfd2702ea398ca6b2e0ae7e94199
3.3 プログラムでロボットを動かす
1. プログラムの作成
q_ros_tutorial以下に,publisher.py を新規作成
code:publisher.py
from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
import time
def create_twist(linear, angular):
twist = Twist()
twist.linear.x = linear
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = angular
return twist
class TwistPublisher(Node):
def __init__(self):
super().__init__('twist_publisher')
qos = QoSProfile(depth=10)
self._publisher = self.create_publisher(Twist, '/simplebot/cmd_vel', qos)
time.sleep(0.2)
twist = create_twist(0.3, 0.0)
self._publisher.publish(twist)
print('START')
stop_timer_period = 15
self._stop_timer = self.create_timer(stop_timer_period, self.stop)
def stop(self):
if self._stop_timer:
self.destroy_timer(self._stop_timer)
self._stop_timer = None
twist = create_twist(0.0, 0.0)
self._publisher.publish(twist)
print('STOP')
def main(args=None):
try:
rclpy.init(args=args)
twist_publisher = TwistPublisher()
rclpy.spin(twist_publisher)
except KeyboardInterrupt as identifier:
pass
finally:
twist_publisher.stop()
time.sleep(1)
twist_publisher.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
2. パッケージ設定の修正
setup.py を修正
code:setup.py
setup(
・・・
entry_points={
'console_scripts': [
'publisher = q_ros_tutorial.publisher:main',
],
},
)
3. プログラムの実行
パッケージを再ビルド
$ cd ~/tutorial_ws/
$ colcon build --symlink-install
$ . ./install/local_setup.bash
プログラムの実行
$ ros2 run q_ros_tutorial publisher