zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

voice lanechange

Browse Source
This commit is contained in:
ajouatom 2025-02-23 15:43:02 +09:00
parent 71a565e3fd
commit af74e44475
6 changed files with 103 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
common/params.cc
View File

@ -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},

45
selfdrive/car/cruise.py
View File

@ -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):

39
selfdrive/carrot/carrot_man.py
View File

@ -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

25
selfdrive/controls/lib/desire_helper.py
View File

@ -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

31
selfdrive/ui/qt/onroad/annotated_camera.cc
View File

@ -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() {

60
selfdrive/ui/qt/onroad/onroad_home.cc
View File

@ -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;