【ROS2】机械臂抓取——gazebo_grasp_plugin配置与踩坑

一脚踩到坑里，两天才出来（悲
如文章内容有误，欢迎在评论内讨论与指正。

---

环境配置：
ubuntu版本：22.04
ros版本：ros2 humble

功能包下载

ros2-humble版本gazebo_grasp_plugin github下载链接：JenniferBuehler/gazebo-pkgs

tips：这个分支不知道为什么没有被完全并到humble分支里，如果直接去JenniferBuehler大佬github的gazebo-pkgs里选humble分支，clone下来的还是ros1的，我给的链接是大佬在问题评论里给的humble文件地址。

修改功能包

把功能包下载到工作空间的src下后，先不着急colcon build，会报错，修改以下内容。

删除未升级到ros2的功能包

把gazebo_state_plugins, gazebo_world_plugin_loader, gazebo_test_tools三个包删除，该分支没有对它们作ros2迁移，用的还是catkin，而且对抓取功能没有影响。

修改src/gazebo-pkgs/gazebo_grasp_plugin/CMakeLists.txt

只截取需要修改的部分（坑了我的地方）
修改前：

install(TARGETS gazebo_grasp_fix gazebo_grasp_msgs
  ARCHIVE DESTINATION lib/${PROJECT_NAME}
  LIBRARY DESTINATION lib/${PROJECT_NAME}
  RUNTIME DESTINATION bin/${PROJECT_NAME}
)

修改后：

install(TARGETS gazebo_grasp_fix gazebo_grasp_msgs
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

如果不改可能会像我一样报错

CMake Error at /home/xxx/xx_ws/install/gazebo_grasp_plugin/share/gazebo_grasp_plugin/cmake/ament_cmake_export_libraries-extras.cmake:48 (message):
  Package 'gazebo_grasp_plugin' exports the library 'gazebo_grasp_fix' which
  couldn't be found
Call Stack (most recent call first):
  /home/xxx/xx_ws/install/gazebo_grasp_plugin/share/gazebo_grasp_plugin/cmake/gazebo_grasp_pluginConfig.cmake:41 (include)
  CMakeLists.txt:15 (find_package)

原因就是找库的时候只会找到lib这一层，不会再往下翻文件夹了，而原来那样写就会在lib下的文件夹里，cmake就找不到了。

src/gazebo-pkgs/gazebo_grasp_plugin/package.xml

建议修改，但好像不改影响也不大
修改前：

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_depend>gazebo</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>gazebo_version_helpers</build_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>gazebo</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>gazebo_version_helpers</run_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- You can specify that this package is a metapackage here: -->
    <!-- <metapackage/> -->
    <!-- Other tools can request additional information be placed here -->
  </export>

修改后：

  <buildtool_depend>ament_cmake</buildtool_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_depend>gazebo</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>rclcpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>gazebo_version_helpers</build_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>gazebo</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>rclcpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>gazebo_version_helpers</run_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <build_type>ament_cmake</build_type>
  </export>

修改完用colcon build编译就好了

使用功能包

在你的机器人模型的基础上加，最好是gazebo相关插件都单独写在一个文件里，然后用xacro把它和你的模型加载到一起。

<gazebo>
    <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
        <arm>
          <arm_name>robot_gripper</arm_name>
          <palm_link> arm_link5  </palm_link>
          <gripper_link> gripper_left </gripper_link>
          <gripper_link> gripper_right </gripper_link>
        </arm>
        <forces_angle_tolerance>100</forces_angle_tolerance>
        <update_rate>100</update_rate>
        <grip_count_threshold>4</grip_count_threshold>
        <max_grip_count>8</max_grip_count>
        <release_tolerance>0.005</release_tolerance>
        <disable_collisions_on_attach>false</disable_collisions_on_attach>
        <contact_topic>__default_topic__</contact_topic>
      </plugin>
  </gazebo>

参数含义和数值可以参考The Gazebo grasp fix plugin-奇幻纬度，我暂时还没开始调这个参数，等我调完，如果有需要注意的点，会回来修改完善的。

在翻github的issue时，看到很多人反映找不到/没有加载.so库，作者给出的解决方法是检查是否设置了GAZEBO_PLUGIN_PATH。
设置方法（一劳永逸版）：
打开~/.bashrc， 在里面添加
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:`locate libgazebo_grasp_fix.so`