|
|
@ -1,140 +1,276 @@
|
|
|
|
#ros lib
|
|
|
|
#!/usr/bin/env python
|
|
|
|
|
|
|
|
# -*- coding: utf-8 -*-
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
颜色识别与HSV参数调整节点
|
|
|
|
|
|
|
|
Color recognition and HSV parameter adjustment node
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
功能说明 (Functionality):
|
|
|
|
|
|
|
|
- 基于HSV颜色空间的目标颜色识别
|
|
|
|
|
|
|
|
- 实时调整HSV颜色范围参数
|
|
|
|
|
|
|
|
- 鼠标选择ROI区域进行颜色学习
|
|
|
|
|
|
|
|
- 发布检测到的目标位置信息
|
|
|
|
|
|
|
|
- 支持实时颜色追踪模式切换
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
作者 (Author): UVPTCCS Team
|
|
|
|
|
|
|
|
日期 (Date): 2024
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ROS2相关库 (ROS2 related libraries)
|
|
|
|
import rclpy
|
|
|
|
import rclpy
|
|
|
|
from rclpy.node import Node
|
|
|
|
from rclpy.node import Node
|
|
|
|
from std_msgs.msg import Bool
|
|
|
|
from std_msgs.msg import Bool
|
|
|
|
from geometry_msgs.msg import Twist
|
|
|
|
from geometry_msgs.msg import Twist
|
|
|
|
from sensor_msgs.msg import CompressedImage, LaserScan, Image
|
|
|
|
from sensor_msgs.msg import CompressedImage, LaserScan, Image
|
|
|
|
from yahboomcar_msgs.msg import Position
|
|
|
|
from yahboomcar_msgs.msg import Position
|
|
|
|
#common lib
|
|
|
|
|
|
|
|
|
|
|
|
# 通用库 (Common libraries)
|
|
|
|
import os
|
|
|
|
import os
|
|
|
|
import threading
|
|
|
|
import threading
|
|
|
|
import math
|
|
|
|
import math
|
|
|
|
from yahboomcar_astra.astra_common import *
|
|
|
|
from yahboomcar_astra.astra_common import *
|
|
|
|
from yahboomcar_msgs.msg import Position
|
|
|
|
from yahboomcar_msgs.msg import Position
|
|
|
|
|
|
|
|
|
|
|
|
print("import finish")
|
|
|
|
print("import finish")
|
|
|
|
cv_edition = cv.__version__
|
|
|
|
cv_edition = cv.__version__
|
|
|
|
print("cv_edition: ",cv_edition)
|
|
|
|
print("cv_edition: ",cv_edition)
|
|
|
|
|
|
|
|
|
|
|
|
class Color_Identify(Node):
|
|
|
|
class Color_Identify(Node):
|
|
|
|
def __init__(self,name):
|
|
|
|
"""
|
|
|
|
|
|
|
|
颜色识别节点类
|
|
|
|
|
|
|
|
Color identification node class
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
功能 (Functions):
|
|
|
|
|
|
|
|
- 实时颜色识别和追踪
|
|
|
|
|
|
|
|
- HSV参数动态调整
|
|
|
|
|
|
|
|
- 鼠标交互式颜色选择
|
|
|
|
|
|
|
|
- 目标位置信息发布
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def __init__(self, name):
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
初始化颜色识别节点
|
|
|
|
|
|
|
|
Initialize color identification node
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Args:
|
|
|
|
|
|
|
|
name (str): 节点名称 (Node name)
|
|
|
|
|
|
|
|
"""
|
|
|
|
super().__init__(name)
|
|
|
|
super().__init__(name)
|
|
|
|
#create a publisher
|
|
|
|
|
|
|
|
self.pub_position = self.create_publisher(Position,"/Current_point", 10)
|
|
|
|
# 创建发布者 (Create publishers)
|
|
|
|
self.pub_cmdVel = self.create_publisher(Twist, '/cmd_vel', 10)
|
|
|
|
self.pub_position = self.create_publisher(Position, "/Current_point", 10) # 发布目标位置 (Publish target position)
|
|
|
|
self.index = 2
|
|
|
|
self.pub_cmdVel = self.create_publisher(Twist, '/cmd_vel', 10) # 发布速度控制命令 (Publish velocity commands)
|
|
|
|
self.Roi_init = ()
|
|
|
|
|
|
|
|
self.hsv_range = ()
|
|
|
|
# 初始化状态变量 (Initialize state variables)
|
|
|
|
self.circle = (0, 0, 0)
|
|
|
|
self.index = 2 # 索引 (Index)
|
|
|
|
self.point_pose = (0, 0, 0)
|
|
|
|
self.Roi_init = () # ROI初始化区域 (ROI initialization area)
|
|
|
|
self.dyn_update = True
|
|
|
|
self.hsv_range = () # HSV颜色范围 (HSV color range)
|
|
|
|
self.Start_state = True
|
|
|
|
self.circle = (0, 0, 0) # 检测到的圆形目标 (x, y, radius)
|
|
|
|
self.select_flags = False
|
|
|
|
self.point_pose = (0, 0, 0) # 目标点位置 (Target point position)
|
|
|
|
self.gTracker_state = False
|
|
|
|
self.dyn_update = True # 动态更新标志 (Dynamic update flag)
|
|
|
|
self.windows_name = 'frame'
|
|
|
|
self.Start_state = True # 启动状态 (Start state)
|
|
|
|
self.Track_state = 'identify'
|
|
|
|
self.select_flags = False # 选择标志 (Selection flag)
|
|
|
|
self.color = color_follow()
|
|
|
|
self.gTracker_state = False # 追踪器状态 (Tracker state)
|
|
|
|
self.cols, self.rows = 0, 0
|
|
|
|
self.windows_name = 'frame' # 窗口名称 (Window name)
|
|
|
|
self.Mouse_XY = (0, 0)
|
|
|
|
self.Track_state = 'identify' # 追踪状态 (Tracking state)
|
|
|
|
|
|
|
|
self.color = color_follow() # 颜色跟随对象 (Color following object)
|
|
|
|
|
|
|
|
self.cols, self.rows = 0, 0 # 图像尺寸 (Image dimensions)
|
|
|
|
|
|
|
|
self.Mouse_XY = (0, 0) # 鼠标坐标 (Mouse coordinates)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 声明参数 (Declare parameters)
|
|
|
|
self.declare_param()
|
|
|
|
self.declare_param()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# HSV参数文件路径 (HSV parameter file path)
|
|
|
|
self.hsv_text = "/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_astra/yahboomcar_astra/colorHSV.text"
|
|
|
|
self.hsv_text = "/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_astra/yahboomcar_astra/colorHSV.text"
|
|
|
|
self.capture = cv.VideoCapture(0)
|
|
|
|
|
|
|
|
if cv_edition[0]=='3': self.capture.set(cv.CAP_PROP_FOURCC, cv.VideoWriter_fourcc(*'XVID'))
|
|
|
|
|
|
|
|
else: self.capture.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
|
|
|
|
|
|
|
|
self.timer = self.create_timer(0.001, self.on_timer)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 初始化摄像头 (Initialize camera)
|
|
|
|
|
|
|
|
self.capture = cv.VideoCapture(0)
|
|
|
|
|
|
|
|
if cv_edition[0] == '3':
|
|
|
|
|
|
|
|
self.capture.set(cv.CAP_PROP_FOURCC, cv.VideoWriter_fourcc(*'XVID'))
|
|
|
|
|
|
|
|
else:
|
|
|
|
|
|
|
|
self.capture.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 创建定时器 (Create timer)
|
|
|
|
|
|
|
|
self.timer = self.create_timer(0.001, self.on_timer)
|
|
|
|
|
|
|
|
|
|
|
|
def declare_param(self):
|
|
|
|
def declare_param(self):
|
|
|
|
#HSV
|
|
|
|
"""
|
|
|
|
self.declare_parameter("Hmin",0)
|
|
|
|
声明ROS参数
|
|
|
|
|
|
|
|
Declare ROS parameters
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
包含HSV颜色空间的最小值和最大值参数
|
|
|
|
|
|
|
|
Contains min and max parameters for HSV color space
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
# HSV最小值参数 (HSV minimum parameters)
|
|
|
|
|
|
|
|
self.declare_parameter("Hmin", 0) # 色调最小值 (Hue minimum)
|
|
|
|
self.Hmin = self.get_parameter('Hmin').get_parameter_value().integer_value
|
|
|
|
self.Hmin = self.get_parameter('Hmin').get_parameter_value().integer_value
|
|
|
|
self.declare_parameter("Smin",85)
|
|
|
|
self.declare_parameter("Smin", 85) # 饱和度最小值 (Saturation minimum)
|
|
|
|
self.Smin = self.get_parameter('Smin').get_parameter_value().integer_value
|
|
|
|
self.Smin = self.get_parameter('Smin').get_parameter_value().integer_value
|
|
|
|
self.declare_parameter("Vmin",126)
|
|
|
|
self.declare_parameter("Vmin", 126) # 明度最小值 (Value minimum)
|
|
|
|
self.Vmin = self.get_parameter('Vmin').get_parameter_value().integer_value
|
|
|
|
self.Vmin = self.get_parameter('Vmin').get_parameter_value().integer_value
|
|
|
|
self.declare_parameter("Hmax",9)
|
|
|
|
|
|
|
|
|
|
|
|
# HSV最大值参数 (HSV maximum parameters)
|
|
|
|
|
|
|
|
self.declare_parameter("Hmax", 9) # 色调最大值 (Hue maximum)
|
|
|
|
self.Hmax = self.get_parameter('Hmax').get_parameter_value().integer_value
|
|
|
|
self.Hmax = self.get_parameter('Hmax').get_parameter_value().integer_value
|
|
|
|
self.declare_parameter("Smax",253)
|
|
|
|
self.declare_parameter("Smax", 253) # 饱和度最大值 (Saturation maximum)
|
|
|
|
self.Smax = self.get_parameter('Smax').get_parameter_value().integer_value
|
|
|
|
self.Smax = self.get_parameter('Smax').get_parameter_value().integer_value
|
|
|
|
self.declare_parameter("Vmax",253)
|
|
|
|
self.declare_parameter("Vmax", 253) # 明度最大值 (Value maximum)
|
|
|
|
self.Vmax = self.get_parameter('Vmax').get_parameter_value().integer_value
|
|
|
|
self.Vmax = self.get_parameter('Vmax').get_parameter_value().integer_value
|
|
|
|
self.declare_parameter('refresh',False)
|
|
|
|
|
|
|
|
|
|
|
|
# 刷新参数 (Refresh parameter)
|
|
|
|
|
|
|
|
self.declare_parameter('refresh', False) # 是否刷新HSV参数 (Whether to refresh HSV parameters)
|
|
|
|
self.refresh = self.get_parameter('refresh').get_parameter_value().bool_value
|
|
|
|
self.refresh = self.get_parameter('refresh').get_parameter_value().bool_value
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def on_timer(self):
|
|
|
|
def on_timer(self):
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
定时器回调函数
|
|
|
|
|
|
|
|
Timer callback function
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
功能 (Functions):
|
|
|
|
|
|
|
|
- 读取摄像头图像
|
|
|
|
|
|
|
|
- 处理图像并进行颜色识别
|
|
|
|
|
|
|
|
- 显示处理结果
|
|
|
|
|
|
|
|
- 处理键盘输入
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
# 读取摄像头图像 (Read camera image)
|
|
|
|
ret, frame = self.capture.read()
|
|
|
|
ret, frame = self.capture.read()
|
|
|
|
action = cv.waitKey(10) & 0xFF
|
|
|
|
action = cv.waitKey(10) & 0xFF
|
|
|
|
frame, binary =self.process(frame, action)
|
|
|
|
|
|
|
|
|
|
|
|
# 处理图像 (Process image)
|
|
|
|
|
|
|
|
frame, binary = self.process(frame, action)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 计算并显示FPS (Calculate and display FPS)
|
|
|
|
start = time.time()
|
|
|
|
start = time.time()
|
|
|
|
end = time.time()
|
|
|
|
end = time.time()
|
|
|
|
fps = 1 / (end - start)
|
|
|
|
fps = 1 / (end - start)
|
|
|
|
text = "FPS : " + str(int(fps))
|
|
|
|
text = "FPS : " + str(int(fps))
|
|
|
|
cv.putText(frame, text, (30, 30), cv.FONT_HERSHEY_SIMPLEX, 0.6, (100, 200, 200), 1)
|
|
|
|
cv.putText(frame, text, (30, 30), cv.FONT_HERSHEY_SIMPLEX, 0.6, (100, 200, 200), 1)
|
|
|
|
if len(binary) != 0: cv.imshow('frame', ManyImgs(1, ([frame, binary])))
|
|
|
|
|
|
|
|
else:cv.imshow('frame', frame)
|
|
|
|
# 显示图像 (Display image)
|
|
|
|
|
|
|
|
if len(binary) != 0:
|
|
|
|
|
|
|
|
cv.imshow('frame', ManyImgs(1, ([frame, binary])))
|
|
|
|
|
|
|
|
else:
|
|
|
|
|
|
|
|
cv.imshow('frame', frame)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 处理退出命令 (Handle exit command)
|
|
|
|
if action == ord('q') or action == 113:
|
|
|
|
if action == ord('q') or action == 113:
|
|
|
|
self.capture.release()
|
|
|
|
self.capture.release()
|
|
|
|
cv.destroyAllWindows()
|
|
|
|
cv.destroyAllWindows()
|
|
|
|
|
|
|
|
|
|
|
|
def process(self, rgb_img, action):
|
|
|
|
def process(self, rgb_img, action):
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
图像处理主函数
|
|
|
|
|
|
|
|
Main image processing function
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Args:
|
|
|
|
|
|
|
|
rgb_img: 输入的RGB图像 (Input RGB image)
|
|
|
|
|
|
|
|
action: 键盘输入动作 (Keyboard input action)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Returns:
|
|
|
|
|
|
|
|
tuple: 处理后的图像和二值化图像 (Processed image and binary image)
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
# 更新参数 (Update parameters)
|
|
|
|
self.get_param()
|
|
|
|
self.get_param()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 调整图像大小 (Resize image)
|
|
|
|
rgb_img = cv.resize(rgb_img, (640, 480))
|
|
|
|
rgb_img = cv.resize(rgb_img, (640, 480))
|
|
|
|
binary = []
|
|
|
|
binary = []
|
|
|
|
if action == 32: self.Track_state = 'tracking'
|
|
|
|
|
|
|
|
elif action == ord('i') or action == ord('I'): self.Track_state = "identify"
|
|
|
|
# 处理键盘输入 (Handle keyboard input)
|
|
|
|
elif action == ord('r') or action == ord('R'): self.Reset()
|
|
|
|
if action == 32: # 空格键 - 切换到追踪模式 (Space key - switch to tracking mode)
|
|
|
|
elif action == ord('q') or action == ord('Q'): self.cancel()
|
|
|
|
self.Track_state = 'tracking'
|
|
|
|
|
|
|
|
elif action == ord('i') or action == ord('I'): # I键 - 切换到识别模式 (I key - switch to identify mode)
|
|
|
|
|
|
|
|
self.Track_state = "identify"
|
|
|
|
|
|
|
|
elif action == ord('r') or action == ord('R'): # R键 - 重置 (R key - reset)
|
|
|
|
|
|
|
|
self.Reset()
|
|
|
|
|
|
|
|
elif action == ord('q') or action == ord('Q'): # Q键 - 退出 (Q key - quit)
|
|
|
|
|
|
|
|
self.cancel()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 初始化模式 - 鼠标选择ROI (Initialization mode - mouse ROI selection)
|
|
|
|
if self.Track_state == 'init':
|
|
|
|
if self.Track_state == 'init':
|
|
|
|
cv.namedWindow(self.windows_name, cv.WINDOW_AUTOSIZE)
|
|
|
|
cv.namedWindow(self.windows_name, cv.WINDOW_AUTOSIZE)
|
|
|
|
cv.setMouseCallback(self.windows_name, self.onMouse, 0)
|
|
|
|
cv.setMouseCallback(self.windows_name, self.onMouse, 0)
|
|
|
|
|
|
|
|
|
|
|
|
if self.select_flags == True:
|
|
|
|
if self.select_flags == True:
|
|
|
|
|
|
|
|
# 绘制选择框 (Draw selection box)
|
|
|
|
cv.line(rgb_img, self.cols, self.rows, (255, 0, 0), 2)
|
|
|
|
cv.line(rgb_img, self.cols, self.rows, (255, 0, 0), 2)
|
|
|
|
cv.rectangle(rgb_img, self.cols, self.rows, (0, 255, 0), 2)
|
|
|
|
cv.rectangle(rgb_img, self.cols, self.rows, (0, 255, 0), 2)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 如果ROI有效,提取HSV范围 (If ROI is valid, extract HSV range)
|
|
|
|
if self.Roi_init[0] != self.Roi_init[2] and self.Roi_init[1] != self.Roi_init[3]:
|
|
|
|
if self.Roi_init[0] != self.Roi_init[2] and self.Roi_init[1] != self.Roi_init[3]:
|
|
|
|
rgb_img, self.hsv_range = self.color.Roi_hsv(rgb_img, self.Roi_init)
|
|
|
|
rgb_img, self.hsv_range = self.color.Roi_hsv(rgb_img, self.Roi_init)
|
|
|
|
self.gTracker_state = True
|
|
|
|
self.gTracker_state = True
|
|
|
|
self.dyn_update = True
|
|
|
|
self.dyn_update = True
|
|
|
|
else: self.Track_state = 'init'
|
|
|
|
else:
|
|
|
|
|
|
|
|
self.Track_state = 'init'
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 识别模式 - 从文件读取HSV参数 (Identify mode - read HSV parameters from file)
|
|
|
|
elif self.Track_state == "identify":
|
|
|
|
elif self.Track_state == "identify":
|
|
|
|
if os.path.exists(self.hsv_text): self.hsv_range = read_HSV(self.hsv_text)
|
|
|
|
if os.path.exists(self.hsv_text):
|
|
|
|
else: self.Track_state = 'init'
|
|
|
|
self.hsv_range = read_HSV(self.hsv_text)
|
|
|
|
|
|
|
|
else:
|
|
|
|
|
|
|
|
self.Track_state = 'init'
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 执行颜色识别 (Execute color recognition)
|
|
|
|
if self.Track_state != 'init':
|
|
|
|
if self.Track_state != 'init':
|
|
|
|
if len(self.hsv_range) != 0:
|
|
|
|
if len(self.hsv_range) != 0:
|
|
|
|
|
|
|
|
# 目标跟随处理 (Object following processing)
|
|
|
|
rgb_img, binary, self.circle = self.color.object_follow(rgb_img, self.hsv_range)
|
|
|
|
rgb_img, binary, self.circle = self.color.object_follow(rgb_img, self.hsv_range)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 动态更新HSV参数 (Dynamic update HSV parameters)
|
|
|
|
if self.dyn_update == True:
|
|
|
|
if self.dyn_update == True:
|
|
|
|
write_HSV(self.hsv_text, self.hsv_range)
|
|
|
|
write_HSV(self.hsv_text, self.hsv_range)
|
|
|
|
self.Hmin = rclpy.parameter.Parameter('Hmin',rclpy.Parameter.Type.INTEGER,self.hsv_range[0][0])
|
|
|
|
|
|
|
|
self.Smin = rclpy.parameter.Parameter('Smin',rclpy.Parameter.Type.INTEGER,self.hsv_range[0][1])
|
|
|
|
# 更新ROS参数 (Update ROS parameters)
|
|
|
|
self.Vmin = rclpy.parameter.Parameter('Vmin',rclpy.Parameter.Type.INTEGER,self.hsv_range[0][2])
|
|
|
|
self.Hmin = rclpy.parameter.Parameter('Hmin', rclpy.Parameter.Type.INTEGER, self.hsv_range[0][0])
|
|
|
|
self.Hmax = rclpy.parameter.Parameter('Hmax',rclpy.Parameter.Type.INTEGER,self.hsv_range[1][0])
|
|
|
|
self.Smin = rclpy.parameter.Parameter('Smin', rclpy.Parameter.Type.INTEGER, self.hsv_range[0][1])
|
|
|
|
self.Smax = rclpy.parameter.Parameter('Smax',rclpy.Parameter.Type.INTEGER,self.hsv_range[1][1])
|
|
|
|
self.Vmin = rclpy.parameter.Parameter('Vmin', rclpy.Parameter.Type.INTEGER, self.hsv_range[0][2])
|
|
|
|
self.Vmax = rclpy.parameter.Parameter('Vmax',rclpy.Parameter.Type.INTEGER,self.hsv_range[1][2])
|
|
|
|
self.Hmax = rclpy.parameter.Parameter('Hmax', rclpy.Parameter.Type.INTEGER, self.hsv_range[1][0])
|
|
|
|
all_new_parameters = [self.Hmin,self.Smin,self.Vmin,self.Hmax,self.Smax,self.Vmax]
|
|
|
|
self.Smax = rclpy.parameter.Parameter('Smax', rclpy.Parameter.Type.INTEGER, self.hsv_range[1][1])
|
|
|
|
|
|
|
|
self.Vmax = rclpy.parameter.Parameter('Vmax', rclpy.Parameter.Type.INTEGER, self.hsv_range[1][2])
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
all_new_parameters = [self.Hmin, self.Smin, self.Vmin, self.Hmax, self.Smax, self.Vmax]
|
|
|
|
self.set_parameters(all_new_parameters)
|
|
|
|
self.set_parameters(all_new_parameters)
|
|
|
|
|
|
|
|
self.dyn_update = False
|
|
|
|
|
|
|
|
|
|
|
|
self.dyn_update = False
|
|
|
|
# 追踪模式 - 发布目标位置 (Tracking mode - publish target position)
|
|
|
|
if self.Track_state == 'tracking':
|
|
|
|
if self.Track_state == 'tracking':
|
|
|
|
self.Start_state = True
|
|
|
|
self.Start_state = True
|
|
|
|
if self.circle[2] != 0: threading.Thread(
|
|
|
|
if self.circle[2] != 0:
|
|
|
|
target=self.execute, args=(self.circle[0], self.circle[1], self.circle[2])).start()
|
|
|
|
threading.Thread(target=self.execute, args=(self.circle[0], self.circle[1], self.circle[2])).start()
|
|
|
|
if self.point_pose[0] != 0 and self.point_pose[1] != 0: threading.Thread(
|
|
|
|
if self.point_pose[0] != 0 and self.point_pose[1] != 0:
|
|
|
|
target=self.execute, args=(self.point_pose[0], self.point_pose[1], self.point_pose[2])).start()
|
|
|
|
threading.Thread(target=self.execute, args=(self.point_pose[0], self.point_pose[1], self.point_pose[2])).start()
|
|
|
|
else:
|
|
|
|
else:
|
|
|
|
|
|
|
|
# 停止机器人运动 (Stop robot movement)
|
|
|
|
if self.Start_state == True:
|
|
|
|
if self.Start_state == True:
|
|
|
|
self.pub_cmdVel.publish(Twist())
|
|
|
|
self.pub_cmdVel.publish(Twist())
|
|
|
|
self.Start_state = False
|
|
|
|
self.Start_state = False
|
|
|
|
|
|
|
|
|
|
|
|
return rgb_img, binary
|
|
|
|
return rgb_img, binary
|
|
|
|
|
|
|
|
|
|
|
|
def execute(self, x, y, z):
|
|
|
|
def execute(self, x, y, z):
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
执行目标位置发布
|
|
|
|
|
|
|
|
Execute target position publishing
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Args:
|
|
|
|
|
|
|
|
x (float): 目标X坐标 (Target X coordinate)
|
|
|
|
|
|
|
|
y (float): 目标Y坐标 (Target Y coordinate)
|
|
|
|
|
|
|
|
z (float): 目标距离/半径 (Target distance/radius)
|
|
|
|
|
|
|
|
"""
|
|
|
|
position = Position()
|
|
|
|
position = Position()
|
|
|
|
position.anglex = x * 1.0
|
|
|
|
position.anglex = x * 1.0 # X轴角度 (X-axis angle)
|
|
|
|
position.angley = y * 1.0
|
|
|
|
position.angley = y * 1.0 # Y轴角度 (Y-axis angle)
|
|
|
|
position.distance = z * 1.0
|
|
|
|
position.distance = z * 1.0 # 距离 (Distance)
|
|
|
|
self.pub_position.publish(position)
|
|
|
|
self.pub_position.publish(position)
|
|
|
|
|
|
|
|
|
|
|
|
def get_param(self):
|
|
|
|
def get_param(self):
|
|
|
|
#hsv
|
|
|
|
"""
|
|
|
|
|
|
|
|
获取当前HSV参数
|
|
|
|
|
|
|
|
Get current HSV parameters
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
从ROS参数服务器获取最新的HSV颜色范围参数
|
|
|
|
|
|
|
|
Get latest HSV color range parameters from ROS parameter server
|
|
|
|
|
|
|
|
"""
|
|
|
|
self.Hmin = self.get_parameter('Hmin').get_parameter_value().integer_value
|
|
|
|
self.Hmin = self.get_parameter('Hmin').get_parameter_value().integer_value
|
|
|
|
self.Smin = self.get_parameter('Smin').get_parameter_value().integer_value
|
|
|
|
self.Smin = self.get_parameter('Smin').get_parameter_value().integer_value
|
|
|
|
self.Vmin = self.get_parameter('Vmin').get_parameter_value().integer_value
|
|
|
|
self.Vmin = self.get_parameter('Vmin').get_parameter_value().integer_value
|
|
|
@ -144,34 +280,65 @@ class Color_Identify(Node):
|
|
|
|
self.refresh = self.get_parameter('refresh').get_parameter_value().bool_value
|
|
|
|
self.refresh = self.get_parameter('refresh').get_parameter_value().bool_value
|
|
|
|
|
|
|
|
|
|
|
|
def Reset(self):
|
|
|
|
def Reset(self):
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
重置颜色识别系统
|
|
|
|
|
|
|
|
Reset color recognition system
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
清除HSV范围、目标圆形、鼠标坐标等状态
|
|
|
|
|
|
|
|
Clear HSV range, target circle, mouse coordinates and other states
|
|
|
|
|
|
|
|
"""
|
|
|
|
self.hsv_range = ()
|
|
|
|
self.hsv_range = ()
|
|
|
|
self.circle = (0, 0, 0)
|
|
|
|
self.circle = (0, 0, 0)
|
|
|
|
self.Mouse_XY = (0, 0)
|
|
|
|
self.Mouse_XY = (0, 0)
|
|
|
|
self.Track_state = 'init'
|
|
|
|
self.Track_state = 'init'
|
|
|
|
for i in range(3): self.pub_position.publish(Position())
|
|
|
|
# 发布空的位置信息 (Publish empty position information)
|
|
|
|
print("succes!!!")
|
|
|
|
for i in range(3):
|
|
|
|
|
|
|
|
self.pub_position.publish(Position())
|
|
|
|
|
|
|
|
print("Reset success!!!")
|
|
|
|
|
|
|
|
|
|
|
|
def cancel(self):
|
|
|
|
def cancel(self):
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
取消操作并关闭窗口
|
|
|
|
|
|
|
|
Cancel operation and close windows
|
|
|
|
|
|
|
|
"""
|
|
|
|
print("Shutting down this node.")
|
|
|
|
print("Shutting down this node.")
|
|
|
|
cv.destroyAllWindows()
|
|
|
|
cv.destroyAllWindows()
|
|
|
|
|
|
|
|
|
|
|
|
def onMouse(self, event, x, y, flags, param):
|
|
|
|
def onMouse(self, event, x, y, flags, param):
|
|
|
|
if event == 1:
|
|
|
|
"""
|
|
|
|
|
|
|
|
鼠标事件回调函数
|
|
|
|
|
|
|
|
Mouse event callback function
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Args:
|
|
|
|
|
|
|
|
event: 鼠标事件类型 (Mouse event type)
|
|
|
|
|
|
|
|
x, y: 鼠标坐标 (Mouse coordinates)
|
|
|
|
|
|
|
|
flags: 鼠标标志 (Mouse flags)
|
|
|
|
|
|
|
|
param: 参数 (Parameters)
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
if event == 1: # 鼠标左键按下 (Left mouse button pressed)
|
|
|
|
self.Track_state = 'init'
|
|
|
|
self.Track_state = 'init'
|
|
|
|
self.select_flags = True
|
|
|
|
self.select_flags = True
|
|
|
|
self.Mouse_XY = (x, y)
|
|
|
|
self.Mouse_XY = (x, y)
|
|
|
|
if event == 4:
|
|
|
|
|
|
|
|
|
|
|
|
if event == 4: # 鼠标左键释放 (Left mouse button released)
|
|
|
|
self.select_flags = False
|
|
|
|
self.select_flags = False
|
|
|
|
self.Track_state = 'mouse'
|
|
|
|
self.Track_state = 'mouse'
|
|
|
|
|
|
|
|
|
|
|
|
if self.select_flags == True:
|
|
|
|
if self.select_flags == True:
|
|
|
|
|
|
|
|
# 计算ROI区域 (Calculate ROI area)
|
|
|
|
self.cols = min(self.Mouse_XY[0], x), min(self.Mouse_XY[1], y)
|
|
|
|
self.cols = min(self.Mouse_XY[0], x), min(self.Mouse_XY[1], y)
|
|
|
|
self.rows = max(self.Mouse_XY[0], x), max(self.Mouse_XY[1], y)
|
|
|
|
self.rows = max(self.Mouse_XY[0], x), max(self.Mouse_XY[1], y)
|
|
|
|
self.Roi_init = (self.cols[0], self.cols[1], self.rows[0], self.rows[1])
|
|
|
|
self.Roi_init = (self.cols[0], self.cols[1], self.rows[0], self.rows[1])
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main():
|
|
|
|
def main():
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
主函数
|
|
|
|
|
|
|
|
Main function
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
初始化ROS节点并启动颜色识别功能
|
|
|
|
|
|
|
|
Initialize ROS node and start color identification functionality
|
|
|
|
|
|
|
|
"""
|
|
|
|
rclpy.init()
|
|
|
|
rclpy.init()
|
|
|
|
color_identify = Color_Identify("ColorIdentify")
|
|
|
|
color_identify = Color_Identify("ColorIdentify")
|
|
|
|
print("start it")
|
|
|
|
print("Color identification node started")
|
|
|
|
rclpy.spin(color_identify)
|
|
|
|
rclpy.spin(color_identify)
|
|
|
|