From 9b0ba2d1216e33a04b9d27ef4d3cad526ac58b6a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 18 Nov 2024 17:47:29 +0900 Subject: [PATCH] fix(factors_panel): sort by distance to stop/decel or point where it starts moving the steering (#9346) Signed-off-by: satoshi-ota --- .../src/velocity_steering_factors_panel.cpp | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) 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 90ad18fe5af6c..b7ea26e2c6895 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 @@ -104,8 +104,15 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: velocity_factors_table_->clearContents(); velocity_factors_table_->setRowCount(msg->factors.size()); - for (std::size_t i = 0; i < msg->factors.size(); i++) { - const auto & e = msg->factors.at(i); + auto sorted = *msg; + + // sort by distance to the decel/stop point. + std::sort(sorted.factors.begin(), sorted.factors.end(), [](const auto & a, const auto & b) { + return a.distance < b.distance; + }); + + for (std::size_t i = 0; i < sorted.factors.size(); i++) { + const auto & e = sorted.factors.at(i); // behavior { @@ -157,8 +164,15 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: steering_factors_table_->clearContents(); steering_factors_table_->setRowCount(msg->factors.size()); - for (std::size_t i = 0; i < msg->factors.size(); i++) { - const auto & e = msg->factors.at(i); + auto sorted = *msg; + + // sort by distance to the point where it starts moving the steering. + std::sort(sorted.factors.begin(), sorted.factors.end(), [](const auto & a, const auto & b) { + return a.distance.front() < b.distance.front(); + }); + + for (std::size_t i = 0; i < sorted.factors.size(); i++) { + const auto & e = sorted.factors.at(i); // behavior {