1.大体实现机械臂跟随物体移动

2.使用线程队列将机器人控制与视觉识别解耦,大大提高了系统的流畅性
This commit is contained in:
GitLab 2024-04-01 13:59:33 +08:00
parent dae25cdf47
commit 559a94c55a
18 changed files with 206 additions and 125 deletions

Binary file not shown.

115
.idea/workspace.xml generated
View File

@ -4,12 +4,25 @@
<option name="autoReloadType" value="SELECTIVE" />
</component>
<component name="ChangeListManager">
<list default="true" id="31e8b3f2-5159-47b8-a798-4b06368c40f5" name="更改" comment="调试了模型预测的输出结果">
<list default="true" id="31e8b3f2-5159-47b8-a798-4b06368c40f5" name="更改" comment="1. 调试了模型预测的输出结果&#10;2. 成功控制机械臂移动">
<change beforePath="$PROJECT_DIR$/.idea/copilot/chatSessions/00000000000.xd" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/copilot/chatSessions/00000000000.xd" afterDir="false" />
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/ObjectClassify.py" beforeDir="false" afterPath="$PROJECT_DIR$/img_process/ObjectClassify.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/orbbec_camera/OrbbecCamera.py" beforeDir="false" afterPath="$PROJECT_DIR$/img_process/orbbec_camera/OrbbecCamera.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/ObjectClassify.py" beforeDir="false" afterPath="$PROJECT_DIR$/ObjectClassify.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/__init__.py" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/__init__.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/orbbec_camera/OrbbecCamera.py" beforeDir="false" afterPath="$PROJECT_DIR$/orbbec_camera/OrbbecCamera.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/orbbec_camera/__init__.py" beforeDir="false" afterPath="$PROJECT_DIR$/orbbec_camera/__init__.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/orbbec_camera/utils.py" beforeDir="false" afterPath="$PROJECT_DIR$/orbbec_camera/utils.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/take_photos.py" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/take_photos.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/train/package-seg.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/train/package-seg.yaml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/train/train.py" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/train/train.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/train/yolov8n-seg.pt" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/train/yolov8n-seg.pt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/train/yolov8n.pt" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/train/yolov8n.pt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/yolo.py" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/yolo.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/yolov8n-seg.pt" beforeDir="false" afterPath="$PROJECT_DIR$/abandon_code/img_process/yolov8n-seg.pt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/img_process/yolov8n.pt" beforeDir="false" afterPath="$PROJECT_DIR$/yolov8n.pt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/main.py" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/robot_control/FRRobot.py" beforeDir="false" afterPath="$PROJECT_DIR$/robot_control/FRRobot.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/robot_control/control.py" beforeDir="false" afterPath="$PROJECT_DIR$/robot_control/control.py" afterDir="false" />
</list>
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
@ -31,22 +44,59 @@
<option name="hideEmptyMiddlePackages" value="true" />
<option name="showLibraryContents" value="true" />
</component>
<component name="PropertiesComponent">{
&quot;keyToString&quot;: {
&quot;Python.FRRobot.executor&quot;: &quot;Run&quot;,
&quot;Python.ObjectClassify.executor&quot;: &quot;Run&quot;,
&quot;RunOnceActivity.OpenProjectViewOnStart&quot;: &quot;true&quot;,
&quot;RunOnceActivity.ShowReadmeOnStart&quot;: &quot;true&quot;,
&quot;last_opened_file_path&quot;: &quot;F:/Jetbrains/PyCharm Professional&quot;,
&quot;node.js.detected.package.eslint&quot;: &quot;true&quot;,
&quot;node.js.detected.package.tslint&quot;: &quot;true&quot;,
&quot;node.js.selected.package.eslint&quot;: &quot;(autodetect)&quot;,
&quot;node.js.selected.package.tslint&quot;: &quot;(autodetect)&quot;,
&quot;nodejs_package_manager_path&quot;: &quot;npm&quot;,
&quot;settings.editor.selected.configurable&quot;: &quot;settings.saveactions&quot;,
&quot;vue.rearranger.settings.migration&quot;: &quot;true&quot;
<component name="PropertiesComponent"><![CDATA[{
"keyToString": {
"Python.FRRobot.executor": "Run",
"Python.ObjectClassify.executor": "Run",
"Python.control.executor": "Run",
"RunOnceActivity.OpenProjectViewOnStart": "true",
"RunOnceActivity.ShowReadmeOnStart": "true",
"last_opened_file_path": "F:/Jetbrains/PyCharm Professional",
"node.js.detected.package.eslint": "true",
"node.js.detected.package.tslint": "true",
"node.js.selected.package.eslint": "(autodetect)",
"node.js.selected.package.tslint": "(autodetect)",
"nodejs_package_manager_path": "npm",
"settings.editor.selected.configurable": "settings.saveactions",
"vue.rearranger.settings.migration": "true"
}
}</component>
}]]></component>
<component name="RecentsManager">
<key name="MoveFile.RECENT_KEYS">
<recent name="F:\Projects\PyCharmProjects\graduation_design\abandon_code" />
<recent name="F:\Projects\PyCharmProjects\graduation_design" />
</key>
</component>
<component name="RunManager">
<configuration name="ObjectClassify" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
<module name="graduation_design" />
<option name="ENV_FILES" value="" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/ObjectClassify.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<recent_temporary>
<list>
<item itemvalue="Python.ObjectClassify" />
</list>
</recent_temporary>
</component>
<component name="SharedIndexes">
<attachedChunks>
<set>
@ -67,7 +117,8 @@
<workItem from="1711856889730" duration="76000" />
<workItem from="1711857077648" duration="18301000" />
<workItem from="1711885626226" duration="118000" />
<workItem from="1711885752611" duration="4246000" />
<workItem from="1711885752611" duration="4821000" />
<workItem from="1711935133710" duration="14551000" />
</task>
<task id="LOCAL-00001" summary="调试了模型预测的输出结果">
<option name="closed" value="true" />
@ -77,7 +128,23 @@
<option name="project" value="LOCAL" />
<updated>1711876153617</updated>
</task>
<option name="localTasksCounter" value="2" />
<task id="LOCAL-00002" summary="调试了模型预测的输出结果">
<option name="closed" value="true" />
<created>1711895189565</created>
<option name="number" value="00002" />
<option name="presentableId" value="LOCAL-00002" />
<option name="project" value="LOCAL" />
<updated>1711895189565</updated>
</task>
<task id="LOCAL-00003" summary="1. 调试了模型预测的输出结果&#10;2. 成功控制机械臂移动">
<option name="closed" value="true" />
<created>1711895216612</created>
<option name="number" value="00003" />
<option name="presentableId" value="LOCAL-00003" />
<option name="project" value="LOCAL" />
<updated>1711895216612</updated>
</task>
<option name="localTasksCounter" value="4" />
<servers />
</component>
<component name="TypeScriptGeneratedFilesManager">
@ -85,10 +152,12 @@
</component>
<component name="VcsManagerConfiguration">
<MESSAGE value="调试了模型预测的输出结果" />
<option name="LAST_COMMIT_MESSAGE" value="调试了模型预测的输出结果" />
<MESSAGE value="1. 调试了模型预测的输出结果&#10;2. 成功控制机械臂移动" />
<option name="LAST_COMMIT_MESSAGE" value="1. 调试了模型预测的输出结果&#10;2. 成功控制机械臂移动" />
</component>
<component name="com.intellij.coverage.CoverageDataManagerImpl">
<SUITE FILE_PATH="coverage/graduation_design$FRRobot.coverage" NAME="FRRobot 覆盖结果" MODIFIED="1711894781434" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/robot_control" />
<SUITE FILE_PATH="coverage/graduation_design$ObjectClassify.coverage" NAME="ObjectClassify 覆盖结果" MODIFIED="1711894875711" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/img_process" />
<SUITE FILE_PATH="coverage/graduation_design$FRRobot.coverage" NAME="FRRobot 覆盖结果" MODIFIED="1711950740512" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/robot_control" />
<SUITE FILE_PATH="coverage/graduation_design$ObjectClassify.coverage" NAME="ObjectClassify 覆盖结果" MODIFIED="1711950908435" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$" />
<SUITE FILE_PATH="coverage/graduation_design$control.coverage" NAME="control 覆盖结果" MODIFIED="1711950717891" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/robot_control" />
</component>
</project>

View File

@ -1,5 +1,6 @@
import asyncio
import os
import threading
from time import sleep
import Robot
@ -8,14 +9,19 @@ from torch import tensor
from ultralytics import YOLO
import cv2
from orbbec_camera.OrbbecCamera import OrbbecCamera
from robot_control import FRRobot
from robot_control.FRRobot import FRRobot
# 全局变量
model = YOLO('yolov8n.pt')
image_width = 640
# 矫正系数,用于映射像素尺寸和实际尺寸
# 开发时系数为25/32 = 0.78125
correction_factor = 1
# 创建摄像头实例
camera = OrbbecCamera('HW', True, image_width=image_width)
center_point = 320
robot = Robot.RPC('192.168.3.102')
# 创建FRRobot实例
robot_controller = FRRobot()
def init():
@ -47,15 +53,14 @@ if __name__ == '__main__':
conf=0.5,
half=True,
imgsz=image_width,
verbose=False,
vid_stride=5)
print("中心点的深度值: ", camera.get_center_distance(), "mm")
verbose=False, )
# print("中心点的深度值: ", camera.get_center_distance(), "mm")
x_pos = 0
y_pos = 0
high = 0
width = 0
for r in results:
if len(r.boxes.xyxy) > 0:
if len(r.boxes.xywh) > 0:
positions = r.boxes.xywh.tolist()
x_pos = positions[0][0]
y_pos = positions[0][1]
@ -66,41 +71,29 @@ if __name__ == '__main__':
# print("宽度", high)
# print("高度", width)
# 计算与中心点的距离
x_distance = x_pos - center_point
y_distance = y_pos - center_point
# print("X距离:", x_distance)
# print("Y距离:", y_distance)
# 移动方向
# dir0 - 负方向1 - 正方向;
x_dir = 0
y_dir = 0
# 移动时间
x_time = 0
y_time = 0
# 确定移动方向
if abs(x_distance) > 25:
x_time = abs(x_distance) / 21
if x_distance > 0:
x_dir = 1
else:
x_dir = 0
robot.StartJOG(ref=4, nb=1, dir=x_dir, max_dis=30, vel=20.0, acc=100.0)
sleep(0.5)
robot.ImmStopJOG()
# 计算与中心点的距离
x_distance = (x_pos - center_point) * correction_factor
y_distance = (y_pos - center_point) * correction_factor
print("X距离:", x_distance)
print("Y距离:", y_distance)
move_commands = []
if abs(x_distance) > 15:
x_time = abs(x_distance) / 21
x_dir = 1 if x_distance > 0 else 0
move_commands.append(("x", x_distance))
if abs(y_distance) > 25:
y_time = abs(y_distance) / 21
if y_distance < 0:
y_dir = 1
else:
y_dir = 0
robot.StartJOG(ref=4, nb=2, dir=y_dir, max_dis=30, vel=20.0, acc=100.0)
sleep(0.5)
robot.ImmStopJOG()
if abs(y_distance) > 15:
y_time = abs(y_distance) / 21
y_dir = 1 if y_distance < 0 else 0
move_commands.append(("y", y_distance))
if len(move_commands) > 0:
if not robot_controller.is_action:
# 添加移动指令
robot_controller.add_move_command(move_commands)
key = cv2.waitKey(1)
if key == ord('q'):
camera.stop()
# cw.stop()
robot_controller.add_move_command("stop")
break

12
main.py
View File

@ -1,12 +0,0 @@
import cv2
import torch
def process_image(image):
# todo:使用OpenCV和Torch处理图像识别物体
pass
def get_object_coordinates(image):
# todo返回识别物体的坐标
pass

View File

@ -25,6 +25,7 @@ class OrbbecCamera:
self.color_image = None # 颜色图像
self.depth_image = None # 深度图像
self.combined_image = None # 叠加图像
self.depth_intrinsics = 0 # 深度图像的内参
# 线程控制
self.thread = None # 线程对象,用于并行执行捕获循环
self.active = False # 控制捕获循环的标志
@ -111,11 +112,12 @@ class OrbbecCamera:
if depth_frame is None:
continue
# 处理深度数据
depth_image = self.process_depth_data(depth_frame)
self.depth_image = depth_image
# 合成深度图像
combined_image = cv2.addWeighted(color_image, 0.5, depth_image, 0.5, 0) # 叠加图像
self.combined_image = combined_image
self.process_depth_data(depth_frame)
# depth_image = self.process_depth_data(depth_frame)
# self.depth_image = depth_image
# # 合成深度图像
# combined_image = cv2.addWeighted(color_image, 0.5, depth_image, 0.5, 0) # 叠加图像
# self.combined_image = combined_image
except KeyboardInterrupt:
break # 捕捉到中断信号则退出
@ -153,17 +155,16 @@ class OrbbecCamera:
# v: 像素坐标y
# depth: 深度值
# K[0]为相机参数
# fx = self.depth_intrinsics.K[0]
# fy = self.depth_intrinsics.K[4]
# cx = self.depth_intrinsics.K[2]
# cy = self.depth_intrinsics.K[5]
fx = self.depth_intrinsics.K[0]
fy = self.depth_intrinsics.K[4]
cx = self.depth_intrinsics.K[2]
cy = self.depth_intrinsics.K[5]
# 单位是毫米
# camera_x = (u - cx) * depth / fx
# camera_y = (v - cy) * depth / fy
# return camera_x, camera_y, depth
camera_x = (u - cx) * depth / fx
camera_y = (v - cy) * depth / fy
return camera_x, camera_y, depth
# 返回值: 相机坐标(x, y, z)
return None
if __name__ == "__main__":

View File

@ -1,3 +1,6 @@
import queue
import threading
import Robot
import time
@ -6,50 +9,63 @@ class FRRobot:
def __init__(self):
self.robotCTL = Robot.RPC('192.168.3.102')
self.is_action = False # 是否正在执行动作
self.speed = 100 # 速度百分比,[0~100]默认20
self.home = [] # 回零点
self.tool = 1 # 工具坐标系编号
self.user = 1 # 工件坐标系编号
# 工具坐标系参数
# self.t_coord = [0, 0, 170, -180, 0, -45]
# 100% 60mm/s; 50% 30mm/s; 20% 12mm/s
self.velocity = 60 # 速度(mm/s)
# 线程相关
self.command_queue = queue.Queue()
self.listener_thread = threading.Thread(target=self.process_commands)
self.listener_thread.daemon = True # 设置为守护线程,确保主程序退出时线程也会退出
self.listener_thread.start()
speed_error_code = self.robotCTL.SetSpeed(self.speed)
if speed_error_code != 0:
print("初始化全局速度错误:", speed_error_code)
# tool_error_code = self.robotCTL.SetToolCoord(1, self.t_coord, 0, 0)
# if tool_error_code != 0:
# print("初始化工具坐标系错误:", tool_error_code)
# 将移动命令添加到队列中
def add_move_command(self, command):
self.command_queue.put(command)
#
if __name__ == '__main__':
robot = FRRobot()
robot.robotCTL.ImmStopJOG()
# # 与机器人控制器建立连接,连接成功返回一个机器人对象
#
# #
# # ref0 - 关节点动, 2 - 基坐标系点动, 4 - 工具坐标系点动, 8 - 工件坐标系点动;
# # nb1 - 1
# # 关节(x轴),2 - 2
# # 关节(y轴),
# # 3 - 3 关节(z轴),
# # 4 - 4 关节(rx),
# # 5 - 5 关节(ry),
# # 6 - 6 关节(rz);
# # dir0 - 负方向1 - 正方向;
# # max_dis单次点动最大角度 / 距离,单位 ° 或mm;
# # 默认参数
# # vel速度百分比[0~100]默认20;
# # acc加速度百分比[0~100]默认100;
# # n = 0
# # while n < 5:
# # X轴负方向移动
# robot.robotCTL.StartJOG(ref=4, nb=1, dir=1, max_dis=300, vel=20.0, acc=100.0)
# time.sleep(5)
# # Y轴负方向移动
# # robot.robotCTL.StartJOG(4, 2, 0, 30, vel=20.0, acc=100.0)
# # time.sleep(0.5)
# # n += 1
# robot.robotCTL.ImmStopJOG()
def process_commands(self):
while True:
move_commands = self.command_queue.get()
if move_commands == "stop":
self.robotCTL.ImmStopJOG()
break # 通过发送一个特定的"stop"命令来停止线程
# axis, distance, direction = move_commands
print("接收到指令: ", move_commands)
for command in move_commands:
self.is_action = True
axis, distance = command
if axis == "x":
print("x移动距离: ", distance)
move_time = abs(distance) / self.velocity
direction = 1 if distance > 0 else 0
self.move_x(distance, direction)
print("x移动时间: ", move_time)
time.sleep(move_time)
self.robotCTL.ImmStopJOG()
elif axis == "y":
print("y移动距离: ", distance)
move_time = abs(distance) / self.velocity
direction = 1 if distance < 0 else 0
self.move_y(distance, direction)
print("y移动时间: ", move_time)
time.sleep(move_time)
self.robotCTL.ImmStopJOG()
self.is_action = False
self.command_queue.task_done()
def move_x(self, distance=30, direction=1):
# direction为0代表负方向为1代表正方向
dis = int(distance)
self.robotCTL.StartJOG(ref=4, nb=1, dir=direction, max_dis=100, vel=20.0, acc=100.0)
def move_y(self, distance=30, direction=1):
dis = int(distance)
self.robotCTL.StartJOG(ref=4, nb=2, dir=direction, max_dis=100, vel=20.0, acc=100.0)

View File

@ -1,3 +1,17 @@
import FRRobot
import queue
import threading
robot = FRRobot.FRRobot()
import Robot
import time
robotCTL = Robot.RPC('192.168.3.102')
tool = 1 # 工具坐标系编号
user = 1 # 工件坐标系编号
speed_error_code = robotCTL.SetSpeed(40)
if speed_error_code != 0:
print("初始化全局速度错误:", speed_error_code)
robotCTL.StartJOG(ref=4, nb=1, dir=1, max_dis=100, vel=20.0, acc=100.0)
time.sleep(1)
robotCTL.ImmStopJOG()