利用ros2 control获取gazebo中六维力传感器数据，使用force_torque_broadcaster

利用ros2 control获取gazebo中六维力传感器数据，使用force_torque_broadcaster

因为目前需要在仿真环境中模拟机械臂末端力传感器的数据，但是目前基于ros2的资料又比较少，笔者在经历数次尝试之后终于总结出一个不错的方案，供大家参考。

关于ROS2 CONTROL

首先让我们来看一下ros2 control的框架，最上面的一层controllers会计算系统的输入输出，比如说最熟悉的joint_trajectory_controller便是输出轨迹，最后通过硬件层来执行计算好的轨迹，这样就省下了我们很多的工作。笔者这里只讲force_torque_broadcaster，他的作用是将传感器的数据广播出去。

一般步骤

首先是对URDF文件进行编写

笔者这里假设你已经有了一个完整的URDF文件，并且可以在RVIZ2中显示出来，如果不知道步骤的话可以参考鱼香ros的教程:《动手学ROS2进阶篇》8.2RVIZ2可视化移动机器人模型
笔者使用的是sw2urdf插件将solidworks里机械臂的模型转换为了urdf文件，并且进行重新的编写和改进，转换成xacro文件，其中有个问题需要注意，对于mesh文件（dae、stl），gazebo那边识别不了 package://这种ros系统的路径查找方式，只能用 $(find xxx)这种方式找到绝对路径并替换。但是，rviz这边又不认这种绝对路径，解决办法可以看这里ROS2中，从SolidWorks导出的urdf，联合moveit、gazebo进行控制及仿真
如果出现零件乱飞或者抽搐的情况，多半是因为惯性参数（inertial）没有编写到位，因为sw2urdf是有bug的，惯性参数会导出错误，这时就需要我们重新赋予零件惯性参数，笔者这里使用meshlab重新计算惯性参数，如果不会使用可以看这个V-rep中机械臂惯性参数的获取方法
这样下来你应该就可以获得一份可以在gazebo中显示的urdf文件了，下面要做的便是笔者的重点

如何添加ros2 control的tag？

如果要使用ros2 control首先要在urdf文件中声明你需要的command interface和state interface，添加形式如下所式，在你的urdf文件中<robot的tag里添加ros2 control的代码块，因为我们要使用的是gazebo，所以这里需要引入gazebo的插件。在harware里joint自不不必多说，是关节的控制，我们主要需要的是sensor的设置，如下所示

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position" />
      <state_interface name="velocity" />
      <state_interface name="effort" />
    </joint>
    <sensor name="ft_sensor">
      <state_interface name="force.x" />
      <state_interface name="force.y" />
      <state_interface name="force.z" />
      <state_interface name="torque.x" />
      <state_interface name="torque.y" />
      <state_interface name="torque.z" />
      <param name="frame_id">ft_sensor_link</param>
      <param name="fx_range">100</param>
      <param name="fy_range">100</param>
      <param name="fz_range">100</param>
      <param name="tx_range">15</param>
      <param name="ty_range">15</param>
      <param name="tz_range">15</param>
    </sensor>
  </ros2_control>

然后将我们的yaml文件作为参数引入进去

这里的路径一定要改成你自己的yaml文件

<gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <parameters>$(find rokae_bringup)/config/rokae_ros2_controller.yaml</parameters>
    </plugin>
  </gazebo>

最后编写launch文件

这样你就可以成功使用ros2control获得gazebo六维力传感器信息了，本教程到此结束。
不，骗你的。
接下来才是最难的部分

重新修改URDF文件

不出意外的话当你完成了以上任务，你的gazebo可以显示模型并且ros2 control已经可以正常使用了，如下图但是当我们使用 ros2 control list_controllers的时候会发现传感器的controller压根没有启动笔者同时也发现在gazebo启动终端中也会发生warning，但是不会报错
打开rqt也获取不到任何信息
这里笔者查阅了一些资料发现是sensor没有定义好，于是在csdn上进行搜索如何在gazebo定义六维力传感器，大部分教程会告诉你要设置plugin name=“ft_sensor” filename="libgazebo_ros_ft_sensor.so"代码块，可是这样是通过libgazebo_ros_ft_sensor.so插件的方式将六维力传感器的数据以topic形式发送出来，那么如何使用ros2 control的方式看到呢。我们可以在终端中看到端倪，ros2control 并没有找到我们的六维力传感器ft_sensor，所以是我们的六维力传感器没有定义好，翻看gazebo官网的教程Force/Torque Sensor
我们从这里可以看出来force torque sensor是在关节里面的，必须要对关节定义好，定义的地方是关节(joint)而不是连杆(link)。由于最后和传感器相连的关节类型是fixed，gazebo那边不可以使用这种类型的关节作为六维力传感器，所以添加以下代码

<gazebo
    reference="joint_sensor">
    <provideFeedback>true</provideFeedback>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>

其中joint_sensor是你定义的连接六维力传感器的关节
然后对其添加六维力传感器

<gazebo reference="joint_sensor">
    <gravity>true</gravity>
    <sensor name="ft_sensor" type="force_torque">
      <always_on>true</always_on>
      <visualize>true</visualize>
    </sensor>
  </gazebo>

这样就可以使用ros2 control在gazebo里添加六维力传感器了。
我们需要注意以下几点：
大部分教程告诉你要添加
plugin name=“ft_sensor” filename="libgazebo_ros_ft_sensor.so"代码块
不要添加，首先是其参数在ros2中有所变化，其次是他会和force_torque_broadcaster这个controller产生冲突，无法正确捕获到传感器。
最终可以显示的形似如下所示，其在gazebo里面可以正确显示而且在rqt中也可以看到ros2 control的数据
完结撒花
还有一点，csdn上的vip文章能不能少一点，找资料真的很难找啊！