Gazebo机器人仿真
本文基于 B站冰达机器人Gazebo教程,针对在仿真过程中出现的问题提出相应解决办法。
一、实验目标和实验准备
目标: 设计出一台具备激光雷达、IMU和相机的机器人仿真模型用于相关实验。
- 获取实验功能包:
git clone https://gitee.com/bingda-robot/bingda_tutorials
- 克隆完成后在工作空间路径下编译功能包
catkin_make --pkg bingda_tutorials
- 安装其他依赖
sudo apt-get install ros-$ROS_DISTRO-gazebo-ros ros-$ROS_DISTRO-gazebo-ros-control ros-$ROS_DISTRO-gazebo-plugins ros-$ROS_DISTRO-joint-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-robot-state-publisher
二、Gazebo的使用和world创建
获取gazebo模型库:
git clone https://gitee.com/bingda-gazebo_models.git
自制实验场景 Edit -> Building Editor
-
保存模型:File -> Save -> 保存路径
-
保存环境模型:File -> Save World As -> 保存路径
这里创建了一个maze.world文件。
创建完成后可将环境模型置于功能包的world文件夹下。
可使用gazebo_world.launch来启动Gazebo加载该环境模型,注意world文件名可替换。
gazebo_world.launch
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find bingda_tutorials)/world/maze.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
</launch>
启动该launch文件
roslaunch bingda_tutorials gazebo_world.launch
Gazebo出现之前创建的环境模型即为成功。
三、URDF和机器人模型
介绍机器人描述文件URDF(Unified Robot Description Format,统一机器人描述格式)
以及<robot>,<link>,<joint>等标签使用方法,读者可由以下链接进入官网查看更详细的说明。
mybot.urdf
<?xml version="1.0"?>
<robot name="mybot">
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="base_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.001" />
</inertial>
<visual>
<geometry>
<box size="0.25 0.16 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.25 0.16 0.05"/>
</geometry>
</collision>
</link>
<link name="right_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="right_wheel_joint" type="continuous">
<axis xyz="0 0 -1"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<origin rpy="1.5707 0 0" xyz=" 0.1 -0.09 -0.03"/>
</joint>
<link name="left_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="left_wheel_joint" type="continuous">
<axis xyz="0 0 -1"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<origin rpy="1.5707 0 0" xyz="0.1 0.09 -0.03"/>
</joint>
<link name="ball_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
<visual>
<geometry>
<sphere radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="ball_wheel_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="ball_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.10 0 -0.03"/>
</joint>
</robot>
- 注意:一般将link的可视化属性<visual>与碰撞属性<collision>的参数设为一致,所见即所仿真。
bingda_tutorials中已经描述了一个带转向轮的三轮小车mybot,可使用check_urdf 命令来检查URDF文件语法
roscd bingda_tutorials/urdf/
check_urdf mybot.urdf
结果显示如上图所示:查看mybot.urdf文件可以发现空link属性的base_footprint,是约定俗成代表该机器人地面投影的link,base_footprint通过base_joint与base_link连接,base_link通过其他joint与3个child link相连,分别是左轮,右轮和万向轮。
可能会提示没有安装该命令相关的包,以下命令补上。
sudo apt install liburdfdom-tools
机器人配置文件设置好后,可启动gazebo_robot.launch文件观察实际效果,其中gazebo_world.launch文件内为自行设置的环境模型。
gazebo_robot.launch
<launch>
<include file="$(find bingda_tutorials)/launch/gazebo_world.launch"/>
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find bingda_tutorials)/urdf/mybot.urdf -urdf -model robot_description" output="screen" />
</launch>
启动launch文件
roslaunch bingda_tutorials gazebo_robot.launch
可以观察到在环境模型中加载了小车模型,但是还不能进行仿真,也没有颜色,需要添加<gazebo>相关标签。
四、传感器和执行文件安装
目标:为车轮增加执行器件,并添加激光雷达、IMU和摄像头。
xacro文件,对urdf文件的补充。不是非用不可,但不失为一种简化代码的手段。
mybot.xacro
<?xml version="1.0"?>
<!-- To declare this code can be explained by xacro -->
<robot name="mybot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Include this file to make a distinction between module file and simulation file-->
<xacro:include filename="$(find bingda_tutorials)/urdf/mybot.gazebo.xacro" />
<!-- The code is same as mybot.urdf in this place -->
<!-- imu sensor -->
<link name="imu">
<visual>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu"/>
<origin xyz="0.08 0 0.025"/>
</joint>
<!-- camera -->
<link name="base_camera_link">
<visual>
<geometry>
<box size="0.02 0.03 0.03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="camera_joint" type="fixed">
<parent link="base_link"/>
<child link="base_camera_link"/>
<origin xyz="0.1 0 0.025"/>
</joint>
<!-- laser lidar -->
<link name="base_laser_link">
<visual>
<geometry>
<cylinder length="0.06" radius="0.04"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="laser_joint" type="fixed">
<parent link="base_link"/>
<child link="base_laser_link"/>
<origin xyz="0 0.0 0.06"/>
</joint>
</robot>
mybot.xacro文件仅定义了模型中增加的传感器的位置大小以及连接方式,并未真正意义上的添加传感器的仿真功能,具体仿真效果见mybot.gazebo.xacro文件。这样做的目的是将模型文件与仿真文件分开存放,互不影响,增加项目文件的复用性,同时使代码理解更简单。
mybot.gazebo.xacro
<?xml version="1.0"?>
<robot name="mybot" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="laser_visual" default="false"/>
<xacro:arg name="camera_visual" default="false"/>
<xacro:arg name="imu_visual" default="false"/>
<gazebo reference="base_link">
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="left_wheel_link">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="right_wheel_link">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="ball_wheel_link">
<mu1>0.1</mu1>
<mu2>0.1</mu2>
<kp>500000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="imu">
<sensor type="imu" name="imu">
<always_on>true</always_on>
<visualize>$(arg imu_visual)</visualize>
</sensor>
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo>
<plugin name="mybot_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometrySource>world</odometrySource>
<publishOdomTF>true</publishOdomTF>
<robotBaseFrame>base_footprint</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishTf>true</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<legacyMode>false</legacyMode>
<updateRate>30</updateRate>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>0.180</wheelSeparation>
<wheelDiameter>0.05</wheelDiameter>
<wheelAcceleration>10</wheelAcceleration>
<wheelTorque>100</wheelTorque>
<rosDebugLevel>na</rosDebugLevel>
</plugin>
</gazebo>
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
<alwaysOn>true</alwaysOn>
<bodyName>imu</bodyName>
<frameName>imu</frameName>
<topicName>imu</topicName>
<serviceName>imu_service</serviceName>
<gaussianNoise>0.0</gaussianNoise>
<updateRate>0</updateRate>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</rate>
<accel>
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
</noise>
</imu>
</plugin>
</gazebo>
<gazebo reference="base_laser_link">
<material>Gazebo/FlatBlack</material>
<sensor type="ray" name="rplidar_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>$(arg laser_visual)</visualize>
<update_rate>7</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>0.5</resolution>
<min_angle>0.0</min_angle>
<max_angle>6.28319</max_angle>
</horizontal>
</scan>
<range>
<min>0.120</min>
<max>12.0</max>
<resolution>0.015</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_rplidar_controller" filename="libgazebo_ros_laser.so">
<topicName>scan</topicName>
<frameName>base_laser_link</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="base_camera_link">
<sensor type="camera" name="csi Camera">
<always_on>true</always_on>
<visualize>$(arg camera_visual)</visualize>
<camera>
<horizontal_fov>1.085595</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.03</near>
<far>100</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>/</cameraName>
<frameName>base_camera_link</frameName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>
mybot.gazebo.xacro具体定义传感器在gazebo中的仿真效果即<gazebo>标签,包括视觉上的颜色以及控制效果等,且只在gezebo中生效,传感器以插件plugin的标签载入。
可启动simulation_robot.launch文件查看效果。
rosrlaunch bingda_tutorials simulation_robot.launch
可以观察到与之前相比,小车已经有了颜色,并且添加了传感器。
验证摄像头功能:
rqt_image_view
选择订阅摄像头发布的话题 /image_raw/compress 。
验证完成!
五、使用Gazebo进行SLAM建图和导航实验
1. 实验预准备
安装建图导航功能包robot_navigation:
cd ~/catkin_ws/src/
git clone https://gitee.com/bingda-robot/robot_navigation.git
cd ~/catkin_ws/
catkin_make
sudo apt-get install ros-$ROS_DISTRO-amcl ros-$ROS_DISTRO-move-base ros-$ROS_DISTRO-slam-gmapping ros-$ROS_DISTRO-slam-karto ros-$ROS_DISTRO-cartographer ros-$ROS_DISTRO-cartographer-ros ros-$ROS_DISTRO-dwa-local-planner ros-$ROS_DISTRO-teb-local-planner ros-$ROS_DISTRO-map-server ros-$ROS_DISTRO-hector-slam*
echo "export BASE_TYPE=NanoRobot" >> ~/.bashrc
source ~/.bashrc
2. 建图实验
- 启动仿真文件simulation_robot.launch
roslaunch bingda_tutorials simulation_robot.launch
- 使用gmapping算法进行建图
roslaunch robot_navigation gmapping.launch simulation:=true
simulation为true表示需要仿真,与gazebo建立同步时间线。
- 打开rviz
roslaunch robot_navigation slam_rviz.launch
- 获取小车摄像头图像
rqt_image_view
- 打开键盘控制节点,控制小车运动并建图
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
若没有安装此功能包,可由以下指令安装:
sudo apt-get install ros-melodic-teleop-twist-keyboard
- 建图完成后,对地图进行保存,这里命名为mazemap
roscd robot_navigation/maps
rosrun map_server map_saver -f mazemap
3. 导航实验
根据刚才建立的地图进行导航
robot_navigation功能包下提供了一个与导航堆栈相关的launch文件navigation_stack.launch
navigation_stack.launch
<launch>
<!-- Arguments -->
<arg name="map_file" default="$(find robot_navigation)/maps/mazemap.yaml"/>
<arg name="simulation" default= "false"/>
<arg name="planner" default="dwa" doc="opt: dwa, teb"/>
<arg name="move_forward_only" default="false"/>
<arg name="use_dijkstra" default= "true"/>
<!-- Map server -->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)">
<param name="frame_id" value="map"/>
</node>
<!-- AMCL -->
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/amcl_params.yaml" command="load" />
<param name="initial_pose_x" value="0.0"/>
<param name="initial_pose_y" value="0.0"/>
<param name="initial_pose_a" value="0.0"/>
</node>
<!-- move_base -->
<include file="$(find robot_navigation)/launch/move_base.launch" >
<arg name="planner" value="$(arg planner)"/>
<arg name="simulation" value="$(arg simulation)"/>
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
<arg name="use_dijkstra" value="$(arg use_dijkstra)"/>
</include>
</launch>
该launch文件会启动地图服务器,amcl定位节点等等。
需要注意的是这里BASE_TYPE为NanoCar,与建立的小车模型在尺寸上会有不同,进而可能导致在进行导航实验时会发生碰撞,读者可在robot_navigation/param文件夹下建立自己的小车尺寸数据。
- 启动仿真文件simulation_robot.launch
roslaunch bingda_tutorials simulation_robot.launch
- 启动navigation_stack.launch
roslaunch robot_navigation navigation_stack.launch simulation:=true
- 打开rviz
roslaunch robot_navigation slam_rviz.launch
- 获取小车摄像头图像
rqt_image_view
Rviz会加载之前保存的地图,此时确定目标点位姿(图中紫色箭头所示),小车即可自动开始进行导航。
小车顺利到达目标点,导航实验成功!
六、从Solidworks中创建URDF模型
使用sw_urdf_exporter工具进行转换,在转换前须确定主要坐标轴。