kinova-jaco2 与 xtion pro live 进行手眼标定（eye on base)

kinova-jaco2 与 xtion pro live 进行手眼标定（eye to hand)（已成功）

机械臂轨迹规划需要手眼协作，所以需要对kinova机械臂和摄像头的坐标系进行计算，用于物品抓取。
easy_handeye 通过将摄像头和机械臂的节点打开，用机械臂抓取一个aucro_market（标定板）,此时，easy_handeye中会出现标定板在机械臂本身坐标系下的坐标和摄像头中识别的aucro_marker 相对于摄像头的坐标，两份坐标为一组。通过识别多组坐标，求解手眼转换矩阵

1 基本的package安装

# -----基本环境为如下------
# 安装ROS-melodic完整版
# 安装opencv3.4.10
# cuda 11.0 
# cudnn 8.0
# 摄像头为openni驱动,每个人摄像头驱动不一样，就不赘述了
sudo apt-get install ros-melodic-openni-camera
sudo apt-get install ros-melodic-openni-launch
# kinova-ros （github原网址）
cd ~/catkin_ws/src
git clone https://github.com/Kinovarobotics/kinova-ros.git

# -----手眼标定部分文件-----
# visp全称是visual servoing platform，是一个可以用于开发视觉跟踪和视觉伺服技术的库。其能够计算机器人的控制律，提供多种可用于追踪的视觉特征
sudo apt-get install ros-melodic-visp

# The ARUCO Library has been developed by the Ava group of the Univeristy of Cordoba(Spain). It provides real-time marker based 3D pose estimation using AR markers.
# 阿鲁科图书馆是由科尔多瓦(西班牙)大学的Ava小组开发的。它使用AR标记提供基于实时标记的3D姿态估计。
cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/pal-robotics/aruco_ros

# easy_handeye
# 该软件包包装了ViSP库中的手眼校准例程（包含在visp_hand2eye_calibration软件包中），以提供一种简单的相机姿态估计解决方案。 预计将以TF中发布的转换形式进行输入。另外，提供了一种用于保存和发布所得到的校准的方法。 通过分别包含/启动单个启动文件，可以实现计算和使用校准。rqt_easy_handeye软件包中提供了用于位置采样和自动机器人移动的（可选）GUI。
cd ~/catkin_ws/src
git clone https://github.com/IFL-CAMP/easy_handeye

2 aruco_ros 验证

// 本身并没有第二步骤，而是直接运行的第3步骤（配置 easy_handeye 的 launch 文件），但是直接运行后发现什么都没有，所以增加了第二步骤，为了试验标定板和相机添加的各个节点是否正确
参考本文进行验证.

cd catkin_ws/src/aruco_ros/launch
# 打开aruco_ros 中的launch文件 做一定的修改 
sudo gedit single.launch

对于xtion pro live 相机的具体参数情况修改如下

<?xml version="1.0"?>
<launch>
     
    <arg name="markerId"        default="582"/>     <!-- ~/catkin_ws/src/aruco_ros/aruco_ros/etc 只能下载这个路径中的标定板，序号也在图中存在，launch文件启动后才能识别  -->
    <arg name="markerSize"      default="0.012"/>    <!-- in m 米为单位 暂时是输入个大致的尺寸，暂时没有多大影响-->
    <arg name="eye"             default="left"/>
    <arg name="marker_frame"    default="aruco_marker_frame"/>
    <arg name="ref_frame"       default=""/>  <!-- leave empty and the pose will be published wrt param parent_name -->
    <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->


    <node pkg="aruco_ros" type="single" name="aruco_single">
        <remap from="/camera_info" to="/camera/rgb/camera_info" />  <!--不怕各位笑话，这个camera_info 我当时找了好久 后来是在终端中先输入rostopic list 然后找到了相对应的彩色图的camera_info节点输入进来，才出现图像的，一定要输入正确，否则启动launch后会出现灰白，不产生任何图像-->
        <remap from="/image" to="/camera/rgb/image_color" />
          <param name="image_is_rectified" value="true"/>
        <param name="marker_size"        value="0.012"/>
        <param name="marker_id"          value="582"/>
        <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>

    <!--node pkg="aruco_ros" type="single" name="aruco_single">
        <remap from="/camera_info" to="/stereo/$(arg eye)/camera_info" />
        <remap from="/image" to="/stereo/$(arg eye)/image_rect_color" />
        <param name="image_is_rectified" value="True"/>
        <param name="marker_size"        value="$(arg markerSize)"/>
        <param name="marker_id"          value="$(arg markerId)"/>
        <param name="reference_frame"    value="$(arg ref_frame)"/> 
        <param name="camera_frame"       value="stereo_gazebo_$(arg eye)_camera_optical_frame"/>
        <param name="marker_frame"       value="$(arg marker_frame)" />
        <param name="corner_refinement"  value="$(arg corner_refinement)" />
    </node-->

</launch>

修改single.launch 文件后

# 启动相机的节点
roslaunch openni_launch openni.launch
# 启动aruco_ros 的launch文件
roslaunch aruco_ros single.launch
# 打开图形看一眼出没出现坐标
rqt_image_view

正常应该出现如下图形，就是ok了

3 配置 easy_handeye 的 launch 文件（有待修改）

Launch文件是用于手眼自动标定的文件
参考:Kinova-Jaco2 与Kinect2进行手眼标定

因为大家用的摄像头不太一样，所以需要对相应参数进行一定的更改。（因为现在在家，所以还没搞明白Xtion pro live 摄像头的具体节点）

<?xml version="1.0"?>
<?xml version="1.0"?>
<launch>
    <!-- launch name 标定板的尺寸和名称-->
    <arg name="namespace_prefix" default="jaco_xtion_handeyecalibration" />
    <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.017" />
    <arg name="marker_id" doc="The ID of the ArUco marker used" default="582" />
    
    <!-- start the Xtion 找到视频驱动 -->
    <include file="$(find openni_launch)/launch/openni.launch" >
        <arg name="depth_registration" value="true" />
    </include>
 
    <!-- start ArUco 连接到相机的节点上-->
    <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_link"/>
        <param name="camera_frame"       value="camera_rgb_optical_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>
 
    <!-- start the robot 启动相应机械臂的节点-->
    <include file="$(find kinova_bringup)/launch/kinova_robot.launch">
    </include>
    <include file="$(find j2n6s300_moveit_config)/launch/j2n6s300_demo.launch">
    </include>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">

    </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="tracking_base_frame" value="camera_rgb_optical_frame" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="j2n6s300_link_base" />
        <arg name="robot_effector_frame" value="j2n6s300_end_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>

将修改好的launch文件放到easy_handeye/launch文件夹下，启动launch文件
启动后出现如下报错，并且只出现两个RVIZ窗口，没有正常的easy_handeye的三个窗口，现在开始想办法解决问题

我尝试了一下，发现更新moveit，安装trac_ik_lib功能包，重新编译以下，就不会出现这么多红色了，但是为什么，我也不知道阿

sudo apt-get install ros-melodic-moveit-*
sudo apt-get install ros-melodic-trac-ik

基本就没有报错了，剩下有一个
[ERROR] [1615040265.841421906]: Failed to find 3D sensor plugin parameters for octomap generation
那不影响我们进行测试

!!!
但是我还是没有别的博客中的Take_sample界面。（-. -.）哭了。。怎么办，继续搜解决办法吧

办法没找到哦，出现了更多的报错。。。看起来像是rqt_plugin之类的东西缺失，然后搜索rqt 果然没有上述这些东西

链接: lianjie.

就差这一个了
之前一直研究错问题了,一直研究的上述红色，直到午睡过后回来，尝试了roslaunch一下 发现有Trackback了，这不好起来了吗。

pip install transforms3d

上述过程中间还出现了别的问题
出现Failed to fetch current robot state
这个问题的原因是程序获取机械臂当前的状态失败导致的
在机械臂启动的launch程序中加入

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
</node>

sudo apt update
sudo apt install ros-<your_ros_version>-joint-state-publisher

之后就能进行正常的roslaunch easy_handeye easy1.launch了

4 手眼标定过程

四元数

其中涉及到四元数，因为欧拉矩阵的方式会出现歧义点，所以现在都用四元数，详细就看视频吧，看个大致明白就行。

链接: 四元数的可视化.
链接: 四元数和三维转动,可互动的探索式视频.

手眼标定过程

正常情况下出现如下几个界面即为正常，但是出现了界面也不好使，遇见了很多问题

遇到的问题

1.没有rqt_image_view

新开个终端 ,输入就可以了

rqt_image_view

2.无法像别人一样直接Check starting pose 和plan

我们不用管，可能本身的demo是按照ur5为原型设计的，我们利用kinova的手柄自己运行20多个点，每个点点一下take_sample 收集点的数据，最后点一下compute就会计算出我们需要的变化矩阵，引用自该博主的 急中生智.

3.点击take_sample直接闪退

在roslaunch easy_handeye easy1.launch之前将jaco恢复到Home位置，之后在重新进入我们想标定坐标的点位