realsense订阅无法进入sub_camera_callback

五谷鱼粉

#!/usr/bin/env python3
import time
import message_filters
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from example_interfaces.srv import AddTwoInts

class MinimalService(Node):
    def __init__(self):
        super().__init__('minimal_service_node')
        self.srv = self.create_service(AddTwoInts, 'camera_sub_add_two_ints', self.add_two_ints_callback)

    def add_two_ints_callback(self, request, response):
        response.sum = request.a + request.b
        flag, rgb, depth = self.sub_camera()
        return response

    def sub_camera(self):
        rgb_subscriber = message_filters.Subscriber(self, Image, "/camera/color/image_raw")
        depth_subscriber = message_filters.Subscriber(self, Image, "/camera/depth/image_rect_raw")

        ts = message_filters.ApproximateTimeSynchronizer(
            fs=[rgb_subscriber, depth_subscriber],
            queue_size=20, slop=0.03)
        vision_rgb_data, vision_depth_data = None, None
        ts.registerCallback(self.sub_camera_callback, vision_rgb_data, vision_depth_data)

        subscription_deadline = time.time() + 3.0

        flag = True
        while not all([vision_rgb_data, vision_depth_data]):
            time.sleep(0.05)
            if time.time() >= subscription_deadline:
                print('Vision data subscription timeout.')
                flag = False
                break

        del ts
        self.destroy_subscription(rgb_subscriber.sub)
        self.destroy_subscription(depth_subscriber.sub)

        print('Vision data subscription finished.')
        return flag, vision_rgb_data, vision_depth_data

    def sub_camera_callback(self, rgb_msg, depth_msg, vision_rgb_data, vision_depth_data):
        print("----------")
        vision_rgb_data = rgb_msg
        vision_depth_data = depth_msg


def main(args=None):
    rclpy.init(args=args)
    minimal_service = MinimalService()
    rclpy.spin(minimal_service)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

五谷鱼粉

想要做的是，client端请求一个服务，然后在服务中先去订阅realsense的rgbd数据；之前存在的问题是，如果频繁的去调用这个订阅图像的函数，就会发生订阅不到的情况，然后至此开始，再怎么请求这个服务，都没法同时订阅rgb和d的数据。

然后我尝试先把这部分代码拆出来，结果发现现在上面的那段代码连订阅图像的那个call back函数都进不去了...可以确定的是，rgbd两个topic的名字是正确的。

五谷鱼粉

命令行发这个就可以调用上面的服务

ros2 service call /camera_sub_add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}"

小鱼

@renpei 如果是高频率订阅数据，还是不要反复订阅销毁，这样消耗的资源会更多

五谷鱼粉

@小鱼在 realsense订阅无法进入sub_camera_callback 中说：

是不要反复订阅销毁，这样消耗的

谢谢鱼总回复。你那边有realsense可以帮忙试试吗？另外一个就是，其实我自己尝试的时候，也就手动一秒一次这样调用这个服务，这算是比较频繁的调用吗？可能实际使用中不会这样。

另外就是如果不做销毁的操作的话，好像就会主线程一直被rgbd两个话题的订阅操作所占用，没法进行之后的操作。

小鱼

@renpei 我这边没有realsense哦，可以把数据接收用单独的callbackgroup