Skip to content

Commit

Permalink
Create a node listener and call service
Browse files Browse the repository at this point in the history
  • Loading branch information
GustavoDCC committed May 2, 2024
1 parent ee8607b commit 0f770ff
Showing 1 changed file with 37 additions and 0 deletions.
37 changes: 37 additions & 0 deletions image_recognition_color_extractor/scripts/get_colors_stream
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
#!/usr/bin/env python
import argparse

import rospy

from sensor_msgs.msg import Image

from image_recognition_msgs.srv import Recognize

def extract_color_client(img_msg):

rospy.wait_for_service('extract_color')
try:
extract_color = rospy.ServiceProxy('extract_color', Recognize)
return extract_color(img_msg)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)

# Create a callback function for the subscriber.
def callback(msg):

# Simply print out values in our custom message.
colors = extract_color_client(msg)
rospy.loginfo(colors)


if __name__ == '__main__':

rospy.init_node('pose_estimation_image_subscriber')

parser = argparse.ArgumentParser(description='Get dominant colors from image')
parser.add_argument('--topic', default='/image', type=str, help='Topic')
args = parser.parse_args()
sub = rospy.Subscriber(args.topic, Image, callback)
rospy.loginfo('Node has been started')

rospy.spin()

0 comments on commit 0f770ff

Please sign in to comment.