最新代码，能够正常编译，运行时没报错。
但好像无法与相机发布者连接，正在查找原因
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped
from cv_bridge import CvBridge,CvBridgeError
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
import message_filters
import time
import math
import pickle
#dis_pub = rclpy.Publisher('/dis',PoseStamped,queue_size=10)
#time_pub = rclpy.Publisher('/time',PoseStamped,queue_size=10)
frame_count = 1
class templatematching(Node):
def __init__(self):
print("02")
self.get_logger().info("01")
# super().__init__(name)
#rospy.on_shutdown(self.cleanup)
self.bridge = CvBridge()
#声明发布者
self.dis_pub = self.create_publisher(PoseStamped,'/dis',10)
self.time_pub = self.create_publisher(PoseStamped,'/time',10)
self.image_pub = self.create_publisher(Image, "cv_bridge_image", 1)
#声明回调组
self.image_sub_callback_group = MutuallyExclusiveCallbackGroup()
self.image_sub = self.create_subscription(Image, "/usb_cam/image_raw", self.image_callback, 1,callback_group=self.image_sub_callback_group)
self.firstFrame = None
self.template = None
def image_callback(self, data):
global frame_count
focal_length_x = 446.6 #相机x，y方向的焦距
focal_length_y = 445.8
camera_height = 255
w = 240 # 模板宽度
h = 240 # 模板长度
w_2 = int(w / 2) # 模板宽度一半
h_2 = int(h / 2) # 模板长度一半
offset_x = 30
offset_y_up = 100 # 上侧
offset_y_down = 100 # control search field
cumulative_u = 0.0
cumulative_v = 0.0
cumulative_pixel = 0.0
# last_time = rclpy.get_time()
last_time = time.time()
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
frame = np.array(cv_image, dtype=np.uint8)
except CvBridgeError as e:
print(e)
if self.firstFrame is None:
self.firstFrame = frame
return
if frame_count == 1:
self.template = self.firstFrame[240 - h_2: 240 + h_2 + 1,320 - w_2: 320 + w_2 + 1] #选取模板240*240
frame_count += 1
return
y = np.shape(frame)[0] # Current frame 行数 图像长度 ?
x = np.shape(frame)[1] #列数 图像宽度 ?
cv2.circle(frame, (320 - w_2, 240 - h_2), 5, (0, 0, 255), 2)
search_field = frame[240 - h_2 - offset_y_up: 240 + h_2 + offset_y_down + 1, 320 - w_2 - offset_x: 320 + w_2 + offset_x + 1]
#search_field = frame[320 - w_2 - offset_x : 320 + w_2 + offset_x + 1, 240 - h_2 - offset_y_up: 240 + h_2 + offset_y_down + 1]
cv2.rectangle(frame, (320 - w_2 - offset_x, 240 - h_2 - offset_y_up), (320 + w_2 + offset_x, 240 + h_2 + offset_y_down), (0, 0, 128), 2)
# match_start_time = rclpy.get_time()
match_start_time = time.time()
match_score = cv2.matchTemplate(search_field, self.template, method=cv2.TM_CCOEFF_NORMED)
# match_end_time = rclpy.get_time()
match_end_time = time.time()
match_time = match_end_time - match_start_time
#rospy.loginfo(match_time)
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(match_score)
top_left = (max_loc[0] + 320 - w_2 - offset_x, max_loc[1] + 240 - h_2 - offset_y_up) # 转换成全局坐标(缩小搜索区域时需要改动)
bottom_right = (top_left[0] + w, top_left[1] + h)
cv2.rectangle(frame, top_left, bottom_right, 255, 2)
last_frame_top_left = (int(x / 2 - w / 2), int(y / 2 - h / 2))
cv2.line(frame, top_left, last_frame_top_left, (0, 255, 0), 2)
cv2.imshow("Image window",frame)
cv2.waitKey(3)
diff_u = top_left[0] - last_frame_top_left[0] #水平，竖直方向位移
diff_v = top_left[1] - last_frame_top_left[1]
diff_pixel = (diff_u ** 2 + diff_v ** 2) ** 0.5 # **表示乘方
cumulative_u = cumulative_u + diff_u
cumulative_v = cumulative_v + diff_v
cumulative_pixel = cumulative_pixel + diff_pixel
diff_x_c = (diff_u) / focal_length_x * camera_height
diff_y_c = (diff_v) / focal_length_y * camera_height
diff_x_u = -diff_x_c #图像坐标系转相机坐标系
diff_y_v = diff_y_c #相机坐标系转机器人坐标系
#self.template = frame[320 - w_2: 320 + w_2 + 1,240 - h_2: 240 + h_2 + 1] #选取模板240*240
self.template = frame[240 - h_2: 240 + h_2 + 1, 320 - w_2: 320 + w_2 + 1]
self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
# current_time = rclpy.get_time()
current_time = time.time()
dt = current_time - last_time
#rospy.loginfo(dt)
dis_msg = PoseStamped()
dis_msg.header.frame_id = "displacement"
# dis_msg.header.stamp = rclpy.Time.now()
dis_msg.header.stamp = time.time()
dis_msg.pose.position.x = diff_x_u
dis_msg.pose.position.y = diff_y_v
self.dis_pub.publish(dis_msg)
time_msg = PoseStamped()
time_msg.header.frame_id = "time"
# time_msg.header.stamp = rclpy.Time.now()
time_msg.header.stamp = time.time()
time_msg.pose.position.x = dt
time_msg.pose.position.y = match_time
self.time_pub.publish(time_msg)
#if __name__ == '__main__':
def main(args =None):
try:
# 初始化ros节点
# rclpy.init_node("templatematching")
# rclpy.loginfo("templatematching node is started...")
# rclpy.loginfo("Please subscribe the ROS image.")
rclpy.init(args=args)
node = rclpy.create_node("templatematching")
node.get_logger().info("templatematching node is started...")
node.get_logger().info("Please subscribe the ROS image.")
print("01")
rclpy.spin(node)
rclpy.shutdown()
except KeyboardInterrupt:
print ("Shutting down motion detector node.")
cv2.destroyAllWindows()
if __name__ == '__main__':
main()
