摘要

一个简单的色块识别及其三维坐标提取的程序，使用OpenCV库，通过深度相机的深度信息与色彩信息进行识别，识别出的色块的三维坐标可以用于简单的机械臂抓取任务。

标签

颜色识别、3D曲线提取、曲线拟合、曲线插补、松灵轻量六轴机械臂PIPER

代码仓库

Github链接

功能演示

---

1.使用前准备

1.1.硬件准备

• 奥比中光Petrel（带对齐的深度图像与RGB图像：640*400@30fps）
• （可选）：intel realsense D435（带对齐的深度图像与RGB图像：640*480@30fps）
• AgileX Robotics PiPER机械臂

1.2.软件环境配置

1. 编译安装PCL点云库，linux编译示例参考官方文档：https://pcl.readthedocs.io/projects/tutorials/en/latest/compiling_pcl_posix.html需要指定编译on_nurbs选项
2. PIPER机械臂驱动部署请参考：https://github.com/agilexrobotics/piper_sdk/blob/1_0_0_beta/README(ZH).MD
3. PIPER机械臂ROS控制节点部署参考：https://github.com/agilexrobotics/piper_ros/blob/noetic/README.MD

---

2.功能操作步骤

2.1.启动色块检测节点

1.启动深度相机ROS驱动节点，以奥比中光Petrel为例

roslaunch astra_camera dabai_dc1.launch

2.启动色块检测节点，弹出2个图像窗口分别是hsv_image和origin_image，其中拖动hsv_image中的滑块条可以改变HSV的参数来搜索特定的颜色；在origin_image中用光标点击需要搜索的颜色所在的位置，即可实现自动提取该位置的颜色并搜索

rosrun cube_det cube_det

3. 接着演示自动搜索颜色功能：在origin_image中用光标点击需要搜索的颜色所在的位置，即可实现自动提取该位置的颜色并搜索

4. 同时也会在rviz可视化蓝色中心点的三维坐标信息

2.2.启动曲线检测节点

1. 启动深度相机ROS驱动节点，以奥比中光Petrel为例

roslaunch astra_camera dabai_dc1.launch

2. 启动曲线检测节点，可以看出由于反光导致曲线颜色不统一，导致色彩识别部分在某些线段上出现空缺，导致出现线段不连续的现象，由于曲线检测节点内置了一个实验性质的曲线拟合器与插补器，可以使输出的曲线对应的点云连续且平滑，可以用作机械臂末端控制的数据

rosrun cube_det line_det 

3. 如上文2.1第三步操作，在origin_image中用光标点击需要搜索的曲线所在的位置，即可实现自动提取该位置的曲线并搜索
4. 开启rviz即可看到将曲线转换为完整的点云

5. 启动机械臂ROS控制节点，将连接到机械臂的can模块插入PC，查找机械臂CAN总线

# 寻找机械臂的CAN端口
./find_all_can_port.sh
# 连接到机械臂的CAN端口
./can_activate.sh

6. 启动机械臂运动节点

roslaunch piper start_single_piper.launch

7. 启动运动学逆解，运动学逆解库pinocchio，（pinocchio环境配置参考
   https://github.com/agilexrobotics/piper_ros/blob/noetic/src/piper/scripts/piper_pinocchio/README.MD）

python piper_pinocchio.py

8. 给定一个机械臂Home点

rostopic pub /pos_cmd piper_msgs/PosCmd "{
x: -0.344,
y: 0.0,
z: 0.110,
roll: 0.0,
pitch: 0.0,
yaw: 0.0,
gripper: 0.0,
mode1: 1,
mode2: 0
}"

9. 然后添加rviz的插件用来可视化生成的末端控制路径/line_path

• 可以观察到生成的末端控制路径/line_path
  

10. 启动机械臂运动路径加载与末端控制程序

rosrun cube_det path_confer.py

• path_confer.py一共有三个操作：分别是'r'：记录当前的这一帧的点云，并将点云从相机坐标系camera_color_frame_optical转换到arm_base坐标系并生成为末端控制路径留作后面两个功能备用；'s':将记录的路径按序列单点发送，按一下's'发送一个坐标点到机械臂；'p'：循环发布整个路径到机械臂。

11. 确定好希望跟踪的路径，按键盘的'r'键后可以看到rviz里生成一条基于坐标系arm_base的路径/transformed_cloud

12. 按下键盘上的's'键或者'p'键即可开始控制机械臂沿着生成的轨迹/transformed_cloud开始跟踪。