From af74e444758bf24e0670a50264f2cc54c5236b63 Mon Sep 17 00:00:00 2001 From: ajouatom Date: Sun, 23 Feb 2025 15:43:02 +0900 Subject: [PATCH] voice lanechange --- common/params.cc | 1 - selfdrive/car/cruise.py | 45 ++++++++-------- selfdrive/carrot/carrot_man.py | 39 +++----------- selfdrive/controls/lib/desire_helper.py | 25 +++++++-- selfdrive/ui/qt/onroad/annotated_camera.cc | 31 ++++++----- selfdrive/ui/qt/onroad/onroad_home.cc | 60 ++++++++++++---------- 6 files changed, 103 insertions(+), 98 deletions(-) diff --git a/common/params.cc b/common/params.cc index 42102f6..bcb14f7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -332,7 +332,6 @@ std::unordered_map keys = { {"SpeedFromPCM", PERSISTENT}, {"MaxTimeOffroadMin", PERSISTENT}, {"DisableDM", PERSISTENT}, - {"CarrotManCommand", PERSISTENT}, {"CarrotException", CLEAR_ON_MANAGER_START}, {"CarName", PERSISTENT}, {"EVTable", PERSISTENT}, diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 73ae163..c1f43d2 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -191,6 +191,12 @@ class VCruiseCarrot: self.desiredSpeed = 250 self.road_limit_kph = 30 + self.carrot_cmd_index_last = 0 + self.carrot_cmd_index = 0 + self.carrot_cmd = "" + self.carrot_arg = "" + + self._cancel_timer = 0 self.d_rel = 0 self.v_rel = 0 @@ -253,6 +259,9 @@ class VCruiseCarrot: carrot_man = sm['carrotMan'] self.nRoadLimitSpeed = carrot_man.nRoadLimitSpeed self.desiredSpeed = carrot_man.desiredSpeed + self.carrot_cmd_index = carrot_man.carrotCmdIndex + self.carrot_cmd = carrot_man.carrotCmd + self.carrot_arg = carrot_man.carrotArg if sm.alive['longitudinalPlan']: lp = sm['longitudinalPlan'] self.xState = lp.xState @@ -400,41 +409,37 @@ class VCruiseCarrot: return button_kph, button_type, self.long_pressed def _carrot_command(self, v_cruise_kph, button_type, long_pressed): - - command = self.params_memory.get("CarrotManCommand") - if command is not None: - command = command.decode() - print("command: ", command) - if command.startswith("CRUISE "): - cruise = command[7:] - if cruise == "OFF": + if self.carrot_cmd_index_last != self.carrot_cmd_index: + self.carrot_cmd_index_last = self.carrot_cmd_index + print(f"Carrot command(cruise.py): {self.carrot_cmd} {self.carrot_arg}") + if self.carrot_cmd == "CRUISE": + if self.carrot_arg == "OFF": self._cruise_control(-2, -1, "Cruise off (carrot command)") - elif cruise == "ON": + elif self.carrot_arg == "ON": self._cruise_control(1, -1, "Cruise on (carrot command)") - elif cruise == "GO": + elif self.carrot_arg == "GO": if button_type == 0: button_type = ButtonType.accelCruise long_pressed = False - self._add_log("Cruise accelCruisep (carrot command)") - elif cruise == "STOP": + self._add_log("Cruise accelCruise (carrot command)") + elif self.carrot_arg == "STOP": v_cruise_kph = 5 self._add_log("Cruise stop (carrot command)") - self.params_memory.put_nonblocking("CarrotManCommand", "") - elif command.startswith("SPEED "): - speed = command[6:] - if speed == "UP": + + elif self.carrot_cmd == "SPEED": + if self.carrot_arg == "UP": v_cruise_kph = self._auto_speed_up(v_cruise_kph) self._add_log("Cruise speed up (carrot command)") - elif speed == "DOWN": + elif self.carrot_arg == "DOWN": if v_cruise_kph > 20: v_cruise_kph -= 10 self._add_log("Cruise speed downup (carrot command)") else: - speed_kph = int(speed) + speed_kph = int(self.carrot_arg) if 0 < speed_kph < 200: v_cruise_kph = speed_kph - self._add_log(f"Cruise speed set to {v_cruise_kph} (carrot command)") - self.params_memory.put_nonblocking("CarrotManCommand", "") + self._add_log(f"Cruise speed set to {v_cruise_kph} (carrot command)") + return v_cruise_kph, button_type, long_pressed def _update_cruise_buttons(self, CS, CC, v_cruise_kph): diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index 55be768..477ac2d 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/selfdrive/carrot/carrot_man.py @@ -966,11 +966,6 @@ class CarrotServ: if self.carrotCmdIndex != self.carrotCmdIndex_last: self.carrotCmdIndex_last = self.carrotCmdIndex command_handlers = { - "SPEED": self._handle_speed_command, - "CRUISE": self._handle_cruise_command, - "LANECHANGE": self._handle_lane_change, - "RECORD": self._handle_record_command, - "DISPLAY": self._handle_display_command, "DETECT": self._handle_detect_command, } @@ -984,29 +979,6 @@ class CarrotServ: self.traffic_light_count = -1 self.traffic_state = 0 - def _handle_speed_command(self, xArg): - self.params_memory.put_nonblocking("CarrotManCommand", "SPEED " + xArg) - - def _handle_cruise_command(self, xArg): - self.params_memory.put_nonblocking("CarrotManCommand", "CRUISE " + xArg) - - def _handle_lane_change(self, xArg): - self.params_memory.put_nonblocking("CarrotManCommand", "LANECHANGE " + xArg) - #if xArg == "RIGHT": - # pass - #elif xArg == "LEFT": - # pass - - def _handle_record_command(self, xArg): - self.params_memory.put_nonblocking("CarrotManCommand", "RECORD " + xArg) - - def _handle_display_command(self, xArg): - self.params_memory.put_nonblocking("CarrotManCommand", "DISPLAY " + xArg) - display_commands = {"MAP": "3", "FULLMAP": "4", "DEFAULT": "1", "ROAD": "2", "TOGGLE": "5"} - command = display_commands.get(xArg) - if command: - pass - def _handle_detect_command(self, xArg): elements = [e.strip() for e in xArg.split(',')] if len(elements) >= 4: @@ -1347,9 +1319,13 @@ class CarrotServ: #print(f"x_dist_to_turn: {x_dist_to_turn}, atc_start_dist: {atc_start_dist}") #print(f"atc_activate_count: {self.atc_activate_count}") if self.atc_activate_count == 2: - self.params_memory.put_nonblocking("CarrotManCommand", "DISPLAY MAP") + self.carrotCmdIndex += 100 + self.carrotCmd = "DISPLAY"; + self.carrotArg = "MAP"; elif self.atc_activate_count == -50: - self.params_memory.put_nonblocking("CarrotManCommand", "DISPLAY ROAD") + self.carrotCmdIndex += 100 + self.carrotCmd = "DISPLAY"; + self.carrotArg = "ROAD"; if check_steer: if 0 <= x_dist_to_turn < atc_start_dist and atc_type in ["fork left", "fork right"]: @@ -1734,10 +1710,11 @@ class CarrotServ: #self._update_system_time(int(json.get("epochTime")), timezone_remote) if "carrotCmd" in json: - print(json.get("carrotCmd"), json.get("carrotArg")) + #print(json.get("carrotCmd"), json.get("carrotArg")) self.carrotCmdIndex = self.carrotIndex self.carrotCmd = json.get("carrotCmd") self.carrotArg = json.get("carrotArg") + print(f"carrotCmd = {self.carrotCmd}, {self.carrotArg}") self.active_count = 80 diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 3141bb0..4bcad24 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -130,6 +130,10 @@ class DesireHelper: self.driver_blinker_state = BLINKER_NONE self.atc_type = "" + self.carrot_lane_change_count = 0 + self.carrot_cmd_index_last = 0 + self.carrot_blinker_state = BLINKER_NONE + def check_lane_state(self, modeldata): self.lane_width_left, self.distance_to_road_edge_left, self.distance_to_road_edge_left_far, lane_prob_left = calculate_lane_width(modeldata.laneLines[0], modeldata.laneLineProbs[0], modeldata.laneLines[1], modeldata.roadEdges[0]) @@ -153,7 +157,7 @@ class DesireHelper: self.laneChangeNeedTorque = self.params.get_bool("LaneChangeNeedTorque") - + self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1) v_ego = carstate.vEgo below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -167,7 +171,14 @@ class DesireHelper: ##### check ATC's blinker state atc_type = carrotMan.atcType atc_blinker_state = BLINKER_NONE - if atc_type in ["turn left", "turn right"]: + if self.carrot_lane_change_count > 0: + atc_blinker_state = self.carrot_blinker_state + elif carrotMan.carrotCmdIndex != self.carrot_cmd_index_last and carrotMan.carrotCmd == "LANECHANGE": + self.carrot_cmd_index_last = carrotMan.carrotCmdIndex + self.carrot_lane_change_count = int(0.2 / DT_MDL) + print(f"Desire lanechange: {carrotMan.carrotArg}") + self.carrot_blinker_state = BLINKER_LEFT if carrotMan.carrotArg == "LEFT" else BLINKER_RIGHT + elif atc_type in ["turn left", "turn right"]: if self.atc_active != 2: below_lane_change_speed = True self.lane_change_timer = 0.0 @@ -223,9 +234,13 @@ class DesireHelper: lane_availabled = not self.lane_available_last and lane_available edge_availabled = not self.edge_available_last and edge_available side_object_detected = self.object_detected_count > -0.3 / DT_MDL - - auto_lane_change_blocked = (atc_blinker_state == BLINKER_LEFT) and (driver_blinker_state != BLINKER_LEFT) - auto_lane_change_available = not auto_lane_change_blocked and lane_availabled and edge_availabled and not side_object_detected + + if self.carrot_lane_change_count > 0: + auto_lane_change_blocked = False + auto_lane_change_available = lane_available + else: + auto_lane_change_blocked = ((atc_blinker_state == BLINKER_LEFT) and (driver_blinker_state != BLINKER_LEFT)) + auto_lane_change_available = not auto_lane_change_blocked and lane_availabled and edge_availabled and not side_object_detected if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index c67e73f..e621148 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -42,25 +42,28 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { experimental_btn->updateState(s); dmon.updateState(s); - Params params_memory{ "/dev/shm/params" }; - QString command = QString::fromStdString(params_memory.get("CarrotManCommand")); - if (command.startsWith("RECORD ")) { - QString recording = command.mid(7); - if (recording == "START") { + static int carrot_cmd_index_last = 0; + SubMaster& sm = *(s.sm); + if (sm.alive("carrotMan")) { + const auto& carrot = sm["carrotMan"].getCarrotMan(); + int carrot_cmd_index = carrot.getCarrotCmdIndex(); + if (carrot_cmd_index != carrot_cmd_index_last) { + carrot_cmd_index_last = carrot_cmd_index; + QString carrot_cmd = QString::fromStdString(carrot.getCarrotCmd()); + QString carrot_arg = QString::fromStdString(carrot.getCarrotArg()); + if (carrot_cmd == "RECORD") { + if (carrot_arg == "START") { recorder->start(); - printf("Start recording\n"); - } - else if (recording == "STOP") { + } + else if (carrot_arg == "STOP") { recorder->stop(); - printf("Stop recording\n"); - } - else if (recording == "TOGGLE") { + } + else if (carrot_arg == "TOGGLE") { recorder->toggle(); - printf("Toggle recording\n"); + } } - params_memory.putNonBlocking("CarrotManCommand", ""); + } } - } void AnnotatedCameraWidget::initializeGL() { diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 28fa5bc..6044e26 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -133,35 +133,41 @@ void OnroadWindow::updateState(const UIState &s) { //update(); } update(); - if (true) { //carrot_display > 0) { + if (true) { int carrot_display = 0; - Params params_memory{ "/dev/shm/params" }; - QString command = QString::fromStdString(params_memory.get("CarrotManCommand")); - if (command.startsWith("DISPLAY ")) { - QString display_cmd = command.mid(8); - if (display_cmd == "TOGGLE") { - carrot_display = 5; - printf("Display toggle\n"); - } - else if (display_cmd == "DEFAULT") { - carrot_display = 1; - printf("Display 1\n"); - } - else if (display_cmd == "ROAD") { - carrot_display = 2; - printf("Display 2\n"); - } - else if (display_cmd == "MAP") { - carrot_display = 3; - printf("Display 3\n"); - } - else if (display_cmd == "FULLMAP") { - carrot_display = 4; - printf("Display 4\n"); - } - params_memory.putNonBlocking("CarrotManCommand", ""); - } + static int carrot_cmd_index_last = 0; + if (sm.alive("carrotMan")) { + const auto& carrot = sm["carrotMan"].getCarrotMan(); + int carrot_cmd_index = carrot.getCarrotCmdIndex(); + if (carrot_cmd_index != carrot_cmd_index_last) { + carrot_cmd_index_last = carrot_cmd_index; + QString carrot_cmd = QString::fromStdString(carrot.getCarrotCmd()); + QString carrot_arg = QString::fromStdString(carrot.getCarrotArg()); + if (carrot_cmd == "DISPLAY") { + if (carrot_arg == "TOGGLE") { + carrot_display = 5; + //printf("Display toggle\n"); + } + else if (carrot_arg == "DEFAULT") { + carrot_display = 1; + //printf("Display 1\n"); + } + else if (carrot_arg == "ROAD") { + carrot_display = 2; + //printf("Display 2\n"); + } + else if (carrot_arg == "MAP") { + carrot_display = 3; + //printf("Display 3\n"); + } + else if (carrot_arg == "FULLMAP") { + carrot_display = 4; + //printf("Display 4\n"); + } + } + } + } if (carrot_display == 5) ss->scene._current_carrot_display = (ss->scene._current_carrot_display % 3) + 1; else if(carrot_display > 0) ss->scene._current_carrot_display = carrot_display;