From 6029eaef5cfc4aa0adaae1770e045824087730c1 Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Mon, 1 Jul 2024 19:49:25 +0300 Subject: [PATCH] feat: add handle angle scale property to signal display (#7774) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: add handle angle scale property to signal display * fix: set default steering angle to 0.0° when not received Signed-off-by: KhalilSelyan * fix: set default steering angle to N/A when not received and check for msg_ptr instead of float value Signed-off-by: KhalilSelyan * chore: update steering wheel font size and wheel icon center is bigger Signed-off-by: KhalilSelyan * chore: update steering wheel display to center the steering angle text Signed-off-by: KhalilSelyan * chore: align steering angle text in both negative and positive cases Signed-off-by: KhalilSelyan --------- Signed-off-by: KhalilSelyan --- .../assets/images/wheel.png | Bin 1632 -> 5802 bytes .../include/signal_display.hpp | 1 + .../include/steering_wheel_display.hpp | 4 +- .../src/signal_display.cpp | 6 ++- .../src/steering_wheel_display.cpp | 50 +++++++++++++----- 5 files changed, 45 insertions(+), 16 deletions(-) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png index 07b1e33132e430476307b9d5f57d35b9da9f0097..04ea298a46829e25db6dc2394a8870308debc37f 100644 GIT binary patch literal 5802 zcmeHKX;f3!7ET-(!XPMERFpI*ijw&zAu)hRPy$4m1r_xsxq%=VNCE^Ei-?E_4p3+n zNB>aM@(9KQBl_OJmNmK65$V-WFRr)3^(Uc|Gf~-cQsp+=vsZho1hWaA8`0Ys5 z!}2K1tl*1q;VFf1fdzBC89a#y2l6Fc2&WWD;mU&x(M2f*c^e@GnhS*q#ZH)^ z`qLP+kne<9Md1)QQa31E=&?x#`EObgz}vKuN9SW)oKZ|A0|pR53J|RnMT+GNr4vSn z%YesPF&=}~nJ6|oVS+h6Xg7%rLIXGeN5Hx(g)tTaGVgvA#tfZ8l4V1 z3Wy*UpwR?akj5uqX&^x3lLdSljQk12N|_L@N-*-%sI*Xg7>YvV5x7Kvi>31@R2Ygt z#nQQCGL}aH=n#=W1c+3M4vNoXER)DYAiSJH5f}#HrQ$H1LQ6Qq)rakbA>jxgB|ed$ zLI68BVZ4RnXyr#mfKUYaD?lxqL>iIq2sk=Y2^0Vz6Nw*NMJA zVZh12#Ddy7g#mPOI2VSS3<4DrS%5?m>4edSgw}d~P;=nz#0M213sgWbD1ihp2s8$P z7(jGn05k?bSVSN*2%p$X_(DP4|FYI@9yD`&$vuQ}c>Xw@XnaTcLs8>zfE)ghVK^f#42J;AW1(&CeEA#^0LfL;FOKdo#T?LphEP;mo zPr~r;62@zL#`}z!`2XO9sWbSb$-sW&GPrrcy%7JQ8NTBTj`yGZyjzQZatbv1>m*;q z?^n9M()C3Qe39|j>iSC87cuZf#$T)J|3(+;<8cZS!~cR};lt9o?kBt8gO(oGYdH(? zR{PCARg?)!45c0+as4(3|HLVX3BkpJjM3Cv9=}v$U3YDW2Ev#Jc-i^8Ve+y1(Nw)pg$)3nit&5O?@c^mjf zS!?vAc_~}$Re04=#IPPNJ*C(PJ>d${>}=GX?%de4X%JM;K8&Ec}`1`U{D5UxD;ls@&wQ#L_XnUt${dbyry;)e8;r`}7i zS`T{K(@Jj)j07a?Fw5(8tnWSFcdMLzJ+=4M?aGKLlkbRYH~L*Fau3Y%TgJMc;aAfJ z#Mf6=4hx0Cez&)jojZ5lvt#$L?c*`ek=KDmbr%oKWp9N8^wc+Psx;kDn{pW$KH*ka z+GN{ulSWZT`k~=$=mvi6iQ!_P3vECRXb>b$y&7Y!Sqz9}VbHb^q8w$?f zf`?nJk{=mA?cDDtp4qrQ%DQO(kxIXpr@#a|-s6MFk@<*P$+WBjTIJhkxPP2=*0GCA}hq87O}@64=z zQbGsg<-^?NtSV7(!_6eKtZC{9=q4^8$~x!QA9AJz1r_~^v;ERyyR6XmP-mn!>vCb$ z)%>TH2%4ah+)(h;saPhtcF4bOt8uV_FL9Dn#$2K*J$;6F=d9#9XEI{cskR2OB=DPR zb}}wHfPJpVc-uEOtn~uL2ky*FxHr@m*rdibJz7QFOm21^yjdf{znqwJi87je=f_qg zVU>JFRc-R%sjXe9-`Y8cJ2xi1KIV0hp40H$<%ev)zY4E6?|a_78#{3KVhAeJ>=3tO zWPa`huZ;Ugcl5Vsg$ay|jKUmpNuxHe07UFcqsKqs*E?r=+Bx@6ub9hjG0pZkLl~Q; zu8^Op)L4yjGOgL2=hBD+b&B??k{J=4Ke_WCVv>eUHviRfns{f_?fB&@s$BddgI6lM zun}Vi-?xWU98DYWf+Buqy58%^acV&+xF)+gw^VAaDNNdYZuoUC9kDgi?1;eU#ElB6 z#!QoEcBs(zb$1%?bym4gMbD~7ZJl?omfcu2=a1Ig%OfJglGV32*Ozx(=y1Ion!h_`&dMyk0_APvM>2GyA=5^KMx|8#P-GQBz=BZY^ z)~X}W3Q_yD;xTh|C<0%I7@e?wrH%8zbxQOCW76!2EhSG%Y&}9pp3hX587_Kbt;SyS z^*Xt- z>zb)`6s%=YyA1bd8z8Fb5M6~k60~j3oH>8#>+9FUE*A&yRy&lg<8unYJlQhM?}oH_ zheVfuO;o8=E7lvIZa-?MS!ZKAmX>d{K^1Ph$fSRDT<;6`zuC% zbxRMeauv_*a_+mctkpi)pHgKS7_A~s&p0{ZWufs*`zvpJ8<`_}nbsPiMSmFXH_v_z z?nxMPPp?C%)r*JbRRLYwB1=ko>Zy8LD(+vmei1gwf8tGx8(TVRS{J=a{l4wm0xM+7 lFW=ABM6KTLGRgkh-$)GCM-cC)*8U11*zPM>J0z@ delta 1615 zcmV-V2C(_6E#M50B!3BTNLh0L01FcU01FcV0GgZ_00001b5ch_0Itp)=>Px#1ZP1_ zK>z@;j|==^1pojA^+`lQRCodHTiaFRNDwWX$M3QctR{kCzRkl#02=|G2!IG+B7lhi zZ3Hk8U<3YMCW6&OFy3FDd#`ADZMh#Mx#i*P>^(;aOWp0hU4K>G)z#&eG=!_GtF`6j z<#qh~zSU~2zr4KgTEpk0R5Zi;5%0L}a19oI4fJ6fr!GBynZEtU%NexscO!bAGp3iHe^MW>x$?aoRX;&hAb92)L(F;*Z6=gXC z_i^t-?Z2+Bu72-yI^QPC42Q$to}Qi(&f=WS@u(}aGr0>H>i!PWcgpg5!q?Z=J>1R} zMY)`$xGuN0wgyrI(4zIFrKJN=l80^gHa0f?5Cv3(VSleD#$J6Gq0AZ7>~pL&)R@mc zq~DVOabwpf5Xo=vsq%yo1Im8wxU{Y9s3dSGjunkAlq^9a{BC*my=};sO;>#@ zVcM<7dw=qd;35zrO&@{kZ+kbR(gBqj85_yu4ZzGX?5oW$U|-*rSBx16`&5||)LS7- z(4@E=IbL(;c2IseC1E4ozZ?yua3ns;8w5PS3lQJ4yqb~nl!T3R#Zl2Ul0O`9c8=uL z!z*J(%2N`?WJT#F@_#X;d}4VuCFORhbBV12MzC@R>X0R{ zI;6L^xBC^VQOb0FtWt*N?iL@$gdxRUM_ZRwC#t$+3)CTPB$(RX8DBg{PMDDkhtbnj ztK>G$OSV8A(&+0K+jDL`$&vr(giUnOhOCM=1+pBpxycqFX>_!!EswtT#i7=?a*l;c zZ+|=5=!+6`0wf~Y0$3(5Oxs$dj&^dBEeLlVl`lnsfX6d0*4TCh(qA3V6aFa(TRRGy z_$C1~OtyfUH2U0ky!uzAY1Fn7A^Tkr*#auko)snGpB85?H1u750zmq|Qo@UlEB`Pc zYWhoCelq+|_`ePkZsZ*HE#m(E{_lKy2!EH5v-Hg5#aYc4R1X9U8bK%wOMuUd?KuJ% ztwN=(q&TaI2WHJ%q=sY~-@x`9yTQC(d-6jW$`is=7KeVQ05mSKJ?Ft5?`>a0*sW+> zxi7LHXu?d0m{|AqQxZnBz~yl7NdRU`mP+$THGv2SscU=ABU|1J36s6e78A{W)PIb8 zleU~`q-*RqvAh};N4Gg)GZS04g2g`42Xhk8SO`~pGRedm$Ez{7_C%wuKwYm0kNAKN zT{5~Ukzs=W!F@TGNT{>R1ZN5R!Xvf_z6}iJ!d8wX5ksRNoo9&b0L~ zuskd3v2K;%q36UT1E;BFbLRBW6Tw&KQIr8!ZyO`c44^}qRl7Vlgv&;qBp6@*iS5M1 zc3iq<(i{?7BXy5L2**)Qt_@NHfs_nuyEEHEE>XTE1~cJV#jt53FdLT(*MEDLmRk42 zoUu{CCB>>?)96s-d89BsYyX`A07#PdmN{PMYVS%bXcEi@^qHKz&hXBq(Mi~-HuET) zE+bQjV3(&F$0P=BhIl5DM|?Qsh}Svd)Pm*%$zCaPw1UYt!I+j= z4?})3-MW;WqRL4m42Q#Y41eS~2Q;)d=jx$ZyIZ0}#Zv}m(Vn3bgHES2?u2Gps_`=d zs_&k54ObI3oe9m3He_b=edlpJ=sc{Oy=)?SRL&WzM3~SfdM*H|*)~?%hgT+C0K|KS zcp{5MW{3v2j5gFJOc;AR(Bv~mK$Lv!xd<09iQK!!MiYsPsEOxM2uM$YJ0p3u@2=D* zUxb{}x<4auAv)nn7=N3~*?V~<&upQjRE!eLaL>3OhNkq(KLm-1e*h6LZ>-G8vcCWT N002ovPDHLkV1hC@{ipx{ diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index 0831ded468c99..5f5c9cee2da43 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -76,6 +76,7 @@ private Q_SLOTS: rviz_common::properties::IntProperty * property_left_; rviz_common::properties::IntProperty * property_top_; rviz_common::properties::ColorProperty * property_signal_color_; + rviz_common::properties::FloatProperty * property_handle_angle_scale_; std::unique_ptr steering_topic_property_; std::unique_ptr gear_topic_property_; std::unique_ptr speed_topic_property_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index dada3cddab911..0e42368f2e017 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -36,7 +36,8 @@ class SteeringWheelDisplay { public: SteeringWheelDisplay(); - void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); + void drawSteeringWheel( + QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_); void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); private: @@ -46,6 +47,7 @@ class SteeringWheelDisplay QImage wheelImage; QImage scaledWheelImage; QImage coloredImage(const QImage & source, const QColor & color); + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index ccfdaa69f5d2a..1fd33a95fb34d 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -49,6 +49,9 @@ SignalDisplay::SignalDisplay() property_signal_color_ = new rviz_common::properties::ColorProperty( "Signal Color", QColor(QString("#00E678")), "Color of the signal arrows", this, SLOT(updateOverlayColor())); + property_handle_angle_scale_ = new rviz_common::properties::FloatProperty( + "Handle Angle Scale", 17.0, "Scale of the steering wheel handle angle", this, + SLOT(updateOverlaySize())); // Initialize the component displays steering_wheel_display_ = std::make_unique(); @@ -285,7 +288,8 @@ void SignalDisplay::drawWidget(QImage & hud) } if (steering_wheel_display_) { - steering_wheel_display_->drawSteeringWheel(painter, backgroundRect); + steering_wheel_display_->drawSteeringWheel( + painter, backgroundRect, property_handle_angle_scale_->getFloat()); } if (speed_display_) { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index d2390b16e5373..28bd2b5c2a52e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -59,19 +59,17 @@ void SteeringWheelDisplay::updateSteeringData( const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { try { - // Assuming msg->steering_angle is the field you're interested in - float steeringAngle = msg->steering_tire_angle; - // we received it as a radian value, but we want to display it in degrees - steering_angle_ = - (steeringAngle * -180 / M_PI) * - 17; // 17 is the ratio between the steering wheel and the steering tire angle i assume + last_msg_ptr_ = msg; + + steering_angle_ = msg->steering_tire_angle; } catch (const std::exception & e) { // Log the error std::cerr << "Error in processMessage: " << e.what() << std::endl; } } -void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect) +void SteeringWheelDisplay::drawSteeringWheel( + QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_) { // Enable Antialiasing for smoother drawing painter.setRenderHint(QPainter::Antialiasing, true); @@ -80,7 +78,7 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & QImage wheel = coloredImage(scaledWheelImage, gray); // Rotate the wheel - float steeringAngle = steering_angle_; // No need to round here + float steeringAngle = std::round(handle_angle_scale_ * (steering_angle_ / M_PI) * -180.0); // Calculate the position int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54; int wheelCenterY = backgroundRect.height() / 2; @@ -99,16 +97,40 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & // Draw the rotated image painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); - QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0); + std::ostringstream steering_angle_ss; + if (last_msg_ptr_) { + steering_angle_ss << std::fixed << std::setprecision(1) << steering_angle_ * 180.0 / M_PI + << "°"; + } else { + steering_angle_ss << "N/A"; + } + + QString steeringAngleString = QString::fromStdString(steering_angle_ss.str()); + // if the string doesn't have a negative sign, add a space to the front of the string + // to align the text in both cases (negative and positive) + if (steeringAngleString[0] != '-') { + steeringAngleString = " " + steeringAngleString; + } // Draw the steering angle text - QFont steeringFont("Quicksand", 9, QFont::Bold); + QFont steeringFont("Quicksand", 8, QFont::Bold); painter.setFont(steeringFont); painter.setPen(QColor(0, 0, 0, 255)); - QRect steeringRect( - wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2, - wheelImage.width(), wheelImage.height()); - painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°"); + // QRect steeringRect( + // wheelCenterX - wheelImage.width() / 2 + 1, wheelCenterY - wheelImage.height() / 2, + // wheelImage.width(), wheelImage.height()); + // painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString); + + // Measure the text + QFontMetrics fontMetrics(steeringFont); + QRect textRect = fontMetrics.boundingRect(steeringAngleString); + + // Center the text + int textX = wheelCenterX - textRect.width() / 2; + int textY = wheelCenterY - textRect.height() / 2; + + QRect steeringRect(textX, textY, textRect.width(), textRect.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString); } QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color)