ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

Subscriber python CameraInfo and Image

asked 2019-04-20 06:39:14 -0500

usamamaq gravatar image

Hello i am new to ros and python. Need little help in how to subscribe to CameraInfo and Image of format sensor_msgs.msg and use its data for further image processing. Here are few lines of code that i am starting with:

from sensor_msgs.msg import CameraInfo, Image
from cv_bridge import CvBridge, CvBridgeError
import cv2

if __name__ == '__main__':
   rospy.init_node('node_name')
   while ~rospy.is_shutdown():
   sub_cam_info = rospy.Subscriber('/camera/rgb/raw_camera_info', CameraInfo)
   sub_rgb = rospy.Subscriber('/camera/rgb/raw_image_color', Image)

From here i want to extract header and data information from 'sub_cam_info' and 'sub_rgb'. Something like this:

camera_info_K = sub_cam_info.K
camera_info_dist_model = sub_cam_info.distortion_model
rgb_image = CvBridge().imgmsg_to_cv2(sub_rgb, encoding="rgb8")

And then use this data for image undistortion:-

rgb_undist = cv2.undistort(rgb_img.data, camera_info_K, camera_info_dist_model)

Mainly i need help in second part i.e. extraction of parameters from CameraInfo and Image msgs. However if someone can help me to sort out body of this whole code! I am already through making up my own CameraInfo and rgb messages, using them for undistortion and publishing on ROS by but now i want to get them from another ROS module and work with them.

Thanks

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2019-08-09 01:37:49 -0500

usamamaq gravatar image

Ok as many people has viewed this so i thought of answering my own question. I solved it like this.

import message_filters
import cv2
import rospy
from cv_bridge import CvBridge

def callback(rgb_msg, camera_info):
   rgb_image = CvBridge().imgmsg_to_cv2(rgb_msg, desired_encoding="rgb8")
   camera_info_K = np.array(camera_info.K).reshape([3, 3])
   camera_info_D = np.array(camera_info.D)
   rgb_undist = cv2.undistort(rgb_image, camera_info_K, camera_info_D)

if __name__ == '__main__':
   rospy.init_node('my_node', anonymous=True)
   image_sub = message_filters.Subscriber('/ardrone/front/image_raw', Image)
   info_sub = message_filters.Subscriber('/ardrone/front/camera_info', CameraInfo)
   ts = message_filters.ApproximateTimeSynchronizer([image_sub, info_sub], 10, 0.2)
   ts.registerCallback(callback)
   rospy.spin()

So my problem was how to subscribe on two different topics and use their data for further processing. "message_filters.ApproximateTimeSynchronizer" synchronized the incoming messages according to the time stamp with each message being received and with "ts.registerCallback(callback)" i was able to use both subscribed messages together for further processing in callback function.

edit flag offensive delete link more

Comments

if this solved your issue you can accept your answer as correct by clicking the tick mark

pavel92 gravatar image pavel92  ( 2019-08-09 02:08:45 -0500 )edit

If the camera driver is written properly, the image and its camera info should have the exactly same timestamps, so you should use ExactTime policy instead of ApproximateTime.

peci1 gravatar image peci1  ( 2021-04-28 08:38:17 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2019-04-20 06:39:14 -0500

Seen: 6,509 times

Last updated: Aug 09 '19