diff --git a/launch_openpilot.sh b/launch_openpilot.sh index d6e3424c3..2d3aac4a1 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,3 +1,3 @@ #!/usr/bin/env bash - +export LOGPRINT="debug" exec ./launch_chffrplus.sh diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py index 6c6e252a5..cad664337 100755 --- a/tools/longitudinal_maneuvers/maneuversd.py +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -2,12 +2,22 @@ import numpy as np from dataclasses import dataclass +import builtins from cereal import messaging, car from opendbc.car.common.conversions import Conversions as CV from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog +# Override print to also send to cloudlog.debug +_builtin_print = builtins.print + +def print(*args, **kwargs): + _builtin_print(*args, **kwargs) + try: + cloudlog.debug(' '.join(str(a) for a in args)) + except Exception: + pass @dataclass class Action: @@ -33,37 +43,46 @@ class Maneuver: _repeated: int = 0 def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float: + print(f"[DEBUG] get_accel: v_ego={v_ego:.2f}, long_active={long_active}, standstill={standstill}, cruise_standstill={cruise_standstill}, initial_speed={self.initial_speed:.2f}") ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill if self.initial_speed < 0.01: ready = ready and standstill self._ready_cnt = (self._ready_cnt + 1) if ready else 0 + print(f"[DEBUG] ready={ready}, _ready_cnt={self._ready_cnt}, _active={self._active}, _finished={self._finished}, _action_index={self._action_index}, _action_frames={self._action_frames}, _repeated={self._repeated}") if self._ready_cnt > (3. / DT_MDL): self._active = True + print(f"[DEBUG] Maneuver activated") if not self._active: + print(f"[DEBUG] Not active, returning speed correction: {min(max(self.initial_speed - v_ego, -2.), 2.)}") return min(max(self.initial_speed - v_ego, -2.), 2.) action = self.actions[self._action_index] action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp) + print(f"[DEBUG] Action index: {self._action_index}, action_frames: {self._action_frames}, action_accel: {action_accel}") self._action_frames += 1 # reached duration of action if self._action_frames > (action.time_bp[-1] / DT_MDL): + print(f"[DEBUG] Action duration reached for index {self._action_index}") # next action if self._action_index < len(self.actions) - 1: self._action_index += 1 self._action_frames = 0 + print(f"[DEBUG] Moving to next action: {self._action_index}") # repeat maneuver elif self._repeated < self.repeat: self._repeated += 1 self._action_index = 0 self._action_frames = 0 self._active = False + print(f"[DEBUG] Repeating maneuver: repeated={self._repeated}") # finish maneuver else: self._finished = True + print(f"[DEBUG] Maneuver finished") return float(action_accel) @@ -142,6 +161,7 @@ def main(): if maneuver is None: maneuver = next(maneuvers, None) + print(f"[DEBUG] Loaded new maneuver: {getattr(maneuver, 'description', None)}") alert_msg = messaging.new_message('alertDebug') alert_msg.valid = True @@ -154,6 +174,7 @@ def main(): v_ego = max(sm['carState'].vEgo, 0) if maneuver is not None: + print(f"[DEBUG] Running maneuver: {maneuver.description}, active={maneuver.active}, finished={maneuver.finished}") accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill) if maneuver.active: @@ -163,6 +184,7 @@ def main(): alert_msg.alertDebug.alertText2 = f'{maneuver.description}' else: alert_msg.alertDebug.alertText1 = 'Maneuvers Finished' + print(f"[DEBUG] All maneuvers finished") pm.send('alertDebug', alert_msg) @@ -180,6 +202,10 @@ def main(): assistance_send = messaging.new_message('driverAssistance') assistance_send.valid = True pm.send('driverAssistance', assistance_send) - + if maneuver is not None and maneuver.finished: + print(f"[DEBUG] Maneuver completed: {maneuver.description}") maneuver = None + +if __name__ == "__main__": + main() \ No newline at end of file