that is my code
#!/usr/bin/env python
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import PointStamped, Point
from std_msgs.msg import Header
class CircleDetection:
def __init__(self):
rospy.init_node('circle_detection_node', anonymous=True)
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
self.camera_info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.camera_info_callback)
self.center_pub = rospy.Publisher("/circle_center_3d", PointStamped, queue_size=1)
self.bridge = CvBridge()
self.K = None # 内参矩阵
self.depth_image = None
def camera_info_callback(self, data):
# 保存内参矩阵
self.K = np.array(data.K).reshape((3, 3))
def depth_callback(self, data):
try:
# 将深度图像转换为OpenCV格式
self.depth_image = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
except CvBridgeError as e:
print(e)
def image_callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
return
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (15, 15), 0)
circles = cv2.HoughCircles(
gray, cv2.HOUGH_GRADIENT, dp=1, minDist=20, param1=50, param2=30, minRadius=0, maxRadius=50
)
if circles is not None and self.K is not None and self.depth_image is not None:
circles = np.uint16(np.around(circles))
for i in circles[0, :]:
center = (i[0], i[1])
cv2.circle(cv_image, center, 1, (0, 100, 100), 3)
cv2.circle(cv_image, center, i[2], (255, 0, 0), 2)
# 获取深度信息
depth_value = self.depth_image[i[1], i[0]]
# 使用内参矩阵进行坐标转换
pixel_coordinates = np.array([[i[0], i[1]]], dtype='float32')
undistorted_pixel = cv2.undistortPoints(pixel_coordinates, self.K, None)
u, v = undistorted_pixel[0, 0]
# 计算三维坐标
x = (u - self.K[0, 2]) * depth_value / self.K[0, 0]
y = (v - self.K[1, 2]) * depth_value / self.K[1, 1]
z = depth_value
# 发布三维坐标
header = Header()
header.stamp = rospy.Time.now()
point_stamped = PointStamped(header=header, point=Point(x, y, z))
self.center_pub.publish(point_stamped)
cv2.imshow("Circle Detection", cv_image)
cv2.waitKey(3)
if __name__ == '__main__':
try:
circle_detection = CircleDetection()
rospy.spin()
except rospy.ROSInterruptException:
pass
During runtime, the target can be detected, but when printed to the terminal, it cannot be converted.
header:
seq: 6
stamp:
secs: 292
nsecs: 277000000
frame_id: ''
point:
x: nan
y: nan
z: nan
I am a beginner, and I would be extremely grateful if you could provide assistance.