基于ArbotiX和rviz控制前轮转向后轮驱动的SmartCar_xacro模型
基于ArbotiX和rviz控制前轮转向后轮驱动的四轮小车xacro模型
- 前言
- 一、前轮转向后轮驱动的SmartCar_URDF模型
-
-
- 1、创建SmartCar工作空间
- 2、创建SmartCar代码包
- 3、创建SmartCar模型描述文件
- 4、创建描述文件的launch文件夹
- 5、编译工程
- 6、模型在rviz显示
-
- 二、前轮转向后轮驱动的四轮小车改进 URDF 模型
-
-
- 1、准备工作
- 2、创建配置文件
- 3、优化urdf模型文件
- 4、创建描述文件的launch文件夹
- 5、发布速度命令
-
- 三、Gazebo仿真小车运动
-
-
- 1、urdf文件讲解
- 2、修改urdf文件
- 3、创建启动文件
- 4、启动仿真
- 5、查看摄像头的图
- 6、SmartCar运动起来
-
- 四、基于ArbotiX和rviz控制小车
-
-
- 1、创建控制包
- 2、加入键盘控制
-
前言
首先感谢古月居大大的资料，学习ROS必看！！！
本文是基于ubuntu 16.04 kinetic版创建的模型
源码获取：链接
一、前轮转向后轮驱动的SmartCar_URDF模型
1、创建SmartCar工作空间
创建工作目录,工作空间为SmartCar_ws,资源文件夹src
mkdir -p ~/SmartCar_ws/src
初始化工作空间
cd ~/SmartCar_ws/src
catkin_init_workspace
2、创建SmartCar代码包
创建SmartCar包
cd ~/SmartCar_ws/src
catkin_create_pkg smartcar rospy geometry_msgs
3、创建SmartCar模型描述文件
创建SmartCar模型描述文件夹smartcar_description
cd ~/SmartCar_ws/src
roscreate-pkg smartcar_description urdf
在模型描述文件夹下创建SmartCar的描述文件SmartCar.urdf
cd ~/SmartCar_ws/src/smartcar_description
mkdir urdf
cd ~/SmartCar_ws/src/smartcar_description/urdf
gedit SmartCar.urdf
附上代码：
<?xml version="1.0"?>
<robot name="smartcar">
<link name="base_link">
<visual>
<geometry>
<box size="0.25 .16 .05"/>
</geometry>
<origin rpy="0 0 1.57075" xyz="0 0 0"/>
<material name="blue">
<color rgba="0 .5 .8 1"/>
</material>
</visual>
</link>
<link name="right_front_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="right_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0.08 0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_back_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="right_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0.08 -0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_front_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="left_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="-0.08 0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_back_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="left_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="-0.08 -0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="head">
<visual>
<geometry>
<box size=".02 .03 .03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="tobox" type="fixed">
<parent link="base_link"/>
<child link="head"/>
<origin xyz="0 0.08 0.025"/>
</joint>
</robot>
建立模型如下图：
URDF 提供了一些命令行工具，可以帮助我们检查、梳理模型文件，需要在终端中独立
安装：
sudo apt-get install liburdfdom-tools
然后使用 check_urdf 命令对SmartCar.urdf 文件进行检查：
cd ~/SmartCar_ws/src/smartcar_description/urdf
check_urdf check_urdf SmartCar.urdf
在终端中生成如下信息：
robot name is: smartcar
---------- Successfully Parsed XML ---------------
root Link: base_link has 5 child(ren)
child(1): left_back_wheel
child(2): left_front_wheel
child(3): right_back_wheel
child(4): right_front_wheel
child(5): camera_link
还可以使用 urdf_to_graphiz 命令查看 URDF 模型的整体结构：
urdf_to_graphiz SmartCar.urdf
执行 urdf_to_graphiz 命令后，会在当前目录下生成一个 pdf 文件，打开该文件，可以看
到模型的整体结构图
4、创建描述文件的launch文件夹
cd ~/SmartCar_ws/src/smartcar_description
mkdir launch
cd ~/SmartCar_ws/src/smartcar_description/launch
gedit base.urdf.rviz.launch
附上代码：
<launch>
<param name="smartcar_description" textfile="$(find smartcar_description)/urdf/SmartCar.urdf" />
<!-- 设置 GUI 参数，显示关节控制插件 -->
<param name="use_gui" value="true"/>
<!-- 运行 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="state_publisher" />
<!-- 运行 rviz 可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/config/mrobot_urdf.rviz" required="true" />
</launch>
5、编译工程
cd ~/SmartCar_ws
catkin_make
source环境变量
source ./devel/setup.bash
6、模型在rviz显示
运行launch文件
roslaunch smartcar_description base.urdf.rviz.launch
修改Fixed Frame为base_link
点击左下角的Add添加我们想看的控件选择RobotModel点击OK
模型在rviz显示结果
二、前轮转向后轮驱动的四轮小车改进 URDF 模型
1、准备工作
安装arbotix功能包，用于发布速度命令并执行
sudo apt-get install ros-kinetic-arbotix-*
或者下载源码
git clone https://github.com/vanadiumlabs/arbotix_ros.git
安装joint-state-publisher发布器，可以控制速度
sudo apt-get install ros-kinetic-joint-state-publisher-gui
2、创建配置文件
创建配置文件夹config
cd ~/SmartCar_ws/src/smartcar_description
mkdir config
创建配置文件
cd config
sudo gedit smartcar_arbotix.yaml
附上代码：
port: /dev/ttyUSB0
baud: 115200
rate: 20
sync_write: True
sync_read: True
read_rate: 20
write_rate: 20
controllers: {
# Pololu motors: 1856 cpr = 0.3888105m travel = 4773 ticks per meter (empirical: 4100)
base_controller: {
type: diff_controller, base_frame_id: base_link, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
}
注：这个配置文件和odom坐标系有关，但在这里无影响
3、优化urdf模型文件
gazebo对urdf的文件支持不如xacro文件，所以将上面写的urdf改成xacro
cd ~/SmartCar_ws/src/smartcar_description/urdf
gedit gazebo.urdf.xacro
gedit smartcar.urdf.xacro
gedit smartcar_body.urdf.xacro
附上代码：
gazebo.urdf.xacro：
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:xacro="http://ros.org/wiki/xacro"
name="smartcar_gazebo">
<!-- ASUS Xtion PRO camera for simulation -->
<!-- gazebo_ros_wge100 plugin is in kt2_gazebo_plugins package -->
<xacro:macro name="smartcar_sim">
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="right_front_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="right_back_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="left_front_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="left_back_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="head">
<material>Gazebo/White</material>
</gazebo>
</xacro:macro>
</robot>
smartcar.urdf.xacro:
<?xml version="1.0"?>
<robot name="smartcar"
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
xmlns:xacro="http://ros.org/wiki/xacro">
<include filename="$(find smartcar_description)/urdf/smartcar_body.urdf.xacro" />
<!-- Body of SmartCar, with plates, standoffs and Create (including sim sensors) -->
<smartcar_body/>
<smartcar_sim/>
</robot>
smartcar_body.urdf.xacro:
<?xml version="1.0"?>
<robot name="smartcar" xmlns:xacro="http://ros.org/wiki/xacro">
<property name="M_PI" value="3.14159"/>
<!-- Macro for SmartCar body. Including Gazebo extensions, but does not include Kinect -->
<include filename="$(find smartcar_description)/urdf/gazebo.urdf.xacro"/>
<property name="base_x" value="0.33" />
<property name="base_y" value="0.33" />
<xacro:macro name="smartcar_body">
<link name="base_link">
<inertial>
<origin xyz="0 0 0.055"/>
<mass value="100.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<box size="0.25 .16 .05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.055"/>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.055"/>
<geometry>
<box size="0.25 .16 .05" />
</geometry>
</collision>
</link>
<link name="left_front_wheel">
<inertial>
<origin xyz="0.08 0.08 0.025"/>
<mass value="1" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="left_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_front_wheel"/>
<origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_front_wheel">
<inertial>
<origin xyz="0.08 -0.08 0.025"/>
<mass value="1" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="right_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_front_wheel"/>
<origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_back_wheel">
<inertial>
<origin xyz="-0.08 0.08 0.025"/>
<mass value="1" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="left_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_back_wheel"/>
<origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_back_wheel">
<inertial>
<origin xyz="-0.08 -0.08 0.025"/>
<mass value="1" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="right_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_back_wheel"/>
<origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="head">
<inertial>
<origin xyz="0.08 0 0.08"/>
<mass value="1" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<geometry>
<box size=".02 .03 .03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0.08 0 0.08"/>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="tobox" type="fixed">
<parent link="base_link"/>
<child link="head"/>
<origin xyz="0.08 0 0.08"/>
</joint>
</xacro:macro>
</robot>
4、创建描述文件的launch文件夹
cd ~/SmartCar_ws/src/smartcar_description/launch
gedit smartcar_display.rviz.launch
附上代码：
<launch>
<param name="/use_sim_time" value="false" />
<!-- Load the URDF/Xacro model of our robot -->
<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" />
<arg name="gui" default="false" />
<param name="robot_description" command="$(arg urdf_file)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" type="double" value="20.0" />
</node>
<!-- We need a static transforms for the wheels -->
<node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" />
<node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/urdf.rviz" />
</launch>
在rviz中的配置如第一节内容一致。
5、发布速度命令
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'
前面三个参数是x,y,z三个方向的速度，后面三个参数是绕x,y,z三个轴旋转的角速度，上述命令就是让小车以0.5m/s的线速度，0.5rad/s的角速度画圆。
在GAZEBO中显示转圈
三、Gazebo仿真小车运动
将模型导入到rviz中并成功的动起来了，但是rviz软件环境主要是用于数据可视化的，处理数据比较方便，真实环境的模型仿真还是需要在gazebo中进行。
1、urdf文件讲解
标签xml
说明：用于xml文件版本声明
参数：version，声明版本
例子：
成对：否
标签robot
说明：声明整体机器人，要包裹将所有机器人组件的标签。
参数：name，用于声明机器人名字
成对：是
例子：
标签link
说明：声明连杆，可以在中设置连杆的可视化，惯性，碰撞属性
参数：name，用于声明连杆的名字，每个连杆必须有一个名字
成对：是
例子：
标签visual
说明：声明可视化属性，放置在link标签中
参数：无
成对：是
例子：
标签 geometry
说明：声明形状，放置在visual标签中
参数：
成对：是
例子：
标签 origin
说明：声明放置位置已经朝向，放置在visual标签中
参数：rpy,声明朝向；xyz，声明位置
成对：是
例子：
标签material
说明：声明材料属性，主要用于颜色，放置在visual标签中
参数：name，声明名字
成对：是
例子：
gazebo属性讲解
上述的标签是模型搭建的基本，可以看到主要创建了模型的形状、大小、颜色、连接关系等，这也就足够在rviz中动起来了，但是在gazebo中还需要配置模型相应的物理属性，如：质量、转动惯量、材质、碰撞等。
标签collision
说明：声明碰撞属性，只需要在里面加上对象的大小即可
参数：无
成对：是
例子：
标签inertial
说明：声明对象的质量，转动惯量
参数：,声明质量；,声明转动惯量矩阵
成对：是
例子：
gazebo标签下的material
说明：声明对象的质量，转动惯量
参数：reference，将对象的颜色与gazebo关联起来
成对：是
例子：
Gazebo/Yellow
gazebo标签下的plugin
说明：声明对象的驱动方式
参数：较多，这里不写了见注释
成对：是
例子：
left_back_wheel_joint
right_back_wheel_joint
base_link
0.14
0.05
true
true
gazebo标签下的sensor
说明：声明传感器
参数：参数较多，见注释
成对：是
例子：
30.0
1.3962634
1280
720
R8G8B8
0.02
300
gaussian
0.0
0.007
true
0.0
/camera
image_raw
camera_info
camera_link
0.07
0.0
0.0
0.0
0.0
0.0
2、修改urdf文件
把之前的SmartCar.urdf文件修改成支持gazebo的文件，代码如下：
<?xml version="1.0"?>
<robot name="smartcar">
<link name="base_link">
<visual>
<geometry>
<box size="0.25 .16 .05"/>
</geometry>
<origin rpy="0 0 1.57075" xyz="0 0 0"/>
<material name="blue">
<color rgba="0 .5 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.25 .16 .05"/>
</geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.0054" iyy="0.0073" izz="0.0023" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<gazebo reference="base_link">
<material>Gazebo/Yellow</material>
</gazebo>
<link name="right_front_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0000189" iyy=".0000189583" izz="0.00003125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<joint name="right_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="0.08 0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<gazebo reference="right_front_wheel">
<material>Gazebo/Black</material>
</gazebo>
<link name="right_back_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0000189" iyy=".0000189583" izz="0.00003125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<joint name="right_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="0.08 -0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<gazebo reference="right_back_wheel">
<material>Gazebo/Black</material>
</gazebo>
<link name="left_front_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0000189" iyy=".0000189583" izz="0.00003125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<joint name="left_front_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_front_wheel"/>
<origin rpy="0 1.57075 0" xyz="-0.08 0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<gazebo reference="left_front_wheel">
<material>Gazebo/Black</material>
</gazebo>
<link name="left_back_wheel">
<visual>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".02" radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.0000189" iyy=".0000189583" izz="0.00003125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<joint name="left_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_back_wheel"/>
<origin rpy="0 1.57075 0" xyz="-0.08 -0.1 -0.03"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<gazebo reference="left_back_wheel">
<material>Gazebo/Black</material>
</gazebo>
<link name="camera_link">
<visual>
<geometry>
<box size=".02 .03 .03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.02 .03 .03"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.000010833" iyy="0.000010833" izz="0.000015" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<joint name="tobox" type="fixed">
<parent link="base_link"/>
<child link="camera_link"/>
<origin xyz="0 0.08 0.025" rpy="0 0 1.57"/>
</joint>
<gazebo reference="camera">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<leftJoint>left_back_wheel_joint</leftJoint>
<rightJoint>right_back_wheel_joint</rightJoint>
<robotBaseFrame>base_link</robotBaseFrame>
<wheelSeparation>0.14</wheelSeparation>
<wheelDiameter>0.05</wheelDiameter>
<legacyMode>true</legacyMode>
<publishWheelJointState>true</publishWheelJointState>
</plugin>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<leftJoint>left_front_wheel_joint</leftJoint>
<rightJoint>right_front_wheel_joint</rightJoint>
<robotBaseFrame>base_link</robotBaseFrame>
<wheelSeparation>0.14</wheelSeparation>
<wheelDiameter>0.05</wheelDiameter>
<legacyMode>true</legacyMode>
<publishWheelJointState>true</publishWheelJointState>
</plugin>
</gazebo>
<gazebo reference="camera_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>
</robot>
3、创建启动文件
创建smartcar_display_gazebo.launch文件
cd ~/SmartCar_ws/src/smartcar_description/launch
gedit smartcar_display_gazebo.launch
代码如下：
<launch>
<!-- 设置launch文件的参数 -->
<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="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 smartcar_description)/urdf/SmartCar.urdf'"/>
<!--运行joint_state_publisher节点，发布机器人关节状态-->
<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "state_publisher">
<param name="publish_frequency" type="double" value="20.0" />
</node>
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model shcrobot -param robot_description"/>
</launch>
4、启动仿真
cd ~/SmartCar_ws
source ./devel/setup.bash
roslaunch smartcar_description smartcar_display_gazebo.launch
5、查看摄像头的图
从gazebo上拖入一个球：
打开新的终端输入：
rqt_image_view
然后选择/camera/image_raw
6、SmartCar运动起来
执行下面语句即可使得小车运动起来
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 5, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'
四、基于ArbotiX和rviz控制小车
基于ArbotiX和rviz，通过键盘来控制SmartCar小车的移动
1、创建控制包
为键盘控制单独建立一个包
cd ~/SmartCar_ws/src
roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
rosmake
2、加入键盘控制
在src目录下添加teleop.py
控制代码如下：
#!/usr/bin/env python
# -*- coding: utf-8 -*
import os
import sys
import tty, termios
import roslib; roslib.load_manifest('smartcar_teleop')
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
# 全局变量
cmd = Twist()
pub = rospy.Publisher('cmd_vel', Twist)
def keyboardLoop():
#初始化
rospy.init_node('smartcar_teleop')
rate = rospy.Rate(rospy.get_param('~hz', 1))
#速度变量
walk_vel_ = rospy.get_param('walk_vel', 0.5)
run_vel_ = rospy.get_param('run_vel', 1.0)
yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
max_tv = walk_vel_
max_rv = yaw_rate_
#显示提示信息
print "Reading from keyboard"
print "Use WASD keys to control the robot"
print "Press Caps to move faster"
print "Press q to quit"
#读取按键循环
while not rospy.is_shutdown():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
#不产生回显效果
old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO
try :
tty.setraw( fd )
ch = sys.stdin.read( 1 )
finally :
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
if ch == 'w':
max_tv = walk_vel_
speed = 1
turn = 0
elif ch == 's':
max_tv = walk_vel_
speed = -1
turn = 0
elif ch == 'a':
max_rv = yaw_rate_
speed = 0
turn = 1
elif ch == 'd':
max_rv = yaw_rate_
speed = 0
turn = -1
elif ch == 'W':
max_tv = run_vel_
speed = 1
turn = 0
elif ch == 'S':
max_tv = run_vel_
speed = -1
turn = 0
elif ch == 'A':
max_rv = yaw_rate_run_
speed = 0
turn = 1
elif ch == 'D':
max_rv = yaw_rate_run_
speed = 0
turn = -1
elif ch == 'q':
exit()
else:
max_tv = walk_vel_
max_rv = yaw_rate_
speed = 0
turn = 0
#发送消息
cmd.linear.x = speed * max_tv;
cmd.angular.z = turn * max_rv;
pub.publish(cmd)
rate.sleep()
#停止机器人
stop_robot();
def stop_robot():
cmd.linear.x = 0.0
cmd.angular.z = 0.0
pub.publish(cmd)
if __name__ == '__main__':
try:
keyboardLoop()
except rospy.ROSInterruptException:
pass
python程序，由于不需要编译所以直接把.py文件建好即可运行。
有时候会出现py文件没法执行的错误，一般来说是执行权限的问题，在py文件所在的位置打开终端，chmod +x XXX.py将.py文件权限更改为允许作为程序执行文件，或者右键属性。
具体操作：
1、打开一个终端
cd SmartCar_ws/src/smartcar_description/launch
roslaunch smartcar_display_gazebo.launch
出现gazebo界面
2、打开新的终端
cd SmartCar_ws
rosrun smartcar_teleop teleop.py
3、选中第二个终端，然后wasd控制小车，按住shift使用WASD可以跑得更快，使用Caps Lock切换大小写也可。
