一步一步制作自己的ROS仿真机器人-键盘+手柄+App控制
第一步：create ROS package
>mkdir-p ~/catkin_ws/src
>cd ~/catkin_ws/src
>catkin_init_workspace
>cd ..
>catkin_make
>source devel/setup.bash
>catkin_create_pkg ros_robotic
hou@houROS:~/catkin_ws/src $ catkin_create_pkg ros_robotic
Created file ros_robotic/CMakeLists.txt
Created file ros_robotic/package.xml
Successfully created files in /home/hou/catkin_ws/src/ros_robotic. Please adjust the values in package.xml.
第二步： 创建差速轮URDF
URDF is an XML format specifically defined to represent robot models down to their
component level. These URDF files can become long and cumbersome on complex
robot systems. Xacro (XML Macros) is an XML macro language created to make
these robot description files easier to read and maintain. Xacro helps you reduce
the duplication of information within the file.
我们将“增量式“的来建立我们的机器人，并在RVIZ里面进行显示。
1. 建立ros_robotic下面的urdf文件夹
hou@houROS:~/catkin_ws/src $ cd ros_robotic/
hou@houROS:~/catkin_ws/src/ros_robotic $ mkdir urdf
hou@houROS:~/catkin_ws/src/ros_robotic $ cd urdf/
hou@houROS:~/catkin_ws/src/ros_robotic/urdf
2. 建立底盘模型(robot chassis)urdf文件
vim dd_robot.urdf
<?xml version="1.0"?>
<robot name ="dd_robot">
<!--Base link-->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0 "/>
<geometry>
<box size="0.5 0.4 0.25" />
</geometry>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
</link>
</robot>
这里建立一个一个长方体的模型，原点是在零点（0, 0, 0），0.5m的长度是均分在x方向，即正负方向的值是+0.25m, -0.25m，同理，y方向也是如此，从后面的图中你也可以发现，这个长方体的重心是在（0, 0, 0）上。使用check_urdf命令来检测模型文件的正确性
check_urdf dd_robot.urdf
得到如下结果
hou@houROS:~/catkin_ws/src/ros_robotic/urdf $ check_urdf dd_robot.urdf
robot name is: dd_robot
---------- Successfully Parsed XML ---------------
root Link: base_link has 0 child(ren)
模型正确，下面来写一个launch程序来把这个模型显示在rviz里面
3. 编写launch显示程序
在ros_robotic下新建一个launch文件夹，在文件夹内新建dd_rviz.launch文件，内容如下
<launch>
<!--Set these parameters on Parameter Server-->
<param name="robot_description"
textfile="$(find ros_robotic)/urdf/$(arg model)"/>
<!--Start 3 nodes: joint_state_publisher
robot_state_publisher and rviz-->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_robotic)/urdf.rviz" required="true"/>
</launch>
这个roslaunch 文件的主要作用有下面三个：
1 通过命令行输入的数据来导入详细模型到参数服务器
2 通过节点发布JointState和坐标变换
3 通过配置urdf.rviz来开启rviz可视化，这个urdf.rviz是打开了rviz后提示你保存的。
使用如下命令来启动rviz，查看机器人模型
roslaunch ros_robotic dd_rviz.launch model:=dd_robot.urdf
此时，不出意外，打开的是一个空白的rviz，因为一开始是没有urdf.rviz这个模型的
这个时候需要手动点击左下角的+号，然后选择RobotModel，这个时候，rviz里面才会有模型，如下
但是这里并没有显示颜色等信息，且左边的Display栏目还有有红色的错误提示，这个时候，继续点击add，添加TF，同时在Displays下面的Fixed frame输入base_link，出现如下所示
关闭rviz，点击保存，细心的你会发现在包ros_robotic下面有一个urdf.rviz，有了这个，下次执行roslaunch
roslaunch ros_robotic dd_rviz.launch model:=dd_robot.urdf
的时候，一开打就能看见这个robot_chasis。注意，这个的模型正中心就是在零点上。
4. 添加轮子
现在，让我们在机器人上添加车轮的shapes和links。 将links元素添加到URDF文件时，必须添加关节joint以描述links之间的关系。关节joint元素定义关节是灵活的（可移动的）还是不灵活的（固定的）。 对于挠性接头，URDF描述了接头的运动学和动力学以及其安全极限。 在URDF中，有六种可能的关节类型，如下所示：
fixed 固定的关节，不能移动 revolute 能够绕一个轴旋转，有角度限制 continuous 能够绕一个轴旋转，无角度限制 prismatic 滑动副 floating 浮动，无约束的 planar 可以在垂直于轴的平面内运动
很明显，我们的轮子和机体之间的关节是一个连续转动无角度限制的，也就是continuous，这个关节，实际上就是电机，因为实际中是电机带动轮子来转动的，就像下面图里一样。
在dd_robot.urdf文件里继续添加如下代码，添加电机和右侧轮子
<!--joint_right_wheel-->
<joint name="joint_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="-0.30 -0.1 0.0" rpy="0.0 1.570795 0.0"/>
<axis xyz="0.0 1 0.0"/>
</joint>
<!--right link-->
<link name="right_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0 "/>
<geometry>
<cylinder radius="0.2" length="0.1"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
</link>
效果如图
另外一点还是需要说一下，是rviz里面的方向，x y z都朝向什么方向得弄清楚，通过设置左边的透明值Alpha为0.5，可以发现base_link透明了,如下图。
在rviz中，默认红色代表x方向，绿色代表y方向，蓝色代表Z方向，记忆起来就是RGB-> XYZ（似乎solidworks里面也是，我们后面再讨论，现在这里埋下一个疑问）。轮的坐标其实是相对绿色的转动了90度，从代码上看，这是体现在轮的joint上面的，所以，轮的joint代码是这个样子：
<!--joint_right_wheel-->
<joint name="joint_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="-0.30 -0.1 0.0" rpy="0.0 1.570795 0.0"/>
<axis xyz="0.0 1 0.0"/>
</joint>
电机是连续转动的，所以joint 的type是continuous，parent link为底盘，这个joint 的子link连接着轮子，即child link="right_wheel"
轮的joint原点(即电机），在-x方向，0.30m处，-y方向0.1m出，z方向和中心一样，rpy="0.0 1.570795 0.0"，代表从y正方向看，绕y转动了90度，用弧度表示为1.570795，这里的r p y，是row, pitch, yaw的缩写，中文意思是滚转，俯仰，偏航。
这句话定义轮子的旋转轴，这里我选择了y轴，但是一看是有问题的，应该是相对蓝色的轴转动，这个我们在后面调整。
轮子的代码
<!--right link-->
<link name="right_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0 "/>
<geometry>
<cylinder radius="0.2" length="0.1"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
</link>
轮子为一个圆柱体cylinder，半径0.2，高度0.1，注意，在rviz里面默认的圆柱体是垂直地面创建的，为了验证，我们把dd_robot.urdf文件中的base_link和right_wheel_joint的代码都删掉，再在rviz中打开，看效果。
看看零点在什么位置。
和之前说的长方体一样，零点还是在正中心，你可以理解为重心的地方。这个地方你就理解了，为什么我把电机的joint放在-0.3的位置了，因为这样看起来轮子刚好是连在地盘上的，0.25+0.05刚好是旋转之后轮子重心的位置。
对于左边的电机和轮子，也是同样的分析道理，下面直接上代码和效果。
<!--joint_left_wheel-->
<joint name="joint_left_wheel" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0.30 -0.1 0.0" rpy="0.0 1.570795 0.0"/>
<axis xyz="0.0 1 0.0"/>
</joint>
<!--left link-->
<link name="left_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0 "/>
<geometry>
<cylinder radius="0.2" length="0.1"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
</link>
</robot>
运行roslaunch 命令
hou@houROS:~/catkin_ws $ roslaunch ros_robotic dd_rviz.launch model:=dd_robot.urdf
效果：
这样的轮子会倒，在前面加上一个球形万向轮，作为支撑，代码为：
<!--front_castor_joint-->
<joint name="front_castor_joint" type="fixed">
<parent link="base_link"/>
<child link="front_castor_link"/>
<origin xyz="0 0.1625 -0.1625" rpy="0.0 0.0 0.0"/>
<axis xyz="0.0 1 0.0"/>
</joint>
<!--front castor link-->
<link name="front_castor_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0 "/>
<geometry>
<sphere radius="0.0375"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
</link>
效果如图：
5. 添加碰撞及惯性参数
添加collision属性
接下来，我们将
<!--Base link-->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0 "/>
<geometry>
<box size="0.5 0.4 0.25" />
</geometry>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
<!--base link collsion-->
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.5 0.4 0.25" />
</geometry>
</collision>
</link>
注意，是在每个Link里面添加这个collision属性，由于本次模型简单，collison里面的尺寸参数和visual里面是一样的。
其他的link也是类似，这里就不再详细阐述，可以参阅最终的代码。
添加惯性inertial参数
惯性参数直接反映了机器人的重量以及实际运动学方面的特性，举个例子来说，大惯量的物体，启动起来就比较慢，这个你可以理解为灵活性的一种表示了。
<inertial>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.005" iyz="0.0" izz="0.005"/>
</inertial>
运行roslaunch ros_robotic dd_rviz.launch model:=dd_robot.urdf 效果如下
此时，可以通过滑动滑杆来操作轮子转动，我们来试一下。
尴尬，大型翻车现场，回到上文（飞机模型下面提到的）这个axis的设置有问题，我们看一下，当前两个轮子的旋转是
<!--joint_right_wheel-->
<joint name="joint_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="-0.30 -0.1 0.0" rpy="0.0 1.570795 0.0"/>
<axis xyz="0.0 1 0.0"/>
</joint>
-----
<!--joint_left_wheel-->
<joint name="joint_left_wheel" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0.30 -0.1 0.0" rpy="0.0 1.570795 0.0"/>
<axis xyz="0.0 1 0.0"/>
</joint>
将两个Joint的
<axis xyz="0.0 0 1.0"/>
再次运行查看效果
OK，就是这个效果，现在可以通过滑杆来让轮子转动了。既然轮子可以转动，那么机器人就可以在rviz里面运动了，
how?
6. 添加运动控制器
ArbotiX控制器可以用来控制差动轮，配合rviz搭建仿真环境，
1. 安装ArbotiX
在当前工作空间的src下面的文件夹内使用如下命令，将ArbotiX的源码从github上下载克隆到本地
git clone https://github.com/vanadiumlabs/arbotix_ros.git
下载成功后，回到工作空间根目录，使用catkin_make编译
2. 配置ArbotiX控制器
这里只需要创建一个启动ArbotiX的节点的launch文件即可
在launch文件夹下创建一个dd_robot_with_arbotix.launch
<launch>
<arg name="model"/>
<param name="/use_sim_time" value="false" />
<!--Set these parameters on Parameter Server-->
<param name="robot_description"
textfile="$(find ros_robotic)/urdf/$(arg model)"/>
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find ros_robotic)/config/fake_robot_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<!--Start 3 nodes: joint_state_publisher
robot_state_publisher and rviz-->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<param name="publish_frequency" type="double" value="20.0" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_robotic)/urdf.rviz" required="true"/>
</launch>
同时在ros_robotic包文件夹下，新建config目录，新建fake_robot_arbotix.yaml配置文件，内容为
controllers: {
base_controller: {type: diff_controller, base_frame_id: base_link, base_width: 0.5, ticks_meter: 400, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
}
运行launch文件
roslaunch ros_robotic dd_robot_with_arbotix.launch model:=dd_robot.urdf
效果如下，这个时候其实看不出什么效果，动起来才能看的见：
7. 使用键盘来控制机器人
安装teleop_twist_key功能包，
在src下面使用如下命令
git clone https://github.com/ros-teleop/teleop_twist_keyboard.git
然后，回到src的上级目录，执行
catkin_make
然后，source一下当前的工作空间
source devel/setup.bash
此时，运行teleop_twist_keyboard功能包节点
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
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%
CTRL-C to quit
currently: speed 0.5 turn 1.0
此时将两个窗口并排放置，如图
按动I键，动起来吧！
翻车again!
本来是前进的，结果横向移动了。why?
这里是方向错了，我们来解释。
第一，I键给的指令是x方向的速度，而我们机器人地盘上x方向是横向的，当i给一个x速度的时候，就平移了，现在是绿色的箭头（y方向）是地盘的正方向，其实，我们给y发速度就行了，看看
hou@houROS:~ $ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 1.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
看怎么走？
此时命令回车之后，显示
hou@houROS:~ $ $ rtopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 1.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
publishing and latching message. Press ctrl-C to terminate
并咩有动，我们后面有空研究咋回事。
这里，我将urdf里面的长宽改变过来.
<box size="0.5 0.4 0.25" />
改为
<box size="0.4 0.5 0.25" />
right joint和left joint改为
origin xyz="-0.1 -0.30 0.0" rpy="1.570795 0 0"/>
支撑轮改为
<origin xyz=" 0.1625 0 -0.1625" rpy="0.0 0.0 0.0"/>
OK，在rviz内重新加载
roslaunch ros_robotic dd_robot_with_arbotix.launch model:=dd_robot.urdf
动起来了，非常爽！
8. 使用游戏手柄来控制机器人
1.安装手柄驱动：
modprobe joydev
2.安装手柄测试软件：
sudo apt-get install joystick （joystick校准手柄）
3.测试手柄：
jstest /dev/input/js0
搞清楚对应的是那个按键
下载teleop_twist_joy功能包，在工作空间下编译，打开launch文件夹下的teleop.launch，修改
<launch>
<arg name="joy_config" default="beitong" />
<arg name="joy_dev" default="/dev/input/js0" />
<arg name="config_filepath" default="$(find teleop_twist_joy)/config/$(arg joy_config).config.yaml" />
<node pkg="joy" type="joy_node" name="joy_node">
<param name="dev" value="$(arg joy_dev)" />
<param name="deadzone" value="0.05" />
<param name="autorepeat_rate" value="0.0" />
</node>
<node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
<rosparam command="load" file="$(arg config_filepath)" />
</node>
</launch>
同时，在config文件夹下面的新建一个手柄配置文件，我这里使用的是北通手柄，这里新建一个beitong.config.yaml，内容为
axis_linear: 1
scale_linear: 1
scale_linear_turbo: 0
axis_angular: 0
scale_angular: 0.4
enable_button: 4
enable_turbo_button: 5
运行
roslaunch teleop_twist_joy teleop.launch
非常爽，这里用左边的摇杆来控制前进和转向，和我之前使用的无人机有点不同，这里让左边摇杆来控制前进，右边摇杆来控制转弯，只需要将配置文件beitong.config.yaml，更改成：
axis_linear: 1
scale_linear: 1
scale_linear_turbo: 0
axis_angular: 2
scale_angular: 0.4
enable_button: 4
enable_turbo_button: 5
OK，非常给力。
9. 使用手机app来控制
下载ros_control app，如图：
新建机器人，连接上当前电脑的ip地址，在高级设置里面设置好速度话题为/cmd_vel，如果你用来控制小乌龟的话，就把这个地方改成/turtle1/cmd_vel，进入界面，移动触干，小车动起来了，爽！
不过有个问题，同时使用遥控器和APP来控制，再将APP关闭之后，用手柄控制起来就显得有点卡顿了，具体原因再找。使用rosnode kill把APP节点干掉之后，不再卡顿。
rosnode kill /android/robot_controller
killing /android/robot_controller
killed
总 结
本文一步步搭建了自己的小车，并通过键盘、手柄、App来实现了控制，对urdf模型的建立、rviz的基础认识有了一个更进一步的体会。但是手写urdf真的是太费劲了，后面我将用solidworks和xacro来实现，同时也会将rviz里面的机器人搬到gazebo里面来跑。
很爽，奥里给！
