M2 MacBook AirでGazeboを少しずつ覚えていきます。
今回やること。
- differential wheeled robotを作成 (*)
- (*)をcmd_velで動くようにする (**)
- ros2通信をGazeboの通信に変換する
- (**)にLiDARモデルを取り付ける
環境。
- M2 MacBook Air
- VMware Fusion
- Ubuntu Server for ARM
- ROS2 Humble
my world 準備
M2 MacBookでも快適に動作する環境を作成します。
(M2 MacBookのVMWareでturtlebot4は快適ではありませんでした。)
$ cd
$ ros/robot_ws/src
$ ros2 pkg create robot_and_world --build-type ament_cmake
$ cd robot_and_world
$ mkdir launch
$ mkdir worlds
$ mkdir maps
$ vi CMakeLists.txt
下記を追加します。
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME})
$ cd worlds
my world 作成 1
differential wheeled robotを作成します。
というかcopyして、理解を進めます。
前回の記事の通りにinstallするとGazebo Fortressがinstallされます。
今はGazebo Harmonicが最新ですがGazebo HarmonicのSDFでは後述するcmd_velで動作しないためGazebo Fortressの記事を参考にします。参考にしますというかcopyします。
下記が車の部分の完成形です。
sensorが付いていません。cmd_velでも動作しません。
SDF : Simulation Description Format
physics tagからreal_time_update_rateが消えてます。
これが1000Hzで更新する設定だから可能な限り小さくするというのを僕はやっていました。
building_robot.sdf (for Gazebo Fortress)
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="car_world">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-user-commands-system"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='vehicle_blue' canonical_link='chassis'>
<pose relative_to='world'>0 0 0 0 0 0</pose> <!--the pose is relative to the world by default-->
<!--chassis-->
<link name='chassis'>
<pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
<inertial> <!--inertial properties of the link mass, inertia matix-->
<mass>1.14395</mass>
<inertia>
<ixx>0.095329</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.381317</iyy>
<iyz>0</iyz>
<izz>0.476646</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
<!--Color of the link-->
<material>
<ambient>0.0 0.0 1.0 1</ambient>
<diffuse>0.0 0.0 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
</collision>
</link>
<!--Left wheel-->
<link name='left_wheel'>
<pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.043333</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.043333</iyy>
<iyz>0</iyz>
<izz>0.08</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<!--The same as left wheel but with different position-->
<link name='right_wheel'>
<pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.043333</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.043333</iyy>
<iyz>0</iyz>
<izz>0.08</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<!--arbitrary frame-->
<frame name="caster_frame" attached_to='chassis'>
<pose>0.8 0 -0.2 0 0 0</pose>
</frame>
<!--caster wheel-->
<link name='caster'>
<pose relative_to='caster_frame'/>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.016</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.016</iyy>
<iyz>0</iyz>
<izz>0.016</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 1 0.0 1</ambient>
<diffuse>0.0 1 0.0 1</diffuse>
<specular>0.0 1 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
<!--left wheel joint-->
<joint name='left_wheel_joint' type='revolute'>
<pose relative_to='left_wheel'/>
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz> <!--can be descired to any frame or even arbitrary frames-->
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<!--right wheel joint-->
<joint name='right_wheel_joint' type='revolute'>
<pose relative_to='right_wheel'/>
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz>
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<!--caster wheel joint--> <!--pose defult value is the child-->
<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>
</model>
</world>
</sdf>
$ vi building_robot.sdf
上記をcopyして保存します。
$ ign gazebo building_robot.sdf
xmlは気合いで読みます。
定義は見てないので、下記は全て予想です。
- physics : 全体の設定
- plugin : 便利なやつ
- light : 光源
- ground_plane : 地面
- collision / normal / z = 1 : 下に落ちない
- vehicle_blue / pose : 位置
- vehicle_blue / chassis : 車体の骨格、シャーシと読むと伝わりやすい、車に限らず基本を為す骨格を示す
- vehicle_blue / mass : 質量
- vehicle_blue / inertia : 慣性モーメント
- vehicle_blue / box : 直方体
- vehicle_blue / size : 大きさ、形状
- 以下略
visualとcollisionの設定を違うと「ん ?」という見え方になるはずです。飛行機に見えるけど当たり判定は丸とか四角みたいな。
jointは3D CADをやっていれば納得の設定。
sdfはfoldingを駆使して戦うことを推奨します。
車の部分の完成形です。
cmd_velで動いてくれません。
my world 作成 2
cmd_velで動いてくれるように設定していきます。設定していきますというかcopyします。
moving_robot.sdf (for Gazebo Fortress)
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="Moving_robot">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-user-commands-system.so"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='vehicle_blue' canonical_link='chassis'>
<pose relative_to='world'>0 0 0 0 0 0</pose> <!--the pose is relative to the world by default-->
<link name='chassis'>
<pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
<inertial> <!--inertial properties of the link mass, inertia matix-->
<mass>1.14395</mass>
<inertia>
<ixx>0.126164</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.416519</iyy>
<iyz>0</iyz>
<izz>0.481014</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.0 1.0 0.5</size> <!--question: this size is in meter-->
</box>
</geometry>
<!--let's add color to our link-->
<material>
<ambient>0.0 0.0 1.0 1</ambient>
<diffuse>0.0 0.0 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'> <!--todo: describe why we need the collision-->
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
</collision>
</link>
<!--let's build the left wheel-->
<link name='left_wheel'>
<pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<!--copy and paste for right wheel but change position-->
<link name='right_wheel'>
<pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<frame name="caster_frame" attached_to='chassis'>
<pose>0.8 0 -0.2 0 0 0</pose>
</frame>
<!--caster wheel-->
<link name='caster'>
<pose relative_to='caster_frame'/>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 1 0.0 1</ambient>
<diffuse>0.0 1 0.0 1</diffuse>
<specular>0.0 1 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
<!--connecting these links together using joints-->
<joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
<pose relative_to='left_wheel'/>
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames-->
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<joint name='right_wheel_joint' type='revolute'>
<pose relative_to='right_wheel'/>
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz>
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<!--different type of joints ball joint--> <!--defult value is the child-->
<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>
<!--diff drive plugin-->
<plugin
filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.2</wheel_separation>
<wheel_radius>0.4</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<topic>cmd_vel</topic>
</plugin>
</model>
<!-- Moving Forward-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777235</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Moving Backward-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777237</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: -0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Rotating right-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777236</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: -0.5}
</output>
</plugin>
<!--Rotating left-->
<plugin filename="libignition-gazebo-triggered-publisher-system.so"
name="ignition::gazebo::systems::TriggeredPublisher">
<input type="ignition.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777234</match>
</input>
<output type="ignition.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: 0.5}
</output>
</plugin>
</world>
</sdf>
$ vi moving_robot.sdf
上記をcopyして保存します。
vimdiffやWinMergeで確認してください。慣性モーメントの値が結構変わってます。なんでや。
$ ign gazebo moving_robot.sdf
play buttonをclickします。
別のterminalを立ち上げます。下記を実行します。cmd_velはありません。
$ ros2 topic list
ignition版ros2 topic list。
$ ign topic -l
下記で動きます。
$ ign topic -t "/cmd_vel" -m ignition.msgs.Twist -p "linear: {x: 0.5}, angular: {z: 0.05}"
my world 作成 3
ちなみに下記とすると自動的にplay buttonが押されます。重要。
$ ign gazebo -r moving_robot.sdf
下記は始めに一通り読んだ方が結果的に効率が良いでしょう。
$ ign gazebo -h
my world 作成 4
ROS2 topicに変換してくれるbridgeは用意されていますので、それを使います。
別のterminalを起動します。
$ ros2 run ros_gz_bridge parameter_bridge /cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist
ros_ign_bridgeはROS 1用と書かれていますが同様に動作します。(ですが大人しくros_gz_bridgeを使いましょう !)
$ ros2 run ros_ign_bridge parameter_bridge /cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist
別のterminalを起動します。
前進。
$ ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.5}}'
後進。
$ ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist '{linear: {x: -0.5}}'
停止。
$ ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist '{linear: {x: 0.0}}'
上記の繰り返しだけでもまあまあ遊べます。
my world 作成 5
LiDARを取り付けます。
下記の公式の記事はきちんと検証していないようで期待の動作にならないのでvisualize_lidar.sdfから持ってくることにします。
moving_robot_lidar.sdf (for Gazebo Fortress)
<?xml version="1.0" ?>
<sdf version="1.9">
<world name="Moving_robot">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-user-commands-system.so"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<plugin
filename="libignition-gazebo-sensors-system.so"
name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<gui fullscreen='false'>
<plugin name='3D View' filename='MinimalScene'>
<ignition-gui>
<title>3D View</title>
<property type='bool' key='showTitleBar'>false</property>
<property type='string' key='state'>docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
</plugin>
<plugin name='Scene Manager' filename='GzSceneManager'>
<ignition-gui>
<property key='resizable' type='bool'>true</property>
<property key='width' type='double'>5</property>
<property key='height' type='double'>5</property>
<property key='state' type='string'>floating</property>
<property key='showTitleBar' type='bool'>false</property>
</ignition-gui>
</plugin>
<plugin name='Entity tree' filename='EntityTree'>
<ignition-gui>
<property type='string' key='state'>docked</property>
</ignition-gui>
</plugin>
<plugin name='Visualize Lidar' filename='VisualizeLidar'/>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='vehicle_blue'>
<pose relative_to='world'>0 0 0 0 0 0</pose> <!--the pose is relative to the world by default-->
<link name='chassis'>
<pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
<inertial> <!--inertial properties of the link mass, inertia matix-->
<mass>1.14395</mass>
<inertia>
<ixx>0.126164</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.416519</iyy>
<iyz>0</iyz>
<izz>0.481014</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.0 1.0 0.5</size> <!--question: this size is in meter-->
</box>
</geometry>
<!--let's add color to our link-->
<material>
<ambient>0.0 0.0 1.0 1</ambient>
<diffuse>0.0 0.0 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'> <!--todo: describe why we need the collision-->
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
</collision>
</link>
<link name='lidar_link'>
<pose>1 0 1 0 0 0</pose>
<inertial>
<mass>0.10000000000000001</mass>
<inertia>
<ixx>0.00016666700000000001</ixx>
<iyy>0.00016666700000000001</iyy>
<izz>0.00016666700000000001</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<bounce/>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name='gpu_lidar' type='gpu_lidar'>
<pose>0 0 0 0 -0 0</pose>
<topic>scan</topic>
<update_rate>10</update_rate>
<enable_metrics>false</enable_metrics>
<lidar>
<scan>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>-1.3962600000000001</min_angle>
<max_angle>1.3962600000000001</max_angle>
</horizontal>
<vertical>
<samples>1</samples>
<min_angle>0</min_angle>
<max_angle>0</max_angle>
<resolution>1</resolution>
</vertical>
</scan>
<range>
<min>0.080000000000000002</min>
<max>10</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>none</type>
<mean>0</mean>
<stddev>0</stddev>
</noise>
<visibility_mask>4294967295</visibility_mask>
</lidar>
</sensor>
<enable_wind>false</enable_wind>
</link>
<!-- <frame name="lidar_frame" attached_to='chassis'> -->
<!-- <pose>0 0 0.1 0 0 0</pose> -->
<!-- </frame> -->
<!--let's build the left wheel-->
<link name='left_wheel'>
<pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<!--copy and paste for right wheel but change position-->
<link name='right_wheel'>
<pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<frame name="caster_frame" attached_to='chassis'>
<pose>0.8 0 -0.2 0 0 0</pose>
</frame>
<!--caster wheel-->
<link name='caster'>
<pose relative_to='caster_frame'/>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 1 0.0 1</ambient>
<diffuse>0.0 1 0.0 1</diffuse>
<specular>0.0 1 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
<!--connecting these links together using joints-->
<joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
<pose relative_to='left_wheel'/>
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz> <!--can be defined as any frame or even arbitrary frames-->
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<joint name='right_wheel_joint' type='revolute'>
<pose relative_to='right_wheel'/>
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz>
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<!--different type of joints ball joint--> <!--defult value is the child-->
<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>
<joint name='lidar_sensor_joint' type='fixed'>
<parent>chassis</parent>
<child>lidar_link</child>
<pose>0 0 0 0 0 0</pose>
</joint>
<!--diff drive plugin-->
<plugin
filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.2</wheel_separation>
<wheel_radius>0.4</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<topic>cmd_vel</topic>
</plugin>
</model>
</world>
</sdf>
実行。再掲。
$ ign topic -t "/cmd_vel" -m ignition.msgs.Twist -p "linear: {x: 0.5}, angular: {z: 0.05}"
簡易に検証する方法です。
$ ign topic -e -t /lidar
mp4もtsも普通に見れるのにuploadしたらおかしくなる。なんでや。