代码文件介绍
任意位置积木分拣
-
重新开启2个终端窗口，分别进入对应目录执行命令。
-
1号终端。
cd /home/HwHiAiUser/E2ESamples/src/E2E-Sample/ros2_robot_armsource setenv.shros2 run dofbot_moveit dofbot_server -
2号终端。
cd /home/HwHiAiUser/E2ESamples/src/E2E-Sample/ros2_robot_armsource setenv.shros2 run dofbot_garbage_yolov5 block_cls
:::note 说明 任意位置抓取时，需要使用带有标定板框一面的地图，并将摄像头对准标定板框。 :::
-
-
查看机械臂分拣结果。
父主题： 快速体验
任意位置积木堆叠
- 重新开启2个终端窗口，分别进入对应目录执行命令。
-
1号终端。
cd /home/HwHiAiUser/E2ESamples/src/E2E-Sample/ros2_robot_armsource setenv.shros2 run dofbot_moveit dofbot_server -
2号终端。
cd /home/HwHiAiUser/E2ESamples/src/E2E-Sample/ros2_robot_armsource setenv.shros2 run robot_arm_color_stacking color_stacking
-
- 查看机械臂分拣结果。
父主题： 快速体验
代码文件介绍
机械臂代码文件目录如下所示：
RobotArm 源码仓
├── dofbot_color_stacking 色块堆叠功能包
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── scripts 代码以及配置文件
│ ├── color_stacking.txt
│ ├── config 配置文件
│ │ ├── dp.bin 透视变换矩阵
│ │ ├── offset.txt 硬件误差配置
│ │ └── XYT_config.txt 相机标定配置
│ ├── det_utils.py ACL推理功能文件
│ ├── dofbot_config.py 机械臂描述文件
│ ├── main.py 主函数入口
│ ├── model 模型文件
│ │ ├── coco_names.txt
│ │ └── yolov5s_bs1.om
│ ├── npu_utils.py ACL推理功能文件
│ ├── stacking_grap.py 色块堆叠抓取功能文件
│ └── stacking_target.py 色块堆叠目标定位文件
├── dofbot_garbage_yolov5 色块分拣功能包
│ ├── dofbot_garbage_yolov5
│ │ ├── a200dk_camera_capture_picture.ipynb
│ │ ├── config 配置文件
│ │ │ ├── dp.bin 透视变换矩阵
│ │ │ ├── offset.txt 硬件误差配置
│ │ │ └── XYT_config.txt 相机标定配置
│ │ ├── det_utils.py ACL推理功能文件
│ │ ├── dofbot_config.py 机械臂描述文件
│ │ ├── garbage_grap_bak.py
│ │ ├── garbage_grap.py 垃圾分拣抓取脚本
│ │ ├── garbage_identify.py 垃圾分拣识别脚本
│ │ ├── __init__.py
│ │ ├── main.py 主函数入口
│ │ ├── model 模型文件
│ │ │ ├── coco_names.txt
│ │ │ └── yolov5s_bs1.om
│ │ ├── npu_utils.py ACL推理功能文件
│ │ ├── __pycache__
│ │ │ ├── det_utils.cpython-310.pyc
│ │ │ ├── dofbot_config.cpython-310.pyc
│ │ │ ├── garbage_grap.cpython-310.pyc
│ │ │ ├── garbage_identify.cpython-310.pyc
│ │ │ ├── npu_utils.cpython-310.pyc
│ │ │ └── single_garbage_identify.cpython-310.pyc
│ │ ├── single_garbage_identify.py
│ │ └── 相机校准.ipynb 相机校准脚本
│ ├── package.xml
│ ├── resource
│ │ └── dofbot_garbage_yolov5_tiny1
│ ├── setup.cfg
│ ├── setup.py
│ └── test
│ ├── test_copyright.py
│ ├── test_flake8.py
│ └── test_pep257.py
├── dofbot_info 机械臂接口功能包
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── srv 服务信息文件
│ └── Kinemarics.srv
├── dofbot_moveit 机械臂描述功能包
│ ├── CMakeLists.txt
│ ├── include
│ │ └── dofbot_moveit
│ │ └── dofbot_kinemarics.h 机械臂运动学头文件
│ ├── launch 工程启动文件
│ │ └── dofbot_moveit.launch.py
│ ├── lib 坐标求解库文件
│ │ └── libdofbot_kinemarics.so
│ ├── meshes 三维结构文件
│ │ ├── base_link.STL
│ │ ├── link1.STL
│ │ ├── link2.STL
│ │ ├── link3.STL
│ │ ├── link4.STL
│ │ └── link5.STL
│ ├── package.xml
│ ├── rviz RVIZ可视化文件
│ │ ├── dofbot.rviz
│ │ └── urdf.rviz
│ ├── src
│ │ └── dofbot_server.cpp 机械臂运动学服务器
│ └── urdf
│ └── dofbot.urdf 机械臂结构描述文件
├── imgs
│ ├── 10.jpg
│ ....
│ └── 9.PNG
└── README.md
父主题： 代码实现
请求与响应配置文件
dofbot_info功能包包含了需要编译的消息文件。“dofbot_info/srv/Kinemarics.srv”文件中，横线上方表示客户端发送请求的消息格式，下方表示服务器回复响应的消息格式。
客户端发送的请求包括以下内容。
- 机械臂基坐标系下的目标坐标（tar_x, tar_y, tar_z）
- 目标姿态角（roll, pitch, yaw）
- 机械臂目前的六个关节信息（cur_joint1, cur_joint2, cur_joint3, cur_joint4, cur_joint5, cur_joint6）
- 用户请求的模式（kin_name）。
除用户请求的模式为字符串类型外，其他消息都应为浮点型。用户请求的模式分为正运动学解（fk）以及逆运动学解（ik）。
- 当模式为正运动学解时，只需填入机械臂目前的六个关节信息。
- 当模式为逆运动学解时，则无需填入机械臂目前的六个关节信息。
服务端响应的消息全为浮点型。
- 当模式为正运动学解时，服务器会响应相对于机械臂基坐标系的到达坐标（x, y, z）和到达姿态角（roll, pitch, yaw）。
- 当模式为逆运动学解时，服务器会响应机械臂的六个关节目标（joint1, joint2, joint3, joint4, joint5, joint6）。
float64 tar_x
float64 tar_y
float64 tar_z
float64 roll
float64 pitch
float64 yaw
float64 cur_joint1
float64 cur_joint2
float64 cur_joint3
float64 cur_joint4
float64 cur_joint5
float64 cur_joint6
string kin_name
---
float64 joint1
float64 joint2
float64 joint3
float64 joint4
float64 joint5
float64 joint6
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
父主题： 代码实现
ROS2服务与机械臂仿真代码
dofbot_moveit功能包包含了ROS2正逆运动学解算服务器、服务器和仿真环境下可视化机械臂的启动文件（launch）、以及用于描述三维机械臂模型的URDF描述文件。
“dofbot_moveit/urdf/dofbot.urdf”文件为约定俗成的机器人三维描述文件（urdf全称为Unified Robot Description Format）。示例代码为机械臂的三维模型定义片段。
<robot name="dofbot">
<!-- 实体描述 -->
<link name="base_link">
<!-- 视觉描述 -->
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0"/>
<geometry>
<mesh
filename="package://dofbot_moveit/meshes/base_link.STL"/>
</geometry>
<material
name="">
<color
rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<!-- 碰撞描述 -->
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0"/>
<geometry>
<mesh
filename="package://dofbot_moveit/meshes/base_link.STL"/>
</geometry>
</collision>
</link>
robot标签定义了机器人的名称为dofbot，在此标签下有多个子标签，如link、joint等，每个子标签定义了当前机器人的组成部分。每个组成部分也拥有属于自己的子标签。这些子标签为该组成部分的属性，例如这个具体关节所在空间的位置和姿态，以及关节的颜色及材质等。通过urdf文件对机器人的描述，可以在仿真环境中可视化机械臂，如图1所示。
图1 机械臂可视化模型
“dofbot_moveit/src/dofbot_server.cpp”文件定义了正逆运动学解的服务器，其核心代码如下。
srvicecallback()函数为正逆运动学解服务器的回调函数，两个参数分别为客户端发送的请求（request）和服务端回复（response）的响应。二者皆由前文中提到的srv消息类型文件定义。当客户端发送的请求模式为求解正运动学（fk），则调用响应函数，进行响应运算，获得目标位置的坐标和姿态角，存入响应变量（response）中，反之同理。
bool srvicecallback(const std::shared_ptr<dofbot_info::srv::Kinemarics::Request> request,
std::shared_ptr<dofbot_info::srv::Kinemarics::Response> response) {
if (request->kin_name == "fk") {
double joints[]{request->cur_joint1, request->cur_joint2, request->cur_joint3, request->cur_joint4,
request->cur_joint5};
// 定义目标关节角容器
vector<double> initjoints;
// 定义位姿容器
vector<double> initpos;
// 目标关节角度单位转换,由角度转换成弧度
for (int i = 0; i < 5; ++i) {
initjoints.push_back((joints[i] - 90) * DE2RA);
}
// 调取正解函数,获取当前位姿
dofbot.dofbot_getFK(urdf_file, initjoints, initpos);
cout << "--------- Fk ---------" << a << "--------- Fk ---------" << endl;
cout << "XYZ坐标 ： " << initpos.at(0) << " ," << initpos.at(1) << " ," << initpos.at(2) << endl;
cout << "Roll,Pitch,Yaw： " << initpos.at(3) << " ," << initpos.at(4) << " ," << initpos.at(5) << endl;
response->x = initpos.at(0);
response->y = initpos.at(1);
response->z = initpos.at(2);
response->roll = initpos.at(3);
response->pitch = initpos.at(4);
response->yaw = initpos.at(5);
}
父主题： 代码实现
任意位置积木分拣代码
dofbot_garbage_yolov5功能包包含了训练好的yolov5模型及功能脚本。主要为通过yolov5进行色块的识别与定位，根据位置控制机械臂进行积木分类的代码。
“dofbot_garbage_yolov5/dofbot_garbage_yolov5/data_collect.py”为摄像头采集数据的逻辑代码。
## 打开摄像头
capture = cv.VideoCapture(0)
## 当摄像头正常打开的情况下循环执行
while capture.isOpened():
# 读取相机的每一帧
_, img = capture.read()
# 统一图像大小
img = cv.resize(img, (640, 480))
try:
write_XYT(XYT_path, [93,130], 140)
except Exception:
print("File XYT_config Error !!!")
dp = np.array([[86, 31], [18, 426], [609, 427], [537, 31]])
img = calibration.Perspective_transform(dp, img)
# ---------- Get Image Index --------------
img_idx = -1
with open("/home/HwHiAiUser/RobotArmProject/ros2_dofbot_formal_ws/src/dofbot_garbage_yolov5/dofbot_garbage_yolov5/config/data_collect_idx.txt", "r") as f:
contents = f.readline()
# print(type(contents))
img_idx = int(contents)
stored_path = "/home/HwHiAiUser/RobotArmProject/ros2_dofbot_formal_ws/src/dofbot_garbage_yolov5/dofbot_garbage_yolov5/data/" + str(img_idx) + ".png"
cv.imwrite(stored_path, img)
with open("/home/HwHiAiUser/RobotArmProject/ros2_dofbot_formal_ws/src/dofbot_garbage_yolov5/dofbot_garbage_yolov5/config/data_collect_idx.txt", "w") as f:
img_idx += 1
f.write(str(img_idx))
# ---------- Done --------------------------
break
“dofbot_garbage_yolov5/dofbot_garbage_yolov5/dofbot_config.py”为图像检测与机器视觉逻辑代码。
def calibration_map(self, image,xy=None, threshold_num=130):
'''
放置方块区域检测函数
:param image:输入图像
:return:轮廓区域边点,处理后的图像
'''
if xy!=None:
self.xy=xy
# 机械臂初始位置角度
joints_init = [self.xy[0], self.xy[1], 0, 0, 90, 0] # REVISE
# 将机械臂移动到标定方框的状态
self.arm.Arm_serial_servo_write6_array(joints_init, 500)
self.image = image
self.threshold_num = threshold_num
# 创建边点容器
dp = []
h, w = self.image.shape[:2]
# print("h",h)
# 获取轮廓点集(坐标)
contours = self.Morphological_processing()
# 遍历点集
for i, c in enumerate(contours):
# 计算轮廓区域。
area = cv.contourArea(c)
low_bound = None
high_bound = None
if self.use_other_cam:
low_bound = h * w / 2 - 40000
else:
low_bound = h * w / 2
high_bound = h * w
# 设置轮廓区域范围
if low_bound < area < high_bound:
# 计算多边形的矩
mm = cv.moments(c)
if mm['m00'] == 0:
continue
cx = mm['m10'] / mm['m00']
cy = mm['m01'] / mm['m00']
# 绘制轮廓区域
cv.drawContours(self.image, contours, i, (255, 255, 0), 2)
# 获取轮廓区域边点
dp = np.squeeze(cv.approxPolyDP(c, 100, True))
# 绘制中心
if not self.data_collect:
cv.circle(self.image, (np.int_(cx), np.int_(cy)), 5, (0, 0, 255), -1)
return dp, self.image
def Morphological_processing(self):
'''
形态学及去噪处理,并获取轮廓点集
'''
# 将图像转为灰度图
gray = cv.cvtColor(self.image, cv.COLOR_BGR2GRAY)
# 使用高斯滤镜模糊图像。
gray = cv.GaussianBlur(gray, (5, 5), 1)
# 图像二值化操作
ref, threshold = cv.threshold(gray, self.threshold_num, 255, cv.THRESH_BINARY)
# 获取不同形状的结构元素
kernel = np.ones((3, 3), np.uint8)
# 形态学开操作
if self.use_other_cam:
blur = cv.morphologyEx(threshold, cv.MORPH_OPEN, kernel, iterations=8)
else:
blur = cv.morphologyEx(threshold, cv.MORPH_OPEN, kernel, iterations=4)
# 提取模式
mode = cv.RETR_EXTERNAL
# 提取方法
method = cv.CHAIN_APPROX_NONE
# 获取轮廓点集(坐标) python2和python3在此处略有不同
# 层级关系 参数一：输入的二值图，参数二：提取模式，参数三：提取方法。
contours, hierarchy = cv.findContours(blur, mode, method)
return contours
def Perspective_transform(self, dp, image):
'''
透视变换
:param dp: 方框边点(左上,左下,右下,右上)
:param image: 原始图像
:return: 透视变换后图像
'''
if len(dp) != 4:
return
upper_left = []
lower_left = []
lower_right = []
upper_right = []
for i in range(len(dp)):
if dp[i][0]<320 and dp[i][1]<240:
upper_left=dp[i]
if dp[i][0]<320 and dp[i][1]>240:
lower_left=dp[i]
if dp[i][0]>320 and dp[i][1]>240:
lower_right=dp[i]
if dp[i][0]>320 and dp[i][1]<240:
upper_right=dp[i]
# 原图中的四个顶点
pts1 = np.float32([upper_left, lower_left, lower_right, upper_right])
# 变换后的四个顶点
pts2 = np.float32([[0, 0], [0, 480], [640, 480], [640, 0]])
# 根据四对对应点计算透视变换。
M = cv.getPerspectiveTransform(pts1, pts2)
# 将透视变换应用于图像。
Transform_img = cv.warpPerspective(image, M, (640, 480))
return Transform_img
“dofbot_garbage_yolov5/dofbot_garbage_yolov5/garbage_grap.py”为机械臂抓取与移动分拣的逻辑代码。
def move(self, joints, joints_down):
'''
移动过程
:param joints: 移动到物体位置的各关节角度
:param joints_down: 机械臂抬起各关节角度
'''
joints_uu = [90, 80, 50, 50, 265, self.grap_joint]
# 抬起
joints_up = [joints_down[0], 80, 50, 50, 265, 30]
# 移动至物体位置上方
self.arm.Arm_serial_servo_write6_array(joints_uu, 1000)
sleep(1)
# 开合夹爪
# for i in range(5):
# self.arm.Arm_serial_servo_write(6, 180, 100)
# sleep(0.08)
# self.arm.Arm_serial_servo_write(6, 30, 100)
# sleep(0.08)
# 松开夹爪
self.arm.Arm_serial_servo_write(6, 0, 500)
sleep(0.5)
# 移动至物体位置
self.arm.Arm_serial_servo_write6_array(joints, 500)
sleep(0.5)
# 进行抓取,夹紧夹爪
self.arm.Arm_serial_servo_write(6, self.grap_joint, 500)
sleep(0.5)
# 架起
self.arm.Arm_serial_servo_write6_array(joints_uu, 1000)
sleep(1)
# 抬起至对应位置上方
self.arm.Arm_serial_servo_write(1, joints_down[0], 500)
sleep(0.5)
# 抬起至对应位置
self.arm.Arm_serial_servo_write6_array(joints_down, 1000)
sleep(1)
# 释放物体,松开夹爪
self.arm.Arm_serial_servo_write(6, 30, 500)
sleep(0.5)
# 抬起
self.arm.Arm_serial_servo_write6_array(joints_up, 1000)
sleep(1)
def arm_run(self, name, joints):
'''
机械臂移动函数
:param name:识别的垃圾名称
:param joints: 反解求得的各关节角度
'''
# 有害垃圾--红色
if name == "Syringe" or name == "Used_batteries" or name == "Expired_cosmetics" or name == "Expired_tablets" and self.move_status == True:
# 此处设置,需执行完本次操作,才能向下运行
self.move_status = False
# print("有害垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
# 获得目标关节角
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# 移动到垃圾桶前对应姿态
# joints_down = [45, 80, 35, 40, 265, self.grap_joint]
# 移动到垃圾桶位置放下对应姿态
joints_down = [45, 50, 20, 60, 265, self.grap_joint]
# 移动
self.move(joints, joints_down)
# 移动完毕
self.move_status = True
# 可回收垃圾--蓝色
if name == "Zip_top_can" or name == "Newspaper" or name == "Old_school_bag" or name == "Book" and self.move_status == True:
self.move_status = False
# print("可回收垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# joints_down = [27, 110, 0, 40, 265, self.grap_joint]
joints_down = [27, 75, 0, 50, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
# 厨余垃圾--绿色
if name == "Fish_bone" or name == "Watermelon_rind" or name == "Apple_core" or name == "Egg_shell" and self.move_status == True:
self.move_status = False
# print("厨余垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# joints_down = [152, 110, 0, 40, 265, self.grap_joint]
joints_down = [147, 75, 0, 50, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
# 其他垃圾--灰色
if name == "Yellow" or name == "Cigarette_butts" or name == "Toilet_paper" or name == "Peach_pit" or name == "Disposable_chopsticks" and self.move_status == True:
self.move_status = False
# print("其他垃圾")
# print(joints[0], joints[1], joints[2], joints[3], joints[4])
joints = [joints[0], joints[1], joints[2], joints[3], 265, 30]
# joints_down = [137, 80, 35, 40, 265, self.grap_joint]
joints_down = [133, 50, 20, 60, 265, self.grap_joint]
self.move(joints, joints_down)
self.move_status = True
父主题： 代码实现
在线提单