Tsai-Lenz:文献地址
opencv有该算法的具体实现。
点击进入官方Kinetic版本配置教程
两者编译工具有区别,3.9及以下使用catkin_make,3.10及以上的使用catkin_make_isolated
UR机器人系统版本 | 驱动选择 |
---|---|
v e r s i o n ≤ 3.9 version \leq 3.9 version≤3.9 | ur_modern_driver |
v e r s i o n ≥ 3.10 version \ge 3.10 version≥3.10 | ur_robot_driver |
源码编译 ur5 & ur_modern_driver
cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/ros-industrial/universal_robot.git
cd universal_robot
git clone -b kinetic-devel https://github.com/ros-industrial/ur_modern_driver.git
cd ~/catkin_ws
catkin_make
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
# source global ros
source /opt/ros/<your_ros_version>/setup.bash
# create a catkin workspace
mkdir -p catkin_ws/src && cd catkin_ws
# clone the driver
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver
# clone fork of the description. This is currently necessary, until the changes are merged upstream.
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot
# install dependencies
sudo apt update -qq
rosdep update
rosdep install --from-paths src --ignore-src -y
# build the workspace. We need an isolated build because of the non-catkin library package.
catkin_make
# activate the workspace (ie: source it)
source devel/setup.bash
提示CMake错误,请使用全源码编译
source /opt/ros/<your_ros_version>/setup.bash
mkdir -p catkin_ws/src && cd catkin_ws
git clone -b boost https://github.com/UniversalRobots/Universal_Robots_Client_Library.git src/Universal_Robots_Client_Library
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot
sudo apt update -qq
rosdep update
rosdep install --from-paths src --ignore-src -y
catkin_make_isolated
source devel_isolated/setup.bash
更多安装信息请到官方仓库查看Universal_Robots_ROS_Driver(ur_robot_driver)
cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/pal-robotics/aruco_ros.git
cd ..
catkin_make
该依赖请联网安装,否则编译时会提示404的情况
cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/lagadic/vision_visp.git
cd ..
catkin_make --pkg visp_hand2eye_calibration
cd ~/catkin_ws/src
git clone https://github.com/IFL-CAMP/easy_handeye
cd ..
catkin_make
官方仓库:https://github.com/orbbec/ros_astra_camera
# 1.安装依赖
sudo apt install ros-$ROS_DISTRO-rgbd-launch ros-$ROS_DISTRO-libuvc ros-$ROS_DISTRO-libuvc-camera ros-$ROS_DISTRO-libuvc-ros
# 2.clone仓库
cd ~/catkin_ws/src
git clone https://github.com/orbbec/ros_astra_camera
# 3.Create astra udev rule
roscd astra_camera
./scripts/create_udev_rules
# 4.编译
cd ~/catkin_ws
catkin_make --pkg astra_camera
启动指令
roslaunch astra_camera astra.launch
ROS+ubuntu 16.04配置IntelRealsense深度摄像头节点
在线生成地址:https://chev.me/arucogen/
cd ~/catkin_ws/src/easy_handeye/docs/example_launch
cp ur5_kinect_calibration.launch ~/catkin_ws/src/easy_handeye/easy_handeye/launch/eye_to_hand_calibration.launch
# 没GUI的用vim代替gedit
sudo gedit ~/catkin_ws/src/easy_handeye/easy_handeye/launch/eye_to_hand_calibration.launch
<launch>
<arg name="namespace_prefix" default="ur5_kinect_handeyecalibration" />
<arg name="robot_ip" doc="The IP address of the UR5 robot" />
<arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.05"/>
<arg name="marker_id" doc="The ID of the ArUco marker used" default="570"/>
<include file="$(find astra_camera)/launch/astra.launch" >
include>
<node name="aruco_tracker" pkg="aruco_ros" type="single">
<remap from="/camera_info" to="/camera/rgb/camera_info" />
<remap from="/image" to="/camera/rgb/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_rgb_optical_frame"/>
<param name="camera_frame" value="camera_rgb_optical_frame"/>
<param name="marker_frame" value="camera_marker" />
node>
<include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
<arg name="limited" value="true" />
<arg name="robot_ip" value="193.169.10.50" />
include>
<include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
<arg name="limited" value="true" />
include>
<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="tracking_base_frame" value="camera_rgb_optical_frame" />
<arg name="tracking_marker_frame" value="camera_marker" />
<arg name="robot_base_frame" value="base_link" />
<arg name="robot_effector_frame" value="wrist_3_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>
roslaunch easy_handeye eye_to_hand_calibration.launch
如果第二个界面没有顶部菜单栏打不开plugins,请看此教程 - ROS系统easy_handeye手眼标定rqt_easy_handeye没有菜单栏无法打开aruco码位姿检测图像(plugins)的解决方法
在image_view先检测/aruco_tracker/result topic返回的图像是否有返回ArUco marker的位姿
操作流程如下图所示:
注意事项:
最终标定结果如下图所示,图片大于两张Tsai-Lenz算法就能求解。
结果为平移向量+四元数模式: x , y , z + q x , q y , q z , q w x, y, z + q_x, q_y, q_z, q_w x,y,z+qx,qy,qz,qw。
在terminal输入rqt命令打开rqt界面,接入摄像头并启动,找到摄像头rgb节点raw图像的topic,打上勾,展开后查看header下面的frame_id填写到easy_handeye的配置文件的reference_frame和camera_frame的value上
不需要, 软件通过摄像头的camera_info节点获取摄像头内参。
<launch>
<arg name="namespace_prefix" default="ur5_kinect_handeyecalibration" />
<arg name="robot_ip" doc="The IP address of the UR5 robot" />
<arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.05"/>
<arg name="marker_id" doc="The ID of the ArUco marker used" default="570"/>
<include file="$(find realsense2_camera)/launch/rs_camera.launch" >
include>
<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_rgb_optical_frame"/>
<param name="camera_frame" value="camera_rgb_optical_frame"/>
<param name="marker_frame" value="camera_marker" />
node>
<include file="$(find ur_robot_driver)/launch/ur5_bringup.launch">
<arg name="limited" value="true" />
<arg name="robot_ip" value="193.169.10.50" />
include>
<include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
<arg name="limited" value="true" />
include>
<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="tracking_base_frame" value="camera_rgb_optical_frame" />
<arg name="tracking_marker_frame" value="camera_marker" />
<arg name="robot_base_frame" value="base_link" />
<arg name="robot_effector_frame" value="wrist_3_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>