From c94ab2b647516e00d712f121636764f8a5352c84 Mon Sep 17 00:00:00 2001
From: minibot-01 <paddy-hofmann@web.de>
Date: Tue, 6 Jun 2023 11:52:41 +0000
Subject: [PATCH] added rvr led status
---
minibot_vision/scripts/SignDetector.py | 26 ++++++++++++++++++++++++++
1 file changed, 26 insertions(+)
diff --git a/minibot_vision/scripts/SignDetector.py b/minibot_vision/scripts/SignDetector.py
index 9c813fd..73df60c 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
--
GitLab