From 328374ed04fda0206024241c67e71e8680e9ee8f Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Tue, 21 May 2024 17:21:48 +0300 Subject: [PATCH] feat: update autoware state panel (#7036) Signed-off-by: KhalilSelyan --- common/tier4_state_rviz_plugin/CMakeLists.txt | 27 + .../icons/assets/active.png | Bin 0 -> 739 bytes .../icons/assets/crash.png | Bin 0 -> 1279 bytes .../icons/assets/danger.png | Bin 0 -> 808 bytes .../icons/assets/none.png | Bin 0 -> 531 bytes .../icons/assets/pending.png | Bin 0 -> 753 bytes common/tier4_state_rviz_plugin/package.xml | 1 + .../src/autoware_state_panel.cpp | 900 +++++++++++------- .../src/custom_button.cpp | 129 +++ .../src/custom_container.cpp | 70 ++ .../src/custom_icon_label.cpp | 80 ++ .../src/custom_label.cpp | 82 ++ .../src/custom_segmented_button.cpp | 75 ++ .../src/custom_segmented_button_item.cpp | 186 ++++ .../src/custom_slider.cpp | 102 ++ .../src/custom_toggle_switch.cpp | 87 ++ .../{ => include}/autoware_state_panel.hpp | 86 +- .../src/include/custom_button.hpp | 69 ++ .../src/include/custom_container.hpp | 52 + .../src/include/custom_icon_label.hpp | 54 ++ .../src/include/custom_label.hpp | 46 + .../src/include/custom_segmented_button.hpp | 53 ++ .../include/custom_segmented_button_item.hpp | 64 ++ .../src/include/custom_slider.hpp | 35 + .../src/include/custom_toggle_switch.hpp | 40 + .../src/include/material_colors.hpp | 88 ++ .../velocity_steering_factors_panel.hpp | 0 .../src/velocity_steering_factors_panel.cpp | 2 +- 28 files changed, 1961 insertions(+), 367 deletions(-) create mode 100644 common/tier4_state_rviz_plugin/icons/assets/active.png create mode 100644 common/tier4_state_rviz_plugin/icons/assets/crash.png create mode 100644 common/tier4_state_rviz_plugin/icons/assets/danger.png create mode 100644 common/tier4_state_rviz_plugin/icons/assets/none.png create mode 100644 common/tier4_state_rviz_plugin/icons/assets/pending.png create mode 100644 common/tier4_state_rviz_plugin/src/custom_button.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_container.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_icon_label.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_label.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_slider.cpp create mode 100644 common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp rename common/tier4_state_rviz_plugin/src/{ => include}/autoware_state_panel.hpp (72%) create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_button.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_container.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_label.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_slider.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp create mode 100644 common/tier4_state_rviz_plugin/src/include/material_colors.hpp rename common/tier4_state_rviz_plugin/src/{ => include}/velocity_steering_factors_panel.hpp (100%) diff --git a/common/tier4_state_rviz_plugin/CMakeLists.txt b/common/tier4_state_rviz_plugin/CMakeLists.txt index afe21f66291b2..6b569ddb6d162 100644 --- a/common/tier4_state_rviz_plugin/CMakeLists.txt +++ b/common/tier4_state_rviz_plugin/CMakeLists.txt @@ -13,8 +13,32 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/autoware_state_panel.cpp src/velocity_steering_factors_panel.cpp + src/custom_toggle_switch.cpp + src/custom_slider.cpp + src/custom_container.cpp + src/custom_button.cpp + src/custom_icon_label.cpp + src/custom_segmented_button.cpp + src/custom_segmented_button_item.cpp + src/custom_label.cpp + src/include/material_colors.hpp + src/include/autoware_state_panel.hpp + src/include/custom_button.hpp + src/include/custom_container.hpp + src/include/custom_icon_label.hpp + src/include/custom_label.hpp + src/include/custom_segmented_button_item.hpp + src/include/custom_segmented_button.hpp + src/include/custom_slider.hpp + src/include/custom_toggle_switch.hpp + src/include/velocity_steering_factors_panel.hpp ) +target_include_directories( + ${PROJECT_NAME} PUBLIC +) + + target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) @@ -22,6 +46,9 @@ target_link_libraries(${PROJECT_NAME} # Export the plugin to be imported by rviz2 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + + + ament_auto_package( INSTALL_TO_SHARE icons diff --git a/common/tier4_state_rviz_plugin/icons/assets/active.png b/common/tier4_state_rviz_plugin/icons/assets/active.png new file mode 100644 index 0000000000000000000000000000000000000000..db9c81211abd5190fca2294362438ed7d9490cec GIT binary patch literal 739 zcmeAS@N?(olHy`uVBq!ia0vp^2_VeD1|%QND7OGojKx9jP7LeL$-HD>V4Cac;uum9 z_x858ACseq+rw8!cB;%eF=6QmuPbw3conE`5^2jYaeTgP3$MaOwpA;SuUz#{vb^~4 z&mCKAa(ePI8rkNqd+5gSVX^}QQv>&b$6FaGr!4yH_wT9NbnKjcT)do&f1#C+^n(d2 zd-HpC=J9VmcCXz<;DBJm-5;}^os~EqFgS$&xxm46gSkMX&Vz-qgki^#`*W_{c$W8d zTTRK1DJ61B4fc`0j{M(eaQ=S${N>A!<)0Tm;J5e6M^=X8;s>M}DxT%9SF~FnZ#u{O znEW5M3m@5eCpiB($n-?0egcE#QF$4MypQax6O#WNWOyP~FTh}Vyr0QHs#pE3?E0|N ztbg5fzyCPMcz5D;YyArW^fq+-1JkNvpKlj)7Oj4dA zV$X5l-VX++%#V8+`;Pf{1SHkeH*|kooBQU>%k^_RuD1j|u4dj+ZqOv?)hEx=;G$t$ zXTZR;uzdVF;=S>OrzfDKhGoI` z(-;5wYS`Ja2{0_|`k~Rl?qvG0ut8X0Ro8r02mY2Tx>fhUS9Dzx^+~%U`xrd2*e|LEAGm>f8+51REG0Fny@Vr3UpwU*BLy!Q^)tM%DlT literal 0 HcmV?d00001 diff --git a/common/tier4_state_rviz_plugin/icons/assets/crash.png b/common/tier4_state_rviz_plugin/icons/assets/crash.png new file mode 100644 index 0000000000000000000000000000000000000000..18175a823ae4ab01675eaad71f5ffc0236c7d245 GIT binary patch literal 1279 zcmVPx(xk*GpRCr$Pok3FTFbsz6!Wue52Ux;MfSbTcfRn&Y;GKjnfhjYE^`m~z9Re*R zwPpXZm7uIsOZq=cwqqx?rf1jg(-U$6rhj}MO)TZ_xl`{;T;f1KueUt$*IeRf;!&c53-gwLB| zW(BYeWZT^sV@{1TQ$#@A06u(vSc(Snxe9q3n-c;GfCTZqRxAKGAs_%q5uX(R7ld2@lEh~R zzyTo#fHd)o06;@X1t3xUVgN`Gm}VQ%69XpuT@xd=LAcFePz2`SYuj(rq$q%mnZPW5 z6MQFxLiWEyC=Q?%LgC7w?Y^oR@Y}=|F$+@B*A>tVsN5qIfC8wjKxOl+8L%_izqH1lE{~=3 z?Q&*nteHCF?%!t5*Ie$i0oa`!-1SoUHl>}JzP>yib3kALFevdG)tMl`0gU$!&z)VK zh8x^voxPa9Jl-M$g98|3@BdO<{;`zMjuo=^)M(2Zdxm^=1FF1NFy3LOj{8eYWz%m2@5HtANzxJ5>bsrRnm@`DEatZ+uQGVd_XO!4o z?F7pZD5D_&Lgj~J1<4JdR2j+!Ftqa1fGt&qvH^t3PoDhL;+ABSy6vg}!1NRVSWs$n3V^B@_Z^agPNCMJOs^|305=_$(;txlmVbn> z;j36B#a}{T^hYLukFLkD^v@ZTIs6d~AV;je{8j-}CZzxrKxG9go7W6b0F@P}Y+f@! z0aR9?vU$w_1yEUm%H}l#6hLJKDw`LV0aS-7>-WPvb%%}`FZM=wFd5A*fD1Tn`;2SE5HG-X3xTIp&0p}dfo5-%A5<**Xv5R_B^-0_kDc%ef7 p8Sg*&K6)Qr_vIULA^4o}{sX}1GoAfbBya!#002ovPDHLkV1lwBE~)?k literal 0 HcmV?d00001 diff --git a/common/tier4_state_rviz_plugin/icons/assets/danger.png b/common/tier4_state_rviz_plugin/icons/assets/danger.png new file mode 100644 index 0000000000000000000000000000000000000000..baed346deea2d7b13096432ad9ca0fdc8784aaa5 GIT binary patch literal 808 zcmeAS@N?(olHy`uVBq!ia0vp^2_VeD1|%QND7OGojKx9jP7LeL$-HD>V0z@~;uum9 z_jZn>pHiTR>;L7l+?*~FkG!^oU)nKgTlk{ALNBMD5a3aHaN~n@!W_X3ui`2~Pp({9 zdSCn8&-cGqGFle43Y}PZH*=nVz#a~PhK2;DgKuXtu&@Yo+{PuvdLu6f>4ik^SR3VOl7m}Me3>-qd6dV?&pHMo$ z5UAP6&^k}Cn}OM7DGQTG)zDWccXKj})cn958}th1-Jb(!JZmZ^y4( zQt0=yZvMHw`daxec^H;HqYd-cESYToF;$E+bYfAjmCEZH3MXN|G#^ix|L8SLZhS6(ex z;lJI!#@Dhp+t%50^VzJzXzl8S0n%y@ZXfsGw!nDJtk?g}2A``8T3#aEx2Si1!(YQM z?v*Py^A_@~-JHXp@=C___J&{f@yT1Ctg4X7{qylNH_+K{ELTI9ax*GYJUzDD1Emh9t)S%xCwQ7p$4}xD%N489ZJ6T-G@yGywqj Crbmwe literal 0 HcmV?d00001 diff --git a/common/tier4_state_rviz_plugin/icons/assets/none.png b/common/tier4_state_rviz_plugin/icons/assets/none.png new file mode 100644 index 0000000000000000000000000000000000000000..c44f9f485dbf1557f3a101f850a532b50a6c958f GIT binary patch literal 531 zcmeAS@N?(olHy`uVBq!ia0vp^2_VeD1|%QND7OGojKx9jP7LeL$-D%z&w9EzhE&A8 zopz9uDNy9dHLg7z-{R*zILamB&EeuUtwJvC$4R>nMPFtM?eMuSZ7*c-k3(oegJI*x z8u39v6__zzzx~&??y~>$`19?*F0M+wKV7}yr$58v(h^328M6Kjz9QdL8JIRd=G@SE zXdjD1LfN19tT*`Ws^|YR|MKR*oA0Uz)VO{NF*M%%$a2FaF`h}mzhYG3@mBK zg%gBZYdISZys2QyS+qePhlw_fW`Q@RGjN>gQy)mf{xcjhQAqq}lk^Z6%M6~belF{r G5}E+RGuJZ! literal 0 HcmV?d00001 diff --git a/common/tier4_state_rviz_plugin/icons/assets/pending.png b/common/tier4_state_rviz_plugin/icons/assets/pending.png new file mode 100644 index 0000000000000000000000000000000000000000..3925162878fd5f7a06aa352c24b74a9279936599 GIT binary patch literal 753 zcmeAS@N?(olHy`uVBq!ia0vp^2_VeD1|%QND7OGojKx9jP7LeL$-HD>U|Q|z;uum9 z_jZP3-mL%;*ZYel)jeE1+g|!jnyRu!<)oKWcu<)TzakT-QC_8JNNxz%KepempSibd zK8yV=6aV?|-5(yDJ51&lI5+%Ib6{XP!gA!J2?Jvi)1wNh1_ndMLK|L(h8YbO^Vk#| zd>mx_nFSQo6#AAkatH_u9KX!K(!tsBIFl;cKHOya@;m(c>d9B<&b~k6W^ZfgvoKee ztG{nnN9m}A&d%1W{r7Ea`K^Y_pS#)3>+f(|tl{dvc>B%QmlB*y&)+XSJD1bKdjFq& zJ0H)kJ$Cx$mu)u}?(f&C{kJdst&2l_L8-jl`E~~0?&HgRHEQc*r(a{bcK(S)=XB=> z`F*^r@6_&?;d`ufziw?^mv=Vf$>&99B~3CNUw;0~EfHh0`**EY#`*6j|2JJe^nCe_ zEqMoSKKOL>&bnYn>;3$zr+t2Teb@bIcJ@bIwGW(`wmI{ypggpx@hLkpSjyoU$D=dAA7*+!3=R#etErTlLKF| z*dPA0Mn9>(O@2p=kDmQ@|6N~xMs8R7=jD-mEH|!UcD~~u^@SgP?pE0n*OteBH%Gia z(ssx1Uw0qWTHi{#^ZVlOYbVP}4u0NVS*o|-r19M3dz=amHRcKf5~;{Z=%2XoukwPN TfY?32G|b@X>gTe~DWM4fL-|8M literal 0 HcmV?d00001 diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 06d57bd947af3..1db152e9632f8 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -6,6 +6,7 @@ The autoware state rviz plugin package Hiroki OTA Takagi, Isamu + Khalil Selyan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index ba961b9ac5b00..f9fb7d3dd3958 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -14,15 +14,13 @@ // limitations under the License. // -#include "autoware_state_panel.hpp" +#include "include/autoware_state_panel.hpp" -#include -#include -#include -#include -#include #include +#include +#include + #include #include @@ -35,181 +33,59 @@ namespace rviz_plugins { AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(parent) { - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - auto * gear_layout = new QHBoxLayout; - gear_layout->addWidget(gear_prefix_label_ptr); - gear_layout->addWidget(gear_label_ptr_); - - // Velocity Limit - velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); - pub_velocity_limit_input_ = new QSpinBox(); - pub_velocity_limit_input_->setRange(-100.0, 100.0); - pub_velocity_limit_input_->setValue(0.0); - pub_velocity_limit_input_->setSingleStep(5.0); - connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); - - // Emergency Button - emergency_button_ptr_ = new QPushButton("Set Emergency"); - connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + // Panel Configuration + this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); // Layout - auto * v_layout = new QVBoxLayout; - auto * velocity_limit_layout = new QHBoxLayout(); - v_layout->addWidget(makeOperationModeGroup()); - v_layout->addWidget(makeControlModeGroup()); - { - auto * h_layout = new QHBoxLayout(); - h_layout->addWidget(makeRoutingGroup()); - h_layout->addWidget(makeLocalizationGroup()); - h_layout->addWidget(makeMotionGroup()); - h_layout->addWidget(makeFailSafeGroup()); - v_layout->addLayout(h_layout); - } - - v_layout->addLayout(gear_layout); - velocity_limit_layout->addWidget(velocity_limit_button_ptr_); - velocity_limit_layout->addWidget(pub_velocity_limit_input_); - velocity_limit_layout->addWidget(new QLabel(" [km/h]")); - velocity_limit_layout->addWidget(emergency_button_ptr_); - v_layout->addLayout(velocity_limit_layout); - setLayout(v_layout); -} - -QGroupBox * AutowareStatePanel::makeOperationModeGroup() -{ - auto * group = new QGroupBox("OperationMode"); - auto * grid = new QGridLayout; - - operation_mode_label_ptr_ = new QLabel("INIT"); - operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); - operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); - - auto_button_ptr_ = new QPushButton("AUTO"); - auto_button_ptr_->setCheckable(true); - connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); - grid->addWidget(auto_button_ptr_, 0, 1); - - stop_button_ptr_ = new QPushButton("STOP"); - stop_button_ptr_->setCheckable(true); - connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_button_ptr_, 0, 2); - - local_button_ptr_ = new QPushButton("LOCAL"); - local_button_ptr_->setCheckable(true); - connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); - grid->addWidget(local_button_ptr_, 1, 1); - - remote_button_ptr_ = new QPushButton("REMOTE"); - remote_button_ptr_->setCheckable(true); - connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); - grid->addWidget(remote_button_ptr_, 1, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeControlModeGroup() -{ - auto * group = new QGroupBox("AutowareControl"); - auto * grid = new QGridLayout; - - control_mode_label_ptr_ = new QLabel("INIT"); - control_mode_label_ptr_->setAlignment(Qt::AlignCenter); - control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(control_mode_label_ptr_, 0, 0); - - enable_button_ptr_ = new QPushButton("Enable"); - enable_button_ptr_->setCheckable(true); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); - grid->addWidget(enable_button_ptr_, 0, 1); - - disable_button_ptr_ = new QPushButton("Disable"); - disable_button_ptr_->setCheckable(true); - connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); - grid->addWidget(disable_button_ptr_, 0, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing"); - auto * grid = new QGridLayout; - routing_label_ptr_ = new QLabel("INIT"); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_button_ptr_ = new QPushButton("Clear Route"); - clear_route_button_ptr_->setCheckable(true); - connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - grid->addWidget(clear_route_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeLocalizationGroup() -{ - auto * group = new QGroupBox("Localization"); - auto * grid = new QGridLayout; - - localization_label_ptr_ = new QLabel("INIT"); - localization_label_ptr_->setAlignment(Qt::AlignCenter); - localization_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(localization_label_ptr_, 0, 0); - - init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); - connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); - grid->addWidget(init_by_gnss_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeMotionGroup() -{ - auto * group = new QGroupBox("Motion"); - auto * grid = new QGridLayout; - - motion_label_ptr_ = new QLabel("INIT"); - motion_label_ptr_->setAlignment(Qt::AlignCenter); - motion_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(motion_label_ptr_, 0, 0); - - accept_start_button_ptr_ = new QPushButton("Accept Start"); - accept_start_button_ptr_->setCheckable(true); - connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); - grid->addWidget(accept_start_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeFailSafeGroup() -{ - auto * group = new QGroupBox("FailSafe"); - auto * grid = new QGridLayout; - - mrm_state_label_ptr_ = new QLabel("INIT"); - mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_state_label_ptr_, 0, 0); - - mrm_behavior_label_ptr_ = new QLabel("INIT"); - mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_behavior_label_ptr_, 1, 0); - - group->setLayout(grid); - return group; + // Create a new container widget + QWidget * containerWidget = new QWidget(this); + containerWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + containerWidget->setStyleSheet( + QString("QWidget { background-color: %1; color: %2; }") + .arg(autoware::state_rviz_plugin::colors::default_colors.background.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + + auto * containerLayout = new QVBoxLayout(containerWidget); + // Set the alignment of the layout + containerLayout->setAlignment(Qt::AlignTop); + containerLayout->setSpacing(1); + + auto * operation_mode_group = makeOperationModeGroup(); + auto * diagnostic_v_layout = new QVBoxLayout; + auto * localization_group = makeLocalizationGroup(); + auto * motion_group = makeMotionGroup(); + auto * fail_safe_group = makeFailSafeGroup(); + auto * routing_group = makeRoutingGroup(); + auto * velocity_limit_group = makeVelocityLimitGroup(); + // auto * diagnostic_group = makeDiagnosticGroup(); + + diagnostic_v_layout->addLayout(routing_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(localization_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(motion_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(fail_safe_group); + + // containerLayout->addLayout(diagnostic_group); + + containerLayout->addLayout(operation_mode_group); + // containerLayout->addSpacing(5); + containerLayout->addLayout(diagnostic_v_layout); + // main_v_layout->addSpacing(5); + containerLayout->addLayout(velocity_limit_group); + + // Create a QScrollArea + QScrollArea * scrollArea = new QScrollArea(this); + scrollArea->setWidgetResizable(true); + scrollArea->setWidget(containerWidget); + + // Main layout for AutowareStatePanel + QVBoxLayout * mainLayout = new QVBoxLayout(this); + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); } void AutowareStatePanel::onInitialize() @@ -268,9 +144,9 @@ void AutowareStatePanel::onInitialize() "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMRMState, this, _1)); - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); + // // Diagnostics + // sub_diag_ = raw_node_->create_subscription( + // "/diagnostics", 10, std::bind(&AutowareStatePanel::onDiagnostics, this, _1)); sub_emergency_ = raw_node_->create_subscription( "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); @@ -280,174 +156,498 @@ void AutowareStatePanel::onInitialize() pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} -void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - auto changeButtonState = [this]( - QPushButton * button, const bool is_desired_mode_available, - const uint8_t current_mode = OperationModeState::UNKNOWN, - const uint8_t desired_mode = OperationModeState::STOP) { - if (is_desired_mode_available && current_mode != desired_mode) { - activateButton(button); + QObject::connect(segmented_button, &CustomSegmentedButton::buttonClicked, this, [this](int id) { + const QList buttons = segmented_button->getButtonGroup()->buttons(); + + // Check if the button ID is within valid range + if (id < 0 || id >= buttons.size()) { + return; + } + + // Ensure the button is not null + QAbstractButton * abstractButton = segmented_button->getButtonGroup()->button(id); + if (!abstractButton) { + return; + } + + QPushButton * button = qobject_cast(abstractButton); + if (button) { + // Call the corresponding function for each button + if (button == auto_button_ptr_) { + onClickAutonomous(); + } else if (button == local_button_ptr_) { + onClickLocal(); + } else if (button == remote_button_ptr_) { + onClickRemote(); + } else if (button == stop_button_ptr_) { + onClickStop(); + } } else { - deactivateButton(button); + // qDebug() << "Button not found with ID:" << id; } - }; + }); +} - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; +QVBoxLayout * AutowareStatePanel::makeOperationModeGroup() +{ + control_mode_switch_ptr_ = new CustomToggleSwitch(this); + connect( + control_mode_switch_ptr_, &QCheckBox::stateChanged, this, + &AutowareStatePanel::onSwitchStateChanged); + + control_mode_label_ptr_ = new QLabel("Autoware Control"); + control_mode_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + CustomContainer * group1 = new CustomContainer(this); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(control_mode_switch_ptr_); + horizontal_layout->addWidget(control_mode_label_ptr_); + + // add switch and label to the container + group1->setContentsMargins(0, 0, 0, 10); + group1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + // Create the CustomSegmentedButton + segmented_button = new CustomSegmentedButton(this); + auto_button_ptr_ = segmented_button->addButton("Auto"); + local_button_ptr_ = segmented_button->addButton("Local"); + remote_button_ptr_ = segmented_button->addButton("Remote"); + stop_button_ptr_ = segmented_button->addButton("Stop"); + + QVBoxLayout * groupLayout = new QVBoxLayout; + // set these widgets to show up at the left and not stretch more than needed + groupLayout->setAlignment(Qt::AlignCenter); + groupLayout->setContentsMargins(10, 0, 0, 0); + groupLayout->addWidget(group1); + // groupLayout->addSpacing(5); + groupLayout->addWidget(segmented_button, 0, Qt::AlignCenter); + return groupLayout; +} - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; +QVBoxLayout * AutowareStatePanel::makeRoutingGroup() +{ + auto * group = new QVBoxLayout; - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + auto * custom_container = new CustomContainer(this); - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; + routing_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + clear_route_button_ptr_ = new CustomElevatedButton("Clear Route"); + clear_route_button_ptr_->setCheckable(true); + clear_route_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } + routing_label_ptr_ = new QLabel("Routing | Unknown"); + routing_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); - updateLabel(operation_mode_label_ptr_, text, style_sheet); + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); - // Control Mode - if (msg->is_autoware_control_enabled) { - updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green - } else { - updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow - } + horizontal_layout->addWidget(routing_icon); + horizontal_layout->addWidget(routing_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(clear_route_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} - // Button - changeButtonState( - auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); - changeButtonState( - stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); - changeButtonState( - local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); - changeButtonState( - remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); - - changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); - changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); +QVBoxLayout * AutowareStatePanel::makeLocalizationGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + init_by_gnss_button_ptr_ = new CustomElevatedButton("Initialize with GNSS"); + init_by_gnss_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + + localization_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + localization_label_ptr_ = new QLabel("Localization | Unknown"); + localization_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(localization_icon); + horizontal_layout->addWidget(localization_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(init_by_gnss_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + return group; +} + +QVBoxLayout * AutowareStatePanel::makeMotionGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + accept_start_button_ptr_ = new CustomElevatedButton("Accept Start"); + accept_start_button_ptr_->setCheckable(true); + accept_start_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); + + motion_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + motion_label_ptr_ = new QLabel("Motion | Unknown"); + motion_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + horizontal_layout->setAlignment(Qt::AlignLeft); + + horizontal_layout->addWidget(motion_icon); + horizontal_layout->addWidget(motion_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(accept_start_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} + +QVBoxLayout * AutowareStatePanel::makeFailSafeGroup() +{ + auto * group = new QVBoxLayout; + auto * v_layout = new QVBoxLayout; + auto * custom_container1 = new CustomContainer(this); + auto * custom_container2 = new CustomContainer(this); + + mrm_state_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + mrm_behavior_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + + mrm_state_label_ptr_ = new QLabel("MRM State | Unknown"); + mrm_behavior_label_ptr_ = new QLabel("MRM Behavior | Unknown"); + + // change text color + mrm_state_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + mrm_behavior_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(mrm_state_icon); + horizontal_layout->addWidget(mrm_state_label_ptr_); + + custom_container1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + auto * horizontal_layout2 = new QHBoxLayout; + horizontal_layout2->setSpacing(10); + horizontal_layout2->setContentsMargins(0, 0, 0, 0); + + horizontal_layout2->addWidget(mrm_behavior_icon); + horizontal_layout2->addWidget(mrm_behavior_label_ptr_); + + custom_container2->getLayout()->addLayout(horizontal_layout2, 0, 0, 1, 1, Qt::AlignLeft); + + v_layout->addWidget(custom_container1); + // v_layout->addSpacing(5); + v_layout->addWidget(custom_container2); + + group->setContentsMargins(10, 0, 0, 0); + + group->addLayout(v_layout); + return group; +} + +/* QVBoxLayout * AutowareStatePanel::makeDiagnosticGroup() +{ + auto * group = new QVBoxLayout; + + // Create the scroll area + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setFixedHeight(66); // Adjust the height as needed + scrollArea->setWidgetResizable(true); + scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded); + scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + + // Create a widget to contain the layout + QWidget * scrollAreaWidgetContents = new QWidget; + // use layout to contain the diagnostic label and the diagnostic level + diagnostic_layout_ = new QVBoxLayout(); + diagnostic_layout_->setSpacing(5); // Set space between items + diagnostic_layout_->setContentsMargins(5, 5, 5, 5); // Set margins within the layout + + // Add a QLabel to display the title of what this is + auto * tsm_label_title_ptr_ = new QLabel("Topic State Monitor: "); + // Set the layout on the widget + scrollAreaWidgetContents->setLayout(diagnostic_layout_); + + // Set the widget on the scroll area + scrollArea->setWidget(scrollAreaWidgetContents); + + group->addWidget(tsm_label_title_ptr_); + group->addWidget(scrollArea); + + return group; +} */ + +QVBoxLayout * AutowareStatePanel::makeVelocityLimitGroup() +{ + // Velocity Limit + velocity_limit_setter_ptr_ = new QLabel("Set Velocity Limit"); + // set its width to fit the text + velocity_limit_setter_ptr_->setFixedWidth( + velocity_limit_setter_ptr_->fontMetrics().horizontalAdvance("Set Velocity Limit")); + + velocity_limit_value_label_ = new QLabel("0"); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance("0")); + + CustomSlider * pub_velocity_limit_slider_ = new CustomSlider(Qt::Horizontal); + pub_velocity_limit_slider_->setRange(0, 100); + pub_velocity_limit_slider_->setValue(0); + pub_velocity_limit_slider_->setMaximumWidth(300); + + connect(pub_velocity_limit_slider_, &QSlider::sliderPressed, this, [this]() { + sliderIsDragging = true; // User starts dragging the handle + }); + + connect(pub_velocity_limit_slider_, &QSlider::sliderReleased, this, [this]() { + sliderIsDragging = false; // User finished dragging + onClickVelocityLimit(); // Call when handle is released after dragging + }); + + connect(pub_velocity_limit_slider_, &QSlider::valueChanged, this, [this](int value) { + this->velocity_limit_value_label_->setText(QString::number(value)); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance(QString::number(value))); + if (!sliderIsDragging) { // If the value changed without dragging, it's a click on the track + onClickVelocityLimit(); // Call the function immediately since it's not a drag operation + } + }); + + // Emergency Button + emergency_button_ptr_ = new CustomElevatedButton("Set Emergency"); + + emergency_button_ptr_->setCursor(Qt::PointingHandCursor); + // set fixed width to fit the text + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + auto * utility_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout; + QLabel * velocity_limit_label = new QLabel("km/h"); + + velocity_limit_layout->addWidget(pub_velocity_limit_slider_); + velocity_limit_layout->addSpacing(5); + velocity_limit_layout->addWidget(velocity_limit_value_label_); + velocity_limit_layout->addWidget(velocity_limit_label); + + // Velocity Limit layout + utility_layout->addSpacing(15); + utility_layout->addWidget(velocity_limit_setter_ptr_); + utility_layout->addSpacing(10); + utility_layout->addLayout(velocity_limit_layout); + utility_layout->addSpacing(25); + utility_layout->addWidget(emergency_button_ptr_); + + utility_layout->setContentsMargins(15, 0, 15, 0); + + return utility_layout; +} + +void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + auto updateButtonState = []( + CustomSegmentedButtonItem * button, bool is_available, + uint8_t current_mode, uint8_t desired_mode, bool disable) { + bool is_checked = (current_mode == desired_mode); + button->setHovered(false); + + button->setActivated(is_checked); + button->setChecked(is_checked); + button->setDisabledButton(disable || !is_available); + button->setCheckableButton(!disable && is_available && !is_checked); + }; + + bool disable_buttons = msg->is_in_transition; + + updateButtonState( + auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS, + disable_buttons); + updateButtonState( + stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP, + disable_buttons); + updateButtonState( + local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL, + disable_buttons); + updateButtonState( + remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE, + disable_buttons); + + // toggle switch for control mode + auto changeToggleSwitchState = [](CustomToggleSwitch * toggle_switch, const bool is_enabled) { + // Flick the switch without triggering its function + bool old_state = toggle_switch->blockSignals(true); + toggle_switch->setCheckedState(!is_enabled); + toggle_switch->blockSignals(old_state); + }; + + if (!msg->is_in_transition) { + // would cause an on/off/on flicker if in transition + changeToggleSwitchState(control_mode_switch_ptr_, !msg->is_autoware_control_enabled); + } } void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString route_state = "Routing | Unknown"; + switch (msg->state) { case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + route_state = "Routing | Unset"; break; case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + route_state = "Routing | Set"; break; case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + route_state = "Routing | Arrived"; break; case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + state = Pending; + route_state = "Routing | Changing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(routing_label_ptr_, text, style_sheet); + routing_icon->updateStyle(state, bgColor); + routing_label_ptr_->setText(route_state); if (msg->state == RouteState::SET) { activateButton(clear_route_button_ptr_); } else { + clear_route_button_ptr_->setStyleSheet( + QString("QPushButton {" + "background-color: %1;color: %2;" + "border: 2px solid %3;" + "font-weight: bold;" + "}") + .arg(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()) + .arg( + autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str())); deactivateButton(clear_route_button_ptr_); } } void AutowareStatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString localization_state = "Localization | Unknown"; + switch (msg->state) { case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + localization_state = "Localization | Uninitialized"; break; - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange + case LocalizationInitializationState::INITIALIZED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + localization_state = "Localization | Initialized"; break; - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green + case LocalizationInitializationState::INITIALIZING: + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + localization_state = "Localization | Initializing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(localization_label_ptr_, text, style_sheet); + localization_icon->updateStyle(state, bgColor); + localization_label_ptr_->setText(localization_state); } void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString motion_state = "Motion | Unknown"; + switch (msg->state) { case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + motion_state = "Motion | Starting"; break; - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange + case MotionState::MOVING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + motion_state = "Motion | Moving"; break; - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green + case MotionState::STOPPED: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + motion_state = "Motion | Stopped"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(motion_label_ptr_, text, style_sheet); + motion_icon->updateStyle(state, bgColor); + motion_label_ptr_->setText(motion_state); if (msg->state == MotionState::STARTING) { activateButton(accept_start_button_ptr_); @@ -458,94 +658,85 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) { - // state - { - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; + IconState state; + QColor bgColor; + QString mrm_state = "MRM State | Unknown"; - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; + switch (msg->state) { + case MRMState::NONE: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Inactive"; + break; - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + case MRMState::MRM_OPERATING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Operating"; + break; - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; + case MRMState::MRM_SUCCEEDED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_state = "MRM State | Successful"; + break; - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + case MRMState::MRM_FAILED: + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_state = "MRM State | Failed"; + break; - updateLabel(mrm_state_label_ptr_, text, style_sheet); + default: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Unknown"; + break; } + mrm_state_icon->updateStyle(state, bgColor); + mrm_state_label_ptr_->setText(mrm_state); + // behavior { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString mrm_behavior = "MRM Behavior | Unknown"; + switch (msg->behavior) { case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Inactive"; break; case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_behavior = "MRM Behavior | Pull Over"; break; case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + mrm_behavior = "MRM Behavior | Comfortable Stop"; break; case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_behavior = "MRM Behavior | Emergency Stop"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Unknown"; break; } - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void AutowareStatePanel::onShift( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; + mrm_behavior_icon->updateStyle(state, bgColor); + mrm_behavior_label_ptr_->setText(mrm_behavior); } } @@ -554,18 +745,37 @@ void AutowareStatePanel::onEmergencyStatus( { current_emergency_ = msg->emergency; if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + emergency_button_ptr_->updateStyle( + "Clear Emergency", + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str())); } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + emergency_button_ptr_->updateStyle( + "Set Emergency", QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_tint.c_str())); + } +} + +void AutowareStatePanel::onSwitchStateChanged(int state) +{ + if (state == 0) { + // call the control mode function + onClickDirectControl(); + } else if (state == 2) { + onClickAutowareControl(); } } void AutowareStatePanel::onClickVelocityLimit() { auto velocity_limit = std::make_shared(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + velocity_limit->max_velocity = velocity_limit_value_label_->text().toDouble() / 3.6; pub_velocity_limit_->publish(*velocity_limit); } diff --git a/common/tier4_state_rviz_plugin/src/custom_button.cpp b/common/tier4_state_rviz_plugin/src/custom_button.cpp new file mode 100644 index 0000000000000..0ef2628577135 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_button.cpp @@ -0,0 +1,129 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_button.hpp" + +#include "src/include/material_colors.hpp" + +CustomElevatedButton::CustomElevatedButton( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor, QWidget * parent) +: QPushButton(text, parent), + backgroundColor(bgColor), + textColor(textColor), + hoverColor(hoverColor), + disabledBgColor(disabledBgColor), + disabledTextColor(disabledTextColor) +{ + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + // set font weight to bold and size to 12 + QFont font = this->font(); + font.setWeight(QFont::Bold); + font.setFamily("Roboto"); + setFont(font); + + // Set up drop shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); + shadowEffect->setOffset(3, 3); + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); + setGraphicsEffect(shadowEffect); +} + +QSize CustomElevatedButton::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 45; // Adding padding + int height = textHeight + 25; // Adding padding + return QSize(width, height); +} + +QSize CustomElevatedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomElevatedButton::updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor) +{ + setText(text); + backgroundColor = bgColor; + this->textColor = textColor; + this->hoverColor = hoverColor; + this->disabledBgColor = disabledBgColor; + this->disabledTextColor = disabledTextColor; + update(); // Force repaint +} + +void CustomElevatedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + QStyleOption opt; + opt.initFrom(this); + QRect r = rect(); + + QColor buttonColor; + QColor currentTextColor = textColor; + if (!isEnabled()) { + buttonColor = disabledBgColor; + currentTextColor = disabledTextColor; + } else if (isHovered) { + buttonColor = hoverColor; + } else { + buttonColor = backgroundColor; + } + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw button background + QPainterPath backgroundPath; + backgroundPath.addRoundedRect(r, cornerRadius, cornerRadius); + if (!isEnabled()) { + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + painter.setOpacity(0.12); + } else { + painter.setBrush(buttonColor); + } + painter.setPen(Qt::NoPen); + painter.drawPath(backgroundPath); + + // Draw button text + if (!isEnabled()) { + painter.setOpacity(0.38); + } + painter.setPen(currentTextColor); + painter.drawText(r, Qt::AlignCenter, text()); +} + +void CustomElevatedButton::enterEvent(QEvent * event) +{ + isHovered = true; + update(); + QPushButton::enterEvent(event); +} + +void CustomElevatedButton::leaveEvent(QEvent * event) +{ + isHovered = false; + update(); + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_container.cpp b/common/tier4_state_rviz_plugin/src/custom_container.cpp new file mode 100644 index 0000000000000..0b0aa1ccd6ed5 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_container.cpp @@ -0,0 +1,70 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_container.hpp" + +#include "src/include/material_colors.hpp" + +CustomContainer::CustomContainer(QWidget * parent) : QFrame(parent), cornerRadius(15) +{ + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setContentsMargins(0, 0, 0, 0); + layout = new QGridLayout(this); + layout->setMargin(0); // Margin between the border and child widgets + layout->setSpacing(0); // Spacing between child widgets + layout->setContentsMargins(10, 0, 10, 0); // Margin between the border and the layout + setLayout(layout); +} + +void CustomContainer::setCornerRadius(int radius) +{ + cornerRadius = radius; + update(); +} + +int CustomContainer::getCornerRadius() const +{ + return cornerRadius; +} + +QGridLayout * CustomContainer::getLayout() const +{ + return layout; // Provide access to the layout +} + +QSize CustomContainer::sizeHint() const +{ + QSize size = layout->sizeHint(); + int width = size.width() + 20; // Adding padding + int height = size.height() + 20; // Adding padding + return QSize(width, height); +} + +QSize CustomContainer::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomContainer::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); // Use height for rounded corners + painter.setPen(Qt::NoPen); + painter.setBrush(QColor( + autoware::state_rviz_plugin::colors::default_colors.surface.c_str())); // Background color + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp new file mode 100644 index 0000000000000..6e4d32d40f7fb --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp @@ -0,0 +1,80 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_icon_label.hpp" + +#include + +CustomIconLabel::CustomIconLabel(const QColor & bgColor, QWidget * parent) +: QLabel(parent), backgroundColor(bgColor) +{ + loadIcons(); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + setAlignment(Qt::AlignCenter); + setFixedSize(35, 35); +} + +void CustomIconLabel::loadIcons() +{ + std::string packagePath = ament_index_cpp::get_package_share_directory("tier4_state_rviz_plugin"); + + iconMap[Active] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/active.png")); + iconMap[Pending] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/pending.png")); + iconMap[Danger] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/danger.png")); + iconMap[None] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/none.png")); + iconMap[Crash] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/crash.png")); + + icon = iconMap[None]; // Default icon +} + +void CustomIconLabel::updateStyle(IconState state, const QColor & bgColor) +{ + icon = iconMap[state]; + backgroundColor = bgColor; + update(); // Force repaint +} + +QSize CustomIconLabel::sizeHint() const +{ + int size = qMax(icon.width(), icon.height()) + 20; // Adding padding + return QSize(size, size); +} + +QSize CustomIconLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomIconLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int diameter = qMin(width(), height()); + int radius = diameter / 2; + + // Draw background circle + QPainterPath path; + path.addEllipse(width() / 2 - radius, height() / 2 - radius, diameter, diameter); + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + painter.drawPath(path); + + // Draw icon in the center + if (!icon.isNull()) { + QSize iconSize = icon.size().scaled(diameter * 0.6, diameter * 0.6, Qt::KeepAspectRatio); + QPoint iconPos((width() - iconSize.width()) / 2, (height() - iconSize.height()) / 2); + painter.drawPixmap( + iconPos, icon.scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + } +} diff --git a/common/tier4_state_rviz_plugin/src/custom_label.cpp b/common/tier4_state_rviz_plugin/src/custom_label.cpp new file mode 100644 index 0000000000000..1f96fc0d45095 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_label.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_label.hpp" + +#include "src/include/material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +CustomLabel::CustomLabel(const QString & text, QWidget * parent) : QLabel(text, parent) +{ + setFont(QFont("Roboto", 10, QFont::Bold)); // Set the font as needed + setAlignment(Qt::AlignCenter); + + // Add shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); // Blur radius for a smoother shadow + shadowEffect->setOffset(3, 3); // Offset for the shadow + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); // Shadow color + setGraphicsEffect(shadowEffect); +} + +QSize CustomLabel::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 40; // Adding padding + int height = textHeight; // Adding padding + return QSize(width, height); +} + +QSize CustomLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw background + QPainterPath path; + path.addRoundedRect(rect().adjusted(1, 1, -1, -1), cornerRadius, cornerRadius); + + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + + painter.drawPath(path); + + // Set text color and draw text + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomLabel::updateStyle( + const QString & text, const QColor & bg_color, const QColor & text_color) +{ + setText(text); + backgroundColor = bg_color; + textColor = text_color; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp new file mode 100644 index 0000000000000..8063bdc0fbc28 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -0,0 +1,75 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_segmented_button.hpp" + +#include + +CustomSegmentedButton::CustomSegmentedButton(QWidget * parent) +: QWidget(parent), buttonGroup(new QButtonGroup(this)), layout(new QHBoxLayout(this)) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + layout->setContentsMargins(0, 0, 0, 0); // Ensure no margins around the layout + layout->setSpacing(0); // Ensure no spacing between buttons + + setLayout(layout); + + buttonGroup->setExclusive(true); + + connect( + buttonGroup, QOverload::of(&QButtonGroup::idClicked), this, + &CustomSegmentedButton::buttonClicked); +} + +CustomSegmentedButtonItem * CustomSegmentedButton::addButton(const QString & text) +{ + CustomSegmentedButtonItem * button = new CustomSegmentedButtonItem(text); + layout->addWidget(button); + buttonGroup->addButton(button, layout->count() - 1); + + return button; +} + +QButtonGroup * CustomSegmentedButton::getButtonGroup() const +{ + return buttonGroup; +} + +QSize CustomSegmentedButton::sizeHint() const +{ + return QSize(400, 40); // Adjust the size hint as needed + + // return QSize( + // layout->count() * (layout->itemAt(0)->widget()->width()), + // layout->itemAt(0)->widget()->height() + 10); +} + +QSize CustomSegmentedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomSegmentedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); + + painter.setPen(Qt::NoPen); + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())); + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp new file mode 100644 index 0000000000000..f2d15260c4e41 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -0,0 +1,186 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_segmented_button_item.hpp" + +CustomSegmentedButtonItem::CustomSegmentedButtonItem(const QString & text, QWidget * parent) +: QPushButton(text, parent), + bgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())), + checkedBgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.secondary_container.c_str())), + inactiveTextColor(QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())), + activeTextColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())), + isHovered(false), + isActivated(false), + isDisabled(false) + +{ + setCheckable(true); + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setCursor(Qt::PointingHandCursor); +} + +void CustomSegmentedButtonItem::setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText) +{ + bgColor = bg; + checkedBgColor = checkedBg; + activeTextColor = activeText; + inactiveTextColor = inactiveText; + update(); +} + +// void CustomSegmentedButtonItem::updateSize() +// { +// QFontMetrics fm(font()); +// int width = fm.horizontalAdvance(text()) + 40; // Add padding +// int height = fm.height() + 20; // Add padding +// setFixedSize(width, height); +// } + +void CustomSegmentedButtonItem::setHovered(bool hovered) +{ + isHovered = hovered; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setCheckableButton(bool checkable) +{ + setCheckable(checkable); + updateCheckableState(); +} + +void CustomSegmentedButtonItem::updateCheckableState() +{ + setCursor( + isDisabled ? Qt::ForbiddenCursor + : (isCheckable() ? Qt::PointingHandCursor : Qt::ForbiddenCursor)); + update(); +} + +void CustomSegmentedButtonItem::setDisabledButton(bool disabled) +{ + isDisabled = disabled; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setActivated(bool activated) +{ + isActivated = activated; + update(); +} + +void CustomSegmentedButtonItem::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Adjust opacity for disabled state + if (isDisabled) { + painter.setOpacity(0.3); + } else { + painter.setOpacity(1.0); + } + + // Determine the button's color based on its state + QColor buttonColor; + if (isDisabled) { + buttonColor = disabledBgColor; + } else if (isHovered && !isChecked() && isCheckable()) { + buttonColor = hoverColor; + } else if (isActivated) { + buttonColor = checkedBgColor; + } else { + buttonColor = isChecked() ? checkedBgColor : bgColor; + } + + // Determine if this is the first or last button + bool isFirstButton = false; + bool isLastButton = false; + + QHBoxLayout * parentLayout = qobject_cast(parentWidget()->layout()); + if (parentLayout) { + int index = parentLayout->indexOf(this); + isFirstButton = (index == 0); + isLastButton = (index == parentLayout->count() - 1); + } + + // Draw button background + + QRect buttonRect = rect().adjusted(0, 1, 0, -1); + + if (isFirstButton) { + buttonRect.setLeft(buttonRect.left() + 1); + } + if (isLastButton) { + buttonRect.setRight(buttonRect.right() - 1); + } + + QPainterPath path; + double radius = (height() - 2) / 2; + + path.setFillRule(Qt::WindingFill); + if (isFirstButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), + buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom Right + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), buttonRect.top(), radius, + radius)); // Top Right + } else if (isLastButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left()), buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom left + path.addRect(QRect((buttonRect.left()), buttonRect.top(), radius, + radius)); // Top Left + } else { + path.addRect(buttonRect); + } + painter.fillPath(path, buttonColor); + + // Draw button border + QPen pen(QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()), 1); + pen.setJoinStyle(Qt::RoundJoin); + pen.setCapStyle(Qt::RoundCap); + painter.setPen(pen); + painter.drawPath(path.simplified()); + + // Draw button text + QColor textColor = (isChecked() ? activeTextColor : inactiveTextColor); + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomSegmentedButtonItem::enterEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = true; + update(); + } + QPushButton::enterEvent(event); +} + +void CustomSegmentedButtonItem::leaveEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = false; + update(); + } + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_slider.cpp b/common/tier4_state_rviz_plugin/src/custom_slider.cpp new file mode 100644 index 0000000000000..cf3f7c3d4638f --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_slider.cpp @@ -0,0 +1,102 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_slider.hpp" + +CustomSlider::CustomSlider(Qt::Orientation orientation, QWidget * parent) +: QSlider(orientation, parent) +{ + setMinimumHeight(40); // Ensure there's enough space for the custom track +} + +void CustomSlider::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(Qt::NoPen); + + // Initialize style option + QStyleOptionSlider opt; + initStyleOption(&opt); + + QRect grooveRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderGroove, this); + int centerY = grooveRect.center().y(); + QRect handleRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderHandle, this); + + int value = this->value(); + int minValue = this->minimum(); + int maxValue = this->maximum(); + + int trackThickness = 14; + int gap = 8; + + QRect beforeRect( + grooveRect.x(), centerY - trackThickness / 2, handleRect.center().x() - grooveRect.x() - gap, + trackThickness); + + QRect afterRect( + handleRect.center().x() + gap, centerY - trackThickness / 2, + grooveRect.right() - handleRect.center().x() - gap, trackThickness); + + QColor inactiveTrackColor( + autoware::state_rviz_plugin::colors::default_colors.primary_container.c_str()); + QColor activeTrackColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor handleColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + // only draw the active track if the value is more than the gap from the minimum + if (value > minValue + gap / 2) { + QPainterPath beforePath; + beforePath.moveTo(beforeRect.left(), centerY + trackThickness / 2); // Start from bottom-left + beforePath.quadTo( + beforeRect.left(), centerY - trackThickness / 2, beforeRect.left() + trackThickness * 0.5, + centerY - trackThickness / 2); + beforePath.lineTo(beforeRect.right() - trackThickness * 0.1, centerY - trackThickness / 2); + beforePath.quadTo( + beforeRect.right(), centerY - trackThickness / 2, beforeRect.right(), centerY); + beforePath.quadTo( + beforeRect.right(), centerY + trackThickness / 2, beforeRect.right() - trackThickness * 0.1, + centerY + trackThickness / 2); + beforePath.lineTo(beforeRect.left() + trackThickness * 0.5, centerY + trackThickness / 2); + beforePath.quadTo(beforeRect.left(), centerY + trackThickness / 2, beforeRect.left(), centerY); + painter.fillPath(beforePath, activeTrackColor); + } + + if (value < maxValue - gap / 2) { + QPainterPath afterPath; + afterPath.moveTo(afterRect.left(), centerY + trackThickness / 2); + afterPath.quadTo( + afterRect.left(), centerY - trackThickness / 2, afterRect.left() + trackThickness * 0.1, + centerY - trackThickness / 2); + afterPath.lineTo(afterRect.right() - trackThickness * 0.5, centerY - trackThickness / 2); + afterPath.quadTo(afterRect.right(), centerY - trackThickness / 2, afterRect.right(), centerY); + afterPath.quadTo( + afterRect.right(), centerY + trackThickness / 2, afterRect.right() - trackThickness * 0.5, + centerY + trackThickness / 2); + afterPath.lineTo(afterRect.left() + trackThickness * 0.1, centerY + trackThickness / 2); + afterPath.quadTo(afterRect.left(), centerY + trackThickness / 2, afterRect.left(), centerY); + painter.fillPath(afterPath, inactiveTrackColor); + } + + painter.setBrush(handleColor); + int handleLineHeight = 30; + int handleLineWidth = 4; + int handleLineRadius = 2; + QRect handleLineRect( + handleRect.center().x() - handleLineWidth / 2, centerY - handleLineHeight / 2, handleLineWidth, + handleLineHeight); + QPainterPath handlePath; + handlePath.addRoundedRect(handleLineRect, handleLineRadius, handleLineRadius); + painter.fillPath(handlePath, handleColor); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp new file mode 100644 index 0000000000000..3b58ded626439 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp @@ -0,0 +1,87 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "include/custom_toggle_switch.hpp" + +CustomToggleSwitch::CustomToggleSwitch(QWidget * parent) : QCheckBox(parent) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + connect(this, &QCheckBox::stateChanged, this, [this]() { + if (!blockSignalsGuard) { + update(); // Force repaint + } + }); +} + +QSize CustomToggleSwitch::sizeHint() const +{ + return QSize(50, 30); // Preferred size of the toggle switch +} + +void CustomToggleSwitch::paintEvent(QPaintEvent *) +{ + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + int margin = 2; + int circleRadius = height() / 2 - margin * 2; + QRect r = rect().adjusted(margin, margin, -margin, -margin); + bool isChecked = this->isChecked(); + + QColor uncheckedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()); + QColor checkedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()); + QColor indicatorColor = isChecked ? checkedIndicatorColor : uncheckedIndicatorColor; + + QColor uncheckedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor checkedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + QColor bgColor = isChecked ? checkedBgColor : uncheckedBgColor; + + QRect borderR = r.adjusted(-margin, -margin, margin, margin); + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(borderR, circleRadius + 4, circleRadius + 4); + + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(r, circleRadius + 4, circleRadius + 4); + + int minX = r.left() + margin * 2; + int maxX = r.right() - circleRadius * 2 - margin; + int circleX = isChecked ? maxX : minX; + QRect circleRect(circleX, r.top() + margin, circleRadius * 2, circleRadius * 2); + p.setBrush(indicatorColor); + p.drawEllipse(circleRect); +} + +void CustomToggleSwitch::mouseReleaseEvent(QMouseEvent * event) +{ + if (event->button() == Qt::LeftButton) { + setCheckedState(!isChecked()); + } + QCheckBox::mouseReleaseEvent(event); +} + +void CustomToggleSwitch::setCheckedState(bool state) +{ + blockSignalsGuard = true; + setChecked(state); + blockSignalsGuard = false; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp similarity index 72% rename from common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 8f67a90215bd1..9863cbbbf802b 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -17,12 +17,26 @@ #ifndef AUTOWARE_STATE_PANEL_HPP_ #define AUTOWARE_STATE_PANEL_HPP_ -#include +#include "custom_button.hpp" +#include "custom_container.hpp" +#include "custom_icon_label.hpp" +#include "custom_label.hpp" +#include "custom_segmented_button.hpp" +#include "custom_segmented_button_item.hpp" +#include "custom_slider.hpp" +#include "custom_toggle_switch.hpp" +#include "material_colors.hpp" + +#include +#include +#include #include -#include #include +#include #include -#include +#include +#include +#include #include #include @@ -36,11 +50,17 @@ #include #include #include +#include +#include #include #include #include +#include + #include +#include +#include namespace rviz_plugins { @@ -56,6 +76,8 @@ class AutowareStatePanel : public rviz_common::Panel using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; Q_OBJECT @@ -75,17 +97,19 @@ public Q_SLOTS: // NOLINT for Qt void onClickAcceptStart(); void onClickVelocityLimit(); void onClickEmergencyButton(); + void onSwitchStateChanged(int state); protected: // Layout - QGroupBox * makeOperationModeGroup(); - QGroupBox * makeControlModeGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeLocalizationGroup(); - QGroupBox * makeMotionGroup(); - QGroupBox * makeFailSafeGroup(); - - void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + QVBoxLayout * makeVelocityLimitGroup(); + QVBoxLayout * makeOperationModeGroup(); + QVBoxLayout * makeRoutingGroup(); + QVBoxLayout * makeLocalizationGroup(); + QVBoxLayout * makeMotionGroup(); + QVBoxLayout * makeFailSafeGroup(); + // QVBoxLayout * makeDiagnosticGroup(); + + // void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; @@ -97,13 +121,15 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Publisher::SharedPtr pub_velocity_limit_; + QLabel * velocity_limit_value_label_{nullptr}; + bool sliderIsDragging = false; + // Operation Mode - //// Gate Mode QLabel * operation_mode_label_ptr_{nullptr}; - QPushButton * stop_button_ptr_{nullptr}; - QPushButton * auto_button_ptr_{nullptr}; - QPushButton * local_button_ptr_{nullptr}; - QPushButton * remote_button_ptr_{nullptr}; + CustomSegmentedButtonItem * stop_button_ptr_{nullptr}; + CustomSegmentedButtonItem * auto_button_ptr_{nullptr}; + CustomSegmentedButtonItem * local_button_ptr_{nullptr}; + CustomSegmentedButtonItem * remote_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_operation_mode_; rclcpp::Client::SharedPtr client_change_to_autonomous_; @@ -112,6 +138,8 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Client::SharedPtr client_change_to_remote_; //// Control Mode + CustomSegmentedButton * segmented_button; + CustomToggleSwitch * control_mode_switch_ptr_{nullptr}; QLabel * control_mode_label_ptr_{nullptr}; QPushButton * enable_button_ptr_{nullptr}; QPushButton * disable_button_ptr_{nullptr}; @@ -123,8 +151,9 @@ public Q_SLOTS: // NOLINT for Qt void changeOperationMode(const rclcpp::Client::SharedPtr client); // Routing + CustomIconLabel * routing_icon{nullptr}; + CustomElevatedButton * clear_route_button_ptr_{nullptr}; QLabel * routing_label_ptr_{nullptr}; - QPushButton * clear_route_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Client::SharedPtr client_clear_route_; @@ -132,8 +161,9 @@ public Q_SLOTS: // NOLINT for Qt void onRoute(const RouteState::ConstSharedPtr msg); // Localization + CustomIconLabel * localization_icon{nullptr}; + CustomElevatedButton * init_by_gnss_button_ptr_{nullptr}; QLabel * localization_label_ptr_{nullptr}; - QPushButton * init_by_gnss_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_localization_; rclcpp::Client::SharedPtr client_init_by_gnss_; @@ -141,8 +171,9 @@ public Q_SLOTS: // NOLINT for Qt void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); // Motion + CustomIconLabel * motion_icon{nullptr}; + CustomElevatedButton * accept_start_button_ptr_{nullptr}; QLabel * motion_label_ptr_{nullptr}; - QPushButton * accept_start_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_motion_; rclcpp::Client::SharedPtr client_accept_start_; @@ -150,7 +181,9 @@ public Q_SLOTS: // NOLINT for Qt void onMotion(const MotionState::ConstSharedPtr msg); // FailSafe + CustomIconLabel * mrm_state_icon{nullptr}; QLabel * mrm_state_label_ptr_{nullptr}; + CustomIconLabel * mrm_behavior_icon{nullptr}; QLabel * mrm_behavior_label_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_mrm_; @@ -158,11 +191,11 @@ public Q_SLOTS: // NOLINT for Qt void onMRMState(const MRMState::ConstSharedPtr msg); // Others - QPushButton * velocity_limit_button_ptr_; + QLabel * velocity_limit_setter_ptr_; QLabel * gear_label_ptr_; QSpinBox * pub_velocity_limit_input_; - QPushButton * emergency_button_ptr_; + CustomElevatedButton * emergency_button_ptr_; bool current_emergency_{false}; @@ -190,6 +223,17 @@ public Q_SLOTS: // NOLINT for Qt label->setText(text); label->setStyleSheet(style_sheet); } + static void updateCustomLabel( + CustomLabel * label, QString text, QColor bg_color, QColor text_color) + { + label->updateStyle(text, bg_color, text_color); + } + + static void updateButton(QPushButton * button, QString text, QString style_sheet) + { + button->setText(text); + button->setStyleSheet(style_sheet); + } static void activateButton(QAbstractButton * button) { diff --git a/common/tier4_state_rviz_plugin/src/include/custom_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp new file mode 100644 index 0000000000000..b10663ce09933 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_BUTTON_HPP_ +#define CUSTOM_BUTTON_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class CustomElevatedButton : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomElevatedButton( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + const QColor & hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()), + const QColor & disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + const QColor & disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()), + QWidget * parent = nullptr); + void updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, + const QColor & hoverColor, const QColor & disabledBgColor, const QColor & disabledTextColor); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.disabled_elevated_button_bg.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); + bool isHovered = false; +}; + +#endif // CUSTOM_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_container.hpp b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp new file mode 100644 index 0000000000000..5142b409ebc88 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_CONTAINER_HPP_ +#define CUSTOM_CONTAINER_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include + +class CustomContainer : public QFrame +{ + Q_OBJECT + +public: + explicit CustomContainer(QWidget * parent = nullptr); + + // Methods to set dimensions and corner radius + void setContainerHeight(int height); + void setContainerWidth(int width); + void setCornerRadius(int radius); + + // Getters + int getContainerHeight() const; + int getContainerWidth() const; + int getCornerRadius() const; + QGridLayout * getLayout() const; // Add a method to access the layout + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QGridLayout * layout; + int cornerRadius; +}; + +#endif // CUSTOM_CONTAINER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp new file mode 100644 index 0000000000000..1b3ab9c8e0c6c --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_ICON_LABEL_HPP_ +#define CUSTOM_ICON_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +enum IconState { Active, Pending, Danger, None, Crash }; + +class CustomIconLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomIconLabel( + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + QWidget * parent = nullptr); + void updateStyle(IconState state, const QColor & bgColor); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + void loadIcons(); + QPixmap icon; + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()); + QMap iconMap; +}; + +#endif // CUSTOM_ICON_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp new file mode 100644 index 0000000000000..a328c4de56e3d --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_LABEL_HPP_ +#define CUSTOM_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +class CustomLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomLabel(const QString & text, QWidget * parent = nullptr); + void updateStyle( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); +}; + +#endif // CUSTOM_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp new file mode 100644 index 0000000000000..c6c589d31b8d4 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_SEGMENTED_BUTTON_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_HPP_ + +#include "custom_segmented_button_item.hpp" +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButton : public QWidget +{ + Q_OBJECT + +public: + explicit CustomSegmentedButton(QWidget * parent = nullptr); + + CustomSegmentedButtonItem * addButton(const QString & text); + QButtonGroup * getButtonGroup() const; + + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +Q_SIGNALS: + void buttonClicked(int id); + +protected: + void paintEvent(QPaintEvent * event) override; + +private: + QButtonGroup * buttonGroup; + QHBoxLayout * layout; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp new file mode 100644 index 0000000000000..33eb9fe16aa31 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp @@ -0,0 +1,64 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButtonItem : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomSegmentedButtonItem(const QString & text, QWidget * parent = nullptr); + + void setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText); + void setActivated(bool activated); + void setCheckableButton(bool checkable); + void setDisabledButton(bool disabled); + void setHovered(bool hovered); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + +private: + void updateCheckableState(); + + QColor bgColor; + QColor checkedBgColor; + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor inactiveTextColor; + QColor activeTextColor; + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_dim.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()); + bool isHovered = false; + bool isActivated = false; + bool isDisabled = false; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp new file mode 100644 index 0000000000000..f0dc9f12aedfe --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp @@ -0,0 +1,35 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_SLIDER_HPP_ +#define CUSTOM_SLIDER_HPP_ +#include "material_colors.hpp" + +#include +#include +#include +#include +#include + +class CustomSlider : public QSlider +{ + Q_OBJECT + +public: + explicit CustomSlider(Qt::Orientation orientation, QWidget * parent = nullptr); + +protected: + void paintEvent(QPaintEvent * event) override; +}; + +#endif // CUSTOM_SLIDER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp new file mode 100644 index 0000000000000..107d45af8c3f3 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CUSTOM_TOGGLE_SWITCH_HPP_ +#define CUSTOM_TOGGLE_SWITCH_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +class CustomToggleSwitch : public QCheckBox +{ + Q_OBJECT + +public: + explicit CustomToggleSwitch(QWidget * parent = nullptr); + QSize sizeHint() const override; + void setCheckedState(bool state); + +protected: + void paintEvent(QPaintEvent * event) override; + void mouseReleaseEvent(QMouseEvent * event) override; + +private: + bool blockSignalsGuard = false; // Guard variable to block signals during updates +}; + +#endif // CUSTOM_TOGGLE_SWITCH_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/material_colors.hpp b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp new file mode 100644 index 0000000000000..d146b599ab600 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MATERIAL_COLORS_HPP_ +#define MATERIAL_COLORS_HPP_ +#include + +namespace autoware +{ +namespace state_rviz_plugin +{ +namespace colors +{ +struct MaterialColors +{ + std::string primary = "#8BD0F0"; + std::string surface_tint = "#8BD0F0"; + std::string on_primary = "#003546"; + std::string primary_container = "#004D64"; + std::string on_primary_container = "#BEE9FF"; + std::string secondary = "#B4CAD6"; + std::string on_secondary = "#1F333C"; + std::string secondary_container = "#354A54"; + std::string on_secondary_container = "#D0E6F2"; + std::string tertiary = "#C6C2EA"; + std::string on_tertiary = "#2F2D4D"; + std::string tertiary_container = "#454364"; + std::string on_tertiary_container = "#E3DFFF"; + std::string error = "#FFB4AB"; + std::string on_error = "#690005"; + std::string error_container = "#93000A"; + std::string on_error_container = "#FFDAD6"; + std::string background = "#0F1417"; + std::string on_background = "#DFE3E7"; + std::string surface = "#0F1417"; + std::string on_surface = "#DFE3E7"; + std::string surface_variant = "#40484C"; + std::string on_surface_variant = "#C0C8CD"; + std::string outline = "#8A9297"; + std::string outline_variant = "#40484C"; + std::string shadow = "#000000"; + std::string scrim = "#000000"; + std::string inverse_surface = "#DFE3E7"; + std::string inverse_on_surface = "#2C3134"; + std::string inverse_primary = "#126682"; + std::string primary_fixed = "#BEE9FF"; + std::string on_primary_fixed = "#001F2A"; + std::string primary_fixed_dim = "#8BD0F0"; + std::string on_primary_fixed_variant = "#004D64"; + std::string secondary_fixed = "#D0E6F2"; + std::string on_secondary_fixed = "#081E27"; + std::string secondary_fixed_dim = "#B4CAD6"; + std::string on_secondary_fixed_variant = "#354A54"; + std::string tertiary_fixed = "#E3DFFF"; + std::string on_tertiary_fixed = "#1A1836"; + std::string tertiary_fixed_dim = "#C6C2EA"; + std::string on_tertiary_fixed_variant = "#454364"; + std::string surface_dim = "#0F1417"; + std::string surface_bright = "#353A3D"; + std::string surface_container_lowest = "#0A0F11"; + std::string surface_container_low = "#171C1F"; + std::string surface_container = "#1B2023"; + std::string surface_container_high = "#262B2E"; + std::string surface_container_highest = "#303538"; + std::string disabled_elevated_button_bg = "#292D30"; + std::string success = "#8DF08B"; + std::string warning = "#EEF08B"; + std::string info = "#8BD0F0"; + std::string danger = "#F08B8B"; +}; + +inline MaterialColors default_colors; +} // namespace colors +} // namespace state_rviz_plugin +} // namespace autoware + +#endif // MATERIAL_COLORS_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp similarity index 100% rename from common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index 6ea142ed66a1b..90ad18fe5af6c 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -14,7 +14,7 @@ // limitations under the License. // -#include "velocity_steering_factors_panel.hpp" +#include "include/velocity_steering_factors_panel.hpp" #include #include