voice lanechange

common/params.cc

@@ -332,7 +332,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SpeedFromPCM", PERSISTENT},
     {"MaxTimeOffroadMin", PERSISTENT},
     {"DisableDM", PERSISTENT},
-    {"CarrotManCommand", PERSISTENT},
     {"CarrotException", CLEAR_ON_MANAGER_START},
     {"CarName", PERSISTENT},
     {"EVTable", PERSISTENT},

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):
-
+    if self.carrot_cmd_index_last != self.carrot_cmd_index:
-    command = self.params_memory.get("CarrotManCommand")
+      self.carrot_cmd_index_last = self.carrot_cmd_index
-    if command is not None:
+      print(f"Carrot command(cruise.py): {self.carrot_cmd} {self.carrot_arg}")
-      command = command.decode()
+      if self.carrot_cmd == "CRUISE":
-      print("command: ", command)
+        if self.carrot_arg == "OFF":
-      if command.startswith("CRUISE "):
-        cruise = command[7:]
-        if cruise == "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)")
+            self._add_log("Cruise accelCruise (carrot command)")
-        elif cruise == "STOP":
+        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 "):
+      elif self.carrot_cmd == "SPEED":
-        speed = command[6:]
+        if self.carrot_arg == "UP":
-        if speed == "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._add_log(f"Cruise speed set to {v_cruise_kph} (carrot command)")       
-        self.params_memory.put_nonblocking("CarrotManCommand", "")
+    
     return v_cruise_kph, button_type, long_pressed
 
   def _update_cruise_buttons(self, CS, CC, v_cruise_kph):

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
 

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)
+    if self.carrot_lane_change_count > 0:
-    auto_lane_change_available = not auto_lane_change_blocked and lane_availabled and edge_availabled and not side_object_detected
+      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

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" };
+  static int carrot_cmd_index_last = 0;
-  QString command = QString::fromStdString(params_memory.get("CarrotManCommand"));
+  SubMaster& sm = *(s.sm);
-  if (command.startsWith("RECORD ")) {
+  if (sm.alive("carrotMan")) {
-      QString recording = command.mid(7);
+    const auto& carrot = sm["carrotMan"].getCarrotMan();
-      if (recording == "START") {
+    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 (carrot_arg == "STOP") {
-      else if (recording == "STOP") {
           recorder->stop();
-          printf("Stop recording\n");
+        }
-      }
+        else if (carrot_arg == "TOGGLE") {
-      else if (recording == "TOGGLE") {
           recorder->toggle();
-          printf("Toggle recording\n");
+        }
       }
-      params_memory.putNonBlocking("CarrotManCommand", "");
+    }
   }
-
 }
 
 void AnnotatedCameraWidget::initializeGL() {

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;