Created
January 26, 2021 08:36
-
-
Save naoki-mizuno/eb4069a72c05b38ead5646963453fb40 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python | |
| import rospy | |
| from sensor_msgs.msg import Image | |
| from cv_bridge import CvBridge | |
| import cv2 | |
| import numpy as np | |
| import argparse | |
| import copy | |
| import math | |
| def parse_args(): | |
| description = "publish image file to a topic" | |
| parser = argparse.ArgumentParser( | |
| description=description, | |
| add_help=False, | |
| ) | |
| parser.add_argument( | |
| "-h", | |
| "--help", | |
| action="help", | |
| help="show this help message and exit", | |
| ) | |
| parser.add_argument( | |
| "-r", | |
| "--rate", | |
| default=None, | |
| help="rate at which to publish the image (default: one-shot)", | |
| ) | |
| parser.add_argument( | |
| "-t", | |
| "--topic", | |
| default="image", | |
| help="output topic name", | |
| ) | |
| parser.add_argument( | |
| "--encoding", | |
| default=None, | |
| help="image file encoding (guessed if unspecified)", | |
| ) | |
| parser.add_argument( | |
| "img_fpath", | |
| help="path to the image file", | |
| ) | |
| args = parser.parse_args() | |
| return args | |
| def main(): | |
| args = parse_args() | |
| rospy.init_node("image_file_pub") | |
| img = cv2.imread(args.img_fpath, cv2.IMREAD_UNCHANGED) | |
| num_channels = img.shape[2] | |
| if args.encoding is not None: | |
| encoding = args.encoding | |
| elif num_channels == 1: | |
| encoding = "mono8" | |
| elif num_channels == 3: | |
| encoding = "bgr8" | |
| elif num_channels == 4: | |
| encoding = "bgra8" | |
| else: | |
| fmt = "Cannot guess encoding for image file {} (shape: {})" | |
| raise RuntimeError(fmt.format(args.img_fpath), img.shape) | |
| img_msg = CvBridge().cv2_to_imgmsg(img, encoding=encoding) | |
| pub = rospy.Publisher(args.topic, Image, queue_size=10, latch=True) | |
| if args.rate is None: | |
| pub.publish(img_msg) | |
| rospy.spin() | |
| else: | |
| rate = rospy.Rate(args.rate) | |
| while not rospy.is_shutdown(): | |
| img_msg.header.stamp = rospy.Time.now() | |
| pub.publish(img_msg) | |
| rate.sleep() | |
| if __name__ == "__main__": | |
| main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment