## 功能 本模块实现相机参数标定、机械臂手眼标定、彩色方块的视觉抓取功能 ## 节点 ### 1. vision_calibration 功能：实现相机内参标定，以及相机与机械臂的手眼变换矩阵标定 ### 2. color_detect_location 功能：检测图像中的色块 ### 3. vision_grasp 功能：实现视觉抓取 ## 原理 ### 1.相机参数标定 相机参数标定的目的是确定相机的内参和畸变参数。用的是张正友棋盘格标定法，参考方法为以下论文： - Z. Zhang. A Flexible New Technique for Camera Calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. - J.Y.Bouguet. MATLAB calibration tool. http://www.vision.caltech.edu/bouguetj/calib_doc/ 实现参考：https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html 相机成像原理如下图所示，展示了世界坐标系中的点P=(X,Y,Z)如何通过相机坐标系Fc映射到像素坐标系(u,v)。  公式为：  简写为：  其中： - (X,Y,Z)为点在世界坐标系中的三维坐标。 - (u,v)为像素坐标系中的坐标。 - A为相机的内参矩阵，包含了焦距和光心坐标。 - (cx,cy)为光心坐标，一般位于图片中心点。 - fx,fy为焦距，以像素为单位 - R和t分别为世界坐标系到相机坐标系的旋转和平移 具体实现采用OpenCV的相机标定模块，主要用了其中的cv::calibrateCamera函数。 输入为N组不同角度拍摄到的棋盘角点坐标、每组对应的实际角点坐标，以及图像尺寸。 输出则为求得的相机内参矩阵、畸变系数、外参矩阵。 ### 2.机械臂手眼标定 机械臂手眼标定用来计算机械臂末端到相机的坐标变换，此处采用的的是ROS中的visp_hand2eye_calibration包，地址为：http://wiki.ros.org/visp_hand2eye_calibration 相机安装在机械臂末端，如下所示：  上一节相机参数标定解决的是上图中右侧的问题，也就是目标到相机的变换。 而机械臂手眼标定则解决的是左侧的问题，也就是相机到机械臂末端的变换。 visp_hand2eye_calibration包中的visp_hand2eye_calibration_calibrator节点就提供了计算手眼标定矩阵的服务。 服务名称：[compute_effector_camera_quick](https://github.com/lagadic/vision_visp/blob/master/visp_hand2eye_calibration/srv/compute_effector_camera_quick.srv) - 客户端： + visp_hand2eye_calibration/TransformArray camera_object + visp_hand2eye_calibration/TransformArray world_effector - 服务端： + geometry_msgs/Transform effector_camera 其中，客户端发送两部分内容： - 一组camera_object：即相机到目标的坐标变换（也就是相机参数标定时求得的相机外参） - 一组对应的world_effector：即拍摄图片时机械臂末端的坐标（可通过机械臂状态接口读取） 调用服务后，会返回手眼标定的结果，即相机到机械臂末端的坐标变换。 手眼标定需要依赖相机参数标定时计算出的每个图片对应的相机外参。所以，进行相机标定及手眼标定的总体步骤为： - 第一步：采集15-20个图片，并记录每个图片采集时对应的机械臂末端姿态。 - 第二步：先利用采集的图片进行相机参数标定，相机参数标定除了会计算出相机的内参矩阵及畸变系数外，还会计算出每个图片对应的相机外参参数。 - 第三步：根据标定结果中的每个图片对应的相机外参参数，结合每个图片对应的机械臂末端姿态，再进行机械臂手眼标定。 ### 3.彩色方块的视觉抓取 相机参数和手眼参数标定完成后，就可以进行基于视觉的抓取了。其步骤为： - 首先将机械臂末端移动到拍照点（拍照高度需要已知，也就是物体到相机光心的距离），并保证相机垂直向下。 - 相机拍摄桌面上的目标，识别其中彩色方块的位置。 - 根据相机内参及拍照高度，将彩色方块中心点的像素坐标转换为相机坐标系下的坐标。 - 根据手眼矩阵，将相机坐标系下的坐标转换为机械臂坐标系下的坐标。 - 控制机械臂末端移动到机械臂坐标系下的坐标，抓取方块。 在`launch/vision_grasp.launch`文件中，配置了抓取时用到的参数。 - `<param name="cali_file" type="string" value="$(find gauss_vision)/data/cfg/"/>`。 此参数指定了抓取所用到的标定参数文件所在目录，包括相机内参和手眼矩阵。 - `<param name="object_hight" type="double" value="0.095"/>`。 抓取对象的高度，调整此参数可控制抓取时机械臂末端距离桌面的高度。如果末端吸盘接触不到方块，则调小此参数。如果末端吸盘接触方块后下压太多，则调大此参数。 - `<param name="camera_to_object_distance" type="double" value="0.20"/>`。 相机光心到物体的距离,即Zc，用来计算物体从像素坐标系到相机坐标系的坐标变换。 - `<param name="handeye_param_x" type="double" value="0.054"/>`等。 这一系列参数设置了手眼矩阵，包括位置(x,y,z)和角度(roll,pitch,yaw)。 - `<param name="robot_capture_image_pose_x" type="double" value="0.2"/>`等。 这一系列参数设置了相机拍照点位姿。 - `<param name="robot_safe_pose_1_x" type="double" value="0.3"/>`等。 这一系列参数设置了机械臂抓取所要经过的点。 - `<param name="robot_place_pose_x" type="double" value="0.17"/>`等。 这一系列参数设置了机械臂抓取方块后，最终放置点。 ## 启动 Gauss机械臂启动后，会自动启动基础控制模块。如果是视觉版机械臂，则开机也会自动启动视觉模块。 不同的启动项通过配置`~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` 文件进行修改。 开机时系统会自动执行`gauss-ros-start`指令，此指令会启动上述文件中配置的所有模块。 ### 对于非视觉版机械臂 非是绝版机械臂只启动基础模块，其配置文件内容为： `~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` ``` processes: - name: 'controllers' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup controllers.launch' args: [] dependencies: [] - name: 'robot_commander' launch_on_startup: true delay_before_start: 4.0 # Additional delay for Moveit to load controllers cmd: 'roslaunch gauss_bringup robot_commander.launch' # cmd: 'roslaunch gauss_bringup robot_commander_pilz.launch' args: [] dependencies: - controllers - name: 'user_interface' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup user_interface.launch' args: [] dependencies: - controllers - robot_commander ``` 此时，如果想启动视觉模块，需要在Gauss基础功能启动后，打开新的客户端，执行以下指令启动视觉模块： ``` $ roslaunch gauss_vision gauss_vision.launch ``` ### 对于视觉版机械臂 视觉版机械臂的 `~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` 文件最后加入了视觉模块启动配置。即： ``` - name: 'gauss_vision' launch_on_startup: true delay_before_start: 10.0 cmd: 'roslaunch gauss_vision gauss_vision.launch' args: [] dependencies: - controllers - robot_commander - user_interface ``` 加入此配置后，执行gauss-ros-start在启动基础模块的基础上，还会启动 `roslaunch gauss_vision gauss_vision.launch`。（gauss_vision包位于`~/Projects/gauss_ws/src/`文件夹下。） 加入视觉模块的完整启动文件内容如下所示： `~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` ``` processes: - name: 'controllers' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup controllers.launch' args: [] dependencies: [] - name: 'robot_commander' launch_on_startup: true delay_before_start: 4.0 # Additional delay for Moveit to load controllers cmd: 'roslaunch gauss_bringup robot_commander.launch' # cmd: 'roslaunch gauss_bringup robot_commander_pilz.launch' args: [] dependencies: - controllers - name: 'user_interface' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup user_interface.launch' args: [] dependencies: - controllers - robot_commander - name: 'gauss_vision' launch_on_startup: true delay_before_start: 10.0 cmd: 'roslaunch gauss_vision gauss_vision.launch' args: [] dependencies: - controllers - robot_commander - user_interface ``` ## 标定功能Service列表 标定功能所提供的Service列表如下： #### (1) gauss/vision/calib/start_stop 功能：启动/关闭标定模块 客户端： * std_msgs/Bool cmd: True表示启动, False表示关闭 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (2) gauss/vision/calib/get_camera_status 功能：获取相机工作状态 客户端： * 空 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (3) gauss/vision/calib/create_flow 功能：创建一个标定流程 客户端： * std_msgs/String flow_name * std_msgs/String remark 服务端： * std_msgs/Int8 flow_id:指定标定流程id * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (4) gauss/vision/calib/get_image_and_pose 功能：获取当前图像及末端位姿 客户端： * 空 服务端： * v_c_image_flow bin_image: 图像数据 * geometry_msgs/Transform pose:末端位姿 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (5) gauss/vision/calib/save_image_and_pose 功能：检验并保存当前图像 客户端： * std_msgs/UInt8 flow_id: 指定流程id 服务端： * v_c_image_flow bin_image: 检验后的图像 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (6) gauss/vision/calib/calc_calib_param 功能：计算相机标定与手眼标定参数 客户端： * std_msgs/UInt8 flow_id: 指定流程id 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描信息 #### (7) gauss/vision/calib/get_calib_param 功能：获取指定标定流程的标定参数 客户端： * std_msgs/UInt8 flow_id: 指定标定流程id 服务端： * geometry_msgs/Vector3 camera_param1 相机内参 * geometry_msgs/Vector3 camera_param2 相机内参 * geometry_msgs/Vector3 camera_param3 相机内参 * std_msgs/Float64 k1, k2, p1, p2, k3: 畸变系数 * geometry_msgs/Transform pose: 手眼变换矩阵 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (8) gauss/vision/calib/get_flow_image_id_list 功能：获取指定流程中存在的图像id列表 客户端： * std_msgs/UInt8 flow_id: 指定标定流程id 服务端： * string[] flow_image_id_list: 该流程图像id数组 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (9) gauss/vision/calib/get_flow_image_and_pose 功能：获取指定标定流程的某一图像及末端位姿 客户端： * std_msgs/UInt8 flow_id: 指定标定流程id * std_msgs/UInt8 image_id: 指定图像id 服务端： * v_c_image_flow bin_image: 图像数据 * geometry_msgs/Transform pose: 机械臂在该图像下的位姿 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (10) gauss/vision/calib/get_local_flow_id_list 功能：获取本地已存在的标定流程id列表 客户端： * 空 服务端： * v_c_flow_info[] local_flow_id_array: 本地的标定流程信息数组 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (11) gauss/vision/calib/set_calib_param 功能：配置指定流程的标定参数作为最终应用 客户端： * std_msgs/UInt8 flow_id: 指定流程id 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (12) gauss/vision/calib/delete_flow_id 功能：删除本地指定的标定流程 客户端： * std_msgs/UInt8 flow_id: 指定流程id(不能删除默认的0号) 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (13) gauss/vision/calib/delete_flow_image 功能：删除本地指定的标定流程中的某图像 客户端： * std_msgs/UInt8 flow_id: 指定流程id(不能删除默认的0号) * std_msgs/UInt8 image_id: 指定图像id 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (14) gauss/vision/calib/get_current_bin_image 功能：获取当前的二进制图像 客户端： * 空 服务端： * v_c_image_flow bin_image: 图像数据 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (15) gauss/vision/calib/get_flow_image_list 功能：获取指定流程中存在的图像 客户端： * std_msgs/UInt8 flow_id: 指定流程id 服务端： * v_c_image_info[] image_info_array: 图像信息数组(string id; v_c_image_flow bin_image) * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (16) gauss/vision/calib/edit_flow_info 功能：编辑流程信息 客户端： * std_msgs/String flow_id: 指定流程id * std_msgs/String flow_name:流程名 * std_msgs/String remark:流程注释 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (17) gauss/vision/calib/get_local_flow_info 功能：获取本地指定流程的信息 客户端： * std_msgs/UInt8 flow_id: 指定流程id 服务端： * v_c_flow_info local_flow_info: 本地的标定流程信息 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 ### 视觉抓取功能Service列表 视觉抓取功能所提供的Service列表如下： #### (1) gauss/vision/grasp/init_task 功能：任务初始化，包括各驱动、算法模块 客户端： * 空 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (2) gauss/vision/grasp/get_camera_status 功能：获取相机工作状态 客户端： * 空 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (3) gauss/vision/grasp/get_tool_status 功能：获取末端工具状态 客户端： * 空 服务端： * std_msgs/Int8 tool_id: 工具id * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (4) gauss/vision/grasp/get_task_status 功能：获取任务状态 客户端： * 空 服务端： * std_msgs/String task_status - 1.“StatusWaitConfig”:等待配置 - 2.“StatusWaitInit”:等待初始化 - 3.“StatusIdle”:空闲态 - 4.“StatusRunning”:运行态 - 5.“StatusSuspend“:暂停态 - 6.“StatusStop“:停止态 - 7.“StatusErr“:错误态 #### (5) gauss/vision/grasp/start_task（注意：此处为Action，并非Service） 功能：开始执行任务 客户端： * gauss_vision/v_g_start_taskAction 服务端： * [实时反馈] + std_msgs/Int8 task_phase:任务执行阶段，取值为： - 1“moving_to_detect_pos”:移动至拍照点 - 2“detecting_cube”:检测色块 - 3“moving_to_cube”:移动至色块抓取位置 - 4“picking_cube”:拾取色块 - 5“safe pose 1“:移动至安全位置1 - 6“safe pose 2“:移动至安全位置2 - 7“moving_to_container”:移动至放置区 - 8“placing_cube”:放置色块 - 9“moving_to_origin”:移动至原点 * [最终执行结果] - std_msgs/Int8 ack_nak: 错误码,0表示成功,非0为失败 - std_msgs/String status_message: 失败描述信息 #### (6) gauss/vision/grasp/pause_task 功能：暂停任务 客户端： * 空 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (7) gauss/vision/grasp/continue_task 功能：恢复任务 客户端： * 空 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (8) gauss/vision/grasp/stop_task 功能：停止任务并归位移动至原点（如果已拾取色块，需将色块放置到圆盘后再归位） 客户端： * 空 服务端： * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息 #### (9) gauss/vision/grasp/get_cube_detection_result 功能：获取色块检测结果 客户端： * 空 服务端： * v_c_image_flow bin_image:色块图像 * std_msgs/Int8 num:检测到的色块个数 * geometry_msgs/Vector3 pose:当前色块坐标 * std_msgs/Int8 ack_nak: 错误码,0表示ok,非0为错误 * std_msgs/String status_message: 错误描述信息