75 lines
2.0 KiB
Python
75 lines
2.0 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
from __future__ import print_function
|
||
|
|
|
||
|
|
import sys
|
||
|
|
import rospy
|
||
|
|
import cv2
|
||
|
|
from std_msgs.msg import String
|
||
|
|
from sensor_msgs.msg import Image
|
||
|
|
from cv_bridge import CvBridge, CvBridgeError
|
||
|
|
|
||
|
|
class image_converter:
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10)
|
||
|
|
self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10)
|
||
|
|
|
||
|
|
self.bridge = CvBridge()
|
||
|
|
self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0)
|
||
|
|
self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1)
|
||
|
|
|
||
|
|
def callback_cam0(self,data):
|
||
|
|
try:
|
||
|
|
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||
|
|
except CvBridgeError as e:
|
||
|
|
print(e)
|
||
|
|
|
||
|
|
imgScale = 0.25
|
||
|
|
newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
|
||
|
|
newimg = cv2.resize(cv_image,(int(newX),int(newY)))
|
||
|
|
|
||
|
|
newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
|
||
|
|
newdata = data
|
||
|
|
newdata.height = newpub.height
|
||
|
|
newdata.width = newpub.width
|
||
|
|
newdata.step = newpub.step
|
||
|
|
newdata.data = newpub.data
|
||
|
|
try:
|
||
|
|
self.image0_pub.publish(newdata)
|
||
|
|
except CvBridgeError as e:
|
||
|
|
print(e)
|
||
|
|
|
||
|
|
def callback_cam1(self,data):
|
||
|
|
try:
|
||
|
|
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||
|
|
except CvBridgeError as e:
|
||
|
|
print(e)
|
||
|
|
|
||
|
|
imgScale = 0.25
|
||
|
|
newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
|
||
|
|
newimg = cv2.resize(cv_image,(int(newX),int(newY)))
|
||
|
|
|
||
|
|
newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
|
||
|
|
newdata = data
|
||
|
|
newdata.height = newpub.height
|
||
|
|
newdata.width = newpub.width
|
||
|
|
newdata.step = newpub.step
|
||
|
|
newdata.data = newpub.data
|
||
|
|
|
||
|
|
try:
|
||
|
|
self.image1_pub.publish(newdata)
|
||
|
|
except CvBridgeError as e:
|
||
|
|
print(e)
|
||
|
|
|
||
|
|
|
||
|
|
def main(args):
|
||
|
|
ic = image_converter()
|
||
|
|
rospy.init_node('image_converter', anonymous=True)
|
||
|
|
try:
|
||
|
|
rospy.spin()
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
print("Shutting down")
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main(sys.argv)
|