本章我们开始机器人仿真
先将程序进行验证再跑实物，或者设计复杂环境测试，主要有三大部分：机器人建模（URDF），创建仿真环境（Gazebo）,以及环境感知（Rviz）
URDF：统一机器人描述格式，可以以一种XML的方式描述机器人的结构，比如底盘，摄像头，激光雷达，以及不同关节的自由度
RVIZ：三维可视化工具，之前显示坐标时已经使用过
Gazebo：显示机器人模型并创建仿真环境，提供传感器模型
第一步是URDF与RVIZ的集成：
先新建一个功能包，导入依赖urdf，xacro
文件的基本组织架构如上
编写launch文件，在参数服务器载入urdf文件，之后启动rviz
<launch>
<param name="robot_description" textfile="$(find jiqiren)/urdf/urdf/demo01_hello.urdf"/>
<node pkg="rviz" type="rviz" name="rviz" />
</launch>
param的name是固定的，注意的是textfile这个参数以前没有使用过，是要加载模型的路径
这样我们就得到了仿真模型，但还要自己手动ADD下RobotModel以及设置Fixed Frame为base_link，我们设置之后把他save到之前建的config目录之下
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find jiqiren)/config/showcar.rviz"/>
学习下URDF语法，ROS对机器人的定义比较简单，简化为两部分，连杆（link标签）与关节（joint标签）
URDF中跟标签是<robot>，后面可以跟neme=命名
Link标签用于描述机器人某个部件的外观和物理属性，比如：机器人底座，轮子，激光雷达，摄像头，每一个部件对应一个link，在link标签内可以设置该部件的形状，尺寸，大小等一系列参数
碰撞参数collision joint惯性
Link标签属性同样只有name
子标签1，visual，描述外观（可视的）{1.geometry：设置连杆形状，2.origin：设置偏移量，3.metrial：设置材料属性}，子标签2，连杆的碰撞属性collision，子标签3，inertial，连杆的惯性矩阵
<link name="base_link">
<!-- 可视化标签 -->
<visual>
<!--子标签分别是形状，偏移量与倾斜弧度，颜色-->
<geometry>
<box size="0.3 0.2 0.1"/>
<!-- <cylinder radius="0.6" length="2.0"/> -->
<!-- <sphere radius="1"/> -->
<!-- <mesh filename="package://jiqiren/meshes/autolabor_mini.stl"/> -->
</geometry>]
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<material name="color">
<!-- color取值0-1之间 -->
<color rgba="0 0 1 1"/>
</material>
</visual>
</link>
这就是link标签中的可视化部分
接下里看joint标签
urdf中的 joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限，机器人的两个部件(分别称之为 parent link 与 child link)以"关节"的形式相连接，不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制....,比如:安装在底座上的轮子可以360度旋转，而摄像头则可能是完全固定在底座上。
joint标签对应的数据在模型中是不可见的
Joint origin用来说明关节在parent连杆的什么位置，joint axis是关节运动方式
属性比较多
Fixed与continuous比较常用
子标签
之后的案例要在底盘上安装摄像头，先创建底盘和摄像头的节点，在创建他们的连接，且有了连接件之后launch文件也要进行调整
注意计算偏移量时，原点是底盘（父级）的中心
<joint name="catobs" type="continuous">
<parent link="base_link"/>
<child link="camera"/>
<origin xyz="0.12 0 0.05" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<robot name="linker">
<link name="base_link">
<!-- 可视化标签 -->
<visual>
<!--子标签分别是形状，偏移量与倾斜弧度，颜色-->
<geometry>
<box size="0.3 0.2 0.1"/>
<!-- <cylinder radius="0.6" length="2.0"/> -->
<!-- <sphere radius="1"/> -->
<!-- <mesh filename="package://jiqiren/meshes/autolabor_mini.stl"/> -->
</geometry>]
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="ba_color">
<!-- color取值0-1之间 -->
<color rgba="0 0 1 1"/>
</material>
</visual>
</link>
<link name="camera">
<!-- 可视化标签 -->
<visual>
<!--子标签分别是形状，偏移量与倾斜弧度，颜色-->
<geometry>
<box size="0.05 0.04 0.05"/>
<!-- <cylinder radius="0.6" length="2.0"/> -->
<!-- <sphere radius="1"/> -->
<!-- <mesh filename="package://jiqiren/meshes/autolabor_mini.stl"/> -->
</geometry>]
<origin xyz="0 0 0.025" rpy="0 0 0"/>
<material name="ca_color">
<!-- color取值0-1之间 -->
<color rgba="0 1 0 0.5"/>
</material>
</visual>
</link>
<joint name="catobs" type="continuous">
<parent link="base_link"/>
<child link="camera"/>
<origin xyz="0.12 0 0.05" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
</robot>
<launch>
<param name="robot_description" textfile="$(find jiqiren)/urdf/urdf/link.urdf"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find jiqiren)/config/showcar.rviz"/>
<!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/> -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui"/>
</launch>
这两组publisher要记得写上
之后看一个优化，默认情况下，原点是机器人的中心，而之前的建模使得机器人是半沉在地下的状况，一种优化思路是将初始的link设置为0.001m的球体，然后再添加底盘上去，这个初始link一般称为base_footprint
参考坐标系也要改成base_footprint
<joint name="footer" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
</joint>
模型创建练习
这个是完成的模型，过程比较简单，接下来看一些小工具
这个用来检验URDF格式是否正确
还有一个命令用于生成PDF格式的关系图，方便分析
URDF优化：xacro
情况：1.太多参数需要手算，太容易错，且参数一旦改变还要重新计算 2.内容高度重复，应该考虑封装，所以后面的实现都是用xacro，他是一种XML语言
案例1：封装驱动轮
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 属性封装 -->
<xacro:property name="wheel_radius" value="0.0325" />
<xacro:property name="wheel_length" value="0.0015" />
<xacro:property name="PI" value="3.1415927" />
<xacro:property name="base_link_length" value="0.08" />
<xacro:property name="lidi_space" value="0.015" />
<!-- 宏 -->
<xacro:macro name="wheel_func" params="wheel_name flag" >
<link name="${wheel_name}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0 0 0" rpy="${PI / 2} 0 0" />
<material name="wheel_color">
<color rgba="0 0 0 0.3" />
</material>
</visual>
</link>
<!-- 3-2.joint -->
<joint name="${wheel_name}2link" type="continuous">
<parent link="base_link" />
<child link="${wheel_name}_wheel" />
<!--
x 无偏移
y 车体半径
z z= 车体高度 / 2 + 离地间距 - 车轮半径
-->
<origin xyz="0 ${0.1 * flag} ${(base_link_length / 2 + lidi_space - wheel_radius) * -1}" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
<xacro:wheel_func wheel_name="left" flag="1" />
<xacro:wheel_func wheel_name="right" flag="-1" />
</robot>
要转换成urdf文件
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
xmlns的声明是必须的
之后先学习参数的封装，比如小车半径和圆周率这种
注意$和大括号{}搭配
接下来看一下宏，类似于函数
xacro:macro注意params=”num1 num2”这样的写法
文件包含：将不同的机器人组件封装成整体
<xacro:include filename="demo03.xacro"/>
之后开始实操，完成之前的那个案例
编写完一小部分后转为urdf文件跑一下check_urdf可以验证对不对
<robot name="carbase" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:property name="basefootprint_r" value="0.001"/>
<xacro:property name="base_link_r" value="0.1"/>
<xacro:property name="base_link_h" value="0.08"/>
<xacro:property name="distoground" value="0.015"/>
<xacro:property name="basepianyi" value="${base_link_h/2+distoground}"/>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="${basefootprint_r}"/>
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${base_link_r}" length="${base_link_h}"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="ba_color">
<color rgba="0 0 1 1"/>
</material>
</visual>
</link>
<joint name="footer" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 ${basepianyi}" rpy="0 0 0"/>
</joint>
</robot>
底盘因为只有一个，不要考虑复用，所以优化只是针对参数定义方面
<param name="robot_description" command="$(find xacro)/xacro $(find jiqiren)/urdf/xacro/carbase.xacro"/>
Launch文件中集成的一种方法，将原来的textfile变成command
<xacro:property name="qvdong_r" value="0.0325"/>
<xacro:property name="qvdong_l" value="0.015"/>
<xacro:property name="qvdong_jiao" value="1.5708"/>
<xacro:property name="qvdongjointz" value="${-base_link_h/2-distoground+qvdong_r}"/>
<xacro:macro name="qvdong" params="name1 flag">
<link name="${name1}_wheel">
<visual>
<geometry>
<cylinder radius="${qvdong_r}" length="${qvdong_l}"/>
</geometry>
<origin xyz="0 0 0" rpy="${qvdong_jiao} 0 0"/>
<material name="ba_color1">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="${name1}2base" type="continuous">
<parent link="base_link"/>
<child link="${name1}_wheel"/>
<origin xyz="0 ${0.1075*flag} ${qvdongjointz}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</xacro:macro>
<xacro:qvdong name1="left" flag="1"/>
<xacro:qvdong name1="right" flag="-1"/>
复用函数来定于俩轮子
至此完成了carbase.xacro的编写，对于制作机器人，一般将不同的组件单独写文件，最后完成集成
<robot name="minefinalcar" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="carbase.xacro"/>
<xacro:include filename="carcamer.xacro"/>
<xacro:include filename="carlaser.xacro"/>
</robot>
完成效果
目前创建的模型可以在rviz显示，但他是静态的，可以调用Arbotix来看动态的效果
上面是他的yaml文件配置框架
之后在launch文件中加载配置的内容
<node pkg="arbotix_python" type="arbotix_driver" name="controler" output="screen">
<rosparam command="load" file="$(find jiqiren)/config/control.yaml"/>
<param name="sim" value="true"/>
</node>
之后直接运行，修改一些配置
首先是坐标系选择非我们自己设置的odom，是机器人根据速度逆向推到出的原点
之后还是发送/cmd_vel控制运动
上述是与Rviz的集成，后面学习的是与Gazebo的集成
Collision是设置碰撞矩阵，如果形状正常直接用原数据就可以
<collision>
<geometry>
<box size="0.5 0.3 0.1"/>
</geometry>
</collision>
Inertial是设置惯性矩阵，如果均质偏移为0，mass用来设置质量，单位Kg
<inertial>
<origin xyz="0 0 0"/>
<mass value="2"/>
<inertial ixx="1" ixy="0" ixz="0" iyy="0" iyz="1" izz="1"/>
</inertial>
颜色设置上，以前RGBA的这种不再生效，Gazebo有他自己的设置方法
<gazebo reference="base_link">
<material>Gazebo/Red</material>
</gazebo>
注意大小写
写完之后就可以配置launch文件了，在参数服务器中载入urdf，启动gazebo仿真环境，在gazebo中添加机器人模型
<launch>
<param name="robot_description" textfile="$(find gezeboo)/urdf/urdf/demo01.urdf"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch"/>
<node pkg="gazebo_ros" type="spawn_model" name="spawmm" args="-urdf -model mycar -param robot_description"/>
</launch>
在编写urdf时与在rviz下主要有这三个区别
如果是标准的几何体形状，与link的visual的属性一致即可，inertial惯性矩阵的计算已经封装
<!-- 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>
比如这个是圆球体计算的宏，传入的参数是质量与外形，因为除了base_footprint外，所有刚体都要计算惯性矩阵，不然模型会晃或者闪现
总结下，和之前rviz集成唯一的变化就是给每个link需要设置碰撞矩阵与惯性矩阵
将原来的empty_world替换成自己创建的环境，先创建World文件夹，导入预先下载的文件
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find gezeboo)/worlds/box_house.world"/>
</include>
通过arg加载世界
环境创建
第一种：
之后自己创建，再save world as保存后缀名.world，如果要建模精细点需要插件
第二种：
分两步，第一步是要划分大体的格局，如上面的围墙，第二步再加模型进去
项目：三个工具的综合调用，URDF用于创建机器人模型，Rviz用于显示机器人感受到的环境信息，Gazebo用于模拟外界环境仿真以及传感器仿真，通过Gazebo模拟机器人的传感器（雷达，摄像头等），并在rviz中显示
首先，学习如何在gazebo中控制机器人运动，使用ros_control组件，相当于type-c
运动控制实现流程：
- 编写一个单独的xacro文件，为机器人添加传动装置以及控制器
- 将此文件集成进机器人主文件
- /cmd_vel控制
传动及控制器的代码修改官网历程，将对应连接件和具体参数进行修改即可
<xacro:include filename="gazebo/move.xacro"/>
<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 传动实现:用于连接控制器与关节 -->
<xacro:macro name="joint_trans" params="joint_name">
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${joint_name}_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${joint_name}_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!-- 每一个驱动轮都需要配置传动装置 -->
<xacro:joint_trans joint_name="left2base" />
<xacro:joint_trans joint_name="right2base" />
<!-- 控制器 -->
<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>left2base</leftJoint> <!-- 左轮 -->
<rightJoint>right2base</rightJoint> <!-- 右轮 -->
<wheelSeparation>0.216 </wheelSeparation> <!-- 车轮间距 -->
<wheelDiameter>${qvdong_r * 2}</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>
</robot>
至此，可以在Gazebo中控制机器人运动了，运动时，机器人还会发送里程消息，需要通过RVIZ来查看可视化的消息
<launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find jiqiren)/config/showcar.rviz"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher_gui"/>
</launch>
只需要在上一步的基础上打开RVIZ
odom是前面控制文件里面设置的，odom原点就是小车的起点
后面传感器的集成都一样的流程了，先编写xacro文件进行配置
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷达 -->
<gazebo reference="laser">
<sensor type="ray" name="rplidar">
<pose>0 0 0 0 0 0</pose>
<visualize>true</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>30.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</frameName>
</plugin>
</sensor>
</gazebo>
</robot>
比较重要的几个点是topicname，framename以及reference
Type是雷达类型ray就是激光雷达，雷达可视化如果为True显示蓝色射线，samples表示旋转一周发射的采样激光数量，resolution意思是隔几个会发射一个测距的，-3到+3的覆盖角度，会有盲区，range是距离上的采样范围，0.1米到30米，resolution是采样的精度，
此时会出来轮廓
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 被引用的link -->
<gazebo reference="camer">
<!-- 类型设置为 camara -->
<sensor type="camer" 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>/camer</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camer</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>
</robot>
摄像头配置也是直接复用，改节点名字即可
左下角就是摄像头的图像
最后是kinect深度相机的仿真，不光有原来的图像，还有与障碍物的距离信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<gazebo reference="camera">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${60.0*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_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</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>camera</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>
</robot>
就可以查看深度或RGB两种图像
点云数据显示，包含深度信息，添加组件PointCloud2
但是发生了错位，因为点云坐标系要自己创建，并将坐标系的信息发布
<node pkg="tf2_ros" type="static_transform_publisher" name="dudu" args="0 0 0 -1.57 0 -1.57 /camera /depthh"/>
depthh为深度相机的frame _id，与原来的相机坐标系位移上无偏转，但X和Z偏转90度
总结下本章学习的这些工具之间的关系，URDF是用于描述机器人的XML文件，但编写机器人代码冗余，xacro可以优化URDF的实现，使代码更为精简，高效，易读，RVIZ是三维可视化工具，强调把已有的数据可视化显示，所以他需要有已有的数据，gezebo是三维物理仿真平台，强调的是创建一个虚拟的仿真环境，他用来创造数据
如果有硬件平台，直接跑RVIZ就可以
扫一扫
688
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
