diff --git a/minibot_vision/scripts/SignDetector.py b/minibot_vision/scripts/SignDetector.py index 9c813fd6b356570595fe527c421a450fe626faf7..73df60c300f0035aef792756a828eef8ac9e2821 100755 --- a/minibot_vision/scripts/SignDetector.py +++ b/minibot_vision/scripts/SignDetector.py @@ -16,6 +16,7 @@ from vision_msgs.msg import Detection2D, ObjectHypothesisWithPose from minibot_msgs.srv import set_url import time from threading import Thread +from sphero_rvr_msgs.srv import TriggerLedEvent, TriggerLedEventRequest # *** CONSTANTS *** @@ -168,9 +169,34 @@ def detect_sign(img_rgb_stream, image_timestamp): pub_result_img.publish(cmprsmsg) +def call_rvr_color_event(event_id: TriggerLedEventRequest, stop_current_event: bool): + # start rvr color event + service = "rvr/trigger_led_event" + + try: + rospy.wait_for_service(service, timeout=0.5) + + call = rospy.ServiceProxy(service, TriggerLedEvent) + response = call.call(TriggerLedEventRequest(stop_current_event=stop_current_event, event_id=event_id)) + if not response.success: + raise rospy.ServiceException("Failed to set rvr status lights") + except rospy.ROSException as e: + # if the service is not available we just ignore this + pass + except rospy.ServiceException as e: + rospy.logerr("Service call failed: %s" % e) + + def set_model_callback(req): + call_rvr_color_event(event_id=TriggerLedEventRequest.DOWNLOAD_MODEL, stop_current_event=False) + success = sign_classifier.setNewModel(req.url) + call_rvr_color_event(stop_current_event=True, event_id=-1) + + if not success: + call_rvr_color_event(stop_current_event=False, event_id=TriggerLedEventRequest.ERROR) + return success