Chapter4：机器人仿真

$ROS1$的基础及应用，基于古月的课，各位可以去看，基于$\mathrm{hawkbot}$机器人进行实际操作。
$\mathrm{ROS}$版本：$ROS1$的$\mathrm{Melodic}$；实际机器人：$\mathrm{Hawkbot}$；

---

---

1.机器人URDF模型优化

1. $\mathrm{URDF}$模型进化版本–$\mathrm{xacro}$模型文件

   1. 精简模型代码
      • 创建宏定义；
      • 文件包含；
   2. 提供可编程接口
      • 常量；
      • 变量；
      • 数学计算；
      • 条件语句；

2. 常量定义

   # 常量定义
   <xacro:property name="M_PI" value="3.14159" /># 常量使用
   <origin xyz="0 0 0" ryp="${M_PI/2} 0 0" />
   

3. 数学计算

   # 数学计算
   <origin xyz="0 ${(motor_length+wheel_length)/2} 0" rpy="0 0 0"/>
   

4. 宏定义

   # 宏定义
   <xacro:macro name="name" params="A B C">
   ...
   </xacro:macro># 宏调用
   <name A="A_value" B="B_value" C="C_value" />
   

5. 文件包含

   # 文件包含
   <xacro:include filename="$(find mbot_description)/urdf/xacro/mbot_base.xacro" />
   

2.使用xacro建立模型

# 1.在urdf文件夹下建立xacro文件夹
mkdir xacro# 在xacro目录下新建mbot_base.xacro和mbot.xacro文件
# 文件内容见下个代码块，请勿直接复制粘贴
touch mbot_base.xacro mbot.xacro# 2.新建display_mbot_base_xacro.launch文件
# 文件内容见下个代码块，请勿直接复制粘贴
touch display_mbot_base_xacro.launch# 3.模型显示
# 法1：将xacro文件转换成URDF文件显示
rosrun xacro xacro.py mbot.xacro > mbot.urdf# 法2：直接调用xacro文件解析器，在.launch文件中写入
<arg name="model" default="$(find xacro)/xacro--inorder'$(find mbot_description)/urdf/xacro/mbot.xacro'" /><param name="robot_description" command="$(arg model)" /># 4.启动.launch文件
roslaunch mbot_description display_mbot_base_xacro.launch

# mbot_base.xacro文件内容
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro"><!-- PROPERTY LIST --><xacro:property name="M_PI" value="3.1415926"/><xacro:property name="base_radius" value="0.20"/><xacro:property name="base_length" value="0.16"/><xacro:property name="wheel_radius" value="0.06"/><xacro:property name="wheel_length" value="0.025"/><xacro:property name="wheel_joint_y" value="0.19"/><xacro:property name="wheel_joint_z" value="0.05"/><xacro:property name="caster_radius" value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) --><xacro:property name="caster_joint_x" value="0.18"/><!-- Defining the colors used in this robot --><material name="yellow"><color rgba="1 0.4 0 1"/></material><material name="black"><color rgba="0 0 0 0.95"/></material><material name="gray"><color rgba="0.75 0.75 0.75 1"/></material><!-- Macro for robot wheel --><xacro:macro name="wheel" params="prefix reflect"><joint name="${prefix}_wheel_joint" type="continuous"><origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/><parent link="base_link"/><child link="${prefix}_wheel_link"/><axis xyz="0 1 0"/></joint><link name="${prefix}_wheel_link"><visual><origin xyz="0 0 0" rpy="${M_PI/2} 0 0" /><geometry><cylinder radius="${wheel_radius}" length = "${wheel_length}"/></geometry><material name="gray" /></visual></link></xacro:macro><!-- Macro for robot caster --><xacro:macro name="caster" params="prefix reflect"><joint name="${prefix}_caster_joint" type="continuous"><origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/><parent link="base_link"/><child link="${prefix}_caster_link"/><axis xyz="0 1 0"/></joint><link name="${prefix}_caster_link"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><sphere radius="${caster_radius}" /></geometry><material name="black" /></visual></link></xacro:macro><xacro:macro name="mbot_base"><link name="base_footprint"><visual><origin xyz="0 0 0" rpy="0 0 0" /><geometry><box size="0.001 0.001 0.001" /></geometry></visual></link><joint name="base_footprint_joint" type="fixed"><origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        <parent link="base_footprint"/><child link="base_link" /></joint><link name="base_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="${base_length}" radius="${base_radius}"/></geometry><material name="yellow" /></visual></link><wheel prefix="left" reflect="-1"/><wheel prefix="right" reflect="1"/><caster prefix="front" reflect="-1"/><caster prefix="back" reflect="1"/></xacro:macro>
</robot>

# mbot.xacro文件内容
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro"><xacro:include filename="$(find mbot_description)/urdf/xacro/mbot_base.xacro" /><mbot_base/></robot>

# display_mbot_base_xacro.launch文件内容
<launch><arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot.xacro'" /><arg name="gui" default="true" /><param name="robot_description" command="$(arg model)" /><!-- 设置GUI参数，显示关节控制插件 --><param name="use_gui" value="$(arg gui)"/><!-- 运行joint_state_publisher节点，发布机器人的关节状态  --><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /><!-- 运行robot_state_publisher节点，发布tf  --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /><!-- 运行rviz可视化界面 --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot.rviz" required="true" /></launch>

3.ArbotiX+rviz功能仿真

1. $\mathrm{ArbotiX}$简介

   • 一款控制电机、舵机的硬件控制板；
   • 提供了相应的$\mathrm{ROS}$功能包；
   • 提供了一个差速控制器，通过接收速度控制指令，更新机器人的里程计状态；

2. $\mathrm{ArbotiX}$安装

   # ros相关信息：Ubuntu 18.04+melodic
   # 1.下载ArbotiX功能包
   cd willard_ws/src/
   git clone -b indigo-devel
   https://github.com/vanadiumlabs/arbotix_ros.git# 2.工作空间下编译
   cd ~/willard_ws/
   catkin_make

3. 配置$\mathrm{ArbotiX}$控制器

   # 1.创建launch文件
   cd willard_ws/src/mbot_description/launch/xacro/# 文件内容见下个代码块，请勿直接复制粘贴
   touch arbotix_mbot_with_camera_xacro.launch# 2.创建配置文件
   cd willard_ws/src/mbot_description/config/# 文件内容见下个代码块，请勿直接复制粘贴
   touch fake_mbot_arbotix.yaml# 3.启动仿真器
   roslaunch mbot_description arbotix_mbot_with_camera_xacro.launch# 4.键盘控制相关
   # 4.1 新建mbot_teleop功能包
   catkin_create_pkg willard_teleop roscpp std_msgs rospy# 4.2 在willard_teleop下创建launch、scripts文件夹
   mkdir launch scripts# 4.3 在/scripts文件夹下创建.py文件
   # 文件内容见下个代码块，请勿直接复制粘贴
   touch willard_teleop.py# 4.4 给.py文件添加可执行权限
   chmod 777 willard_teleop.py# 4.5 新建.launch文件
   # 文件内容见下个代码块，请勿直接复制粘贴
   touch willard_teleop.launch # 5.启动键盘控制
   roslaunch willard_teleop willard_teleop.launch

   # arbotix_mbot_with_camera_xacro.launch文件内容
   <launch><arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot_with_camera.xacro'" /><arg name="gui" default="false" /><param name="robot_description" command="$(arg model)" /><!-- 设置GUI参数，显示关节控制插件 --><param name="use_gui" value="$(arg gui)"/><node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen"><rosparam file="$(find mbot_description)/config/fake_mbot_arbotix.yaml" command="load" /><param name="sim" value="true"/></node><!-- 运行joint_state_publisher节点，发布机器人的关节状态  --><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /><!-- 运行robot_state_publisher节点，发布tf  --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /><!-- 运行rviz可视化界面 --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_arbotix.rviz" required="true" /></launch>

   # fake_mbot_arbotix.yaml文件内容
   controllers: {base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
   }

   # willard_teleop.py文件内容
   #!/usr/bin/env python
   # -*- coding: utf-8 -*-import rospy
   from geometry_msgs.msg import Twist
   import sys, select, termios, ttymsg = """
   Control mbot!
   ---------------------------
   Moving around:u    i    oj    k    lm    ,    .q/z : increase/decrease max speeds by 10%
   w/x : increase/decrease only linear speed by 10%
   e/c : increase/decrease only angular speed by 10%
   space key, k : force stop
   anything else : stop smoothlyCTRL-C to quit
   """moveBindings = {'i':(1,0),'o':(1,-1),'j':(0,1),'l':(0,-1),'u':(1,1),',':(-1,0),'.':(-1,1),'m':(-1,-1),}speedBindings={'q':(1.1,1.1),'z':(.9,.9),'w':(1.1,1),'x':(.9,1),'e':(1,1.1),'c':(1,.9),}def getKey():tty.setraw(sys.stdin.fileno())rlist, _, _ = select.select([sys.stdin], [], [], 0.1)if rlist:key = sys.stdin.read(1)else:key = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keyspeed = .2
   turn = 1def vels(speed,turn):return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)rospy.init_node('mbot_teleop')pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)x = 0th = 0status = 0count = 0acc = 0.1target_speed = 0target_turn = 0control_speed = 0control_turn = 0try:print msgprint vels(speed,turn)while(1):key = getKey()# 运动控制方向键（1：正方向，-1负方向）if key in moveBindings.keys():x = moveBindings[key][0]th = moveBindings[key][1]count = 0# 速度修改键elif key in speedBindings.keys():speed = speed * speedBindings[key][0]  # 线速度增加0.1倍turn = turn * speedBindings[key][1]    # 角速度增加0.1倍count = 0print vels(speed,turn)if (status == 14):print msgstatus = (status + 1) % 15# 停止键elif key == ' ' or key == 'k' :x = 0th = 0control_speed = 0control_turn = 0else:count = count + 1if count > 4:x = 0th = 0if (key == '\x03'):break# 目标速度=速度值*方向值target_speed = speed * xtarget_turn = turn * th# 速度限位，防止速度增减过快if target_speed > control_speed:control_speed = min( target_speed, control_speed + 0.02 )elif target_speed < control_speed:control_speed = max( target_speed, control_speed - 0.02 )else:control_speed = target_speedif target_turn > control_turn:control_turn = min( target_turn, control_turn + 0.1 )elif target_turn < control_turn:control_turn = max( target_turn, control_turn - 0.1 )else:control_turn = target_turn# 创建并发布twist消息twist = Twist()twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turnpub.publish(twist)except:print efinally:twist = Twist()twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0pub.publish(twist)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

   # willard_teleop.launch
   <launch><node name="willard_teleop" pkg="willard_teleop" type="willard_teleop.py" output="screen"><param name="scale_linear" value="0.1" type="double"/><param name="scale_angular" value="0.4" type="double"/></node>
   </launch>

4. 仿真效果图

   

4.Gazebo物理仿真环境搭建

4.1 ros_control

1. $ros\_control$简介

   • $\mathrm{ROS}$为开发者提供的机器人控制中间件；
   • 包含一系列控制器接口、传动装置接口、硬件接口、控制器工具等；
   • 可以帮助机器人应用功能包快速落地，提高开发效率；

2. $ros\_control$框架

   

   

   • 控制器管理器：提供一种通用的接口来管理不同的控制器；
   • 控制器：读取硬件状态，发布控制命令，完成每个$\mathrm{joint}$的控制；
   • 硬件资源：为上下两层提供硬件资源的接口；
   • 机器人硬件抽象：机器人硬件抽象和硬件资源直接打交道，通过$\mathrm{write}$和$\mathrm{read}$方法完成硬件操作；
   • 真实机器人：执行接收到的命令；

   

3. 控制器$(\mathrm{controllers})$介绍

   

4.2 仿真步骤

1. 配置机器人模型；
2. 创建仿真环境；
3. 开始仿真；

4.2.1 配置物理仿真模型

• $STEP1$：为$\mathrm{link}$添加惯性参数和碰撞属性；
• $STEP2$：为$\mathrm{link}$添加$\mathrm{gazebo}$标签；
• $STEP3$：为$\mathrm{joint}$添加传动装置；
• $STEP4$：添加$\mathrm{gazebo}$控制器插件；
  • <$\mathrm{robotNamespace}$>：机器人的命名空间；
  • <$\mathrm{leftJoint}$>和<$\mathrm{rightJoint}$>：左右轮转动的关节$\mathrm{joint}$；
  • <$\mathrm{wheelSeparation}$>和<$\mathrm{wheelDiameter}$>：机器人模型的相关尺寸，在计算差速参数时需要使用；
  • <$\mathrm{commandTopic}$>：控制器订阅的速度控制指令，生成全局命名时需要结合<$\mathrm{robotNamespace}$>中设置的命名空间；
  • <$\mathrm{odometryFrame}$>：里程计数据的参考坐标系，$\mathrm{ROS}$中一般命名为$\mathrm{odom}$；

物理仿真模型配置实例：

# mbot_base_gazebo.xacro文件内容<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro"><!-- PROPERTY LIST --><xacro:property name="M_PI" value="3.1415926"/><xacro:property name="base_mass"   value="20" /> <xacro:property name="base_radius" value="0.20"/><xacro:property name="base_length" value="0.16"/><xacro:property name="wheel_mass"   value="2" /><xacro:property name="wheel_radius" value="0.06"/><xacro:property name="wheel_length" value="0.025"/><xacro:property name="wheel_joint_y" value="0.19"/><xacro:property name="wheel_joint_z" value="0.05"/><xacro:property name="caster_mass"    value="0.5" /> <xacro:property name="caster_radius"  value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) --><xacro:property name="caster_joint_x" value="0.18"/><!-- Defining the colors used in this robot --><material name="yellow"><color rgba="1 0.4 0 1"/></material><material name="black"><color rgba="0 0 0 0.95"/></material><material name="gray"><color rgba="0.75 0.75 0.75 1"/></material><!-- Macro for inertia matrix --><xacro:macro name="sphere_inertial_matrix" params="m r"><inertial><mass value="${m}" /><inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"iyy="${2*m*r*r/5}" iyz="0" izz="${2*m*r*r/5}" /></inertial></xacro:macro><xacro:macro name="cylinder_inertial_matrix" params="m r h"><inertial><mass value="${m}" /><inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"iyy="${m*(3*r*r+h*h)/12}" iyz = "0"izz="${m*r*r/2}" /> </inertial></xacro:macro><!-- Macro for robot wheel --><xacro:macro name="wheel" params="prefix reflect"><joint name="${prefix}_wheel_joint" type="continuous"><origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/><parent link="base_link"/><child link="${prefix}_wheel_link"/><axis xyz="0 1 0"/></joint><link name="${prefix}_wheel_link"><visual><origin xyz="0 0 0" rpy="${M_PI/2} 0 0" /><geometry><cylinder radius="${wheel_radius}" length = "${wheel_length}"/></geometry><material name="gray" /></visual><collision><origin xyz="0 0 0" rpy="${M_PI/2} 0 0" /><geometry><cylinder radius="${wheel_radius}" length = "${wheel_length}"/></geometry></collision><cylinder_inertial_matrix  m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" /></link><gazebo reference="${prefix}_wheel_link"><material>Gazebo/Gray</material></gazebo><!-- Transmission is important to link the joints and the controller --><transmission name="${prefix}_wheel_joint_trans"><type>transmission_interface/SimpleTransmission</type><joint name="${prefix}_wheel_joint" ><hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface></joint><actuator name="${prefix}_wheel_joint_motor"><hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission></xacro:macro><!-- Macro for robot caster --><xacro:macro name="caster" params="prefix reflect"><joint name="${prefix}_caster_joint" type="continuous"><origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/><parent link="base_link"/><child link="${prefix}_caster_link"/><axis xyz="0 1 0"/></joint><link name="${prefix}_caster_link"><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><sphere radius="${caster_radius}" /></geometry><material name="black" /></visual><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><sphere radius="${caster_radius}" /></geometry></collision>      <sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" /></link><gazebo reference="${prefix}_caster_link"><material>Gazebo/Black</material></gazebo></xacro:macro><xacro:macro name="mbot_base_gazebo"><link name="base_footprint"><visual><origin xyz="0 0 0" rpy="0 0 0" /><geometry><box size="0.001 0.001 0.001" /></geometry></visual></link><gazebo reference="base_footprint"><turnGravityOff>false</turnGravityOff></gazebo><joint name="base_footprint_joint" type="fixed"><origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        <parent link="base_footprint"/><child link="base_link" /></joint><link name="base_link"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="${base_length}" radius="${base_radius}"/></geometry><material name="yellow" /></visual><collision><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="${base_length}" radius="${base_radius}"/></geometry></collision>   <cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" /></link><gazebo reference="base_link"><material>Gazebo/Blue</material></gazebo><wheel prefix="left"  reflect="-1"/><wheel prefix="right" reflect="1"/><caster prefix="front" reflect="-1"/><caster prefix="back"  reflect="1"/><!-- controller --><gazebo><plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so"><rosDebugLevel>Debug</rosDebugLevel><publishWheelTF>true</publishWheelTF><robotNamespace>/</robotNamespace><publishTf>1</publishTf><publishWheelJointState>true</publishWheelJointState><alwaysOn>true</alwaysOn><updateRate>100.0</updateRate><legacyMode>true</legacyMode><leftJoint>left_wheel_joint</leftJoint><rightJoint>right_wheel_joint</rightJoint><wheelSeparation>${wheel_joint_y*2}</wheelSeparation><wheelDiameter>${2*wheel_radius}</wheelDiameter><broadcastTF>1</broadcastTF><wheelTorque>30</wheelTorque><wheelAcceleration>1.8</wheelAcceleration><commandTopic>cmd_vel</commandTopic><odometryFrame>odom</odometryFrame> <odometryTopic>odom</odometryTopic> <robotBaseFrame>base_footprint</robotBaseFrame></plugin></gazebo> </xacro:macro></robot>

4.2.2 创建仿真环境

# view_mbot_gazebo_empty_world.launch文件内容<launch><!-- 设置launch文件的参数 --><arg name="paused" default="false"/><arg name="use_sim_time" default="true"/><arg name="gui" default="true"/><arg name="headless" default="false"/><arg name="debug" default="false"/><!-- 运行gazebo仿真环境 --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="debug" value="$(arg debug)" /><arg name="gui" value="$(arg gui)" /><arg name="paused" value="$(arg paused)"/><arg name="use_sim_time" value="$(arg use_sim_time)"/><arg name="headless" value="$(arg headless)"/></include><!-- 加载机器人模型描述参数 --><param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_gazebo.xacro'" /> <!-- 运行joint_state_publisher节点，发布机器人的关节状态  --><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> <!-- 运行robot_state_publisher节点，发布tf  --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" ><param name="publish_frequency" type="double" value="50.0" /></node><!-- 在gazebo中加载机器人模型--><node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"args="-urdf -model mrobot -param robot_description"/> </launch>

# 1.启动空环境下的.launch文件
roslaunch mbot_gazebo view_mbot_gazebo_empty_world.launch # 2.启动键盘控制节点
roslaunch mbot_teleop mbot_teleop.launch# 3.添加环境模型
# 3.1 直接添加，把模型保存到~/.gazebo/models/下
cd ~/.gazebo/models/
git clone https://github.com/osrf/gazebo_models.git# 添加好模型后，保存为.world文件# 3.2 使用Buiding Editor，在gazebo的edit下
# 4.把仿真模型路径写入.launch文件，即可加载

效果如下图：

4.3 传感器仿真

4.3.1 摄像头仿真

• <$\mathrm{sensor}$>标签：描述传感器；

  • $\mathrm{type}$：传感器类型，$\mathrm{camera}$；
  • $\mathrm{name}$：摄像头命名，自由设置；

• <$\mathrm{camera}$>标签：描述摄像头参数；

  • 分辨率、编码格式、图像范围、噪音参数等；

• <$\mathrm{plugin}$>标签：加载摄像头仿真插件$libgazebo\_ros\_camera.so$；

  • 设置插件的命名空间、发布图像的话题、参考坐标系等；

• 摄像头仿真的$.xacro$文件实例

  # camera_gazebo.xacro文件内容<?xml version="1.0"?>
  <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera"><xacro:macro name="usb_camera" params="prefix:=camera"><!-- Create laser reference frame --><link name="${prefix}_link"><inertial><mass value="0.1" /><origin xyz="0 0 0" /><inertia ixx="0.01" ixy="0.0" ixz="0.0"iyy="0.01" iyz="0.0"izz="0.01" /></inertial><visual><origin xyz=" 0 0 0 " rpy="0 0 0" /><geometry><box size="0.01 0.04 0.04" /></geometry><material name="black"/></visual><collision><origin xyz="0.0 0.0 0.0" rpy="0 0 0" /><geometry><box size="0.01 0.04 0.04" /></geometry></collision></link><gazebo reference="${prefix}_link"><material>Gazebo/Black</material></gazebo><gazebo reference="${prefix}_link"><sensor type="camera" name="camera_node"><update_rate>30.0</update_rate><camera name="head"><horizontal_fov>1.3962634</horizontal_fov><image><width>1280</width><height>720</height><format>R8G8B8</format></image><clip><near>0.02</near><far>300</far></clip><noise><type>gaussian</type><mean>0.0</mean><stddev>0.007</stddev></noise></camera><plugin name="gazebo_camera" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><updateRate>0.0</updateRate><cameraName>/camera</cameraName><imageTopicName>image_raw</imageTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>camera_link</frameName><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></xacro:macro>
  </robot>

# 1.在~/willard_ws/src/mbot_description/urdf/xacro/sensors/下新建camera_gazebo.xacro文件，内容见上一代码块
touch camera_gazebo.xacro# 2.启动仿真环境
# view_mbot_with_camera_gazebo.launch内容见下一代码块
roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch# 3.查看图像
rqt_image_view# 4.启动键盘控制机器人移动
roslaunch mbot_teleop mbot_teleop.launch

# view_mbot_with_camera_gazebo.launch文件内容<launch><!-- 设置launch文件的参数 --><arg name="world_name" value="$(find mbot_gazebo)/worlds/playground.world"/><arg name="paused" default="false"/><arg name="use_sim_time" default="true"/><arg name="gui" default="true"/><arg name="headless" default="false"/><arg name="debug" default="false"/><!-- 运行gazebo仿真环境 --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(arg world_name)" /><arg name="debug" value="$(arg debug)" /><arg name="gui" value="$(arg gui)" /><arg name="paused" value="$(arg paused)"/><arg name="use_sim_time" value="$(arg use_sim_time)"/><arg name="headless" value="$(arg headless)"/></include><!-- 加载机器人模型描述参数 --><param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_with_camera_gazebo.xacro'" /> <!-- 运行joint_state_publisher节点，发布机器人的关节状态  --><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> <!-- 运行robot_state_publisher节点，发布tf  --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" ><param name="publish_frequency" type="double" value="50.0" /></node><!-- 在gazebo中加载机器人模型--><node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"args="-urdf -model mrobot -param robot_description"/> </launch>

4.3.2 RGB-D摄像头(kinect)仿真

# 1.在~/willard_ws/src/mbot_description/urdf/xacro/sensors/下新建kinect_gazebo.xacro文件，内容见下一代码块
touch kinect_gazebo.xacro# 2.启动仿真环境
# view_mbot_with_kinect_gazebo.launch内容见下一代码块
roslaunch mbot_gazebo view_mbot_with_kinect_gazebo.launch# 3.查看图像
rosrun rviz rviz# 4.启动键盘控制机器人移动
roslaunch mbot_teleop mbot_teleop.launch

# kinect_gazebo.xacro文件内容<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera"><xacro:macro name="kinect_camera" params="prefix:=camera"><!-- Create kinect reference frame --><!-- Add mesh for kinect --><link name="${prefix}_link"><origin xyz="0 0 0" rpy="0 0 0"/><visual><origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/><geometry><mesh filename="package://mbot_description/meshes/kinect.dae" /></geometry></visual><collision><geometry><box size="0.07 0.3 0.09"/></geometry></collision></link><joint name="${prefix}_optical_joint" type="fixed"><origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/><parent link="${prefix}_link"/><child link="${prefix}_frame_optical"/></joint><link name="${prefix}_frame_optical"/><gazebo reference="${prefix}_link"><sensor type="depth" name="${prefix}"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov><image><format>R8G8B8</format><width>640</width><height>480</height></image><clip><near>0.05</near><far>8.0</far></clip></camera><plugin name="kinect_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so"><cameraName>${prefix}</cameraName><alwaysOn>true</alwaysOn><updateRate>10</updateRate><imageTopicName>rgb/image_raw</imageTopicName><depthImageTopicName>depth/image_raw</depthImageTopicName><pointCloudTopicName>depth/points</pointCloudTopicName><cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName><depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName><frameName>${prefix}_frame_optical</frameName><baseline>0.1</baseline><distortion_k1>0.0</distortion_k1><distortion_k2>0.0</distortion_k2><distortion_k3>0.0</distortion_k3><distortion_t1>0.0</distortion_t1><distortion_t2>0.0</distortion_t2><pointCloudCutoff>0.4</pointCloudCutoff></plugin></sensor></gazebo></xacro:macro>
</robot>

# view_mbot_with_kinect_gazebo.launch文件内容<launch><!-- 设置launch文件的参数 --><arg name="world_name" value="$(find mbot_gazebo)/worlds/playground.world"/><arg name="paused" default="false"/><arg name="use_sim_time" default="true"/><arg name="gui" default="true"/><arg name="headless" default="false"/><arg name="debug" default="false"/><!-- 运行gazebo仿真环境 --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(arg world_name)" /><arg name="debug" value="$(arg debug)" /><arg name="gui" value="$(arg gui)" /><arg name="paused" value="$(arg paused)"/><arg name="use_sim_time" value="$(arg use_sim_time)"/><arg name="headless" value="$(arg headless)"/></include><!-- 加载机器人模型描述参数 --><param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_with_kinect_gazebo.xacro'" /> <!-- 运行joint_state_publisher节点，发布机器人的关节状态  --><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> <!-- 运行robot_state_publisher节点，发布tf  --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" ><param name="publish_frequency" type="double" value="50.0" /></node><!-- 在gazebo中加载机器人模型--><node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"args="-urdf -model mrobot -param robot_description"/> </launch>

仿真效果图：

4.3.3 激光雷达仿真

# 1.在~/willard_ws/src/mbot_description/urdf/xacro/sensors/下新建lidar_gazebo.xacro文件，内容见下一代码块
touch lidar_gazebo.xacro# 2.启动仿真环境
# view_mbot_with_laser_gazebo.launch内容见下一代码块
roslaunch mbot_gazebo view_mbot_with_laser_gazebo.launch# 3.查看图像
rosrun rviz rviz# 4.启动键盘控制机器人移动
roslaunch mbot_teleop mbot_teleop.launch

# lidar_gazebo.xacro文件内容<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser"><xacro:macro name="rplidar" params="prefix:=laser"><!-- Create laser reference frame --><link name="${prefix}_link"><inertial><mass value="0.1" /><origin xyz="0 0 0" /><inertia ixx="0.01" ixy="0.0" ixz="0.0"iyy="0.01" iyz="0.0"izz="0.01" /></inertial><visual><origin xyz=" 0 0 0 " rpy="0 0 0" /><geometry><cylinder length="0.05" radius="0.05"/></geometry><material name="black"/></visual><collision><origin xyz="0.0 0.0 0.0" rpy="0 0 0" /><geometry><cylinder length="0.06" radius="0.05"/></geometry></collision></link><gazebo reference="${prefix}_link"><material>Gazebo/Black</material></gazebo><gazebo reference="${prefix}_link"><sensor type="ray" name="rplidar"><pose>0 0 0 0 0 0</pose><visualize>false</visualize><update_rate>5.5</update_rate><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3</min_angle><max_angle>3</max_angle></horizontal></scan><range><min>0.10</min><max>6.0</max><resolution>0.01</resolution></range><noise><type>gaussian</type><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so"><topicName>/scan</topicName><frameName>laser_link</frameName></plugin></sensor></gazebo></xacro:macro>
</robot>

仿真效果图：