小车模型建模
安装gzgarden
sudo apt install ros-humble-ros-gzgarden
新建building_robot.sdf文件写入下面内容,创建一个小车模型参考官网教程https://gazebosim.org/docs/garden/building_robot,这里增加了一个地球坐标插件spherical_coordinates用以实现gps功能,与imu插件。
模拟真实情况IMU增加高斯噪声
gps增加高斯误差
下面是完整代码
<?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="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<!-- 地球坐标 -->
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<world_frame_orientation>ENU</world_frame_orientation>
<latitude_deg>38.328636</latitude_deg>
<longitude_deg>114.390793</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<!-- gps -->
<plugin name='gz::sim::systems::NavSat' filename='gz-sim-navsat-system'/>
<!-- imu插件 -->
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<!-- 后 -->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777237</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: -3}, angular: {z: 0}
</output>
</plugin>
<!--前-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777235</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 3}, angular: {z: 0}
</output>
</plugin>
<!--左-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777234</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0}, angular: {z: 0.3}
</output>
</plugin>
<!-- 右-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777236</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0}, angular: {z: -0.3}
</output>
</plugin>
<!-- 停-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/keyboard/keypress">
<match field="data">16777220</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0}, angular: {z: 0}
</output>
</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.2 0.2 0.2 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>
<mass>1.14395</mass>
<inertia>
<ixx>0.00095329</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00381317</iyy>
<iyz>0</iyz>
<izz>0.00476646</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>0
<size>2. 1.0 0.5</size>
</box>
</geometry>
<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>
<!--imu高斯误差-->
<sensor name="imu_sensor" type="imu">
<pose relative_to='chassis'>-0.5 0 0 0 0 0</pose>
<always_on>1</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
</linear_acceleration>
</imu>
</sensor>
<!-- gps -->
<sensor name='navsat' type='navsat'>
<always_on>true</always_on>
<update_rate>10</update_rate>
<topic>navsat</topic>
<navsat>
<position_sensing>
<horizontal>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.00001</stddev>
<bias_stddev>0.00001</bias_stddev>
</noise>
</horizontal>
<vertical>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.00001</stddev>
<bias_stddev>0.00001</bias_stddev>
</noise>
</vertical>
</position_sensing>
</navsat>
</sensor>
</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.00043333</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00043333</iyy>
<iyz>0</iyz>
<izz>0.0008</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.00043333</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00043333</iyy>
<iyz>0</iyz>
<izz>0.0008</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.00016</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00016</iyy>
<iyz>0</iyz>
<izz>0.00016</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>
<!-- 控制移动 -->
<plugin
filename="gz-sim-diff-drive-system"
name="gz::sim::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>100</odom_publish_frequency>
<topic>cmd_vel</topic>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
</plugin>
</model>
</world>
</sdf>
gazebo sim 仿真
运行刚写好的.sdf文件,自动打开gazebo并显示一个蓝色的小车
gz sim building_robot.sdf
在gazebo上面的搜索中找到key_publisher插件,将输出名为/keyboard/keypress的主题这样用键盘上下左右箭头控制小车移动,点击左下角的run the simulink按键启动仿真
查看按键主题,在gazebo中按下上下左右键盘控制小车前后移动左右转动,回车停止。gz 查看主题的方式和ros不太一样可以使用gz topic -h命令查看使用方法
gz topic -e -t /keyboard/keypress
查看imu数据
gz topic -e -t /imu
gz ros 转换
ros2 是无法查看gz的主题的需要进行一下转换
使用ros_gz_bridge将gz主题转换成ros能见的主题
转换gps主题
ros2 run ros_gz_bridge parameter_bridge /navsat@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat
转换imu主题
ros2 run ros_gz_bridge parameter_bridge /imu@sensor_msgs/msg/Imu@gz.msgs.IMU
转换里程计主题
ros2 run ros_gz_bridge parameter_bridge /model/vehicle_blue/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry
转换真实位移
ros2 run ros_gz_bridge parameter_bridge /world/car_world/dynamic_pose/info@geometry_msgs/msg/PoseArray@gz.msgs.Pose_V
使用ros2 topic list 查看转换的主题,这样可以通过ros2 操作处理仿真的数据了