testrelasense

mail@pastecode.io avatar
unknown
plain_text
2 months ago
2.3 kB
17
Indexable
Never
#!/usr/bin/env python

# Capturing synchronized images from multiple Intel RealSense cameras

import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from apriltag_ros.msg import AprilTagDetectionArray

# Initialize the X Window System for multi-threading
cv2.startWindowThread()

class image_converter:
    def __init__(self):
        self.bridge = CvBridge()
        self.cam1_image = None
        self.cam2_image = None

        rospy.Subscriber('/apriltag_sync/camsync', AprilTagDetectionArray, self.callback)

    def callback(self, msg):
        if len(msg.detections) >= 2:
            self.cam1_image = self.bridge.imgmsg_to_cv2(msg.detections[0].image, "bgr8")
            self.cam2_image = self.bridge.imgmsg_to_cv2(msg.detections[1].image, "bgr8")

            cv2.imshow("Camera 1", self.cam1_image)
            cv2.imshow("Camera 2", self.cam2_image)
            cv2.waitKey(1)

def main():
    rospy.init_node('image_converter', anonymous=True)
    ic = image_converter()
    rospy.spin()

if __name__ == '__main__':
    main()




import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

def color_callback(msg):
    # Process the synchronized color image
    bridge = CvBridge()
    cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
    # Do something with the image

def depth_callback(msg):
    # Process the synchronized depth image
    bridge = CvBridge()
    cv_image = bridge.imgmsg_to_cv2(msg, "16UC1")
    # Do something with the image

rospy.init_node('sync_node')
color_sub = rospy.Subscriber('/apriltag_sync/camsync', Image, color_callback)
depth_sub = rospy.Subscriber('/apriltag_sync/detsync', Image, depth_callback)
rospy.spin()


<launch>
  <!-- Launch camera nodes -->
  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="camera" value="cam_1" />
  </include>
  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="camera" value="cam_2" />
  </include>

  <!-- Launch the apriltag synchronizer node -->
  <node pkg="apriltag_ros" type="apriltag_sync" name="apriltag_sync" output="screen">
    <remap from="/cam_1/color/image_raw" to="/cam_1/image_raw" />
    <remap from="/cam_2/color/image_raw" to="/cam_2/image_raw" />
  </node>
</launch>




Leave a Comment