关于发布订阅模型订阅者回调函数里放发布者，无 .publish功能的问题
-
#!/usr/bin/env python # -*- coding: utf-8 -*- import rclpy from rclpy 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 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) dis_pub = rclpy.Node.create_publisher(PoseStamped,'/dis',10) time_pub = rclpy.Node.create_publisher(PoseStamped,'/time',10) frame_count = 1 class templatematching(Node): def __init__(self,name): super().__init__(name) #rospy.on_shutdown(self.cleanup) self.bridge = CvBridge() self.image_pub = self.create_publisher(Image, "cv_bridge_image", queue_size=1) self.image_sub = self.create_subscription(Image, "/usb_cam/image_raw", self.image_callback, queue_size=1) 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() 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_score = cv2.matchTemplate(search_field, self.template, method=cv2.TM_CCOEFF_NORMED) match_end_time = rclpy.get_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() 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.pose.position.x = diff_x_u dis_msg.pose.position.y = diff_y_v dis_pub.publish(dis_msg) time_msg = PoseStamped() time_msg.header.frame_id = "time" time_msg.header.stamp = rclpy.Time.now() time_msg.pose.position.x = dt time_msg.pose.position.y = match_time 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.") rclpy.spin(node) rclpy.shutdown() except KeyboardInterrupt: print ("Shutting down motion detector node.") cv2.destroyAllWindows() if __name__ == '__main__': main()
!
-
@熵 ROS1里可以这样做，但是ros2不行，ros1在设计的时候开了多个线程，对使用规范要求很低，ros2的代码做了很多优化，比如同一个节点默认只开了一个线程，所以无法像ros1那样可以随便写。你的回调函数写了太多的内容，估计要用多线程回调组才行，我简单针对你的问题改了下，你还需要加上一个回调组的设置。
在回调函数中发布数据方式参考代码如下：
import rclpy from rclpy.node import Node from std_msgs.msg import String,UInt32 from village_interfaces.srv import SellNovel from queue import Queue,LifoQueue,PriorityQueue from rclpy.executors import MultiThreadedExecutor from rclpy.callback_groups import MutuallyExclusiveCallbackGroup class TestNode(Node): """ 对象编写 """ def __init__(self,name): super().__init__(name) self.get_logger().info("大家好，我是%s,我是一名订阅者！" % name) self.pub_money = self.create_publisher(UInt32,"sexy_girl_money", 10) # 声明回调组 self.sell_novel_callback_group = MutuallyExclusiveCallbackGroup() self.sell_service = self.create_subscription(String,"sexy_girl", self.recv_msg_callback, 10,callback_group=self.sell_novel_callback_group) def recv_msg_callback(self,msg): self.msg = msg.data self.get_logger().info('穷汉王五：我已经收到了李四的稿子，%s' % self.msg) # 组装发布数据 money = UInt32() money.data = 10 self.pub_money.publish(money) #将金钱发给李四 self.queue_list.put(self.msg) def main(args=None): rclpy.init(args=args) # 初始化rclpy node = TestNode("wangwu") # 新建一个节点 executor = MultiThreadedExecutor() executor.add_node(node) executor.spin() rclpy.shutdown() # 关闭rclpy -
@小鱼
鱼哥，我看了下官方文档后，简单加了回调组的声明，并改了一下有关time的部分。不知怎么的
self.image_pub.publish
self.time_pub.publish(time_msg)
self.dis_pub.publish(dis_msg)
这三个中,self.image_pub.publish的 .publish是黄色，正常调用，另两个是白色。
我编译运行后，果然报错了，不过我看报的错误和.publish貌似没啥关系，我注释掉回调组，问题没有任何变化。修改后代码和编译报错如下#!/usr/bin/env python # -*- coding: utf-8 -*- import rclpy from rclpy 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): # super().__init__(name) #rospy.on_shutdown(self.cleanup) self.bridge = CvBridge() #声明发布者 self.dis_pub = rclpy.Node.create_publisher(PoseStamped,'/dis',10) self.time_pub = rclpy.Node.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.") rclpy.spin(node) rclpy.shutdown() except KeyboardInterrupt: print ("Shutting down motion detector node.") cv2.destroyAllWindows() if __name__ == '__main__': main()rr@rr-TM1709:~/d2lros2/chapt2/photo_ws$ colcon build Starting >>> example_py --- stderr: example_py /usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools. warnings.warn( --- Finished <<< example_py [1.08s] Summary: 1 package finished [1.39s] 1 package had stderr output: example_py rr@rr-TM1709:~/d2lros2/chapt2/photo_ws$ source install/setup.bash rr@rr-TM1709:~/d2lros2/chapt2/photo_ws$ ros2 run example_py node_01 Traceback (most recent call last): File "/home/rr/d2lros2/chapt2/photo_ws/install/example_py/lib/example_py/node_01", line 33, in <module> sys.exit(load_entry_point('example-py==0.0.0', 'console_scripts', 'node_01')()) File "/home/rr/d2lros2/chapt2/photo_ws/install/example_py/lib/example_py/node_01", line 25, in importlib_load_entry_point return next(matches).load() File "/usr/lib/python3.10/importlib/metadata/__init__.py", line 171, in load module = import_module(match.group('module')) File "/usr/lib/python3.10/importlib/__init__.py", line 126, in import_module return _bootstrap._gcd_import(name[level:], package, level) File "<frozen importlib._bootstrap>", line 1050, in _gcd_import File "<frozen importlib._bootstrap>", line 1027, in _find_and_load File "<frozen importlib._bootstrap>", line 1006, in _find_and_load_unlocked File "<frozen importlib._bootstrap>", line 688, in _load_unlocked File "<frozen importlib._bootstrap_external>", line 883, in exec_module File "<frozen importlib._bootstrap>", line 241, in _call_with_frames_removed File "/home/rr/d2lros2/chapt2/photo_ws/install/example_py/lib/python3.10/site-packages/example_py/node_01.py", line 4, in <module> from rclpy import Node ImportError: cannot import name 'Node' from 'rclpy' (/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py) [ros2run]: Process exited with failure 1 -
@熵 在 关于发布订阅模型订阅者回调函数里放发布者，无 .publish功能的问题 中说：
self.dis_pub = rclpy.Node.create_publisher(PoseStamped,'/dis',10)
self.time_pub = rclpy.Node.create_publisher(PoseStamped,'/time',10)
self.image_pub = self.create_publisher(Image, "cv_bridge_image", 1)正确创建发布者的方式是第三行，使用self.create_publisher,而不是rclpy.Node.create_publisher
-
@小鱼 嗯嗯，哥，改了后变黄了。
果然，编译的报错和发布者一点关系没有....难难难，我去搜搜 -
@小鱼 在 关于发布订阅模型订阅者回调函数里放发布者，无 .publish功能的问题 中说：
from rclpy.node import Node
正确的导入node方式是这样的，要细心些，仔细对比即可发现
-
@小鱼
谢谢鱼哥，我好粗心 -
最新代码，能够正常编译，运行时没报错。
但好像无法与相机发布者连接，正在查找原因#!/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()
