|
| 1 | +#! /usr/bin/env python |
| 2 | +# Software License Agreement (BSD License) |
| 3 | +# |
| 4 | +# Copyright (c) 2022, Kei Okada |
| 5 | +# All rights reserved. |
| 6 | +# |
| 7 | +# Redistribution and use in source and binary forms, with or without |
| 8 | +# modification, are permitted provided that the following conditions |
| 9 | +# are met: |
| 10 | +# |
| 11 | +# * Redistributions of source code must retain the above copyright |
| 12 | +# notice, this list of conditions and the following disclaimer. |
| 13 | +# * Redistributions in binary form must reproduce the above |
| 14 | +# copyright notice, this list of conditions and the following |
| 15 | +# disclaimer in the documentation and/or other materials provided |
| 16 | +# with the distribution. |
| 17 | +# * Neither the name of Kei Okada nor the names of its |
| 18 | +# contributors may be used to endorse or promote products derived |
| 19 | +# from this software without specific prior written permission. |
| 20 | +# |
| 21 | +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 22 | +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 23 | +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 24 | +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 25 | +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 26 | +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 27 | +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 28 | +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 29 | +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 30 | +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 31 | +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 32 | +# POSSIBILITY OF SUCH DAMAGE. |
| 33 | + |
| 34 | + |
| 35 | +from __future__ import print_function |
| 36 | + |
| 37 | +try: |
| 38 | + input = raw_input |
| 39 | +except: |
| 40 | + pass |
| 41 | + |
| 42 | +import argparse |
| 43 | +import cv2 |
| 44 | +import cv_bridge |
| 45 | +import os |
| 46 | +import sys |
| 47 | +import time |
| 48 | +import rospy |
| 49 | + |
| 50 | +from opencv_apps.msg import Rect |
| 51 | +from opencv_apps.srv import SetImages, SetImagesRequest |
| 52 | + |
| 53 | + |
| 54 | +if __name__ == '__main__': |
| 55 | + parser = argparse.ArgumentParser(description='Send ROI (and template image) to the tracker') |
| 56 | + parser.add_argument('center_x', type=int, help='X coordinates of center of ROI') |
| 57 | + parser.add_argument('center_y', type=int, help='Y coordinates of center of ROI') |
| 58 | + parser.add_argument('width', type=int, help='Width of ROI') |
| 59 | + parser.add_argument('height', type=int, help='Height of ROI') |
| 60 | + parser.add_argument('image_file_name', nargs='?', help='File name of template image') |
| 61 | + parser.add_argument('--debug-view', action='store_true', help='Display input_file with ROI') |
| 62 | + args, unknown = parser.parse_known_args(); |
| 63 | + |
| 64 | + rospy.init_node("set_image") |
| 65 | + rospy.wait_for_service('set_roi') |
| 66 | + set_roi = rospy.ServiceProxy('set_roi', SetImages) |
| 67 | + |
| 68 | + req = SetImagesRequest() |
| 69 | + |
| 70 | + # set roi |
| 71 | + rospy.loginfo("Set ROI({},{}, {}, {})".format(args.center_x, args.center_y, args.width, args.height)) |
| 72 | + rect = Rect(x=args.center_x, y=args.center_y, width=args.width, height=args.height) |
| 73 | + req.rects=[rect] |
| 74 | + |
| 75 | + # set images |
| 76 | + if args.image_file_name: |
| 77 | + fname = args.image_file_name |
| 78 | + if os.path.exists(fname): |
| 79 | + im = cv2.imread(fname) |
| 80 | + im_msg = cv_bridge.CvBridge().cv2_to_imgmsg(im, "bgr8") |
| 81 | + rospy.loginfo("Set Image{} from {}".format(im.shape, fname)) |
| 82 | + req.images = [im_msg] |
| 83 | + if args.debug_view: |
| 84 | + cv2.rectangle(im, |
| 85 | + (int(rect.x - rect.width/2), int(rect.y - rect.height/2)), |
| 86 | + (int(rect.x + rect.width/2), int(rect.y + rect.height/2)), |
| 87 | + (0, 255, 0), 3) |
| 88 | + cv2.imshow('template image', im) |
| 89 | + cv2.waitKey(100) |
| 90 | + else: |
| 91 | + rospy.logerr("{} not found exitting".format(fname)) |
| 92 | + sys.exit(1) |
| 93 | + |
| 94 | + ret = set_roi(images=req.images, rects=req.rects) |
| 95 | + rospy.loginfo("'set_roi' returns\n {}".format(ret)) |
| 96 | + if args.debug_view: |
| 97 | + time.sleep(2) |
| 98 | + cv2.destroyAllWindows() |
0 commit comments