一、起因
学校的这学期课程是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>catkin</buildtool_depend>
<!--编译依赖项 -->
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<build_depend>gazebo_plugins</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>gazebo_ros_control</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<!-- 导出依赖项 -->
<build_export_depend>urdf</build_export_depend>
<build_export_depend>xacro</build_export_depend>
<build_export_depend>gazebo_plugins</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend>
<build_export_depend>gazebo_ros_control</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<!-- 运行依赖项 -->
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>gazebo_plugins</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>gazebo_ros_control</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_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文件也是能够直接导入进来的.
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5" >
<!-- common stuff -->
<xacro:include filename="$(find ur_description)/urdf/ur5/common.gazebo.xacro" />
<!-- ur5 -->
<xacro:include filename="$(find ur_description)/urdf/ur5/ur5.urdf.xacro" />
<!-- arm -->
<xacro:ur5_robot prefix="" joint_limited="false"/>
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
</robot>
类似于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的官方文档明确指出,必须正确指定和配置每个<link>
元素中的<inertial>
元素,urdf模型才能在gazebo中正常工作,因为gazebo里使用的文件格式并不是urdf格式,而是将urdf转换为它自己独有的的SDF格式再进行仿真。
- 背景
尽管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个 <transmission name =“ simple_trans”>
2 <type> transmission_interface / SimpleTransmission </ type>
3 <joint name =“ foo_joint”>
4 <hardwareInterface> EffortJointInterface </ hardwareInterface>
5 </ joint>
6 <actuator name =“ foo_motor”>
7 <mechanicalReduction> 50 </ mechanicalReduction>
8 <hardwareInterface> EffortJointInterface </ hardwareInterface>
9 </ actuator>
10 </ transmission>
<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:<gazebo>
元素是URDF的扩展,用于指定在gazebo中进行仿真所需的其他属性。
1.可选择的<gazebo>
标签添加方式
· 为<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">
<!-- Plugin update rate in Hz -->
<!--插件更新率(Hz)-->
<updateRate>${update_rate}</updateRate>
<!-- Name of left joint, defaults to `left_joint` -->
<!--左关节的名称,默认为“左关节”-->
<leftJoint>base_link_left_wheel_joint</leftJoint>
<!-- Name of right joint, defaults to `right_joint` -->
<!--右关节的名称,默认为“右关节”-->
<rightJoint>base_link_right_wheel_joint</rightJoint>
<!-- The distance from the center of one wheel to the other, in meters, defaults to 0.34 m -->
<!--从一个车轮中心到另一个车轮中心的距离(以米为单位)默认为0.34米-->
<wheelSeparation>0.5380</wheelSeparation>
<!-- Diameter of the wheels, in meters, defaults to 0.15 m -->
<!--车轮直径(以米为单位)默认为0.15米-->
<wheelDiameter>0.2410</wheelDiameter>
<!-- Wheel acceleration, in rad/s^2, defaults to 0.0 rad/s^2 -->
<!--车轮加速度,单位为rad/s^2,默认为0.0 rad/s^2-->
<wheelAcceleration>1.0</wheelAcceleration>
<!-- Maximum torque which the wheels can produce, in Nm, defaults to 5 Nm -->
<!--车轮可产生的最大扭矩(Nm)默认为5 Nm-->
<wheelTorque>20</wheelTorque>
<!-- Topic to receive geometry_msgs/Twist message commands, defaults to `cmd_vel` -->
<!--主题接收几何图形/扭曲消息命令,默认为“cmd”级别-->
<commandTopic>cmd_vel</commandTopic>
<!-- Topic to publish nav_msgs/Odometry messages, defaults to `odom` -->
<!--主题发布导航消息/里程计消息,默认为“odom”-->
<odometryTopic>odom</odometryTopic>
<!-- Odometry frame, defaults to `odom` -->
<!--里程计帧,默认为“odom”-->
<odometryFrame>odom</odometryFrame>
<!-- Robot frame to calculate odometry from, defaults to `base_footprint` -->
<!--要从中计算里程表的Robot帧,默认为“base_footprint”-->
<robotBaseFrame>base_footprint</robotBaseFrame>
<!-- Odometry source, 0 for ENCODER, 1 for WORLD, defaults to WORLD -->
<!--里程计源,0代表编码器,1代表世界,默认为世界-->
<odometrySource>1</odometrySource>
<!-- Set to true to publish transforms for the wheel links, defaults to false -->
<!--设置为true可发布控制盘链接的变换,默认值为false-->
<publishWheelTF>true</publishWheelTF>
<!-- Set to true to publish transforms for the odometry, defaults to true -->
<!--设置为true以发布里程计的变换,默认为true-->
<publishOdom>true</publishOdom>
<!-- Set to true to publish sensor_msgs/JointState on /joint_states for the wheel joints, defaults to false -->
<!--如果设置为true,则发布车轮接头的sensor_msgs/JointState on/joint_状态,则默认为false-->
<publishWheelJointState>true</publishWheelJointState>
<!-- Set to true to swap right and left wheels, defaults to true -->
<!--设置为“真”可交换左右控制盘,默认为“真-->
<legacyMode>false</legacyMode>
</plugin>
</gazebo>
插件使用具体教程:Tutorial: Using Gazebo plugins with ROS
七、urdf结合xacro构建机器人并书写launch文件
模型文件及启动文件联系结构如图:
(1)机器人主体模型 hulu_base_description.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.14159265354"/>
<!-- body sphere -->
<xacro:property name="body_mass" value="20" />
<xacro:property name="body_radius" value="0.20"/>
<xacro:property name="body_length" value="0.16"/>
<!-- head sphere -->
<xacro:property name="head_mass" value="5" />
<xacro:property name="head_radius" value="0.1"/>
<!-- wheel cylinder -->
<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"/>
<!-- caster sphere -->
<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"/>
<!-- ears box -->
<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"/>
<!-- eye sphere -->
<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"/>
<!-- eyeball sphere -->
<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"/>
<!-- 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>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="orange">
<color rgba="1 0.27 0 1"/>
</material>
<!-- 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>
<!-- 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="body_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>
<!-- Specifies the color of the limb in gazebo -->
<gazebo reference="${prefix}_wheel_link">
<material>Gazebo/DarkGrey</material>
</gazebo>
<!-- electric machinery -->
<!-- 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 ${-caster_joint_z}" rpy="0 0 0"/>
<parent link="body_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/GreyTransparent</material>
</gazebo>
</xacro:macro>
<!-- Macro for robot ears -->
<xacro:macro name="ears" params="prefix reflect" >
<joint name="${prefix}_ears_joint" type="fixed">
<origin xyz="0 ${reflect*ears_joint_y} ${ears_joint_z}" rpy="0 0 0" />
<parent link="head_link"/>
<child link="${prefix}_ears_link" />
</joint>
<link name="${prefix}_ears_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="${ears_x} ${ears_y} ${ears_z}"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="${ears_x} ${ears_y} ${ears_z}"/>
</geometry>
</collision>
<box_inertial_matrix m="${ears_mass}" l="${ears_x}" w="${ears_y}" h="${ears_z}" />
</link>
<gazebo reference="${prefix}_ears_link">
<material>Gazebo/Orange</material>
</gazebo>
</xacro:macro>
<!-- Macro for robot eye -->
<xacro:macro name="eye" params="prefix reflect">
<joint name="${prefix}_eye_joint" type="fixed">
<origin xyz="${eye_joint_x} ${reflect*eye_joint_y} ${eye_joint_z}" rpy="0 0 0" />
<parent link="head_link"/>
<child link="${prefix}_eye_link" />
</joint>
<link name="${prefix}_eye_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${eye_radius}" />
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${eye_radius}" />
</geometry>
</collision>
<sphere_inertial_matrix m="${eye_mass}" r="${eye_radius}" />
</link>
<gazebo reference="${prefix}_eye_link">
<material>Gazebo/White</material>
</gazebo>
</xacro:macro>
<!-- Macro for robot eyeball -->
<xacro:macro name="eyeball" params="prefix">
<joint name="${prefix}_eyeball_link_joint" type="fixed">
<origin xyz="${eyeball_joint_x} 0 0" rpy="0 0 0" />
<parent link="${prefix}_eye_link"/>
<child link="${prefix}_eyeball_link" />
</joint>
<link name="${prefix}_eyeball_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${eyeball_radius}"/>
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${eyeball_radius}"/>
</geometry>
</collision>
<sphere_inertial_matrix m="${0.02}" r="${eyeball_radius}" />
</link>
<gazebo reference="${prefix}_eyeball_link">
<material>Gazebo/Black</material>
</gazebo>
</xacro:macro>
<!-- Macro for robot -->
<xacro:macro name="hulu_body_gazebo">
<!-- shadow , odom reference resources-->
<link name="body_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="body_footprint">
<!-- cutdown gravuity -->
<turnGravityOff>false</turnGravityOff>
</gazebo>
<!-- body joint -->
<joint name="body_footprint_joint" type="fixed">
<origin xyz="0 0 ${body_length/2 + caster_radius*2}" rpy="0 0 0" />
<parent link="body_footprint"/>
<child link="body_link" />
</joint>
<!-- body -->
<link name="body_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${body_radius}"/>
</geometry>
<material name="orange" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${body_radius}"/>
</geometry>
</collision>
<sphere_inertial_matrix m="${body_mass}" r="${body_radius}" />
</link>
<gazebo reference="body_link">
<material>Gazebo/RedBright</material>
</gazebo>
<joint name="body_link_joint" type="fixed">
<origin xyz="0 0 0.28" rpy="0 0 0" />
<parent link="body_link"/>
<child link="head_link" />
</joint>
<!-- head -->
<link name="head_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${head_radius}"/>
</geometry>
<material name="orange" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${head_radius}"/>
</geometry>
</collision>
<sphere_inertial_matrix m="${head_mass}" r="${head_radius}" />
</link>
<gazebo reference="head_link">
<material>Gazebo/RedBright</material>
</gazebo>
<joint name="hand_link_joint" type="fixed">
<origin xyz="0 -0.19 0.1" rpy="0 1.4 -1.5705" />
<parent link="body_link"/>
<child link="hand_link" />
</joint>
<!-- hand -->
<link name="hand_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.02" length="0.3"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.02" length="0.3"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="0.05" r="0.02" h="0.3"/>
</link>
<gazebo reference="hand_link">
<material>Gazebo/RedBright</material>
</gazebo>
<joint name="palm_link_joint" type="fixed">
<origin xyz="0 0 0.2" rpy="1.75 0 1.5705" />
<parent link="hand_link"/>
<child link="palm_link" />
</joint>
<!-- palm -->
<link name="palm_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.01"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.01"/>
</geometry>
</collision>
<box_inertial_matrix m="0.1" l="0.1" w="0.1" h="0.01" />
</link>
<gazebo reference="palm_link">
<material>Gazebo/RedBright</material>
</gazebo>
<wheel prefix="left" reflect="-1"/>
<wheel prefix="right" reflect="1"/>
<caster prefix="front" reflect="-1"/>
<caster prefix="back" reflect="1"/>
<eye prefix="left" reflect="-1" />
<eye prefix="right" reflect="1" />
<ears prefix="left" reflect="-1" />
<ears prefix="right" reflect="1" />
<eyeball prefix="left"/>
<eyeball prefix="right"/>
<!-- 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>body_footprint</robotBaseFrame>
</plugin>
</gazebo>
</xacro:macro>
</robot>
(2)机器人传感器的模型
- 照相机 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.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/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>
- 3D体感摄影机 kinect 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://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>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>
- 激光雷达 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.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/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>
(3)机器人主体添加传感器后的模型 hulu_with_sensors.xacro
<?xml version="1.0"?>
<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" />
<!-- Camera -->
<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"/>
<!-- lidar -->
<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"/>
<!-- kinect -->
<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)" />
<!-- 设置GUI参数,显示关节控制插件 -->
<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>
<!-- 运行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 hulu_gazebo)/config/hulu_arbotix.rviz" required="true" />
</launch>
(6)创建在gazebo进行仿真的launch文件 view_hulu_with__gazebo.launch
<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"/>
<!-- 运行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 hulu_gazebo)/urdf/xacro/gazebo/hulu_with_sensors.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 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