@小鱼 谢谢鱼哥
重要提示
鱼香小铺正式开业,最低499可入手一台能建图会导航的移动机器人,淘宝搜店:鱼香ROS 或点击链接查看。
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
熵 发布的最新帖子
-
关于PoseStamped数据转path数据可行性问题
请问下,接受PoseStamped数据后可以直接将其作为Path数据的子部分发布出去然后在rviz中显示吗,还是需要做什么特殊操作
-
RE: 关于发布订阅模型订阅者回调函数里放发布者,无 .publish功能的问题
最新代码,能够正常编译,运行时没报错。
但好像无法与相机发布者连接,正在查找原因#!/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()
-
RE: 关于发布订阅模型订阅者回调函数里放发布者,无 .publish功能的问题
@小鱼
鱼哥,我看了下官方文档后,简单加了回调组的声明,并改了一下有关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功能的问题
#!/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()
!