参考资料：

[1]

[2]

步骤1：手眼标定（摄像头与 机械臂相对位置的参数标定）。即根据摄像头获取的图像的像素点坐标获得机械臂末端的位置。

准备工作，启动机器人，放置好标定板，打开机械臂（机械臂开关盘的电源指示灯亮即可确定机械臂为打开状态）

具体步骤：

cd ~/spark

./onekey.sh

选择第8项（根据提示完成标定）。具体过程大致如下：

根据提示调整摄像头的位置，然后按任意键继续，标定程序在收集了绿色方框内（红色标定块）的hsv信息后，开始移动机械臂，机械臂移动的位置如下图中的pose x,y，机械臂上的红色标定块在相机坐标系中的像素坐标为

下图中的cam x,y:

机械臂坐标系的说明：

坐标原点为机械臂的底座中心点，机械臂的正前方（也是机器人正前方）为X轴的正方向，

其左侧为Y轴的正方向，垂直于地面（机械臂底座平面）向上为机械臂的Z轴。

关于相机像素坐标系的说明，如下图：

标定的结果写入home目录的thefile.txt文件

例如，上面的标定结果对应的文件内容如下图：

如标定有关的节点（ROS程序）如下：

核心代码如下：

注意到其中机械臂移动的z坐标是一样的，但固定设置为：

这个值需要根据抓取物的实际高度来设置。

步骤2、根据参考资料[2]实现地图上物品的抓取

（实验验证和完善中）

建议在标定后关闭所有终端，开始步骤2.

正常情况下，步骤2直接通过下面指令启动：

roslaunch move2grasp move2grasp.launch

（注意：roslaunch move2grasp move2grasp.launch 为自动对抗赛的启动方式，如果为半自动对抗赛，改为  roslaunch move2grasp teleop2grasp.launch ）

在该指令启动过程中如果出现下面的错误：

可以按如下方式修改相关的launch文件中的参数：

（1）新建终端，通过下面指令转到包spark_description包的launch目录

roscd  spark_description/launch

通过下面指令编辑spark_description_norviz.launch文件

gedit  spark_description_norviz.launch

将其中的<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/>

改为

<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)   camera_types:=astrapro"/>

存盘退出。

（2）修改 move2grasp包中 move2grasp.launch文件

通过以下指令转到包的目录

roscd move2grasp/launch

通过以下指令编辑 move2grasp.launch文件

gedit move2grasp.launch

将该文件中的astra_launch改为astra_camera,然后保存退出。

进行以上修改后，可以继续步骤2：

关闭所有终端后，新建一个终端，输入下面的指令：

roslaunch move2grasp move2grasp.launch

将启动建图、导航、抓取的一系列节点，rviz典型的界面如下，则启动成功。

进一步的测试：

通过"Publish Piont"在地图中设置目标点，机器人将移动到该目标点，然后实现抓取，抓取后会回到起始点，然后将目标放下。

说明：设定目标点导航并返回已测试，实验发现，抓取的时候，机械臂在z轴没u有 移动到位，请同学们自行测试，改进代码。

代码所在位置：

新建一个终端通过下面指令转到move2grasp包的scripts目录

roscd move2grasp/scripts/

其中有两个文件：

grasp.py  move.py

可以对他们编辑来改进。

例如：

在目前的grasp.py中，其抓取的代码中，有以下设置：

 

可以看出，该设置设定的z为-50，应该设置得更小，如大家可以设置为-110看效果（注意到：这也是标定的代码中设置的值），注意到-110已经比较接近地面，大家需要精确地测量。

辅助实验：

在上面步骤2的基础上，我们可以结合对grasp.py的阅读来手动地发布相关指令来对机械臂进行控制实现对目标的抓取，

如上图，如何实现机械臂抓取其前方的蓝色物体呢？

上面的grasp.py的部分代码，由此可知，要控制机械臂移动，其实是向position_write_topic 话题发布spark_carry_object/position消息，

例如，通过下面的指令，机械臂将移动到(200,50,-30)处（这里单位为mm)

rostopic pub /position_write_topic spark_carry_object/position : "x: 200.0

y: 50.0

z: -30.0" 

下面指令进一步调整机械臂末端的位置

rostopic pub /position_write_topic spark_carry_object/position : "x: 220.0

y: 0.0

z: -40.0" 

(以上指令在spark-7392设备上测试)结果如图：

进一步精细控制：

rostopic pub /position_write_topic spark_carry_object/position : 200.0

y: 0.0

z: -50.0" 

效果如下如图：

由此可以初步得到，机械臂z的坐标为-120左右，则此时其吸盘在地面上方10cm处（正好是一块10cm*10cm)的方块的上方。

通过向pump_topic话题发布1（如下）可以吸附目标，

rostopic pub /pump_topic swiftpro/status "status: 1"

如果吸附到，则如图

注意，这里有一个重要问题，由于吸附是基于负压的原理，因此，只有目标与吸盘没有空隙才能被吸附！（上图的结果，是手动将目标往上移动了一点才被吸附到的），因此，真正要实现自动吸附，机械臂还应该往下一点（策略：x,y的位置对准后，机械臂从上网下移动的-122左右（待测试）！

物体放下：

通过下面指令可以方向物体

rostopic pub /pump_topic swiftpro/status "status: 0"

注意，如果把机械臂抬得太高，那么，放下时可能导致物体滚动，从而跑到范围之外。

2023.5.29

关于视觉感知及机械臂移动范围的测试

启动move2grasp.launch文件，在rviz的相机框可以获得类似如下的图像，可以根据该图像（通过实际测量）大致获得摄像头可以感知的范围，获得图像的四个角点（左下角、右下角、左上角、右上角）在本体坐标系下的坐标。

由此获得摄像头感知的范围。

==========

关于标定精度的进一步思考：

步骤1的标定，其实是启动了如下指令：

roslaunch   spark_carry_object   spark_carry_cal_cv3.launch  camera_type_tel:=astrapro 

其中，控制机械臂移动的节点为

spark_carry_object包下的cali_pos.py

通过如下指令转到spark_carry_object包的节点目录

roscd  spark_carry_object/nodes

通过如下指令来编辑cali_pos.py文件

gedit  cali_pos.py

该文件中，有以下几行

print ("start to move the uarm----")

for i in range(20):

if rospy.is_shutdown():

break

r1.sleep()

pos.x = 180 + i * 5

pos.y = 200 - i * 10

pos.z = -110

pub1.publish(pos)

其中，pos.z = -110就表示机械臂的高度（将它改为 pos.z = -122就可以标定在机械臂高度为-122(mm）的情况下的标定结果。（实验发现，不同高度的标定文件的参数不一样）

实训任务：在teleop.py中增加一个按键，其功能为检测当前机器人位姿下能否检测到目标？如果能检测到，则返回其在摄像头中的位置，否则显示没有找到。在此基础，进行可靠性试验。

实训任务：在teleop.py中增加一个按键，其功能为直接抓取其前方的物体（根据经验给定机械臂的目标位置，实现抓取）。

实训任务：分析机械臂的抓取范围。

扩展思考：深度相机的利用

启动以下launch文件，该文件将根据深度相机获得laserscan数据，并发布到scan2话题。利用scan2话题的信息（激光雷达信息）对分米立方块进行识别。

例如，（在7990编号设备上的实验），当机器人前方约25cm处有分米立方块是，scan2的显示结果如下图：

对激光雷达信息分析，发现其距离（距摄像头的距离）约为0.57m,如下图

可以利用这些信息来判断机器人前方是否有分米立方块，以及其相对与机器人的水平距离。（测量机器人摄像头的高度，分析计算摄像头向下的角度）