【UR5机械臂Moveit避障】【5】在可以用moveit驱动机械臂的基础上添加octomap

目录

有错误或没说清楚的地方欢迎评论指正，我会修改的，谢谢大家

修改moveit中sensor参数

定期清除体素

---

有错误或没说清楚的地方欢迎评论指正，我会修改的，谢谢大家

在启动roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch的时候，ur5_moveit_planning_execution.launch主要是为了启动move_group.launch,这个就是moveit的主要部分。而传感器就是move_group.launch中的sensor manager部分，也就是需要改sensor_manager.launch.xml

修改moveit中sensor参数

参考：http://t.csdnimg.cn/TBiWq

move_group.launch中的Sensors Functionality部分:

找到同目录下的sensor_manager.launch.xml，备份一下，再修改：

<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->

  <!-- Params for 3D sensors config -->
  <rosparam command="load" file="$(find ur5_moveit_config)/config/sensors_3d.yaml" />

  <!-- Params for the octomap monitor -->
  <param name="octomap_frame" type="string" value="base" />
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="3.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="ur5" />
  <include file="$(find ur5_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />

</launch>

这里面注意几点，sensors_3d.yaml这个文件一会要改，octomap_frame和当时相机标定时设置的机器人坐标系保持一致，我这里填base。

在launch旁边的config目录下，新建sensors_3d.yaml：

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
  - filtered_cloud_topic: filtered_cloud
    max_range: 2.5
    max_update_rate: 5.0
    padding_offset: 0.01
    padding_scale: 1.0
    point_cloud_topic: /camera/depth/color/points
    # point_cloud_topic: /kinect2/sd/points
    point_subsample: 1
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

这里不同的相机要改的就一个，就是订阅的pointscloud2的话题，其余的都一样。我这里的订阅的话题是realsense的，注释的是kinect2的。

设置好之后启动我们整合好的launch，详情见前节，链接：

【UR5机械臂Moveit避障】【4】UR5和kinectv2/realsense手眼标定（眼在手外）-CSDN博客我这里是眼在手外，也就是相机在固定位置，如下图所示。把frame改为base，添加点云话题，就可以看见标定成功，至于说有点偏差，那是正常的，因为这种传统的手眼标定会有一系列的多种误差累积，觉得差的可以寻找新式的标定方法（本帖末尾会给出我找到的方法），现在有点偏差，到时候建包围盒建大一点就行。reference_frame和camera_frame，填相机rgb的frame，一般在rviz测试相机的时候可以看到除了xxx_link以外还有其他frame，填带有rgb，color的关于彩色的frame。https://blog.csdn.net/weixin_45702459/article/details/139293764其中包含启动机器人+启动相机+运行tf变换节点

roslaunch my_bag tfpc.launch

可以看到moveit已经生成了octomap，但是其中包括了机械臂自己的点云，所以现在还无法直接规划。（会报错说初始位置碰撞）

定期清除体素

网上有人评论里说在移开障碍物之后体素不消失，我双系统+kinect2体素不消失，但是后面虚拟机+realsense同样步骤，体素却是消失的。针对不消失的情况，其实motionplanning已经给了clear octomap 的服务，只要我们定期调用这个服务即可。

在自己包的scripts中新建clearoctomap.py

#!/usr/bin/env python

import rospy
from std_srvs.srv import Empty

def clear_octomap():
    rospy.wait_for_service('/clear_octomap')
    try:
        clear_octomap_service = rospy.ServiceProxy('/clear_octomap', Empty)
        clear_octomap_service()
        # rospy.loginfo("Octomap cleared successfully.")
    except rospy.ServiceException as e:
        rospy.logerr("Failed to call clear_octomap service: %s" % e)

if __name__ == '__main__':
    rospy.init_node('octomap_clearer')
    rate = rospy.Rate(1)  # 设置每秒运行一次
    while not rospy.is_shutdown():
        clear_octomap()
        rate.sleep()

这次要在cmakelists.txt中添加了（可以先直接运行看看是不是没问题），过程参考：

http://www.autolabor.com.cn/book/ROSTutorials/index.html

再把这个节点加入launch中

 <!-- Launch your clear_octomap node -->
  <node name="clear_octomap_node" pkg="my_bag" type="clearoctomap.py" output="screen"/>

运行一下看看是不是一秒刷新一次，应该没问题

roslaunch my_bag tfpc.launch