ROS Nonetic: 机械臂UR5 + RobotIQ FT300 传感器 配置

今天需要使用 ROBOTIQ FT300的力/力矩传感器，所以记录一下自己的配置情况。

环境： Ubuntu 20.04, ROS Noetic

机械臂：UR5 (关于如何通过序列号判断UR型号，可以参照这里。)

工作空间名: ~/ur5_ws/

前提：已经安装 fmauch_universal_robot 和 Universal_Robots_ROS_Driver 模型和驱动2个packages。具体安装细节可以参见这里。

操作步骤：

• Setp1: 下载robotiq的ros package

cd ~/ur5_ws/src
git clone https://github.com/jr-robotics/robotiq.git

Note: 因为https://github.com/ros-industrial/robotiq 中没有noetic release, 所以采用了上图中的git仓库。

• Setp2: 编译环境

cd ur5_ws
sudo apt update -qq
rosdep update
rosdep install --from-paths src --ignore-src -y
catkin_make
source ~/ur5_ws/devel/setup.zsh

Note: 作者使用oh-my-zsh，故最后source的文件后缀为“.zsh”. 一般情况，用“.bash”即可。

Note: 如果不需要在Gazebo仿真器中添加力传感器，则可以直接跳转到Step 7.

• Step3: 建立自己的package

        为了减小对fmauch_universal_robot 和 Universal_Robots_ROS_Driver源代码的修改，这里参考大家的做法，自己建立一个package，名为 myur5:

cd ~/ur5_ws/src
mkdir -p myur5
cd myur5
catkin_create_pkg myur5_description std_msgs rospy roscpp
catkin_create_pkg myur5_gazebo std_msgs rospy roscpp
catkin_create_pkg myur5_moveit_config std_msgs rospy roscpp
cd ~/ur5_ws
catkin_make
source ~/ur5_ws/devel/setup.zsh

Note: 这里是std_msgs是复数，有s. 不要问我怎么知道的，问就看这里。

之后把fmauch_universal_robot/ur_description包中的关于ur5的文件按照同样的文件夹结构复制一份到 myur5_description. 之后针对自己应用的修改都可以在myur5_description中的代码上操作。同理，fmauch_universal_robot/ur_gazebo 和 fmauch_universal_robot/ur5_moveit_config 中关于ur5的文件夹也需要复制一份到 myur5_gazebo 和 myur5_moveit_config.

Note: 不要复制fmauch_universal_robot/ur_description等中的CMakeLists.txt 和 package.xml.

 在myur5_xxx的3个文件中，可以把一些 find ur_description、find ur_gazebo、find ur5_moveit_config的部分修改为find myur5_description、find myur5_gazebo 、find myur5_moveit_config。否则系统还是调用fmauch_universal_robot中的原始代码。

测试一下：

 roslaunch myur5_description view_ur5.launch 

如何看到机械臂就表示基本没啥问题。这个代码后面可以用来对比安装FT300后的机械臂。

• Step4: 安装传感器

在 myur5_description/urdf中复制ur5.xacro，命名为ur5_robot_ft.xacro，并增加ft300的相关信息。

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur5_robot_ft">  
  <xacro:include filename="$(find myur5_description)/urdf/ur5.xacro"/>
  <xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro"/> 
  <!--robotiq_ft_sensor-->
  <xacro:robotiq_ft300 prefix="" parent="tool0">
      <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_ft300>
</robot>

Note:第3行，加载机械臂，第4行加载ft300，5-8行表征ft300和ur5的安装关系。

Note:  tool0是ur5机械臂的末端frame. ft300装在ur5末端。

•  Step5: launch文件修改

        在~/ur5_ws/src/myur5/myur5_description/launch中先后增加(复制+修改)3个files:         view_ur5_ft.launch，load_ur5_ft.launch，load_ur_ft.launch. 内容分别是如下。

        view_ur5_ft.launch

<?xml version="1.0"?>
<launch>
  <include file="$(find myur5_description)/launch/load_ur5_ft.launch"/>

  <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" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find myur5_description)/cfg/view_robot.rviz" required="true" />
</launch>

        load_ur5_ft.launch              

<?xml version="1.0"?>
<launch>
  <!--ur5 parameters files -->
  (.....无需修改......)
  <!--common parameters -->
  (.....无需修改......)

  <!-- use common launch file and pass all arguments to it -->
  <include file="$(find myur5_description)/launch/load_ur_ft.launch" pass_all_args="true"/>
</launch>

Note: 就是修改最后一行 “$(find myur5_description)/launch/load_ur_ft.launch”，让系统加载load_ur_ft.launch.

        load_ur_ft.launch

<?xml version="1.0"?>
<launch>
  <!--ur parameters files -->
  (.....无需修改.....)
  <!--common parameters  -->
  (.....无需修改.....)

  <param name="robot_description" command="$(find xacro)/xacro '$(find myur5_description)/urdf/ur5_robot_ft.xacro'
    (.....无需修改......)
    />
</launch>

Note: 修改为“$(find myur5_description)/urdf/ur5_robot_ft.xacro”，即让系统加载第4步中新增的ur5_robot_ft.xacro文件。 

• Step6: 最后效果

        改好后，打开末端，看一下效果，看看机械臂末端有没有多出ft300的模型。

roslaunch myur5_description view_ur5_ft.launch

        可以看到ft300

         再看看原先没有装ft300的机械臂样子，使用第3步中的代码

 roslaunch myur5_description view_ur5.launch 

        看到没有ft300.

• Step7: ft300和PC之间连接

        找到ft300的连接线，有个VGA接头，如下图

         通过参考FT300 Instruction Manual(2018),

        完成传感器和UR控制盒之间的连线        

        

        可以找到最末端的USB接口，连接到PC.

•  Step8: 读取ft300的数据

     0. 开放串口权限（开一个终端） 

# Linux串口是默认普通用户没有读写权限的
sudo chmod a+rw /dev/ttyUSB0

1. 开一个终端（Ctrl+Alt+T）(a.k.a. T1)：开启ros master

roscore

      2. 开一个终端（Ctrl+Alt+T）(a.k.a. T2): 开启rq_sensor node (路径：~/ur5_ws/src/robotiq/robotiq_ft_sensor/nodes)--这是publisher/talker, 往/robotiq_ft_sensor和/robotiq_ft_wrench两个topics发布信息。

rosrun robotiq_ft_sensor rq_sensor

        如果第7步连接没有问题，那么会看到如下的信息。 

$ rosrun robotiq_ft_sensor rq_sensor
[ INFO] [1650284097.356151697]: No device filename specified. Will attempt to discover Robotiq force torque sensor.
[ INFO] [1650284098.040120336]: Waiting for sensor connection...
[ INFO] [1650284099.182842730]: Sensor connected!
[ INFO] [1650284100.260359416]: Starting Sensor

     Note: 这个时候可以发现FT300的灯变成蓝色，表示连接正常，ready。

     3. 开一个终端（Ctrl+Alt+T）(a.k.a. T3): 查看topic实时数据

rostopic echo /robotiq_ft_sensor

    4. 开一个终端（Ctrl+Alt+T）(a.k.a. T4)，打开subscriber/listener.

rosrun robotiq_ft_sensor rq_test_sensor

        可以看到从/robotiq_ft_sensor中获得信息：

...
[ INFO] [1650286760.040227612]: I heard: FX[0.020000] FY[0.020000] FZ[-0.030000] MX[0.000000] MY[0.001000] MZ[0.000000]
[ INFO] [1650286760.052497302]: I heard: FX[0.010000] FY[0.040000] FZ[0.040000] MX[0.000000] MY[0.000000] MZ[0.000000]
[ INFO] [1650286760.068783576]: I heard: FX[0.040000] FY[0.040000] FZ[-0.030000] MX[-0.001000] MY[0.000000] MZ[0.000000]
...

P.S. 开一个终端（Ctrl+Alt+T） 查看当前active的topics

rostopic list

   可以看到当前active的topics,如下图

/robotiq_ft_sensor
/robotiq_ft_wrench
/rosout
/rosout_agg

• Step 9. 串口权限问题

在Step 8-0的时候手动开启了串口权限，但是每次开机都要再次开启，非常麻烦。

于是可以写一个脚本每次开机自动开启串口权限(参考：“三、串口权限问题 ” in page)。

---

References:

机械臂—ROS下UR10e+ft300+2f-140gripper ur机械臂配置夹爪及问题解决

ROS - UR3机械臂添加robotiq FT sensor和robotiq 140 2指夹爪

关于连接FT300后需要给串口设置权限的操作

ubuntu关于串口的操作