一、实现过程
1、 从零开始仿真ROS小车(一)urdf模型+rviz可视化
2、从零开始仿真ROS小车(二)在gazebo中显示小车模型
3、从零开始仿真ROSR小车(三)运动控制、激光雷达仿真、深度相机仿真
4、从零开始仿真ROS小车(四)建图与导航
二、实现效果及包含文件
蓝色的相机和黑色的激光雷达没有,我这是已经加好的,我会在之后的文章中写
三、优化xacro模型
在手写urdf模型+rviz可视化基础上,我以方形全向轮车为基础,进行功能实现,xacro模型其实就是调用全局变量并结合函数
原理:
Xacro 可以声明变量,可以通过数学运算求解,使用流程控制控制执行顺序,还可以通过类似函数的实现,封装固定的逻辑,将逻辑中需要的可变的数据以参数的方式暴露出去,从而提高代码复用率以及程序的安全性。
四、jubot_box.xacro
讲解关键代码,全部代码可以在https://github.com/Grizi-ju
上下载
<!--wheel-->
<xacro:property name="wheel_radius" value="0.13" />
<xacro:property name="wheel_length" value="0.04" />
<xacro:property name="wheel_mass" value="0.05" />
<xacro:property name="PI" value="3.1415927" />
<xacro:macro name="wheel_func" params="wheel_name flag1 flag2">
<joint name="${wheel_name}_joint" type="continuous">
<origin xyz="${0.20*flag2} ${0.298*flag1} 0.07" rpy="${PI/2} 0.0 0.0"/>
<parent link="base_link"/>
<child link="${wheel_name}_link"/>
<axis xyz="0 0 1"/>
</joint>
<link name="${wheel_name}_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}"/>
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
</collision>
<xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${wheel_name}_link">
<material>Gazebo/Black</material>
</gazebo>
</xacro:macro>
<xacro:wheel_func wheel_name="left_back" flag1="1" flag2="-1"/>
<xacro:wheel_func wheel_name="right_back" flag1="-1" flag2="-1"/>
<xacro:wheel_func wheel_name="left_front" flag1="1" flag2="1"/>
<xacro:wheel_func wheel_name="right_front" flag1="-1" flag2="1"/>
以车轮为例
1、将轮子半径radius、长length、质量mass进行属性封装,在下面进行调用
2、写macro宏,参数为轮子名和用于±1的flag1、2,并在最后调用宏,这样就只需要写一次代码,实现4个轮子模型,不会显得冗余
3、为link添加碰撞参数、惯性矩阵、gazebo中的颜色设置
小车其他部分
1、举一反三,其他link、joint也进行一下属性封装
2、轮子、支架、这类多个的进行宏调用
3、所有link部分添加collision、inertial_matrix、gazebo material
五、head.xacro
我把惯性矩阵部分另写了一个头文件,分别包含球体、圆柱体、方体,只需要在jubot_box.xacro里引用就行了,添加一行代码:<xacro:include filename="head.xacro"/>
<robot name="base" xmlns:xacro="http://wiki.ros.org/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="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>
<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>
</robot>
六、display_jubot_box_urdf.launch
这是一个在rviz当中显示机器人模型的launch文件,文件名可以自己修改
<launch>
<!-- 设置机器人模型路径参数 -->
<param name="robot_description" command="$(find xacro)/xacro $(find jubot_demo)/urdf_box/jubot_box.xacro" />
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!-- 运行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 jubot_demo)/config/jubot_box_urdf.rviz" required="true" />
</launch>
七、gazebo_box.launch
这是在gazebo中显示机器人模型的文件
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find jubot_demo)/urdf_box/jubot_box.xacro" />
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>
八、Gazebo仿真环境搭建
之前我们用的是gazebo空白世界,现在我们搭建下自己想要的障碍物环境
1、打开gazebo终端直接输入gazebo
2、左上角edit->building editor
3、拖拉左边的组件到白色区域,开始创建
4、创建好后file->save world as 以.world格式保存在功能包下,最好新建个world文件夹,再在model中打开文件,添加障碍物模型
5、添加gazebo_box.launch文件中的地图路径,改为自己保存地图位置的路径:
<include file="$(find gazebo_ros)/launch/empty_world.launch" >
<arg name="world_name" value="$(find jubot_gazebo)/world/robot_room.world"/>
</include>
小车在gazebo中的显示模型:
上一篇:从零开始仿真ROS小车(一)urdf模型+rviz可视化
下一篇:从零开始仿真ROS小车(三)运动控制、激光雷达仿真、深度相机仿真
评论(0)
您还未登录,请登录后发表或查看评论