diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index bb5a45bfc..6841ae6c2 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -353,6 +353,27 @@ FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00 FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05 FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18 FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 +FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DISTANCE = 40.0 +FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED = 0.5 +FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED = 1.8 +FAR_RADAR_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE = 0.12 +FAR_RADAR_COMFORT_BRAKE_CAP_MIN_TTC = 20.0 +FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN = 0.55 +FAR_RADAR_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 0.95 +FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL = 0.04 +FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL = 0.14 +FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05 +EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME = 0.75 +EXPERIMENTAL_RELEASE_ACCEL_MIN_SPEED = 12.0 +EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_SPEED = 5.0 +EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_DELTA = -1.0 +EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_DELTA = 1.5 +EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_BRAKE = 0.2 +EXPERIMENTAL_RELEASE_ACCEL_MIN_MODEL_PROB = 0.9 +EXPERIMENTAL_RELEASE_ACCEL_MAX_LATERAL_OFFSET = 1.5 +EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN = 0.0 +EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A = 0.12 +EXPERIMENTAL_RELEASE_ACCEL_STEP = 0.06 MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 20.0 MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.25 MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 0.75 @@ -653,6 +674,8 @@ class LongitudinalPlanner: self.lead_depart_accel_hold_floor = None self.post_departure_follow_settle_until = 0.0 self.duplicate_vision_comfort_lead_source = None + self.prev_experimental_mode = None + self.experimental_release_accel_until = 0.0 if self.is_preap: try: @@ -2025,23 +2048,45 @@ class LongitudinalPlanner: if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED: return None + relative_speed = float(v_ego) - float(lead.vLead) + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + headway_margin = actual_headway - float(base_t_follow) + if bool(getattr(lead, "radar", False)): - return None + if not (FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED): + return None + if lead_brake > FAR_RADAR_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE: + return None + if float(lead.dRel) < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN: + return None + + ttc = float(lead.dRel) / max(relative_speed, 1e-3) + if ttc < FAR_RADAR_COMFORT_BRAKE_CAP_MIN_TTC: + return None + + cap_decel = float(np.interp( + relative_speed, + [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED, FAR_RADAR_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED], + [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_DECEL, FAR_RADAR_COMFORT_BRAKE_CAP_MAX_DECEL], + )) + relax_decel = float(np.interp( + headway_margin, + [FAR_RADAR_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN, FAR_RADAR_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN], + [0.0, FAR_RADAR_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL], + )) + return -max(0.0, cap_decel - relax_decel) lead_prob = float(getattr(lead, "modelProb", 0.0)) if lead_prob < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_MODEL_PROB: return None - relative_speed = float(v_ego) - float(lead.vLead) if not (FAR_LEAD_COMFORT_BRAKE_CAP_MIN_CLOSING_SPEED <= relative_speed <= FAR_LEAD_COMFORT_BRAKE_CAP_MAX_CLOSING_SPEED): return None - lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) if lead_brake > FAR_LEAD_COMFORT_BRAKE_CAP_MAX_LEAD_BRAKE: return None - actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) - headway_margin = actual_headway - float(base_t_follow) if float(lead.dRel) < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DISTANCE or headway_margin < FAR_LEAD_COMFORT_BRAKE_CAP_MIN_HEADWAY_MARGIN: return None @@ -2061,6 +2106,42 @@ class LongitudinalPlanner: )) return -max(0.0, cap_decel - relax_decel) + def update_experimental_release_accel_state(self, experimental_mode, now_t): + if self.prev_experimental_mode is True and not experimental_mode: + self.experimental_release_accel_until = now_t + EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME + elif experimental_mode: + self.experimental_release_accel_until = 0.0 + self.prev_experimental_mode = bool(experimental_mode) + + def get_experimental_release_accel_target(self, lead, v_ego, base_t_follow, + prev_output_a_target, output_a_target, + release_active): + if not release_active or lead is None or not lead.status or v_ego < EXPERIMENTAL_RELEASE_ACCEL_MIN_SPEED: + return None + + lead_speed = float(lead.vLead) + lead_delta = lead_speed - float(v_ego) + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + lead_radar = bool(getattr(lead, "radar", False)) + lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + if lead_speed < EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_SPEED: + return None + if not (EXPERIMENTAL_RELEASE_ACCEL_MIN_LEAD_DELTA <= lead_delta <= EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_DELTA): + return None + if lead_brake > EXPERIMENTAL_RELEASE_ACCEL_MAX_LEAD_BRAKE: + return None + if not lead_radar and lead_prob < EXPERIMENTAL_RELEASE_ACCEL_MIN_MODEL_PROB: + return None + if abs(float(getattr(lead, "yRel", 0.0))) > EXPERIMENTAL_RELEASE_ACCEL_MAX_LATERAL_OFFSET: + return None + if actual_headway < float(base_t_follow) + EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN: + return None + if float(output_a_target) - float(prev_output_a_target) < EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A: + return None + + return min(float(output_a_target), float(prev_output_a_target) + EXPERIMENTAL_RELEASE_ACCEL_STEP) + def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target, current_source, tracking_lead_active): if lead is None or not lead.status: @@ -2527,7 +2608,8 @@ class LongitudinalPlanner: self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel") self.generation = getattr(starpilot_toggles, "model_version", None) - self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + experimental_mode = bool(sm['selfdriveState'].experimentalMode) + self.mode = 'blended' if experimental_mode else 'acc' self.mpc.mode = 'acc' if not self.mlsim: self.mpc.mode = self.mode @@ -2656,6 +2738,7 @@ class LongitudinalPlanner: # Lead stability estimation and recent-brake timer now_t = time.monotonic() + self.update_experimental_release_accel_state(experimental_mode, now_t) # relative speed (ego - lead) positive when closing v_rel = (v_ego - self.lead_one.vLead) if lead_one_active else 0.0 if self.prev_lead_dist is None: @@ -3487,6 +3570,24 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, inside_gap_closing_cap) output_a_target = min(output_a_target, inside_gap_closing_cap) + experimental_release_accel_target = self.get_experimental_release_accel_target( + comfort_follow_lead, + scene_v_ego, + effective_t_follow, + prev_output_a_target, + output_a_target, + bool( + now_t < self.experimental_release_accel_until and + not output_should_stop and + not vision_low_speed_stop_active and + not getattr(sm['starpilotPlan'], 'forcingStop', False) and + not getattr(sm['starpilotPlan'], 'redLight', False) + ), + ) + if experimental_release_accel_target is not None: + self.a_desired = min(self.a_desired, experimental_release_accel_target) + output_a_target = min(output_a_target, experimental_release_accel_target) + self.output_a_target = output_a_target self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 8a60e9e4e..c78c16894 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2760,6 +2760,113 @@ def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead(): assert cap < -0.05 +def test_far_lead_soft_brake_cap_limits_spacious_mild_closing_radar_lead(): + v_ego = 25.61 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=55.1, v_lead=24.11, a_lead=0.22, radar=True, model_prob=0.99) + + cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.13) + + assert cap is not None + assert -0.11 < cap < -0.04 + + +@pytest.mark.parametrize( + "v_lead,a_lead", + [ + (22.9, 0.0), + (24.11, -0.5), + ], +) +def test_far_lead_soft_brake_cap_rejects_urgent_radar_lead(v_lead, a_lead): + v_ego = 25.61 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=55.1, v_lead=v_lead, a_lead=a_lead, radar=True, model_prob=0.99) + + assert planner.get_far_lead_brake_cap(lead, v_ego, 1.13) is None + + +def test_experimental_release_state_arms_only_on_falling_edge(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP) + + planner.update_experimental_release_accel_state(True, 10.0) + assert planner.experimental_release_accel_until == 0.0 + + planner.update_experimental_release_accel_state(False, 10.1) + assert planner.experimental_release_accel_until > 10.1 + + planner.update_experimental_release_accel_state(True, 10.2) + assert planner.experimental_release_accel_until == 0.0 + + +def test_experimental_release_accel_transition_damps_moving_lead_handoff(): + v_ego = 23.96 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=55.15, v_lead=23.73, a_lead=0.40, radar=True, model_prob=0.997) + + target = planner.get_experimental_release_accel_target( + lead, + v_ego, + 1.13, + prev_output_a_target=0.03, + output_a_target=0.44, + release_active=True, + ) + + assert target == pytest.approx(0.09) + + +def test_experimental_release_accel_transition_does_not_mask_stopped_lead(): + v_ego = 23.96 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=35.0, v_lead=0.0, a_lead=-1.0, radar=True, model_prob=0.997) + + target = planner.get_experimental_release_accel_target( + lead, + v_ego, + 1.13, + prev_output_a_target=-0.4, + output_a_target=0.4, + release_active=True, + ) + + assert target is None + + +def test_planner_arms_experimental_release_accel_only_on_mode_exit(): + v_ego = 23.96 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=55.15, v_lead=23.73, a_lead=0.40, radar=True, model_prob=0.997) + sm = make_sm( + v_ego, + desired_accel=0.0, + min_accel=-1.0, + experimental_mode=True, + tracking_lead=True, + lead_one=lead, + ) + + release_states = [] + original = planner.get_experimental_release_accel_target + + def record_release_state(self, *args, **kwargs): + release_states.append(bool(args[-1])) + return original(*args, **kwargs) + + planner.get_experimental_release_accel_target = types.MethodType(record_release_state, planner) + planner.update(sm, make_toggles()) + sm["selfdriveState"].experimentalMode = False + planner.update(sm, make_toggles()) + + assert release_states == [False, True] + + def test_matched_follow_transition_target_damps_large_comfort_sign_flip(): v_ego = 20.3 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index fcfcde64d..58fbc0e2c 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -92,7 +92,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._render_overlays() self._render_road_name() if self._draw_road_overlays: - self._render_path_features(rect) + self._render_path_features(self._content_rect) def _draw_border(self, rect: rl.Rectangle): border_width = self._get_border_width() diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 4727f7260..1f3ad5745 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -46,7 +46,7 @@ class ConditionalExperimentalMode: STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN = 10.0 STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0 - STOP_LIGHT_DETECTED_HOLD_TIME = 1.75 + STOP_LIGHT_DETECTED_HOLD_TIME = 4.0 STOP_APPROACH_LATCH_TIME = 1.0 STOP_APPROACH_MAX_LEAD_SPEED = 4.5 STOP_APPROACH_MIN_MODEL_PROB = 0.9 diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index 4e10f2ace..7198b7902 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -15,6 +15,7 @@ import numpy as np from openpilot.common.constants import CV from openpilot.common.realtime import set_core_affinity +from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.system.hardware import PC RUNTIME_LOOP_HZ = 20 @@ -62,6 +63,8 @@ CHANGE_REPEAT_MIN_CONFIDENCE = 0.70 LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS = 2 LOW_SPEED_CHANGE_MIN_CONFIDENCE = 0.90 LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS = True +LARGE_SET_SPEED_DELTA_MPH = 30.0 +LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS = 3 MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE = 0.65 PUBLISHED_HOLD_SECONDS = 300.0 PUBLISHED_CHANGE_COOLDOWN_SECONDS = 1.4 @@ -311,7 +314,7 @@ class SpeedLimitVisionDaemon: self.Ratekeeper = Ratekeeper self.VisionIpcClient = VisionIpcClient self.VisionStreamType = VisionStreamType - self.sm = messaging.SubMaster(["deviceState", "mapdOut", "userBookmark", "livePose"]) + self.sm = messaging.SubMaster(["carState", "deviceState", "mapdOut", "userBookmark", "livePose"]) self.client = None self.stream_name = "" @@ -337,6 +340,7 @@ class SpeedLimitVisionDaemon: self.started_prev = False self.history: deque[HistoryEntry] = deque() + self.current_cruise_set_speed_ms = 0.0 self.published_speed_limit_mph = 0 self.published_confidence = 0.0 self.previous_published_speed_limit_mph = 0 @@ -438,6 +442,24 @@ class SpeedLimitVisionDaemon: conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS return speed_value * conversion + def _update_current_cruise_set_speed(self): + self.current_cruise_set_speed_ms = 0.0 + if self.sm is None: + return + try: + v_cruise_kph = float(self.sm["carState"].vCruise) + except Exception: + return + if math.isfinite(v_cruise_kph) and 0.0 < v_cruise_kph < V_CRUISE_UNSET: + self.current_cruise_set_speed_ms = v_cruise_kph * CV.KPH_TO_MS + + def _has_large_cruise_set_speed_delta(self, speed_limit): + cruise_set_speed_ms = getattr(self, "current_cruise_set_speed_ms", 0.0) + return ( + cruise_set_speed_ms > 0.0 and + abs(self._speed_value_to_ms(speed_limit) - cruise_set_speed_ms) >= LARGE_SET_SPEED_DELTA_MPH * CV.MPH_TO_MS + ) + def _speed_value_from_ms(self, speed_ms): conversion = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH return int(round(speed_ms * conversion)) if speed_ms > 0.0 else 0 @@ -2205,6 +2227,10 @@ class SpeedLimitVisionDaemon: has_strong_consensus = any(entry.strong_consensus for entry in matching_entries) current_speed_limit = self.published_speed_limit_mph current_count = counts.get(current_speed_limit, 0) if current_speed_limit > 0 else 0 + large_set_speed_delta = ( + candidate_speed_limit != current_speed_limit and + self._has_large_cruise_set_speed_delta(candidate_speed_limit) + ) if current_speed_limit > 0 and candidate_speed_limit != current_speed_limit: required_count = CHANGE_CONSISTENT_DETECTIONS @@ -2219,6 +2245,9 @@ class SpeedLimitVisionDaemon: ) if best_confidence < LOW_SPEED_CHANGE_MIN_CONFIDENCE: return None + if large_set_speed_delta: + required_count = max(required_count, LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS) + allow_single_frame_confirmation = False if candidate_count < required_count and not allow_single_frame_confirmation: return None if ( @@ -2230,6 +2259,13 @@ class SpeedLimitVisionDaemon: return None return candidate_speed_limit, best_confidence + if large_set_speed_delta: + if candidate_count < LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS: + return None + if matching_confidences[LARGE_SET_SPEED_DELTA_CONSISTENT_DETECTIONS - 1] < CHANGE_REPEAT_MIN_CONFIDENCE: + return None + return candidate_speed_limit, best_confidence + if has_strong_consensus or best_confidence >= STRONG_DETECTION_CONFIDENCE or candidate_count >= CONSISTENT_DETECTIONS: return candidate_speed_limit, best_confidence return None @@ -2472,6 +2508,7 @@ class SpeedLimitVisionDaemon: now = time.monotonic() self.loop_count += 1 self.sm.update(0) + self._update_current_cruise_set_speed() if self.sm.updated["userBookmark"]: if self.ignore_next_user_bookmark: diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index f433187d4..8c2b930f4 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -30,6 +30,8 @@ class StaticClassifierNet: def daemon_with_history(current_speed, entries): daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.is_metric = False + daemon.current_cruise_set_speed_ms = 0.0 daemon.published_speed_limit_mph = current_speed daemon.history = deque(HistoryEntry(speed, confidence, float(index)) for index, (speed, confidence) in enumerate(entries)) return daemon @@ -189,6 +191,34 @@ def test_low_speed_change_rejects_low_confidence_sequence(): assert daemon._confirm_detection() is None +@pytest.mark.parametrize( + ("is_metric", "set_speed", "candidate_speed", "conversion"), + ( + (False, 75, 45, slv.CV.MPH_TO_MS), + (True, 100, 50, slv.CV.KPH_TO_MS), + ), +) +def test_large_cruise_set_speed_delta_requires_three_reads(is_metric, set_speed, candidate_speed, conversion): + daemon = daemon_with_history(0, [(candidate_speed, 0.99)]) + daemon.is_metric = is_metric + daemon.current_cruise_set_speed_ms = set_speed * conversion + assert daemon._has_large_cruise_set_speed_delta(candidate_speed) + assert daemon._confirm_detection() is None + + daemon.history.append(HistoryEntry(candidate_speed, 0.95, 1.0, strong_consensus=True)) + assert daemon._confirm_detection() is None + + daemon.history.append(HistoryEntry(candidate_speed, 0.91, 1.5)) + assert daemon._confirm_detection() == pytest.approx((candidate_speed, 0.99)) + + +def test_normal_cruise_set_speed_delta_keeps_single_read_confirmation(): + daemon = daemon_with_history(0, [(50, 0.99)]) + daemon.current_cruise_set_speed_ms = 75 * slv.CV.MPH_TO_MS + + assert daemon._confirm_detection() == pytest.approx((50, 0.99)) + + def textured_track_frame(offset_x=0, offset_y=0): frame = np.zeros((120, 180, 3), dtype=np.uint8) x1, y1, x2, y2 = 90 + offset_x, 30 + offset_y, 130 + offset_x, 90 + offset_y