Merge pull request #214 from dragonpilot-community/tetoo

Tē-Tôo module
dragonpilot-community · Jul 10, 2024 · bbb15c7 · bbb15c7
2 parents f9ff917 + 9ad7112
commit bbb15c7
Show file tree

Hide file tree

Showing 87 changed files with 6,957 additions and 6 deletions.
diff --git a/cereal/custom.capnp b/cereal/custom.capnp
@@ -26,7 +26,33 @@ enum LaneChangeAssistMode {
  auto @3;
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct TeToo @0xf35cc4560bbf6ec2 {
+ lat @0 :Float32;
+ lon @1 :Float32;
+ bearing @2 :Float32;
+ name @3 :Text;
+ maxspeed @4 :Float32;
+ tags @5 :Text;
+
+ updatingData @6 :Bool;
+ nearestFeatures @7 :List(Feature);
+
+ struct Feature {
+ id @0 :Text;
+ type @1 :FeatureType;
+ lat @2 :Float32;
+ lon @3 :Float32;
+ bearing @4 :Float32;
+ distance @5 :Float32;
+ tags @6 :Text;
+ probability @7 :Float32;
+ }
+
+ enum FeatureType {
+ trafficSignal @0;
+ speedCamera @1;
+ }
+
 }
 
 struct CustomReserved3 @0xda96579883444c35 {

diff --git a/cereal/log.capnp b/cereal/log.capnp
@@ -2351,7 +2351,7 @@ struct Event {
  # *********** Custom: reserved for forks ***********
  controlsStateExt @107 :Custom.ControlsStateExt;
  longitudinalPlanExt @108 :Custom.LongitudinalPlanExt;
- customReserved2 @109 :Custom.CustomReserved2;
+ teToo @109 :Custom.TeToo;
  customReserved3 @110 :Custom.CustomReserved3;
  customReserved4 @111 :Custom.CustomReserved4;
  customReserved5 @112 :Custom.CustomReserved5;

diff --git a/cereal/services.py b/cereal/services.py
@@ -91,6 +91,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int]
  # dp
  "controlsStateExt": (False, 100., 10),
  "longitudinalPlanExt": (False, 20., 5),
+ "teToo": (False, 5),
 }
 SERVICE_LIST = {name: Service(*vals) for
  idx, (name, vals) in enumerate(_services.items())}

diff --git a/common/params.cc b/common/params.cc
@@ -237,6 +237,10 @@ std::unordered_map<std::string, uint32_t> keys = {
  {"dp_toyota_auto_brake_hold", PERSISTENT},
  {"dp_device_offroad", CLEAR_ON_MANAGER_START},
  {"dp_toyota_sng", PERSISTENT},
+ {"dp_tetoo", PERSISTENT},
+ {"dp_tetoo_data", PERSISTENT},
+ {"dp_tetoo_gps", PERSISTENT},
+ {"dp_tetoo_taiwan_speed_camera", PERSISTENT},
 };
 
 } // namespace

diff --git a/dp_ext b/dp_ext
diff --git a/dp_priv b/dp_priv
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
@@ -76,6 +76,15 @@ function launch {
  # hardware specific init
  if [ -f /AGNOS ]; then
  agnos_init
+ # dp
+ if ! (LD_LIBRARY_PATH=$DIR/third_party/libspatialindex/larch64/lib/ python -c "import rtree") &> /dev/null; then
+ pip install "$DIR/dp_ext/selfdrive/tetood/pkgs/Rtree-1.2.0-cp311-cp311-linux_aarch64.whl"
+ fi
+ else
+ # dp
+ if ! (LD_LIBRARY_PATH=$DIR/third_party/libspatialindex/x86_64/lib/ python -c "import rtree") &> /dev/null; then
+ pip install "$DIR/dp_ext/selfdrive/tetood/pkgs/Rtree-1.2.0-cp312-cp312-linux_x86_64.whl"
+ fi
  fi
 
  # write tmux scrollback to a file

diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
@@ -47,6 +47,7 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par
  rainbow_path = new RainbowPath;
  flight_panel = new FlightPanel;
  chevron_ext = new ChevronExt;
+ tt_indicator = new TeTooIndicator;
  #endif
 
 }
@@ -94,6 +95,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
  rainbow_path->update_states(s);
  flight_panel->update_states(s, is_metric);
  chevron_ext->update_states(s, is_metric);
+ tt_indicator->update_states(s);
  #endif
 }
 
@@ -145,6 +147,11 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
  p.setFont(InterFont(66));
  drawText(p, rect().center().x(), 290, speedUnit, 200);
 
+ #ifdef DP
+ tt_indicator->paint_speed_camera(p, rect(), speedStr);
+ tt_indicator->paint_maxspeed(p, set_speed_rect.topRight());
+ #endif
+
  p.restore();
 }
 
@@ -395,6 +402,7 @@ void AnnotatedCameraWidget::paintGL() {
  update_model(s, model);
 
  #ifdef DP
+ tt_indicator->paint_road_name(painter, rect().left(), rect().bottom(), rect().width());
  flight_panel->paint(painter, width(), height());
  #endif
 

diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h
@@ -44,7 +44,8 @@ class AnnotatedCameraWidget : public CameraWidget {
  PersonalityButton *personality_btn;
  KnightScanner *knight_scanner;
  RainbowPath *rainbow_path;
- ChevronExt * chevron_ext;
+ ChevronExt *chevron_ext;
+ TeTooIndicator *tt_indicator;
  #endif
 
 protected:

diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts
@@ -224,7 +224,7 @@ Your vehicle must support openpilot longitudinal control.</source>
 Once activate, the lead vehicle indicator will display an &quot;A&quot;</source>
  <translation>當速度低於設定值時，openpilot將會切換到指定的「駕駛風格」。
 1 km/h = 0.62 mph。
-一旦啟用，前車指示將顯示一個「A」。</translation>
+一旦啟用，前車指示將顯示一個「A」</translation>
  </message>
  <message>
  <source>OFF</source>
@@ -525,6 +525,40 @@ Reboot Required.</source>
 感謝 Alexandre Sato 的貢獻：https://github.com/AlexandreSato/
 需要重新啟動。</translation>
  </message>
+ <message>
+ <source>Enable Stop and Go (SnG) Hack</source>
+ <translation>啟用 Stop and Go (SnG) 修正</translation>
+ </message>
+ <message>
+ <source>When enabled, openpilot will stop sending standstill signal when the car is fully stopped.
+ONLY WORK ON SOME VEHICLES.
+Reboot Required.</source>
+ <translation>啟用後，openpilot 在車輛完全停止時將不再發送停止信號。
+僅適用於部分車輛。
+需要重新啟動。</translation>
+ </message>
+ <message>
+ <source>Display Road Name</source>
+ <translation>顯示道路名稱</translation>
+ </message>
+ <message>
+ <source>When enabled, openpilot will display live road name using OpenStreetMap data.
+Internet required.</source>
+ <translation>啟用後，openpilot 將使用 OpenStreetMap 資料顯示即時道路名稱。
+需要網路連線。</translation>
+ </message>
+ <message>
+ <source>Display Speed Camera Warning (Taiwan)</source>
+ <translation>顯示測速照相警告（台灣）</translation>
+ </message>
+ <message>
+ <source>Tē-Tôo (Map)</source>
+ <translation>地圖</translation>
+ </message>
+ <message>
+ <source>When enabled, openpilot will display speed camera warning using offline database.</source>
+ <translation>啟用後，openpilot 將使用離線資料庫顯示測速照相警告。</translation>
+ </message>
 </context>
 <context>
  <name>DeclinePage</name>
@@ -1322,6 +1356,26 @@ This may take up to a minute.</source>
  <source>never</source>
  <translation>從未更新</translation>
  </message>
+ <message>
+ <source>Onroad/Offroad Mode</source>
+ <translation>上線/離線 模式</translation>
+ </message>
+ <message>
+ <source>Go Offroad</source>
+ <translation>切換到離線</translation>
+ </message>
+ <message>
+ <source>Are you sure you want to switch driving mode?</source>
+ <translation>您確定要切換模式嗎？</translation>
+ </message>
+ <message>
+ <source>CONFIRM</source>
+ <translation>確認</translation>
+ </message>
+ <message>
+ <source>Go Onroad</source>
+ <translation>切換到上線</translation>
+ </message>
 </context>
 <context>
  <name>SshControl</name>

diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
@@ -252,6 +252,8 @@ UIState::UIState(QObject *parent) : QObject(parent) {
  "carControl", "controlsStateExt",
  // de2e
  "longitudinalPlanExt",
+ // TeToo
+ "teToo",
  });
 
  Params params;

diff --git a/system/manager/manager.py b/system/manager/manager.py
@@ -75,6 +75,10 @@ def manager_init() -> None:
  ("dp_toyota_enhanced_bsm", "0"),
  ("dp_toyota_auto_brake_hold", "0"),
  ("dp_toyota_sng", "0"),
+ ("dp_tetoo", "0"),
+ ("dp_tetoo_data", ""),
+ ("dp_tetoo_gps", ""),
+ ("dp_tetoo_taiwan_speed_camera", "0"),
  ]
  if not PC:
  default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))

diff --git a/system/manager/process_config.py b/system/manager/process_config.py
@@ -53,6 +53,9 @@ def dp_onroad_uploads(started, params, CP: car.CarParams) -> bool:
 def dpdmonitoringd(started, params, CP: car.CarParams) -> bool:
  return params.get_bool("dp_device_dm_unavailable") and started
 
+def tetood(started, params, CP: car.CarParams) -> bool:
+ return started and (params.get_bool("dp_tetoo") or params.get_bool("dp_tetoo_taiwan_speed_camera"))
+
 procs = [
  DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"),
 
@@ -100,6 +103,7 @@ def dpdmonitoringd(started, params, CP: car.CarParams) -> bool:
 
  #dp
  PythonProcess("dpdmonitoringd", "dp_ext.selfdrive.monitoring.dmonitoringd", dpdmonitoringd, enabled=not PC),
+ NativeProcess("tetood", "dp_ext/selfdrive/tetood", ["./tetood"], tetood),
 ]
 
 managed_processes = {p.name: p for p in procs}
diff --git a/third_party/libspatialindex/larch64/include/spatialindex/Ball.h b/third_party/libspatialindex/larch64/include/spatialindex/Ball.h
@@ -0,0 +1,98 @@
+/******************************************************************************
+ * Project: libspatialindex - A C++ library for spatial indexing
+ * Author: Peter Labadorf - plaba3.1415@gmail.com
+ ******************************************************************************
+ * Copyright (c) 2023, Peter Labadorf
+ *
+ * All rights reserved.
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+******************************************************************************/
+
+#pragma once
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace SpatialIndex
+{
+
+ class SIDX_DLL Ball: public Tools::IObject, public virtual IShape
+ {
+ public:
+ Ball();
+ Ball(double radius, const Point& center);
+ Ball(double radius, const double *pCoords, uint32_t dimension);
+ Ball(const Ball& b);
+ ~Ball() override;
+
+ virtual Ball& operator=(const Ball& b);
+ virtual bool operator==(const Ball& b) const;
+
+ //
+ // IObject interface
+ //
+
+ Ball* clone() override;
+
+ //
+ // ISerializable interface
+ //
+
+ uint32_t getByteArraySize() override;
+ void loadFromByteArray(const uint8_t *data) override;
+ void storeToByteArray(uint8_t **data, uint32_t &length) override;
+
+ //
+ // IShape interface
+ //
+
+ bool intersectsShape(const IShape &in) const override;
+ bool containsShape(const IShape &in) const override;
+ bool touchesShape(const IShape &in) const override;
+ void getCenter(SpatialIndex::Point &out) const override;
+ uint32_t getDimension() const override;
+ void getMBR(SpatialIndex::Region &out) const override;
+ double getArea() const override;
+ double getMinimumDistance(const IShape &in) const override;
+
+ virtual bool containsLineSegment(const SpatialIndex::LineSegment *line) const;
+ virtual bool containsRegion(const SpatialIndex::Region *region) const;
+
+ inline bool containsPoint(const Point *point) const
+ {
+ return getMinimumDistance(*point) <= m_centerPoint.m_dimension;
+ }
+
+ inline bool containsBall(const Ball *ball) const
+ {
+ return getMinimumDistance(ball->m_centerPoint) + ball->m_radius <= m_radius;
+ }
+
+ public:
+ double m_radius{0.0};
+ Point m_centerPoint;
+
+ }; // Ball
+
+ SIDX_DLL std::ostream& operator<<(std::ostream& os, const Ball& ball);
+
+}
+