硬件资源:
机械臂viper300
摄像头realsense d435i
软件版本:
操作系统:Ubuntu18.04
ROS: Melodic
标定算法:
采用easy handeye 算法包 https://gitee.com/casiaros/easy_handeye.git
踩过的坑:
1、easy handeye基于opencv的calibrateHandEye函数,只在opencv4(实测opencv4.2)以后才引入了这个算法。
解决办法,下载opencv的4.2版本的源码。
2、由于melodic二进制安装默认使用的是opencv3.x版本,而如果要使用ROS,则必须配置其环境变量(现在发现,其实配置的是python的环境变量),因此会导致无法使用自己编译的opencv
启动报错:后补
解决办法:
单独为此节点配置.bashrc,具体如下:
# 加入以下内容:
export PYTHONPATH=/usr/bin/python2:/usr/local/lib:/usr/local/lib/python2.7/dist-packages
#export PYTHONPATH=/usr/bin/python3:/usr/local/lib:/usr/local/lib/python3.6/dist-packages
# 分别是 python3安装目录,opencv安装目录,python包所在目录。不确定这样设置对切换cv2版本一定有用,仅供参考
export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig # “/usr/local/lib/ ” 部分根据自己的opencv-python安装目录确定
export LD_LIBRARY_PATH=/usr/local/lib # “/usr/local/lib/ ” 部分根据自己的opencv-python安装目录确定
这样就将可以使用自己编译版本的opencv。
easy handeye使用方法
aruco ros
easy handeye需要知道摄像头坐标系到机械臂末端(手)的转换关系,这里使用arcuo ros进行计算;而arcuo ros就成了必须要了解的算法。aruco ros就是利用aruco进行识别定位,输出aruco标定板到摄像机坐标系的转换关系。aruco蕴含了size和id信息,size也就是实际打印的aruco的边长,id为aruco内容。我使用的是maker582_5cm.jpg如图所示,可能是由于我打印没有设置好,理论上marker_size=0.05,marker_id=582,而实际marker_size=0.13(这个是根据rviz中深度彩色点云数据和发布的maker重合程度进行不断试探的调试得出的)
制作标定板
下面是整理的制作标定板的代码,cv2.aruco.DICT_6x6_250表示,maker_size=0.06,maker_id=250
#encoding: utf8
import cv2
import cv2.aruco as aruco
import numpy as np
# 生成aruco标记
# 加载预定义的字典
dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
# 生成标记
markerImage = np.zeros((600, 200), dtype=np.uint8)
# markerImage[:,:]=255
markerImage = cv2.aruco.drawMarker(dictionary, 22, 200, markerImage, 1)
cv2.imshow("aruco",markerImage)
print(dictionary.markerSize)
# print(dictionary.markerId)
key = cv2.waitKey(0)
if key is 27:
cv2.imwrite("aruco.jpg",markerImage)
exit
配置自己的启动launch
如下图所示,很重要的两个参数,marker_size 和marker_id,前者表示标定板的尺寸,maker_id为标定板的id,注:实际制作或者生成的标定板会将这两部分信息保存进去。
<launch>
<arg name="namespace_prefix" default="vx300" />
<arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.13"/>
<arg name="marker_id" doc="The ID of the ArUco marker used" default="582"/>
<!-- start ArUco -->
<node name="aruco_tracker" pkg="aruco_ros" type="single">
<remap from="/camera_info" to="/camera/color/camera_info" />
<remap from="/image" to="/camera/color/image_raw" />
<param name="image_is_rectified" value="true"/>
<param name="marker_size" value="$(arg marker_size)"/>
<param name="marker_id" value="$(arg marker_id)"/>
<param name="reference_frame" value="camera_link"/>
<param name="camera_frame" value="camera_color_optical_frame"/>
<param name="marker_frame" value="camera_maker" />
</node>
<!-- start easy_handeye -->
<include file="$(find easy_handeye)/launch/calibrate.launch" >
<arg name="namespace_prefix" value="$(arg namespace_prefix)" />
<arg name="eye_on_hand" value="false" />
<arg name="start_rviz" value="false"/>
<arg name="move_group_namespace" value="/vx300" />
<arg name="move_group" value="interbotix_arm" />
<!-- need to copy iiwa_stack_config.rviz to launch/ folder -->
<arg name="rviz_config_file" value="$(find easy_handeye)/launch/iiwa_stack_config.rviz" />
<arg name="tracking_base_frame" value="camera_link" />
<arg name="tracking_marker_frame" value="camera_maker" />
<arg name="robot_base_frame" value="vx300/base_link" />
<arg name="robot_effector_frame" value="vx300/ee_gripper_link" />
<arg name="freehand_robot_movement" value="false" />
<arg name="robot_velocity_scaling" value="0.5" />
<arg name="robot_acceleration_scaling" value="0.2" />
</include>
</launch>
标定结果
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)