Basic sliders with acceleration adjustments

This commit is contained in:
jakethesake420
2023-11-10 02:22:02 -06:00
parent 0f0ab321f2
commit c91ed41f82
12 changed files with 496 additions and 6 deletions
+3
View File
@@ -86,6 +86,9 @@ private:
};
std::unordered_map<std::string, uint32_t> keys = {
{"BehaviordInitialiazed", PERSISTENT},
{"MaxDeacceleration", PERSISTENT},
{"AccelCruiseMaxFactor", PERSISTENT},
{"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
{"ApiCache_Device", PERSISTENT},
{"ApiCache_NavDestinations", PERSISTENT},
+85
View File
@@ -0,0 +1,85 @@
#!/usr/bin/env python3
""" gets behavior input from UI and saves it to params"""
from common.params import Params
import cereal.messaging as messaging
import time
# Update this dict and run slider_gen.py to generate the code for the sliders, params, and log.capnp
PARAMS = {
"MaxDeacceleration": {"default": -1.2, "range": (-3.0, 0.0), "label": "Minimum Cruise Accel:", "units": "m/s<sup>2</sup>"},
"AccelCruiseMaxFactor": {"default": 1.0, "range": (0.0, 3.0), "label": "Cruise Accel Factor:", "units": "Coef."},
}
class LiveBehavior:
def __init__(self, log=False):
self.log = log
self.sm = messaging.SubMaster(['behavior'])
self.params = {param: info["default"] for param, info in PARAMS.items()}
def update(self) -> None:
self.sm.update(0)
if self.sm.updated['behavior']:
for param in PARAMS.keys():
self.params[param] = self.get_param(param)
def get_param(self, param: str) -> float:
# make the first letter lowercase to match the cereal message
lowercase_param = param[0].lower() + param[1:]
value = getattr(self.sm['behavior'], lowercase_param)
min_val, max_val = PARAMS[param]["range"]
return value if min_val <= value <= max_val else PARAMS[param]["default"]
def get_live_param(self, param: str) -> float:
self.update()
if self.log:
print(f"{param}: ", self.params[param])
return self.params[param]
''' Example:
lb = LiveBehavior()
x = lb.get_live_param("MaxDeacceleration")
print(x)
'''
class Behaviord:
def __init__(self):
self.sm = messaging.SubMaster(['behavior'])
self.p = Params()
self.init_params()
self.initialized = False
def init_params(self):
if any(self.p.get(param) is None for param in PARAMS):
for param, info in PARAMS.items():
self.p.put(param, str(info["default"]))
def get_param(self, param: str) -> float:
# make the first letter lowercase to match the cereal message
lowercase_param = param[0].lower() + param[1:]
value = getattr(self.sm['behavior'], lowercase_param)
min_val, max_val = PARAMS[param]["range"]
return value if min_val <= value <= max_val else PARAMS[param]["default"]
def save(self):
if self.sm.updated['behavior']:
#print("behavior updated")
for param in PARAMS.keys():
self.p.put(param, str(self.get_param(param)))
if not self.initialized:
self.p.put_bool("BehaviordInitialiazed", True)
self.initialized = True
def behaviord_thread(self):
while True:
self.sm.update(0)
self.save()
time.sleep(1)
def main():
behaviord = Behaviord()
behaviord.behaviord_thread()
if __name__ == "__main__":
main()
+10
View File
@@ -0,0 +1,10 @@
from common.params import Params
from selfdrive.controls.behaviord import PARAMS
p = Params()
# delete all params in PARAMS
for param in PARAMS.keys():
p.remove(param)
print(f"deleted {param}")
+15 -2
View File
@@ -1,10 +1,18 @@
import math
from cereal import log
<<<<<<< HEAD
from openpilot.common.numpy_fast import interp
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
=======
from common.numpy_fast import interp
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.pid import PIDController
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from selfdrive.controls.behaviord import LiveBehavior
>>>>>>> 26d90863f... WIP sliders
# At higher speeds (25+mph) we can assume:
# Lateral acceleration achieved by a specific car correlates to
@@ -19,7 +27,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_G
LOW_SPEED_X = [0, 10, 20, 30]
LOW_SPEED_Y = [15, 13, 10, 5]
lb = LiveBehavior()
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
@@ -30,6 +38,7 @@ class LatControlTorque(LatControl):
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
self.use_steering_angle = self.torque_params.useSteeringAngle
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
self.torque_params.latAccelFactor = latAccelFactor
@@ -38,7 +47,11 @@ class LatControlTorque(LatControl):
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message()
#self.torque_params.latAngleFactor = lb.get_live_param("LatAngleFactor")
#self.torque_params.latAccelFactor = lb.get_live_param("LatAccelFactor")
#self.torque_params.latAccelOffset = lb.get_live_param("LatAccelOffset")
#self.torque_params.friction = lb.get_live_param("Friction")
if not active:
output_torque = 0.0
pid_log.active = False
@@ -16,6 +16,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
from openpilot.system.swaglog import cloudlog
from openpilot.selfdrive.controls.behaviord import LiveBehavior
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
@@ -26,9 +27,10 @@ A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
lb = LiveBehavior()
def get_max_accel(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
return interp(v_ego, A_CRUISE_MAX_BP, np.array(A_CRUISE_MAX_VALS) * lb.get_live_param("AccelCruiseMaxFactor"))
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
@@ -107,7 +109,7 @@ class LongitudinalPlanner:
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if self.mpc.mode == 'acc':
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
accel_limits = [lb.get_live_param("MaxDeacceleration"), get_max_accel(v_ego)]
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
else:
accel_limits = [ACCEL_MIN, ACCEL_MAX]
+48
View File
@@ -0,0 +1,48 @@
#!/usr/bin/env python3
from selfdrive.controls.behaviord import PARAMS
def generate_slider_code():
cpp_code = 'BehaviorPanel::BehaviorPanel(SettingsWindow *parent) : ListWidget(parent){\n\n // Add sliders here\n // name, label, units, min, max, default, setter function\n std::vector<SliderDefinition> slider_defs{\n'
for param, info in PARAMS.items():
min_val, max_val = info["range"]
default_val = info["default"]
label = info["label"]
units = info["units"]
setter_func = f'[](cereal::Behavior::Builder &behavior, double value) {{\n behavior.set{param}(static_cast<float>(value));\n }}'
cpp_code += f' {{"{param}", tr("{label}"), "{units}", {min_val}, {max_val}, {default_val},\n {setter_func}\n }},\n'
cpp_code += ' };\n\n'
return cpp_code
def generate_param_code():
cpp_code = ''
for param in PARAMS.keys():
cpp_code += f' {{"{param}", PERSISTENT}},\n'
return cpp_code
def generate_log_code():
cpp_code = 'struct Behavior {\n'
for i, param in enumerate(PARAMS.keys()):
cpp_code += f' {param} @{i} :Float32;\n'
cpp_code += '};'
return cpp_code
if __name__ == "__main__":
print("paste the output of this into selfdrive/ui/qt/settings.cc")
print(generate_slider_code())
print("paste the output of this into common/params.cc")
print(generate_param_code())
print("paste the output of this into cereal/log.capnp")
print(generate_log_code())
+34
View File
@@ -43,6 +43,7 @@ def only_offroad(started, params, CP: car.CarParams) -> bool:
procs = [
DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),
<<<<<<< HEAD
NativeProcess("camerad", "system/camerad", ["./camerad"], driverview),
NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad),
@@ -81,6 +82,39 @@ procs = [
PythonProcess("updated", "selfdrive.updated", only_offroad, enabled=not PC),
PythonProcess("uploader", "system.loggerd.uploader", always_run),
PythonProcess("statsd", "selfdrive.statsd", always_run),
=======
NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], enabled=(not PC or WEBCAM), callback=driverview),
NativeProcess("encoderd", "system/loggerd", ["./encoderd"]),
NativeProcess("loggerd", "system/loggerd", ["./loggerd"], onroad=False, callback=logging),
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False),
NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False),
NativeProcess("sensord", "system/sensord", ["./sensord"], enabled=not PC),
NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)),
NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True),
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False),
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
PythonProcess("torqued", "selfdrive.locationd.torqued"),
PythonProcess("controlsd", "selfdrive.controls.controlsd"),
PythonProcess("deleter", "system.loggerd.deleter", offroad=True),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
PythonProcess("laikad", "selfdrive.locationd.laikad"),
PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI, onroad=False, callback=qcomgps),
PythonProcess("navd", "selfdrive.navd.navd"),
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], enabled=TICI, onroad=False, callback=ublox),
PythonProcess("pigeond", "system.sensord.pigeond", enabled=TICI, onroad=False, callback=ublox),
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
PythonProcess("radard", "selfdrive.controls.radard"),
PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),
PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True),
PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True),
PythonProcess("uploader", "system.loggerd.uploader", offroad=True),
PythonProcess("statsd", "selfdrive.statsd", offroad=True),
PythonProcess("behaviord", "selfdrive.controls.behaviord", offroad=True),
>>>>>>> d1acac47a... basic sliders
# debug procs
NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
+2 -2
View File
@@ -20,8 +20,8 @@ if arch == "Darwin":
qt_env['FRAMEWORKS'] += ['OpenCL']
qt_util = qt_env.Library("qt_util", ["#selfdrive/ui/qt/api.cc", "#selfdrive/ui/qt/util.cc"], LIBS=base_libs)
widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/wifi.cc",
"qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc",
widgets_src = ["ui.cc", "qt/widgets/input.cc","qt/widgets/wifi.cc",
"qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/slider.cc", "qt/widgets/controls.cc",
"qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc",
"qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc",
"qt/request_repeater.cc", "qt/qt_window.cc", "qt/network/networking.cc", "qt/network/wifi_manager.cc"]
+67
View File
@@ -19,6 +19,7 @@
#include "selfdrive/ui/qt/widgets/scrollview.h"
#include "selfdrive/ui/qt/widgets/ssh_keys.h"
#include "selfdrive/ui/qt/widgets/toggle.h"
#include "selfdrive/ui/qt/widgets/slider.h"
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/qt_window.h"
@@ -336,6 +337,71 @@ void DevicePanel::poweroff() {
}
}
BehaviorPanel::BehaviorPanel(SettingsWindow *parent) : ListWidget(parent){
// Add sliders here
// name, label, units, min, max, default, setter function
std::vector<SliderDefinition> slider_defs{
{"MaxDeacceleration", tr("Minimum Cruise Accel:"), "m/s<sup>2</sup>", -3.0, 0.0, -1.2,
[](cereal::Behavior::Builder &behavior, double value) {
behavior.setMaxDeacceleration(static_cast<float>(value));
}
},
{"AccelCruiseMaxFactor", tr("Cruise Accel Factor:"), "Coef.", 0.0, 3.0, 1.0,
[](cereal::Behavior::Builder &behavior, double value) {
behavior.setAccelCruiseMaxFactor(static_cast<float>(value));
}
},
};
// Loop through the slider definitions and create sliders
for (const auto &slider_def : slider_defs) {
// Get the setter function from the map
CustomSlider::CerealSetterFunction cerealSetFunc = slider_def.cerealSetFunc;
CustomSlider *slider = new CustomSlider(slider_def.paramName, \
cerealSetFunc, \
slider_def.unit, \
slider_def.title, \
slider_def.paramMin, \
slider_def.paramMax, \
slider_def.defaultVal, \
this);
sliders[slider_def.paramName] = slider; // Store the slider pointer in the map
sliderItems[slider_def.paramName] = slider->getSliderItem(); // Store the slider item pointer in the map
addItem(slider->getSliderItem()); // Add the slider item to the list widget
}
// create a pubmaster for all the sliders
pm = std::make_unique<PubMaster, const std::initializer_list<const char *>>(
{"behavior"});
timer = new QTimer(this);
timer->setInterval(1000); // Send all slider values every interval
timer->start();
connect(timer, &QTimer::timeout, this, &BehaviorPanel::sendAllSliderValues);
}
void BehaviorPanel::sendAllSliderValues()
{
MessageBuilder msg;
auto behavior = msg.initEvent().initBehavior();
// Iterate through all sliders and call their setter functions
for (const auto &slider : sliders)
{
double dValue = slider->paramMin + (slider->paramMax - slider->paramMin) * (slider->value() - slider->sliderMin) / (slider->sliderMax - slider->sliderMin);
slider->cerealSetFunc(behavior, dValue);
}
// Send the message with all slider values
pm->send("behavior", msg);
}
void SettingsWindow::showEvent(QShowEvent *event) {
setCurrentPanel(0);
}
@@ -389,6 +455,7 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
{tr("Network"), new Networking(this)},
{tr("Toggles"), toggles},
{tr("Software"), new SoftwarePanel(this)},
{tr("Behavior"), new BehaviorPanel(this)},
};
nav_btns = new QButtonGroup(this);
+33
View File
@@ -9,10 +9,15 @@
#include <QPushButton>
#include <QStackedWidget>
#include <QWidget>
#include <QSlider>
#include <QScrollArea>
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/widgets/slider.h"
// ********** settings window + top-level panels **********
class SettingsWindow : public QFrame {
@@ -72,6 +77,34 @@ private:
void updateToggles();
};
struct SliderDefinition {
std::string paramName;
QString title;
QString unit;
double paramMin;
double paramMax;
double defaultVal;
CustomSlider::CerealSetterFunction cerealSetFunc;
};
class BehaviorPanel : public ListWidget {
Q_OBJECT
public:
explicit BehaviorPanel(SettingsWindow *parent = nullptr);
public slots:
void sendAllSliderValues();
private:
std::unique_ptr<PubMaster> pm;
Params params;
std::map<std::string, QWidget *> sliderItems;
QMap<std::string, CustomSlider *> sliders;
QTimer *timer;
};
class SoftwarePanel : public ListWidget {
Q_OBJECT
public:
+113
View File
@@ -0,0 +1,113 @@
#include "selfdrive/ui/qt/widgets/slider.h"
#include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/qt/offroad/settings.h"
CustomSlider::CustomSlider(const std::string &param,
CerealSetterFunction cerealSetFunc,
const QString &unit,
const QString &title,
double paramMin,
double paramMax,
double defaultVal,
QWidget *parent) // Define the constructor
: // Call the base class constructor
QSlider(Qt::Horizontal, parent),
param(param), title(title), unit(unit),
paramMin(paramMin), paramMax(paramMax), defaultVal(defaultVal),
cerealSetFunc(cerealSetFunc) // Initialize the setter function
{
initialize(); // Call the initialize function
} // End of constructor
void CustomSlider::initialize()
{
// Create UI elements
sliderItem = new QWidget(parentWidget()); // Create a new widget
// Create a vertical layout to stack the title and reset button on top of the slider
QVBoxLayout *mainLayout = new QVBoxLayout(sliderItem);
// Create a horizontal layout to put the title and reset on left and right respectively
QHBoxLayout *titleLayout = new QHBoxLayout();
mainLayout->addLayout(titleLayout);
// Create the title label
label = new QLabel(title);
label->setStyleSheet(LabelStyle);
label->setTextFormat(Qt::RichText);
titleLayout->addWidget(label, 0, Qt::AlignLeft);
// Create the reset button
ButtonControl *resetButton = new ButtonControl(" ", tr("RESET"));
titleLayout->addWidget(resetButton, 0, Qt::AlignRight);
// Connect the reset button to set the slider value to the default value
connect(resetButton, &ButtonControl::clicked, [&]() {
if (ConfirmationDialog::confirm(tr("Are you sure you want to reset ") + QString::fromStdString(param) + "?", tr("Reset"), this)) {
this->setValue(sliderMin + (defaultVal - paramMin) / (paramMax - paramMin) * (sliderRange));
}
});
// slider settings
setFixedHeight(100);
setMinimum(sliderMin);
setMaximum(sliderMax);
// Set the default value of the slider to begin with
setValue(sliderMin + (defaultVal - paramMin) / (paramMax - paramMin) * (sliderRange));
label->setText(title + " " + QString::number(defaultVal, 'f', 2) + " " + unit);
try // Try to get the value of the param from params. If it doesn't exist, catch the error
{
QString valueStr;
double value;
if (Params().getBool("BehaviordInitialiazed")){
valueStr = QString::fromStdString(Params().get(param));
value = QString(valueStr).toDouble();
} else{
value = defaultVal;
}
// Set the value of the param in the behavior struct
MessageBuilder msg;
auto behavior = msg.initEvent().initBehavior();
cerealSetFunc(behavior, value);
setValue(sliderMin + (value - paramMin) / (paramMax - paramMin) * (sliderRange)); // Set the value of the slider. The value is scaled to the slider range
label->setText(title + " " + QString::number(value, 'f', 2) + " " + unit);
// Set the slider to be enabled or disabled depending on the lock status
bool locked = Params().getBool((param + "Lock"));
setEnabled(!locked);
setStyleSheet(locked ? lockedSliderStyle : SliderStyle);
label->setStyleSheet(locked ? lockedLabelStyle : LabelStyle);
}
catch (const std::invalid_argument &e)
{
// Handle the error, e.g. lock the slider and display an error message as the label
setValue(0);
label->setText(title + "Error: Param not found. Add param to behaviord");
setEnabled(false);
setStyleSheet(lockedSliderStyle);
}
mainLayout->addWidget(this);
connect(this, &CustomSlider::valueChanged, [=](int value)
{
// Update the label as the slider is moved. Don't save the value to params here
double dValue = paramMin + (paramMax - paramMin) * (value - sliderMin) / (sliderRange);
label->setText(title + " " + QString::number(dValue, 'f', 2) + " " + unit);
});
connect(this, &CustomSlider::sliderReleasedWithValue, [this]() {
// Call the sendAllSliderValues method from the BehaviorPanel
auto parentBehaviorPanel = qobject_cast<BehaviorPanel *>(this->parentWidget());
if (parentBehaviorPanel)
{
parentBehaviorPanel->sendAllSliderValues();
}
});
}
+82
View File
@@ -0,0 +1,82 @@
#pragma once
#include <QTimer>
#include <QLabel>
#include <QSlider>
#include <QWidget>
#include <QMouseEvent>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QPushButton>
#include <functional>
#include "common/params.h"
#include "selfdrive/ui/ui.h"
class CustomSlider : public QSlider {
Q_OBJECT
public:
using CerealSetterFunction = std::function<void(cereal::Behavior::Builder&, double)>;
CustomSlider(const std::string &param, CerealSetterFunction cerealSetFunc,
const QString &unit, const QString &title,
double paramMin, double paramMax, double defaultVal, QWidget *parent = nullptr);
QWidget *getSliderItem() {
return sliderItem;
}
CerealSetterFunction cerealSetFunc;
double paramMin;
double paramMax;
int sliderMin = 0;
int sliderMax = 10000;
signals:
void sliderReleasedWithValue(int value);
protected:
void mouseReleaseEvent(QMouseEvent *event) override {
QSlider::mouseReleaseEvent(event);
emit sliderReleasedWithValue(value());
}
private:
void initialize();
double defaultVal;
double scaleFactor;
std::string param;
QString title;
QString unit;
QWidget *sliderItem;
QLabel *label;
int sliderRange = sliderMax - sliderMin;
QString SliderStyle = R"(
QSlider::groove:horizontal
{border: none;height: 60px;background-color: #393939;border-radius: 30px;}
QSlider::handle:horizontal
{background-color: #fafafa;border: none;width: 80px;height: 80px;margin-top: -10px;margin-bottom: -10px;border-radius: 40px;}
)";
QString lockedSliderStyle = R"(
QSlider::groove:horizontal
{border: none;height: 60px;background-color: #393939;border-radius: 30px;}
QSlider::handle:horizontal
{background-color: #787878;border: none;width: 80px;height: 80px;margin-top: -10px;margin-bottom: -10px;border-radius: 40px;}
)";
// label
QString LabelStyle = R"(
QLabel {
color: #fafafa;
}
)";
QString lockedLabelStyle = R"(
QLabel {
color: #787878;
}
)";
};