最近做了一个机器人视觉抓取的项目,目标是让机器人准确地拿到桌子上摆放的咖啡,递给旁边的人。目前的实现结果是能够抓取到一定区域内任意摆放的纸杯,水平方向定位精度在20mm以内,竖直方向定位精度较差,会有20~35mm的误差,原因是竖直方向对应着相机的深度方向。
下面总结一下项目中一些代码使用和bug解决,大部分代码都是已经开源的方案,不过用的时候总会出现各式各样的问题,都是靠着其他人的博客和代码库的issue才解决的。
AprilTag有专门写了一个ROS版本的代码(https://github.com/AprilRobotics/apriltag_ros),跑AprilTag代码之前得先装好相机驱动,并且做好相机标定。
启动节点前要先设置一下apirltag_ros/apriltag_ros/config目录下的tags.yaml和settings.yaml。
apirltag_ros/apriltag_ros/config/settings.yaml
tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
tag_threads: 2 # default: 2
tag_decimate: 1.0 # default: 1.0
settings.yaml得注意tag_family和你打印的tag一致,一般好像都用36h11。
apirltag_ros/apriltag_ros/config/tags.yaml
standalone_tags:
[
# {id: 1, size: 0.058}, #size对应标签的大小
# {id: 0, size: 0.048},
# {id: 0, size: 0.04},
# {id: 1, size: 0.04},
# {id: 2, size: 0.04},
# {id: 3, size: 0.04},
# {id: 4, size: 0.04}
]
tag_bundles:
[
{
name: 'my_bundle',
layout:
[
{id: 0, size: 0.044, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
{id: 4, size: 0.044, x: 0.0300, y: -0.0830, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
{id: 3, size: 0.044, x: -0.0300, y: -0.0830, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
{id: 2, size: 0.044, x: 0.0300, y: 0.0830, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
{id: 1, size: 0.044, x: -0.0300, y: 0.0830, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}
]
}
]
tags.yaml的话就得注意standalone_tags和tag_bundles的区别。
我源码安装了usb_cam驱动包,不过源码也没什么用,没看也没改,整个项目就只用到了usb_cam-test.launch一个文件。
usb_cam/launch/usb_cam-test.launch
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="camera_info_url" type="string" value="file:///home/jeremy/.ros/camera_info/head_camera.yaml"/>
<param name="video_device" value="/dev/video2" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
node>
<node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
<remap from="image" to="/usb_cam/image_raw"/>
<param name="autosize" value="true" />
node>
launch>
第3行代码camera_info_url是加上了标定后相机参数的位置,源码应该没有这一句,我是看到手眼标定里面有个启动文件这样写了,就加上去了。(其实单目相机标定的作用我也没看出来,删掉这句我观察输出图像也没看到啥变化。但为了心理安慰,还是别删,要不然我精度又没了… …)
还有第4行的video_device,如果启动launch文件后发现打开的是笔记本自带相机(突然出现自己的脸,吓一跳),就把video2改成video0,或者video0改成video2,反正就是换一下。不过别改成video1,会说找不到的。
[ WARN] [1639733001.211902775]: Requested description of standalone tag ID [0], but no description was found...
这个问题是因为tags.yaml中没有正确设置二维码id,要注意id必须和打印出来二维码一致,还有就是单个tag设置standalone_tags,多个就设置后面的tag_bundles。
启动相机后发现图像黑屏
遇到这个问题我本来上网找半天,按照其他博主的方法改各种.launch文件中的参数,但是都没用,最后哭笑不得地发现是单目相机的光圈调到最小了,调大光圈就出现图像了,亏得我还linux和window下反复测试相机能不能用。
输出位置信息都是0
未进行相机标定,相机参数没有保存至.ros/camera_info目录下。
JAKA有官方的API二次开发文档,python的文档写得详细一点,不过我的机器人控制代码用的C++写的,所以开发的时候我python和C++的API都用了一遍。一般用python先试一下,成功了再换成C++。
Linux平台下使用机器人API的话,需要将JAKA给的libjakaAPI.so, jkrc.so
两个编译好的库放到和python代码相同的文件夹,运行代码前还需要代码文件夹路径添加到环境变量中,就是在终端中运行下面这行代码。我的代码就放在grab/script路径下。
export LD_LIBRARY_PATH=~/project/jaka/SendCoffee/src/grab/script
还有一点需要注意的是,节卡机器人的API使用环境是python3.5,如果你的linux默认python环境是2.7的话,可以用python3 ***.py
的命令来运行代码。我python3的环境是3.6.9,与官方要求的3.5不一致,但是目前运行也没遇到问题。
C++要编译,所以设置会比python麻烦,我是通过参考JAKA给的文件中的linux示例代码才编译成功的。具体步骤是先在grab功能包建立grab/include文件夹,放入JAKAZuRobot.h, jkerr.h, jktypes.h
3个头文件。接着建立grab/lib文件夹,放入编译好的C++静态库libjakaAPI.a
。最后只需要正确编写Cmake编译文件,就可以用catkin_make命令编译。哦对了,还得将你要运行的C++代码放到grab/src文件夹中。
grab/CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(grab)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
apriltag_ros
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES test
# CATKIN_DEPENDS roscpp
# DEPENDS system_lib
)
include_directories(
${catkin_INCLUDE_DIRS}
)
include_directories( "/usr/include/eigen3" )
# include 头文件目录
include_directories(include)
LINK_DIRECTORIES($(SRC_ROOT_PATH))
LINK_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/lib)
LINK_LIBRARIES(pthread)
# 编译成可执行文件,如果编译动态库可以用:target_link_libraries
add_executable(control_robot src/control_robot.cpp)
target_link_libraries(control_robot jakaAPI)
target_link_libraries(control_robot ${catkin_LIBRARIES})
find_package(Threads)
target_link_libraries (control_robot ${CMAKE_THREAD_LIBS_INIT})
眼在手外的手眼标定是为了求解相机相对于机器人底部的位姿,在本项目中,AprilTag定位能够得到tag在相机坐标系下的位姿,而抓取目标与tag的相对位置是固定的。因此,完成手眼标定后,即可实现抓取目标在tag坐标系–>相机坐标系–>机器人坐标系的位姿转换。
求解得到抓取目标在机器人坐标系下的位置后,即可控制机器人运动至目标点,进行抓取。
我用的是鱼香ROS写的handeye-calib包(https://gitee.com/ohhuo/handeye-calib),代码给出了操作步骤的详细介绍,我是跟着在线标定的流程实现的。handeye-calib其实与easy_handeye功能包的原理一样,都是调用OpenCV的cv2.calibrateHandEye()函数完成手眼标定。easy_handeye有一个更好的可视化界面,但是相应的代码也更难读懂。
很多手眼标定博客都是用moveit驱动机器人,通过tf树来获取机器人位姿的,但是我不知道怎样用moveit,所以我决定直接调用JAKA SDK来获取机器人位姿,并且通过ros的geometry_msgs::PoseStamped消息格式发布话题/robot_pos。这样标定程序就能直接订阅机器人位姿话题,下面是修改后的手眼标定launch文件。
handeye-calib/launch/online/online_hand_to_eye_calib.launch
<launch>
<arg name="camera_pose_topic" default="/aruco_single/pose" />
<arg name="arm_pose_topic" default="/robot_pos" />
<node pkg="handeye-calib" type="online_hand_to_eye_calib.py" name="online_hand_to_eye_calib" output="screen" >
<param name="arm_pose_topic" value="$(arg arm_pose_topic)" />
<param name="camera_pose_topic" value="$(arg camera_pose_topic)" />
node>
launch>
no moudle named 'rospkg'
no moudle named 'transforms3d'
'module' object has no attribute 'CALIB_HAND_EYE_TSAI'
super().__init() Error: super() takes at least 1 argument(0 given)
标定结果精度很差