其他文章
ROS自学笔记(一)
ROS自学笔记(三)
提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档
文章目录
四、ROS中的仿真
1. 简单的二维机器人仿真
先安装STDR功能包。(安装教程)
启动STDR
roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch
运行结果
小圆圈代表机器人
红色线表示激光雷达传感器的仿真激光射线
绿色代表仿真声呐信号
控制机器人运动的主题：/robot0/cmd_vel。通过指令rostopic info /robot0/cmd_vel检查主题，可以看出，此主题有两个订阅方，没有发布器，消息体类型是geometry_msgs/Twist。
通过指令rosmsg show geometry_msgs/Twist可以查看消息体的具体内容，如下：
//速度向量 线速度
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
//角度向量 角速度
geometry_msgs/Vector3 angular
float64 x //翻滚
float64 y //俯仰
float64 z //偏航
对于二维机器人，只有线速度的x与角速度的y有用。机器人只能向前运动和绕中心旋转。
下面指令让机器人向前运动。向前以0.5m/s运动。
rostopic pub -r 1 /robot0/cmd_vel geometry_msgs/Twist "linear:
x: 0.5
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
经传感器测量，移动机器人应该以消息类型nav_msgs/Odommetry向/robot0/odom主题发布仿真机器人的速度和角速度信息。
2. URDF rviz Gazebo 简介
本节将以URDF描述机器人建模。
相关组件
-
URDF– 是 Unified Robot Description Format 的首字母缩写，直译为统一(标准化)机器人描述格式，可以以一种 XML 的方式描述机器人的部分结构，比如底盘、摄像头、激光雷达、机械臂以及不同关节的自由度…,该文件可以被 C++ 内置的解释器转换成可视化的机器人模型，是 ROS 中实现机器人仿真的重要组件 -
rviz– 是 ROS Visualization Tool 的首字母缩写，直译为ROS的三维可视化工具。它的主要目的是以三维方式显示ROS消息，可以将 数据进行可视化表达。例如:可以显示机器人模型，可以无需编程就能表达激光测距仪（LRF）传感器中的传感 器到障碍物的距离，RealSense、Kinect或Xtion等三维距离传感器的点云数据（PCD， Point Cloud Data），从相机获取的图像值等 -
Gazebo– 是一款3D动态模拟器，用于显示机器人模型并创建仿真环境,能够在复杂的室内和室外环境中准确有效地模拟机器人。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。
3. URDF集成Rviz基本流程
创建一个新的功能包，名称自定义，导入依赖包 : urdf与xacro
在当前功能包下，再新建几个目录:
urdf: 存储 urdf 文件的目录
meshes:机器人模型渲染文件(暂不使用)
config: 配置文件
launch: 存储 launch 启动文件
(1) 编写urdf文件
在src/urdf中，再新建一个子级文件夹:urdf(可选)，文件夹中添加一个hao_box.urdf文件，内容如下：
<robot name="haobox">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
</visual>
</link>
</robot>
(2) 编写launch文件集成 URDF 与 Rviz
<launch>
<!--设置参数-->
<param name="robot_description" textfile="$(find hao_urdf_rviz)/urdf/urdf/hao_box.urdf" />\
<!--启动rviz-->
<!--args设置，让rviz每次启动时带有之前的配置信息-->
<node pkg="rviz" type="rviz" name="rviz" args=" -d $(find hao_urdf_rviz)/config/urdf/hao_urdf.config.rviz"/>
</launch>
(3) 在 Rviz 中显示机器人模型
rviz 启动后，会发现并没有盒装的机器人模型，这是因为默认情况下没有添加机器人显示组件，需要手动添加，添加方式如下:
点击按钮add --> 选择RobotModel --> 点击OK --> 在Global Options中设置Fixed Frame的值为base_link
4. URDF 语法
URDF 文件是一个标准的 XML 文件，在 ROS 中预定义了一系列的标签用于描述机器人模型
<robot>
<robot name="机器人的名称"></robot>
<link>
urdf 中的 link 标签用于描述机器人某个部件(也即刚体部分)的外观和物理属性，比如: 机器人底座、轮子、激光雷达、摄像头等等
每一个部件都对应一个 link, 在 link 标签内，可以设计该部件的形状、尺寸、颜色、惯性矩阵、碰撞参数等一系列属性
子标签
- visual
+ geometry
* 标签1 box (盒状)
- 属性: size="x y z" 长宽高
* 标签2 cylinder (圆柱)
- 属性: radius=半径 length=高度
* 标签3 sphere (球体)
- 属性: radius=半径
* 标签4 mesh (为连杆添加皮肤)
-属性: filename=资源路径(格式:package://<packagename>/<path>/文件)
+ origin 设置刚体偏移量与倾斜弧度
* 属性1: xyz=“x偏移 y便宜 z偏移"
* 属性2: rpy="x翻滚 y俯仰 z偏航 (单位是弧度)"
+ material 设置刚体材料属性(颜色)
* 属性: name
* 标签: color
- 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])
- collision 连杆的碰撞属性
- inertial 连杆的惯性矩阵
案例一
urdf文件编写：
<robot name="test">
<link name="base_link">
<visual>
<geometry>
<box size="0.3 0.1 0.5"/>
</geometry>
<origin xyz="0 0 1" rpy="0 0 0"/>
<material name="test_material">
<color rgba="0.8 0 0.8 0.5"/>
</material>
</visual>
<!-- <collision>
</collision>
<inertial>
</inertial> -->
</link>
</robot>
launch文件编写
<launch>
<param name="robot_description" textfile="$(find hao_urdf_rviz)/urdf/urdf/urdf_demo.urdf" />
<node pkg="rviz" name="rviz" type="rviz" args=" -d $(find hao_urdf_rviz)/config/urdf/hao_urdf.config.rviz"/>
</launch>
<joint>
joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限，机器人的两个部件(分别称之为 parent link 与 child link)以"关节"的形式相连接，不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制。joint标签对应的数据在模型中是不可见的
属性
- name 为关节命名
- type 关节运动形式
+ continuous: 旋转关节，可以绕单轴无限旋转
+ revolute: 旋转关节，类似于 continues,但是有旋转角度限制
+ prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
+ planer: 平面关节，允许在平面正交方向上平移或旋转
+ floating: 浮动关节，允许进行平移、旋转运动
+ fixed: 固定关节，不允许运动的特殊关节
子标签
- parent(必需的)
属性：link是一个强制的属性：
link:父级连杆的名字，是这个link在机器人结构树中的名字。
- child(必需的)
属性：link的名字是一个强制的属性：
link:子级连杆的名字，是这个link在机器人结构树中的名字。
- origin
属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。
- axis
属性: xyz用于设置围绕哪个关节轴运动。
案例二
urdf文件
<robot name="hao_car">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="base_link_color">
<color rgba="0.2 0.4 0.9 0.2" />
</material>
</visual>
</link>
<link name="camera">
<visual>
<geometry>
<box size="0.02 0.05 0.05" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="camera_color">
<color rgba="0.6 0 0.1 0.2" />
</material>
</visual>
</link>
<joint name="camera2baselink" type="continuous">
<parent link="base_link"/>
<child link="camera"/>
<!-- 关节的坐标系是相对于父连杆的 -->
<!-- 关节的坐标系的原点以向量【0.2，0，0.075】偏离base_link坐标系 -->
<!-- rpy是指关节的坐标系旋转 -->
<!-- 并且子连杆的坐标系会与关节的坐标系重合 -->
<origin xyz="0.2 0 0.075" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
</robot>
joint标签的子标签origin的属性xyz，描述的是此关节坐标原点相对于parant link坐标的偏移量。
launch文件
<launch>
<param name="robot_description" textfile="$(find hao_urdf_rviz)/urdf/urdf/joint_demo.urdf" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find hao_urdf_rviz)/config/urdf/joint_demo.config.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>
其中关节状态发布节点和机器人状态发布节点是必须的，用于控制关节运动的节点是可选的。
案例二优化
前面实现的机器人模型是半沉到地下的，因为默认情况下: 底盘的中心点位于地图原点上。可以将初始 link 设置为一个尺寸极小的 link，然后再在初始 link 上添加base_link，令base_link向上偏移，就可以解决问题。
<robot name="hao_car">
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="base_link_color">
<color rgba="0.2 0.4 0.9 0.2" />
</material>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 0.05" rpy="0 0 0" />
</joint>
<link name="camera">
<visual>
<geometry>
<box size="0.02 0.05 0.05" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="camera_color">
<color rgba="0.6 0 0.1 0.2" />
</material>
</visual>
</link>
<joint name="camera2baselink" type="continuous">
<parent link="base_link"/>
<child link="camera"/>
<origin xyz="0.2 0 0.075" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
</robot>
5. xacro
Xacro 是一种 XML 宏语言，是可编程的 XML。Xacro 可以声明变量，可以通过数学运算求解，使用流程控制控制执行顺序，还可以通过类似函数的实现，封装固定的逻辑，将逻辑中需要的可变的数据以参数的方式暴露出去，从而提高代码复用率以及程序的安全性。
属性与算数运算
属性定义
<xacro:property name="xxxx" value="yyyy" />
属性调用${属性名称}
算数运算${数学表达式}
宏
类似于函数实现，提高代码复用率，优化代码结构，提高安全性
<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">
.....
参数调用格式: ${参数名}
</xacro:macro>
宏调用<xacro:宏名称 参数1=xxx 参数2=xxx/>
文件包含
机器人由多部件组成，不同部件可能封装为单独的 xacro 文件，最后再将不同的文件集成，组合为完整机器人，可以使用文件包含实现
<robot name="xxx" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_base.xacro" />
<xacro:include filename="my_camera.xacro" />
<xacro:include filename="my_laser.xacro" />
....
</robot>
案例
编写 Xacro 文件
<!--
使用 xacro 优化 URDF 版的小车底盘实现：
实现思路:
1.将一些常量、变量封装为 xacro:property
比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮
-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 封装变量、常量 -->
<xacro:property name="PI" value="3.141"/>
<!-- 宏:黑色设置 -->
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
<!-- 底盘属性 -->
<xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径 -->
<xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
<xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
<xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->
<!-- 底盘 -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="${base_footprint_radius}" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${base_link_radius}" length="${base_link_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="yellow">
<color rgba="0.5 0.3 0.0 0.5" />
</material>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
</joint>
<!-- 驱动轮 -->
<!-- 驱动轮属性 -->
<xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
<xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
<!-- 驱动轮宏实现 -->
<xacro:macro name="add_wheels" params="name flag">
<link name="${name}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
<xacro:add_wheels name="left" flag="1" />
<xacro:add_wheels name="right" flag="-1" />
<!-- 支撑轮 -->
<!-- 支撑轮属性 -->
<xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 -->
<!-- 支撑轮宏 -->
<xacro:macro name="add_support_wheel" params="name flag" >
<link name="${name}_wheel">
<visual>
<geometry>
<sphere radius="${support_wheel_radius}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black" />
</visual>
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
<axis xyz="1 1 1" />
</joint>
</xacro:macro>
<xacro:add_support_wheel name="front" flag="1" />
<xacro:add_support_wheel name="back" flag="-1" />
</robot>
集成launch文件
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find demo01_urdf_helloworld)/urdf/xacro/my_base.urdf.xacro" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find demo01_urdf_helloworld)/config/helloworld.rviz" />
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />
</launch>
核心代码:<param name="robot_description" command="$(find xacro)/xacro $(find demo01_urdf_helloworld)/urdf/xacro/my_base.urdf.xacro" />
加载robot_description时使用command属性，属性值就是调用 xacro 功能包的 xacro 程序直接解析 xacro 文件。
6. URDF xacro 与rviz综合实例
需求描述:
在前面小车底盘基础之上，添加摄像头和雷达传感器。
(1) 摄像头 Xacro 文件实现
hao_camera.xacro 文件代码如下：
<!-- 摄像头相关的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 摄像头属性 -->
<xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) -->
<xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) -->
<xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) -->
<xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 -->
<xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
<xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2 -->
<!-- 摄像头关节以及link -->
<link name="camera">
<visual>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="camera2base_link" type="fixed">
<parent link="base_link" />
<child link="camera" />
<origin xyz="${camera_x} ${camera_y} ${camera_z}" />
</joint>
</robot>
(2) 雷达 Xacro 文件实现
hao_laser.xacro 代码如下：
<!--
小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷达支架 -->
<xacro:property name="support_length" value="0.15" /> <!-- 支架长度 -->
<xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 -->
<xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 -->
<xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 -->
<xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2 -->
<link name="support">
<visual>
<geometry>
<cylinder radius="${support_radius}" length="${support_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="red">
<color rgba="0.8 0.2 0.0 0.8" />
</material>
</visual>
</link>
<joint name="support2base_link" type="fixed">
<parent link="base_link" />
<child link="support" />
<origin xyz="${support_x} ${support_y} ${support_z}" />
</joint>
<!-- 雷达属性 -->
<xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 -->
<xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
<xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
<xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
<xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2 -->
<!-- 雷达关节以及link -->
<link name="laser">
<visual>
<geometry>
<cylinder radius="${laser_radius}" length="${laser_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="laser2support" type="fixed">
<parent link="support" />
<child link="laser" />
<origin xyz="${laser_x} ${laser_y} ${laser_z}" />
</joint>
</robot>
(3) 机器车底盘 Xacro 实现
hao_base.xacro 代码如下：
<!--
使用 xacro 优化 URDF 版的小车底盘实现：
实现思路:
1.将一些常量、变量封装为 xacro:property
比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮
-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 封装变量、常量 -->
<xacro:property name="PI" value="3.141"/>
<!-- 宏:黑色设置 -->
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
<!-- 底盘属性 -->
<xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径 -->
<xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
<xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
<xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->
<!-- 底盘 -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="${base_footprint_radius}" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${base_link_radius}" length="${base_link_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="yellow">
<color rgba="0.5 0.3 0.0 0.5" />
</material>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
</joint>
<!-- 驱动轮 -->
<!-- 驱动轮属性 -->
<xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
<xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
<!-- 驱动轮宏实现 -->
<xacro:macro name="add_wheels" params="name flag">
<link name="${name}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
<xacro:add_wheels name="left" flag="1" />
<xacro:add_wheels name="right" flag="-1" />
<!-- 支撑轮 -->
<!-- 支撑轮属性 -->
<xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 -->
<!-- 支撑轮宏 -->
<xacro:macro name="add_support_wheel" params="name flag" >
<link name="${name}_wheel">
<visual>
<geometry>
<sphere radius="${support_wheel_radius}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black" />
</visual>
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
<axis xyz="1 1 1" />
</joint>
</xacro:macro>
<xacro:add_support_wheel name="front" flag="1" />
<xacro:add_support_wheel name="back" flag="-1" />
</robot>
(5) 组合小车底盘与摄像头与雷达
hao_base_camera_laser.xacro 代码如下：
<!-- 组合小车底盘与摄像头与雷达 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="hao_base.xacro" />
<xacro:include filename="hao_camera.xacro" />
<xacro:include filename="hao_laser.xacro" />
</robot>
(6) Arbotix引入
Arbotix 是一款控制电机、舵机的控制板，并提供相应的 ros 功能包，这个功能包的功能不仅可以驱动真实的 Arbotix 控制板，它还提供一个差速控制器，通过接受速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在 rviz 中的运动。
a. 安装 Arbotix
sudo apt-get install ros-<VersionName()>-arbotix
VersionName() 为ros版本
b. 添加 arbotix 所需的配置文件
配置文件为yaml格式。该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂等
car_move.yaml 代码实现如下：
# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
# 单控制器设置
base_controller: {
#类型: 差速控制器
type: diff_controller,
#参考坐标
base_frame_id: base_footprint,
#两个轮子之间的间距
base_width: 0.2,
#控制频率
ticks_meter: 2000,
#PID控制参数，使机器人车轮快速达到预期速度
Kp: 12,
Kd: 12,
Ki: 0,
Ko: 50,
#加速限制
accel_limit: 1.0
}
}
(7) launch 文件中配置 arbotix 节点
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find hao_urdf_xacro)/urdf/xacro/hao_base_camera_laser.xacro" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find hao_urdf_xacro)/config/hao_base_camera_laser.config.rviz"/>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />
<!--launch 文件中配置 arbotix 节点-->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find hao_urdf_xacro)/config/car_move.yaml" command="load" />
<param name="sim" value="true" />
</node>
</launch>
(8) 启动 launch 文件并控制机器人模型运动
启动launch:roslaunch xxxx …launch
配置 rviz:
发布 cmd_vel 话题消息控制小车运动。例如
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'
7. URDF集成Gazebo
如果要在 Gazebo 中显示机器人模型，URDF 需要做的一些额外配置。
(1) URDF与Gazebo基本集成流程
a. 创建功能包
创建新功能包，导入依赖包: urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins
b. 编写URDF文件
<!--
创建一个机器人模型(盒状即可)，显示在 Gazebo 中
-->
<robot name="mycar">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="yellow">
<color rgba="0.5 0.3 0.0 1" />
</material>
</visual>
<collision>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="6" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<gazebo reference="base_link">
<material>Gazebo/Black</material>
</gazebo>
</robot>
注意， 当 URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别:
1.必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。
2.必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。
3.颜色设置，也需要重新使用 gazebo 标签标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项。
c. 启动Gazebo并显示模型
<launch>
<!-- 将 Urdf 文件的内容加载到参数服务器 -->
<param name="robot_description" textfile="$(find hao_urdf_gazebo)/urdf/urdf/gazebo_test.urdf" />
<!-- 启动 gazebo -->
<!-- 启动 Gazebo 的仿真环境，当前环境为空环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 在 gazebo 中显示机器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
<!--
在 Gazebo 中加载一个机器人模型，该功能由 gazebo_ros 下的 spawn_model 提供:
-urdf 加载的是 urdf 文件
-model mycar 模型名称是 mycar
-param robot_description 从参数 robot_description 中载入模型
-x 模型载入的 x 坐标
-y 模型载入的 y 坐标
-z 模型载入的 z 坐标
-->
</launch>
(2)URDF集成Gazebo相关设置
collision设置
如果机器人link是标准的几何体形状，和link的 visual 属性设置一致即可。
inertial设置
惯性矩阵的设置需要结合link的质量与外形参数动态生成，标准的球体、圆柱与立方体的惯性矩阵公式如下(已经封装为 xacro 实现):
球体惯性矩阵
<!-- 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="Box_inertial_matrix" params="m l w h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
iyy="${m*(w*w + l*l)/12}" iyz= "0"
izz="${m*(w*w + h*h)/12}" />
</inertial>
</xacro:macro>
性矩阵必须经计算得出，如果随意定义刚体部分的惯性矩阵，那么可能会导致机器人在 Gazebo 中出现抖动，移动等现象。
颜色设置
在 gazebo 中显示 link 的颜色，必须要使用指定的标签:
<gazebo reference="link节点名称">
<material>Gazebo/Blue</material>
</gazebo>
五、ROS常用组件
TF坐标变换
tf:TransForm Frame,坐标变换
坐标系:ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
在 ROS 中用于实现不同坐标系之间的点或向量的转换
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:
-
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
-
tf2: 封装了坐标变换的常用消息。
-
tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
【需求描述】
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
-
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
-
坐标系相对关系，可以通过发布方发布
#include "ros/ros.h" #include "tf2_ros/static_transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h" #include "tf2/LinearMath/Quaternion.h" int main(int argc, char *argv[]) { ros::init(argc, argv, "static_broadcast"); // 创建静态坐标转换广播器 tf2_ros::StaticTransformBroadcaster broadcaster; // 创建坐标系信息 geometry_msgs::TransformStamped ts; ts.header.seq = 100; ts.header.frame_id = "base_link"; ts.header.stamp = ros::Time::now(); ts.child_frame_id = "laser"; ts.transform.translation.x = 0.2; ts.transform.translation.y = 0.0; ts.transform.translation.z = 0.5; tf2::Quaternion qtn; //设置四元数 qtn.setRPY(0,0,0); //将欧拉角数据转换成四元数 ts.transform.rotation.w = qtn.getW(); ts.transform.rotation.x = qtn.getX(); ts.transform.rotation.y = qtn.getY(); ts.transform.rotation.z = qtn.getZ(); //广播器发布坐标系信息 broadcaster.sendTransform(ts); ros::spin(); return 0; } -
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
#include "ros/ros.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/PointStamped.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件,不然会报错 int main(int argc, char *argv[]) { ros::init(argc, argv, "tf_sub"); ros::NodeHandle nh; tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); ros::Rate rate(1); while (ros::ok()) { //生成一个坐标点(相对于子级坐标系) 即在雷达坐标系下的座标点 geometry_msgs::PointStamped ps_laser; ps_laser.header.frame_id = "laser"; ps_laser.header.stamp = ros::Time::now(); ps_laser.point.x = 1; ps_laser.point.y = 2; ps_laser.point.z = 7.3; //转换坐标点(将雷达坐标系下坐标转换成父级坐标系下的坐标) try { geometry_msgs::PointStamped ps_base; /** * 参数一：要转换的坐标点 * 参数二：要转换到的坐标系，目标坐标系 * 返回值：返回在目标坐标系下的坐标 */ ps_base = buffer.transform(ps_laser,"base_link"); ROS_INFO("transformed : (%.2f,%.2f,%.2f) the axis is: %s\n",ps_base.point.x,ps_base.point.y,ps_base.point.z, ps_base.header.frame_id.c_str()); } catch(const std::exception& e) { ROS_INFO("error"); } rate.sleep(); ros::spinOnce(); } return 0; }
补充1:
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
也建议使用该种方式直接实现静态坐标系相对信息发布。
补充2:
可以借助于rviz显示坐标系关系，具体操作:
新建窗口输入命令:rviz;
在启动的 rviz 中设置Fixed Frame 为 base_link;
点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。
动态坐标变换
指两个坐标系之间的相对位置是变化的
【需求描述】
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
【实现分析】
乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成 坐标系相对信息并发布
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
