Embedded Files
URDFの定義に従ってゼロから移動台車を構成します．
ROS 2のNavigation2を最後に使えるようにします．
Navigation2の移動ロボットの構築ページを参考に作成をします．
移動ロボットでいちばん簡単な構成は差動２輪タイプで，左右にある２車輪を独立に制御可能な状態で，１輪を補助輪（キャスタなど）として搭載したモデルとなります．
キャスタのモデルを作るのは面倒なので，参考ページと同じく球体を補助輪の代わりにして，理想的な平面に対して点接地している状態で前後左右にある程度の摩擦力で滑る状態にします．
TFの関係の確認
TFの関係の確認
移動ロボットに設定する座標系は，全体のマップを管理する座標系を「map」として，mapに対する移動ロボットの位置姿勢を表す座標系を「odom」とします．
次に，移動ロボットの本体に固定された座標系を「base_link」として設定し，移動ロボットの位置を表す「odom」と「base_link」を接続します．
最後に，ロボット本体に取り付けられた各種センサの位置を固有の座標系で設定し，「base_link」と接続します．例えば，Lidarをロボットに搭載したときに，Lidarの座標系を「base_laser」としたとき，「base_link」と「base_laser」を固定した値で接続します．
URDFファイルに記載する方法がありますが，単体でtf2を使っても座標系を定義しておくことが可能です．
下記に，「base_link」と「base_laser」をコマンドラインでtf2を使って宣言する方法を示します．
１つ目の端末
ros2 run tf2_ros static_transform_publisher 0.1 0 0.2 0 0 0 base_link base_laser
２つ目の端末
ros2 run tf2_ros tf2_echo base_link base_laser
２つ目の端末で，１つ目の端末で設定したbase_linkとbase_laserの位置関係を確認することができます．
URDFの作成
URDFの作成
２輪の差動移動の構成＋キャスタ（ボール）で移動ロボットを構築します．
まずは，接続を管理するためのパッケージとxacroに関するパッケージをインストールします．
sudo apt install ros-humble-joint-state-publisher-gui
sudo apt install ros-humble-xacro
つぎに，移動ロボットのモデルを管理するパッケージを作成します．
/home/ユーザ名/ros2_ws/srcでソースを管理していると仮定して説明します．
また，パッケージ名は本家と同じくsam_bot_descriptionとします．
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake sam_bot_description ＃C++で一応宣言
cd sam_bot_description/src ＃作成したパッケージのsrcに移動
mkdir description #srcの中にURDFを保存するdescriptionのディレクトリ作成
cd description
gedit sam_bot_description.urdf #テキストエディタでsam_bot_description.urdfを作成
最後のgeditはテキストエディタであれば何でもOK．
sam_bot_description.urdfファイルのソースは下記の通り．
<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Define robot constants -->
<xacro:property name="base_width" value="0.31"/>
<xacro:property name="base_length" value="0.42"/>
<xacro:property name="base_height" value="0.18"/>
<xacro:property name="wheel_radius" value="0.10"/>
<xacro:property name="wheel_width" value="0.04"/>
<xacro:property name="wheel_ygap" value="0.025"/>
<xacro:property name="wheel_zoff" value="0.05"/>
<xacro:property name="wheel_xoff" value="0.12"/>
<xacro:property name="caster_xoff" value="0.14"/>
<!-- Robot Base -->
<link name="base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
</link>
<!-- Robot Footprint -->
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
</joint>
<!-- Wheels -->
<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
<link name="${prefix}_link">
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<material name="Gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
</link>
<joint name="${prefix}_joint" type="continuous">
<parent link="base_link"/>
<child link="${prefix}_link"/>
<origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</xacro:macro>
<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
<!-- Caster Wheel -->
<link name="front_caster">
<visual>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
</link>
<joint name="caster_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster"/>
<origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
</joint>
</robot>
次に，package.xmlを修正する．
<buildtool_depend>の下に下記を追加．どこでも良いが，<buildtool_depend>の後ろが推奨．
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>xacro</exec_depend>
次にlaunchディレクトリを作成して，display.launch.pyを生成
cd ~/ros2_ws/src/sam_bot_description
mkdir launch
cd launch
gedit display.launch.py
下記がdisplay.launch.pyのソース．
import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os
def generate_launch_description():
pkg_share = launch_ros.substitutions.FindPackageShare(package='sam_bot_description').find('sam_bot_description')
default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf')
default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz') #rvizの設定．次のステップで記載
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
)
joint_state_publisher_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
parameters=[{'robot_description': Command(['xacro ', default_model_path])}],
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = launch_ros.actions.Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)
rviz_node = launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
description='Absolute path to robot urdf file'),
launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
description='Absolute path to rviz config file'),
joint_state_publisher_node,
joint_state_publisher_gui_node,
robot_state_publisher_node,
rviz_node
])
次に，rvizディレクトリをパッケージ中に作成して，表示時にrvizの設定を読み込んで表示できるようにする．(launchファイル内ですでに記載してしまっている）
cd ~/ros2_ws/src/sam_bot_description
mkdir rviz
gedit urdf_config.rviz
urdf_config.rvizの記載内容は下記．
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1/Links1
- /TF1
Splitter Ratio: 0.5
Tree Height: 557
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Name: Grid
- Alpha: 0.6
Class: rviz_default_plugins/RobotModel
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Name: RobotModel
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Name: TF
Marker Scale: 0.3
Show Arrows: true
Show Axes: true
Show Names: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Name: Current View
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Saved: ~
最後に，CmakeLists.txtを修正します．
if(BUILD_TESTING) の上に下記を追記．src launch rvizをインストールディレクトリにコピーするため，
ファイル内のinstallの記載を下記のように追記．
install(
DIRECTORY src launch rviz
DESTINATION share/${PROJECT_NAME}
)
上記記載が終わって，保存をしたらビルドをしてパスを通して下さい．
cd ~/ros2_ws
colcon build
source install/setup.bash
では，launchファイルを立ち上げて表示をしてみましょう．
ros2 launch sam_bot_description display.launch.py
上記コマンドでlaunchファイルを起動すると下記のrvizの画面が表示されます．
TFの座標が確認できるので，問題なく接続されているか確認すること．
JointStatePublisherのGUIウィンドウで左右のタイヤをスライドバーで回転させることができます．
物理パラメータの追加
物理パラメータの追加
座標系と形状のみの規定しかできていないため，質量・慣性や接触判定のボリュームを設定する．
物理パラメータの設定がないとGazeboなどの物理シミュレーションで正確な動作ができません．
NAV2のHPの記載の通り，特定の形状（直方体，円柱，球）に対して，寸法を入れることで密度が均等と仮定した公式から重心位置，慣性モーメントを計算するマクロを設定します．
下記のマクロの記載をsam_bot_description.urdfの<xacro:property>で各種パラメータを設定していた行の後ろにペーストします．
<!-- Define intertial property macros -->
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
<mass value="${m}"/>
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<mass value="${m}"/>
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}"/>
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
</inertial>
</xacro:macro>
次に，各種オブジェクトの接触判定のボリュームを設定します．
まずベースリンクの接触判定は下記の通り．
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
マクロで直方体の質量と寸法をいれることで，慣性モーメントを設定しています．
上記の記載は，<link name="base_link">で記載しているタグの中にペーストします．（わかりにくいと思いますので，最後に修正後のファイルを提示してあります）
また，車輪については下記記載の通り接触判定とマクロを使った質量と慣性モーメントの設定を行います．
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
</collision>
<xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
こちらも車輪を表す<link name="${prefix}_link">タグの中にペーストしてください．
最後に，キャスター部分の球体の接触判定と質量・慣性モーメントを下記の記載で実装します．
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
</collision>
<xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
<link name="front_caster">タグの中に上記の記載をペーストしてください．
これまでの内容を修正したsam_bot_description.urdfファイルを下記に示します．
sam_bot_description.urdf
<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Define robot constants -->
<xacro:property name="base_width" value="0.31"/>
<xacro:property name="base_length" value="0.42"/>
<xacro:property name="base_height" value="0.18"/>
<xacro:property name="wheel_radius" value="0.10"/>
<xacro:property name="wheel_width" value="0.04"/>
<xacro:property name="wheel_ygap" value="0.025"/>
<xacro:property name="wheel_zoff" value="0.05"/>
<xacro:property name="wheel_xoff" value="0.12"/>
<xacro:property name="caster_xoff" value="0.14"/>
<!-- Define intertial property macros -->
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
<mass value="${m}"/>
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<mass value="${m}"/>
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}"/>
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
</inertial>
</xacro:macro>
<!-- Robot Base -->
<link name="base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
</link>
<!-- Robot Footprint -->
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
</joint>
<!-- Wheels -->
<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
<link name="${prefix}_link">
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<material name="Gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
</collision>
<xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
</link>
<joint name="${prefix}_joint" type="continuous">
<parent link="base_link"/>
<child link="${prefix}_link"/>
<origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</xacro:macro>
<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
<!-- Caster Wheel -->
<link name="front_caster">
<visual>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
</collision>
<xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</link>
<joint name="caster_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster"/>
<origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
</joint>
</robot>
設定がうまくできているか確認するために，rvizを立ち上げて確認します．
とりあえず，buildをしてからurdfを見るためのlaunchファイルを立ち上げます．
cd ~/ros2_ws
colcon build
source install/setup.bash
ros2 launch sam_bot_description display.launch.py
上記コマンドでlaunchファイルで起動したrvizの画面（下図）の左メニューのRobotModelの中にあるVisual Enableのチェックを外して，Collision Enableのチェックを付けて車体と車輪が表示されていれば接触判定のボリュームが設定できていることを確認できます．
このページは以上で，Gazeboの表示は別ページで説明します．
Page updated
Google Sites
Report abuse