鱼香ROS社区
    • 版块
    • 最新
    • 未解决
    • 已解决
    • 群组
    • 注册
    • 登录
    紧急通知:禁止一切关于政治&VPN翻墙等话题,发现相关帖子会立马删除封号
    提问前必看的发帖注意事项: 社区问答规则(小鱼个人)更新 | 高质量帖子发布指南

    关于发布订阅模型订阅者回调函数里放发布者,无 .publish功能的问题

    已定时 已固定 已锁定 已移动
    ROS 2相关问题
    ros2 发布订阅
    2
    8
    751
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • 熵熵
      熵
      最后由 小鱼 编辑

      #!/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()
      

      143_1658843633_hd.jpeg

      !144_1658843654_hd.jpeg

      小鱼小 1 条回复 最后回复 回复 引用 0
      • 小鱼小
        小鱼 技术大佬 @熵
        最后由 编辑

        @熵 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
        
        

        新书配套视频:https://www.bilibili.com/video/BV1GW42197Ck/

        熵熵 小鱼小 2 条回复 最后回复 回复 引用 0
        • 熵熵
          熵 @小鱼
          最后由 编辑

          @小鱼
          鱼哥,我看了下官方文档后,简单加了回调组的声明,并改了一下有关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
          
          小鱼小 1 条回复 最后回复 回复 引用 0
          • 小鱼小
            小鱼 技术大佬 @熵
            最后由 编辑

            @熵 在 关于发布订阅模型订阅者回调函数里放发布者,无 .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

            新书配套视频:https://www.bilibili.com/video/BV1GW42197Ck/

            熵熵 1 条回复 最后回复 回复 引用 0
            • 熵熵
              熵 @小鱼
              最后由 编辑

              @小鱼 嗯嗯,哥,改了后变黄了。
              果然,编译的报错和发布者一点关系没有....难难难,我去搜搜

              1 条回复 最后回复 回复 引用 0
              • 小鱼小
                小鱼 技术大佬 @小鱼
                最后由 编辑

                @小鱼 在 关于发布订阅模型订阅者回调函数里放发布者,无 .publish功能的问题 中说:

                from rclpy.node import Node

                正确的导入node方式是这样的,要细心些,仔细对比即可发现

                新书配套视频:https://www.bilibili.com/video/BV1GW42197Ck/

                熵熵 1 条回复 最后回复 回复 引用 0
                • 熵熵
                  熵 @小鱼
                  最后由 编辑

                  @小鱼 😥 😥 😥 谢谢鱼哥,我好粗心

                  1 条回复 最后回复 回复 引用 0
                  • 熵熵
                    熵
                    最后由 编辑

                    最新代码,能够正常编译,运行时没报错。
                    但好像无法与相机发布者连接,正在查找原因

                    #!/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()
                    
                    1 条回复 最后回复 回复 引用 0
                    • 第一个帖子
                      最后一个帖子
                    皖ICP备16016415号-7
                    Powered by NodeBB | 鱼香ROS