From f0e38411b7d22a1d204687a342b9240cfe86458c Mon Sep 17 00:00:00 2001 From: Vivek Ponnaiyan Date: Thu, 16 Mar 2017 21:47:31 -0700 Subject: [PATCH 1/8] [vp] Added a test system for wavevisiond talking to controlsd --- common/services.py | 1 + selfdrive/controls/controlsd.py | 8 +++++++ selfdrive/controls/lib/alertmanager.py | 1 + selfdrive/manager.py | 5 +++-- selfdrive/wavevisiond/__init__.py | 0 selfdrive/wavevisiond/wavevisiond.py | 31 ++++++++++++++++++++++++++ 6 files changed, 44 insertions(+), 2 deletions(-) create mode 100644 selfdrive/wavevisiond/__init__.py create mode 100644 selfdrive/wavevisiond/wavevisiond.py diff --git a/common/services.py b/common/services.py index 1c85cd82edae51..8aa1d728053caa 100644 --- a/common/services.py +++ b/common/services.py @@ -36,6 +36,7 @@ def __init__(self, port, should_log): "carState": Service(8021, True), # 8022 is reserved for sshd "carControl": Service(8023, True), + "waveModel":Service(8024,True) } # manager -- base process to manage starting and stopping of all others diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 02566a8696487b..4fb2820217d1b1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -46,6 +46,8 @@ def controlsd_thread(gctx, rate=100): #rate in Hz model = messaging.sub_sock(context, service_list['model'].port) health = messaging.sub_sock(context, service_list['health'].port) + waveModel = messagine.sub_sock(contexst, service_list['waveModel'].port) + # connects to can and sendcan CI = CarInterface() VP = CI.getVehicleParams() @@ -144,6 +146,12 @@ def controlsd_thread(gctx, rate=100): #rate in Hz prof.checkpoint("Buttons") + wave_model_data = messaging.recv_sock(waveModel) + if wave_model_data is not None: + if ( wave_model_data.path.prob == 0.743 ): + AM.add("waveModelSending",enabled) + + # *** health checking logic *** hh = messaging.recv_sock(health) if hh is not None: diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 57246c54c81ec1..049d21b308b370 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -58,6 +58,7 @@ class AlertManager(object): "wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), "outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), "ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "waveModelSending": alert("Wave Model Working", "Alls Good", ET.WARNING, "steerRequired", "chimeRepeated",1., 2.,3.) "startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.), } def __init__(self): diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 997168679f074f..b01ee21704c679 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -35,7 +35,8 @@ "boardd": ("boardd", ["./boardd"]), # switch to c++ boardd "ui": ("ui", ["./ui"]), "visiond": ("visiond", ["./visiond"]), - "sensord": ("sensord", ["./sensord"]), } + "sensord": ("sensord", ["./sensord"]), + "wavevisiond":"selfdrive.wavevisiond.wavevisiond" } running = {} @@ -45,7 +46,7 @@ # processes to end with SIGINT instead of SIGTERM interrupt_processes = ['loggerd'] -car_started_processes = ['controlsd', 'loggerd', 'sensord', 'radard', 'calibrationd', 'visiond'] +car_started_processes = ['controlsd', 'loggerd', 'sensord', 'radard', 'calibrationd', 'visiond','wavevisiond'] # ****************** process management functions ****************** diff --git a/selfdrive/wavevisiond/__init__.py b/selfdrive/wavevisiond/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/wavevisiond/wavevisiond.py b/selfdrive/wavevisiond/wavevisiond.py new file mode 100644 index 00000000000000..6512d0516884eb --- /dev/null +++ b/selfdrive/wavevisiond/wavevisiond.py @@ -0,0 +1,31 @@ +import zmq +import selfdrive.messaging as messaging +from common.services import service_list +import numpy as np + +import time + +def wavevisiond_thread(gctx,rate=100): + set_realtime_priority(1) + + context = zmq.Context() + + waveModel = messaging.pub_sock(context, service_list['waveModel'].port) + + while True: + msg = messaging.new_message() + msg.init('model') + msg.path.points=[1.0,5.0,4.0,4.0,5.0] + msg.path.prob = 0.743 + msg.path.std = 0.2 + + msg.leftLane.points = [5.0, 4.0, 3.0, 2.0, 1.0 ] + msg.leftLane.prob = 0.987 + msg.leftLane.std = 0.1 + + waveModel.send(msg.to_bytes()) + time.sleep(1) + + +def main(gctx=None): + wavevisiond_thread(gctx, rate) From 9a56054702b064bd27319eab63088512e02518e2 Mon Sep 17 00:00:00 2001 From: Vivek Ponnaiyan Date: Thu, 16 Mar 2017 21:52:27 -0700 Subject: [PATCH 2/8] [vp] oops forgot to import set_realtime_priority. --- selfdrive/wavevisiond/wavevisiond.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/wavevisiond/wavevisiond.py b/selfdrive/wavevisiond/wavevisiond.py index 6512d0516884eb..a01819d5e348ba 100644 --- a/selfdrive/wavevisiond/wavevisiond.py +++ b/selfdrive/wavevisiond/wavevisiond.py @@ -2,6 +2,7 @@ import selfdrive.messaging as messaging from common.services import service_list import numpy as np +from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper import time From 38104c6c363ecfdb7cb889434e5e5c84a611fc33 Mon Sep 17 00:00:00 2001 From: Vivek Ponnaiyan Date: Thu, 16 Mar 2017 21:56:00 -0700 Subject: [PATCH 3/8] [vp] Changed sleep to 6 seconds --- selfdrive/wavevisiond/wavevisiond.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/wavevisiond/wavevisiond.py b/selfdrive/wavevisiond/wavevisiond.py index a01819d5e348ba..a67929c6c4261f 100644 --- a/selfdrive/wavevisiond/wavevisiond.py +++ b/selfdrive/wavevisiond/wavevisiond.py @@ -25,7 +25,7 @@ def wavevisiond_thread(gctx,rate=100): msg.leftLane.std = 0.1 waveModel.send(msg.to_bytes()) - time.sleep(1) + time.sleep(6) def main(gctx=None): From 317aad35a9d7cd16953d3b58d48d9fed141e4b0c Mon Sep 17 00:00:00 2001 From: Vivek Ponnaiyan Date: Thu, 16 Mar 2017 21:59:30 -0700 Subject: [PATCH 4/8] [vp] typo --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4fb2820217d1b1..6c50de092003bf 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -46,7 +46,7 @@ def controlsd_thread(gctx, rate=100): #rate in Hz model = messaging.sub_sock(context, service_list['model'].port) health = messaging.sub_sock(context, service_list['health'].port) - waveModel = messagine.sub_sock(contexst, service_list['waveModel'].port) + waveModel = messaging.sub_sock(contexst, service_list['waveModel'].port) # connects to can and sendcan CI = CarInterface() From c28766fccd15d07e2dffd82c8dc78e74a75a049c Mon Sep 17 00:00:00 2001 From: Vivek Ponnaiyan Date: Thu, 16 Mar 2017 23:10:29 -0700 Subject: [PATCH 5/8] [vp] Fixed typo --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6c50de092003bf..b9bbe63add8671 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -46,7 +46,7 @@ def controlsd_thread(gctx, rate=100): #rate in Hz model = messaging.sub_sock(context, service_list['model'].port) health = messaging.sub_sock(context, service_list['health'].port) - waveModel = messaging.sub_sock(contexst, service_list['waveModel'].port) + waveModel = messaging.sub_sock(context, service_list['waveModel'].port) # connects to can and sendcan CI = CarInterface() From c8d798e346f3c69d2686ca9347c6f49c38893034 Mon Sep 17 00:00:00 2001 From: Vivek Ponnaiyan Date: Thu, 16 Mar 2017 23:13:08 -0700 Subject: [PATCH 6/8] [vp] Fixed another typo --- selfdrive/wavevisiond/wavevisiond.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/wavevisiond/wavevisiond.py b/selfdrive/wavevisiond/wavevisiond.py index a67929c6c4261f..2bb43d327d1ff4 100644 --- a/selfdrive/wavevisiond/wavevisiond.py +++ b/selfdrive/wavevisiond/wavevisiond.py @@ -29,4 +29,4 @@ def wavevisiond_thread(gctx,rate=100): def main(gctx=None): - wavevisiond_thread(gctx, rate) + wavevisiond_thread(gctx, 100) From 07cc810d409769e00614070752ed413da1bda2fc Mon Sep 17 00:00:00 2001 From: Bill Zito Date: Fri, 17 Mar 2017 10:09:44 -0700 Subject: [PATCH 7/8] Add print statements --- selfdrive/wavevisiond/wavevisiond.py | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/selfdrive/wavevisiond/wavevisiond.py b/selfdrive/wavevisiond/wavevisiond.py index 2bb43d327d1ff4..d91bd2ac9157dc 100644 --- a/selfdrive/wavevisiond/wavevisiond.py +++ b/selfdrive/wavevisiond/wavevisiond.py @@ -1,4 +1,5 @@ import zmq +import sys import selfdrive.messaging as messaging from common.services import service_list import numpy as np @@ -7,26 +8,40 @@ import time def wavevisiond_thread(gctx,rate=100): + print('line 10') set_realtime_priority(1) - + print('line 12') context = zmq.Context() - + print('line 14') waveModel = messaging.pub_sock(context, service_list['waveModel'].port) - + print('line 16') + while True: + print('line 19') msg = messaging.new_message() + print('line 21') msg.init('model') + print('line 23') msg.path.points=[1.0,5.0,4.0,4.0,5.0] msg.path.prob = 0.743 msg.path.std = 0.2 + print('line 27') msg.leftLane.points = [5.0, 4.0, 3.0, 2.0, 1.0 ] msg.leftLane.prob = 0.987 msg.leftLane.std = 0.1 + print('line 32') waveModel.send(msg.to_bytes()) + print('line 35') time.sleep(6) + print('line 37') def main(gctx=None): + print('line 41') wavevisiond_thread(gctx, 100) + +if __name__ == "__main__": + print('sys version', sys.version) + main() \ No newline at end of file From e4ea2dc72651cb9b825deff3e1b05c327aea2ae0 Mon Sep 17 00:00:00 2001 From: Mez Date: Fri, 17 Mar 2017 12:59:40 -0700 Subject: [PATCH 8/8] fixed typo --- selfdrive/controls/lib/alertmanager.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 049d21b308b370..10c3333545e4e4 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -58,7 +58,7 @@ class AlertManager(object): "wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), "outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), "ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "waveModelSending": alert("Wave Model Working", "Alls Good", ET.WARNING, "steerRequired", "chimeRepeated",1., 2.,3.) + "waveModelSending": alert("Wave Model Working", "Alls Good", ET.WARNING, "steerRequired", "chimeRepeated",1., 2.,3.), "startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.), } def __init__(self): @@ -83,7 +83,7 @@ def add(self, alert_type, enabled = True): if not enabled and this_alert.alert_type > ET.NO_ENTRY: this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.) - # ignore no entries if we are enabled + # ignore no entries if we are enabled if enabled and this_alert.alert_type < ET.WARNING: return @@ -127,4 +127,3 @@ def process_alerts(self, cur_time): self.activealerts = [] return alert_text_1, alert_text_2, visual_alert, audible_alert -