【ros学习】14.urdf、xacro机器人建模与rviz、gazebo仿真详解
一、起因
学校的这学期课程是ros机器人开发实战，我们学习小组也要搞一个自己的机器人模型，我们组又叫葫芦组，所以我就做了个葫芦形状的机器人，虽说有点丑，本来想用maya建模再导入的，奈何不太懂maya，于是乎就用基础三形状构建了这个机器人模型。
接下来我将会将urdf建模与gazebo仿真过程详细写出，共大家参考与互相学习，如有疏漏，敬请指正。
源码：hulu机器人源码-百度网盘 提取码：hctm
不过这个葫芦机器人走路实在是不稳定，所以又写了另外的俩个机器人，源码和演示视频在文末。
二、机器人结构图
首先我们得把要构建的机器人画出来，把每个肢节的长宽高和半径都标注好，听说可以用CAD/SW/UG等软件画，我当时懒得画，画了个躯体就没画了，纯粹靠手调调节相对位置，耗费了不少时间，如果你会CAD可以减少很多调整时间，效果图大概是这样子的，找了个E100的图作为参考
三、准备工作
- 创建一个机器人建模的功能包
virtual-machine:~$ catkin_crate_pkg hulu_gazebo urdf xacro
依赖urdf、xacro功能包
- 创建各个放置文件的文件夹
总源码包文件联系结构如图：
- config: 配置文件
- launch：启动文件
- meshes：dae模型文件
- scripts：脚本文件
- urdf：机器人模型文件
- world：gazebo地图文件
- CMakeLists.txt: 依赖及编译规则
- package.xml: 包信息
- 修改Cmake编译系统的规则文件CMakeLists.txt，添加依赖
find_package(catkin REQUIRED COMPONENTS
urdf
xacro
gazebo_plugins
gazebo_ros
gazebo_ros_control
geometry_msgs
roscpp
rospy
)
- 修改软件包的描述文件package.xml
<buildtool_depend>catkinbuildtool_depend>
<build_depend>urdfbuild_depend>
<build_depend>xacrobuild_depend>
<build_depend>gazebo_pluginsbuild_depend>
<build_depend>gazebo_rosbuild_depend>
<build_depend>gazebo_ros_controlbuild_depend>
<build_depend>geometry_msgsbuild_depend>
<build_depend>roscppbuild_depend>
<build_depend>rospybuild_depend>
<build_export_depend>urdfbuild_export_depend>
<build_export_depend>xacrobuild_export_depend>
<build_export_depend>gazebo_pluginsbuild_export_depend>
<build_export_depend>gazebo_rosbuild_export_depend>
<build_export_depend>gazebo_ros_controlbuild_export_depend>
<build_export_depend>geometry_msgsbuild_export_depend>
<build_export_depend>roscppbuild_export_depend>
<build_export_depend>rospybuild_export_depend>
<exec_depend>urdfexec_depend>
<exec_depend>xacroexec_depend>
<exec_depend>gazebo_pluginsexec_depend>
<exec_depend>gazebo_rosexec_depend>
<exec_depend>gazebo_ros_controlexec_depend>
<exec_depend>geometry_msgsexec_depend>
<exec_depend>roscppexec_depend>
<exec_depend>rospyexec_depend>
关于CMakeLists.txt可以参考我的这篇文章【ros学习】9.ros话题通讯之publisher与subscribe的c++实现详解
四、Arbotix的安装
- 由于rviz不像gazebo，没有提供控制器接口，想要让机器人在rviz中动起来，我们需要一款差速控制器，而ArbotiX这款控制电机、舵机的硬件控制板的ros功能包就提供了一个差速控制器，通过接收速度控制指令，更新机器人的里程计状态。
$ git clone http://github.com/vanadiumlabs/arbotis_ros.git
$ catkin_make
- 注意：arbotis_ros中的python文件需要添加可执行权限
五、xacro是什么？怎么用？
什么是Xacro? 我们可以把它理解成为针对URDF的扩展性和配置性而设计的宏语言(macro language)。有了Xacro，我们就可以像编程一样来写URDF文件。XACRO格式提供了一些更高级的方式来组织编辑机器人描述. 主要提供了三种方式来使得整个描述文件变得简单。
（1）Constants
<xacro:property name="WIDTH" value="2.0"/>
类似于C语言中的宏定义, 在头部定义后就可以${body_width}进行引用其数值，有了这个，至少我们可以把需要配置的变量进行统一管理和使用。
（2）Macros
<xacro:macro name="default_origin">
<origin xyz="0 0 0" rpy="0 0 0"/>
xacro:macro>
<xacro:default_origin />
Macros是xacro文件中最重要的部分. 就像宏函数一样, 完成一些最小模块的定义, 方便重用, 以及可以使用参数来标识不同的部分.
（3）Include
很多模型都是已宏的形式进行定义, 并以最小集团分成很多个文件. 而最终的机器人描述就变得非常简单了. 下面摘录一个ur5的描述文件. 从中可以看出来xacro的强大优势. 在最后的示例中我们还能够看到, urdf文件也是能够直接导入进来的.
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5" >
<xacro:include filename="$(find ur_description)/urdf/ur5/common.gazebo.xacro" />
<xacro:include filename="$(find ur_description)/urdf/ur5/ur5.urdf.xacro" />
<xacro:ur5_robot prefix="" joint_limited="false"/>
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world" />
类似于C语言中的include, 先将该文件扩展到包含的位置. 但包含进来的文件很有可能只是一个参数宏的定义. 并没有被调用.\quad
举例说明打开新的终端，输入roslaunch urdf_demo display_xacro.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，并且小车颜色和形状已经固定完成。
（4）数学运算
六、URDF所有的标签解析
主体是俩个标签，joint和link标签，joint代表关节,link代表肢节。transmission代表变速器，他控制关节的转动，gazebo设置gazebo相关参数。官方文档：XML Specifications
joint:关节
- origin:
- xyz 代表孩子肢节相对与父肢节坐标系的相对位移，x相对父坐标系x轴原点前后移动，以此类推yz。
- rpy 代表该关节的横滚角-roll、俯仰角-pitch、偏航角-yaw
一般定义载体的右、前、上三个方向构成右手系，绕向前的轴[x轴]旋转就是横滚角，绕向右的轴[y轴]旋转就是俯仰角，绕向上的轴[z轴]旋转就是航向角
逆时针为正，比如rpy=“1.5705 0 0”,代表该关节及关节上绑定的其他关节作为一个整体绕关节的x轴逆时针旋转90度，为什么是1.5705呢？因为是用弧度来表示的，一度等于0.01745弧度，不过也取决于你的π的精度，比如3.14159/180=0.01745结果就是一弧度的大小, - parent：
- 父肢节 - child:
- 子肢节 - axis：
- 旋转轴，在关节框架中指定的关节轴。这是旋转关节的旋转轴，棱柱形关节的平移轴，平面关节的表面法线。轴在参考关节框架中指定。固定关节和浮动关节不使用轴字段。 - joint的属性type：关节的类型
-
link:肢节
- visual：视觉，描述机器人肢节部分的外观
- origin 视觉元素的参考框架相对于肢节的参考框架。和上面关节的 origin一样的意思。
- geometry （必需）视觉对象的形状。这可以是一个如下：
<geometry>
<box> 盒子，长方体、正方体、立方体
<box size="${x} ${y} ${z}"/>
size属性包含盒子的三个边长。盒子的原点在中间。
<cylinder> 气缸，其实就是个圆柱体
<cylinder radius="0.02" length="0.3"/>
指定半径和长度。圆柱体的原点位于其中心。
<sphere> 球体
<sphere radius="0.05" />
指定半径。球体的原点位于其中心。
<mesh> 网格，导入通过其他建模软件建的dae文件
<mesh filename="package://hulu_gazebo/meshes/kinect.dae" />
由filename指定的trimesh元素，以及用于缩放网格的axis-aligned-bounding-box的可选比例。任何几何格式都是可以接受的，
但是特定的应用程序兼容性取决于实现。最佳的纹理和颜色支持的推荐格式是Collada .dae文件。不能在引用相同模型的机器之间传输网格文件。
它必须是本地文件。
geometry>
- material：材料
视觉元素的材料。可以在顶级“机器人”元素中指定“链接”对象之外的材料元素。然后， 您可以从链接元素中按名称引用材料。
<material>
<color> （可选）
<color rgba="1 0.27 0 1"/>
rgba由一组代表红色/绿色/蓝色/ alpha的四个数字指定的材料的颜色，
每个数字的范围为[0,1]。
最后的那个参数是透明度，0全透明，1不透明
rgb(207 166 87 1) -> rgba(207/255 166/255 87/255 1)
<texture> （可选）
材质的纹理由文件名指定
material>
- inertial：惯性参数
- Gazebo的官方文档明确指出，必须正确指定和配置每个元素中的
- 背景
尽管URDF在ROS中是一种有用且标准化的格式，但它们缺少许多功能，并且尚未进行更新以应对机器人技术的不断发展的需求。URDF只能单独指定单个机器人的运动学和动力学特性。URDF无法指定世界中机器人本身的姿势。它也不是通用的描述格式，因为它不能指定关节环（平行连接），并且缺乏摩擦和其他特性。此外，它不能指定不是机器人的东西，例如灯光，高度图等。
在实现方面，URDF语法大量使用XML属性破坏了正确的格式设置，这反过来又使URDF更加不灵活。也没有向后兼容的机制。
为了解决此问题，创建了一种称为仿真描述格式（SDF）的新格式
，供凉亭使用，以解决URDF的缺点。SDF是从世界级到机器人级的所有内容的完整描述。它具有可伸缩性，并易于添加和修改元素。SDF格式本身使用XML进行描述，这有助于使用简单的升级工具将旧版本迁移到新版本。它也是自我描述的。
作者的意图是使URDF尽可能完整地记录在凉亭中，并在凉亭中提供支持，但与读者相关的是要了解为什么存在两种格式以及两者都有缺点。如果在URDF中进行更多工作以将其更新为当前的机器人技术，那将是很好的。
- origin 这是惯性参考系相对于肢节参考系的姿势。
- 惯性参考系的原点必须位于重心。惯性基准帧的轴线做不需要与惯性的主轴对齐。
inertial>
<mass value="${m}" />
肢节的质量由此元素的value属性表示
<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>
<!-- Macro for inertia matrix -->
<!-- 立方体惯性矩阵 -->
<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>
<!-- 球体惯性矩阵 -->
<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>
矩阵原理及计算方法：机器人的惯性、视觉、碰撞特征计算与表示
- collision：碰撞属性
- origin 碰撞元素的参考框架相对于肢节的参考框架。
- geometry （必需）碰撞对象的形状。
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.01"/>
geometry>
collision>
这个元素是表明机器人的虚拟预留碰撞范围的，视觉上是绿色的范围是模型，但实际给肢节的碰撞范围比视觉效果大，碰撞范围是红框框，以避免机器人真的撞墙，不过经常为了简便，就直接复制一遍visual的origin、geometry就行了
transmission:变速器
这个我也不是很懂，大意就是为你的轮子的关节选择一种gazebo提供的电机进行驱动，然后电机会订阅cmd_vel话题去接收速度控制指令然后控制你绑定的关节按axis旋转轴转动。
1个
2 <type> transmission_interface / SimpleTransmission
3
4 <hardwareInterface> EffortJointInterface
5
6
7 <mechanicalReduction> 50
8 <hardwareInterface> EffortJointInterface
9
10
<transmission>
<type> (one occurrence)
指定传输类型。
<joint> (one or more occurrences)
变速箱所连接的关节。
<hardwareInterface> (one or more occurrences)
指定支持的联合空间硬件接口。
<actuator> (one or more occurrences)
变速器连接到的执行器。（电机）
<mechanicalReduction> (optional)
指定关节/执行器传动机构的机械减速。
<hardwareInterface> (optional) (one or more occurrences)
指定支持的联合空间硬件接口。
actuator>
transmission>
gazebo:
1.可选择的
· 为<link>元素添加一个<gazebo>元素
1.将视觉颜色转换为凉亭格式
2.将Stl文件转换为dae文件以获得更好的纹理
3.添加传感器插件
· 为<joint>元素添加一个<gazebo>元素
1.设置适当的阻尼动力学
2.添加执行器控制插件
· 为<robot>元素添加一个<gazebo>元素
· 如果应该将机器人牢固地附加到world / base_link，请添加链接<link name="world"/>
- link内的可选属性 ，以后有空再翻译成中文
- joint内的可选属性
- 官方文档：教程：在gazebo中使用URDF
- gazebo插件
gazebo插件为您的URDF模型提供了更大的功能，并且可以将ROS消息和服务呼叫联系在一起，以实现传感器输出和电动机输入。
差动驱动
说明：模型插件，为凉亭中的差动驱动机器人提供基本控制器。您需要一个定义明确的差动驱动器机器人才能使用此插件。
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<updateRate>${update_rate}updateRate>
<leftJoint>base_link_left_wheel_jointleftJoint>
<rightJoint>base_link_right_wheel_jointrightJoint>
<wheelSeparation>0.5380wheelSeparation>
<wheelDiameter>0.2410wheelDiameter>
<wheelAcceleration>1.0wheelAcceleration>
<wheelTorque>20wheelTorque>
<commandTopic>cmd_velcommandTopic>
<odometryTopic>odomodometryTopic>
<odometryFrame>odomodometryFrame>
<robotBaseFrame>base_footprintrobotBaseFrame>
<odometrySource>1odometrySource>
<publishWheelTF>truepublishWheelTF>
<publishOdom>truepublishOdom>
<publishWheelJointState>truepublishWheelJointState>
<legacyMode>falselegacyMode>
plugin>
gazebo>
插件使用具体教程：Tutorial: Using Gazebo plugins with ROS
七、urdf结合xacro构建机器人并书写launch文件
模型文件及启动文件联系结构如图：
（1）机器人主体模型 hulu_base_description.xacro
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.14159265354"/>
<xacro:property name="body_mass" value="20" />
<xacro:property name="body_radius" value="0.20"/>
<xacro:property name="body_length" value="0.16"/>
<xacro:property name="head_mass" value="5" />
<xacro:property name="head_radius" value="0.1"/>
<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_x" value="0.15"/>
<xacro:property name="wheel_joint_y" value="0.15"/>
<xacro:property name="wheel_joint_z" value="0.16"/>
<xacro:property name="caster_mass" value="0.5" />
<xacro:property name="caster_radius" value="0.06"/>
<xacro:property name="caster_joint_x" value="0.1"/>
<xacro:property name="caster_joint_y" value="0.1"/>
<xacro:property name="caster_joint_z" value="0.16"/>
<xacro:property name="ears_mass" value="0.02" />
<xacro:property name="ears_x" value="0.008" />
<xacro:property name="ears_y" value="0.008" />
<xacro:property name="ears_z" value="0.04" />
<xacro:property name="ears_joint_y" value="0.099"/>
<xacro:property name="ears_joint_z" value="0.02"/>
<xacro:property name="eye_mass" value="0.02" />
<xacro:property name="eye_radius" value="0.01" />
<xacro:property name="eye_joint_x" value="0.085"/>
<xacro:property name="eye_joint_y" value="0.05"/>
<xacro:property name="eye_joint_z" value="0.02"/>
<xacro:property name="eyeball_mass" value="0.02" />
<xacro:property name="eyeball_radius" value="0.0025" />
<xacro:property name="eyeball_joint_x" value="0.0095"/>
<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>
<material name="white">
<color rgba="1 1 1 1"/>
material>
<material name="orange">
<color rgba="1 0.27 0 1"/>
material>
<xacro:macro name="box_inertial_matrix" params="m l w h">
<inertial>
<mass value="${m}" />
（2）机器人传感器的模型
- 照相机 camera_gazebo.xacro
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<xacro:macro name="usb_camera" params="prefix:=camera">
<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.02 0.02" />
geometry>
<material name="black"/>
visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.02 0.02" />
geometry>
collision>
link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Blackmaterial>
gazebo>
<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0update_rate>
<camera name="head">
<horizontal_fov>1.3962634horizontal_fov>
<image>
<width>1280width>
<height>720height>
<format>R8G8B8format>
image>
<clip>
<near>0.02near>
<far>300far>
clip>
<noise>
<type>gaussiantype>
<mean>0.0mean>
<stddev>0.007stddev>
noise>
camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>truealwaysOn>
<updateRate>0.0updateRate>
<cameraName>/cameracameraName>
<imageTopicName>image_rawimageTopicName>
<cameraInfoTopicName>camera_infocameraInfoTopicName>
<frameName>camera_linkframeName>
<hackBaseline>0.07hackBaseline>
<distortionK1>0.0distortionK1>
<distortionK2>0.0distortionK2>
<distortionK3>0.0distortionK3>
<distortionT1>0.0distortionT1>
<distortionT2>0.0distortionT2>
plugin>
sensor>
gazebo>
xacro:macro>
robot>
- 3D体感摄影机 kinect kinect_gazebo.xacro
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">
<xacro:macro name="kinect_camera" params="prefix:=camera">
<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://hulu_gazebo/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>truealways_on>
<update_rate>20.0update_rate>
<camera>
<horizontal_fov>${60.0*M_PI/180.0}horizontal_fov>
<image>
<format>R8G8B8format>
<width>640width>
<height>480height>
image>
<clip>
<near>0.05near>
<far>8.0far>
clip>
camera>
<plugin name="kinect_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>${prefix}cameraName>
<alwaysOn>truealwaysOn>
<updateRate>10updateRate>
<imageTopicName>rgb/image_rawimageTopicName>
<depthImageTopicName>depth/image_rawdepthImageTopicName>
<pointCloudTopicName>depth/pointspointCloudTopicName>
<cameraInfoTopicName>rgb/camera_infocameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_infodepthImageCameraInfoTopicName>
<frameName>${prefix}_frame_opticalframeName>
<baseline>0.1baseline>
<distortion_k1>0.0distortion_k1>
<distortion_k2>0.0distortion_k2>
<distortion_k3>0.0distortion_k3>
<distortion_t1>0.0distortion_t1>
<distortion_t2>0.0distortion_t2>
<pointCloudCutoff>0.4pointCloudCutoff>
plugin>
sensor>
gazebo>
xacro:macro>
robot>
- 激光雷达 lidar_gazebo.xacro
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">
<xacro:macro name="rplidar" params="prefix:=laser">
<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.005"/>
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/Blackmaterial>
gazebo>
<gazebo reference="${prefix}_link">
<sensor type="ray" name="rplidar">
<pose>0 0 0 0 0 0pose>
<visualize>falsevisualize>
<update_rate>5.5update_rate>
<ray>
<scan>
<horizontal>
<samples>360samples>
<resolution>1resolution>
<min_angle>-3min_angle>
<max_angle>3max_angle>
horizontal>
scan>
<range>
<min>0.10min>
<max>6.0max>
<resolution>0.01resolution>
range>
<noise>
<type>gaussiantype>
<mean>0.0mean>
<stddev>0.01stddev>
noise>
ray>
<plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
<topicName>/scantopicName>
<frameName>laser_linkframeName>
plugin>
sensor>
gazebo>
xacro:macro>
robot>
（3）机器人主体添加传感器后的模型 hulu_with_sensors.xacro
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find hulu_gazebo)/urdf/xacro/gazebo/hulu_base_description.xacro" />
<xacro:include filename="$(find hulu_gazebo)/urdf/xacro/sensors/camera_gazebo.xacro" />
<xacro:include filename="$(find hulu_gazebo)/urdf/xacro/sensors/lidar_gazebo.xacro" />
<xacro:include filename="$(find hulu_gazebo)/urdf/xacro/sensors/kinect_gazebo.xacro" />
<xacro:property name="camera_offset_x" value="0.082" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.06" />
<xacro:property name="lidar_offset_x" value="0" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.125" />
<xacro:property name="kinect_offset_x" value="0" />
<xacro:property name="kinect_offset_y" value="0" />
<xacro:property name="kinect_offset_z" value="-0.03" />
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${-camera_offset_z}" rpy="0 0.27483 0" />
<parent link="head_link"/>
<child link="camera_link"/>
joint>
<xacro:usb_camera prefix="camera"/>
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="head_link"/>
<child link="laser_link"/>
joint>
<xacro:rplidar prefix="laser"/>
<joint name="kinect_joint" type="fixed">
<origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="-3.14159 0 0" />
<parent link="palm_link"/>
<child link="kinect_link"/>
joint>
<xacro:kinect_camera prefix="kinect"/>
<hulu_body_gazebo/>
robot>
（4）创建arbotix与rviz的配置文件
- arbotix配置文件 fake_hulu_arbotix.yaml
controllers: {
base_controller: {
type: diff_controller,
base_frame_id: body_footprint,
base_width: 0.26,
ticks_meter: 4100,
Kp: 12,
Kd: 12,
Ki: 0,
Ko: 50,
accel_limit: 1.0
}
}
- rviz配置文件 hulu_arbotix.rviz
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Odometry1/Shape1
Splitter Ratio: 0.5
Tree Height: 631
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
省略以下没有营养的内容
（5）创建在rviz进行仿真的launch文件 arbotix_hulu_with_rviz.launch
<launch>
<arg name="model" default="$(find xacro)/xacro --inorder '$(find hulu_gazebo)/urdf/xacro/gazebo/hulu_with_sensors.xacro'" />
<arg name="gui" default="false" />
<param name="robot_description" command="$(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find hulu_gazebo)/config/fake_hulu_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find hulu_gazebo)/config/hulu_arbotix.rviz" required="true" />
launch>
（6）创建在gazebo进行仿真的launch文件 view_hulu_with__gazebo.launch
<launch>
<arg name="world_name" value="$(find hulu_gazebo)/worlds/maze.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"/>
<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 hulu_gazebo)/urdf/xacro/gazebo/hulu_with_sensors.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >node>
<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>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model hulu -param robot_description"/>
launch>
（6）创建键盘发布控制指令的launch文件 hulu_teleop.launch
<launch>
<node name="hulu_teleop" pkg="hulu_gazebo" type="hulu_teleop.py" output="screen">
<param name="scale_linear" value="0.1" type="double"/>
<param name="scale_angular" value="0.4" type="double"/>
node>
launch>
（7）键盘速度控制脚本文件 hulu_teleop.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Control hulu!
---------------------------
Moving around:
u i o
j k l
m , .
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 smoothly
CTRL-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 key
speed = .2
turn = 1
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('hulu_teleop')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
x = 0
th = 0
status = 0
count = 0
acc = 0.1
target_speed = 0
target_turn = 0
control_speed = 0
control_turn = 0
try:
print msg
print 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 = 0
print vels(speed,turn)
if (status == 14):
print msg
status = (status + 1) % 15
# 停止键
elif key == ' ' or key == 'k' :
x = 0
th = 0
control_speed = 0
control_turn = 0
else:
count = count + 1
if count > 4:
x = 0
th = 0
if (key == '\x03'):
break
# 目标速度=速度值*方向值
target_speed = speed * x
target_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_speed
if 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 = 0
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = control_turn
pub.publish(twist)
except:
print e
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
八、仿真演示
1.rviz展示模型
(1)直接启动
$ roslaunch hulu_gazebo arbotix_hulu_with_rviz.launch
(2)启动键盘控制节点
$ roslaunch hulu_gazebo hulu_teleop.launch
2.创建自己的gazebo地图
3.gazebo仿真
(1) 摄像头仿真
启动gazebo环境
$ roslaunch hulu_gazebo view_hulu_with_gazebo.launch
查看当前ROS计算图
$ rqt_graph
查看当前发布的话题
$ rostopic list
打开摄像头
$ rqt_image_view
打开键盘操控节点
$ roslaunch hulu_gazebo hulu_teleop.launch
(2) 激光雷达仿真
打开rviz
$ rosrun rviz rviz
固定坐标系选择laser_link肢节
添加一个LaserScan模块，订阅/scan话题
将激光点尺寸改大点方便看
键盘控制移动
由于我的激光雷达装在葫芦娃的头顶，所以比它头矮的物品他都扫描不到，这就很尴尬
(3) kinect仿真
把rviz关掉再重进比较快
固定坐标系选择kinect_link肢节
添加一个piontcloud2模块，订阅/kinect/depth/points话题
添加一个image模块，订阅/kinect/rgb/image_raw话题
键盘控制移动
查看当前ROS计算图
$ rqt_graph
九、后记
测试发现这个葫芦机器人走路很不稳定，基本有坎或者撞墙就翻车，而且它的激光雷达扫描不到比自己矮的物体，调整激光雷达的扫描角度也扫不到，可能是被偷挡住了。所以又写了俩个机器人，一个叫阶梯机器人，一个叫雷达机器人，把他们的传感器都放在了低位置，这次就还行了。
stairs_sensor_lidar.xacro
这样摆放老是让激光雷达把后面俩个方块当成障碍物，所以还是把后面俩个东西去掉了好扫描一点
stairs_sensors.xacro
两个模型的名字取反了，别弄混
新的代码和演示视频：百度网盘
提取码：t9nn
后续再写个ppt