How do i subscribe to two topics in a ros service?

asked 2023-05-12 12:15:56 -0500

hamzaay gravatar image

I want to make a snapshot of a scene and for this I need to subsribe to two topics. I want to make a service take a picture, use the scene and call that service again when the scene updates. How do I do that? I have a code that can subscribe to the topics and I have a made a service but I cannot combine the two i.e put the subribers inside the servers. How do I this? This is my code. Its ros1.

Service:

#!/usr/bin/env python
from intention_application.srv import snapshot
import rospy
import numpy as np  

def callback(req):
    return True

def takesnapshot():
    rospy.init_node("Snap_The_Scene",anonymous=True)
    service=rospy.Service("response", snapshot,callback)
    rospy.spin()

if __name__=="__main__":

    takesnapshot()

snapshot does nothing except sending a bool.

this is my subscriber:

#!/usr/bin/env python
# license removed for brevity

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import open3d as o3d
import numpy as np

import message_filters


def callback(rgb : Image, depth : Image):
    cv_bridge = CvBridge()
    rgb_img = cv_bridge.imgmsg_to_cv2(rgb, "rgb8")
    depth_img = cv_bridge.imgmsg_to_cv2(depth, "16UC1")
    mtx= np.array([[570.3405082258201, 0.0, 319.5], [0.0, 570.3405082258201, 239.5], [0.0, 0.0, 1.0]])
    Im_Rgbd=o3d.geometry.RGBDImage.create_from_color_and_depth(o3d.geometry.Image(rgb_img), o3d.geometry.Image(depth_img),convert_rgb_to_intensity=False)
    intrinsic=o3d.camera.PinholeCameraIntrinsic(640,480 ,mtx[0][0],mtx[1][1],mtx[0][2],mtx[1][2])
    print("we here broooo")


def ImageFeed():
    rospy.init_node("Camera_Feed",anonymous=True)
    rgb_sub=message_filters.Subscriber("camera/color/image_raw",Image)
    depth_sub = message_filters.Subscriber('/camera/depth/image_raw', Image)
    ts = message_filters.ApproximateTimeSynchronizer([rgb_sub,depth_sub],10,1)
    ts.registerCallback(callback)

    rospy.spin()


if __name__ == '__main__':
    print("HE")
    ImageFeed()

Is my approach completly wrong?

edit retag flag offensive close merge delete