From 6dd150d7627c722c062736848e5dfa84a30d60cf Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 26 Nov 2024 12:01:51 +0900 Subject: [PATCH 1/4] add abrupt deceleration checking feature to velocity steering factors panel Signed-off-by: mitukou1109 --- common/tier4_state_rviz_plugin/package.xml | 1 + .../velocity_steering_factors_panel.hpp | 22 +++++ .../src/velocity_steering_factors_panel.cpp | 92 ++++++++++++++++++- 3 files changed, 113 insertions(+), 2 deletions(-) diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index aa7f5fe41c295..14ad8a22a52e2 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -14,6 +14,7 @@ ament_index_cpp autoware_adapi_v1_msgs + autoware_motion_utils autoware_vehicle_msgs libqt5-core libqt5-gui diff --git a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp index 70f35ed3793ab..d0391e8a8a697 100644 --- a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include @@ -49,6 +51,14 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel void onInitialize() override; protected: + static constexpr double MAX_JERK_MIN = 0.0; + static constexpr double MAX_JERK_DEFAULT = 1.0; + static constexpr double MAX_JERK_MAX = 2.0; + + static constexpr double MAX_DECELERATION_MIN = 0.0; + static constexpr double MAX_DECELERATION_DEFAULT = 1.0; + static constexpr double MAX_DECELERATION_MAX = 2.0; + // Layout QGroupBox * makeVelocityFactorsGroup(); QGroupBox * makeSteeringFactorsGroup(); @@ -58,12 +68,24 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel // Planning QTableWidget * velocity_factors_table_{nullptr}; QTableWidget * steering_factors_table_{nullptr}; + QSlider * max_jerk_slider_{nullptr}; + QLabel * max_jerk_label_{nullptr}; + QSlider * max_deceleration_slider_{nullptr}; + QLabel * max_deceleration_label_{nullptr}; + nav_msgs::msg::Odometry::ConstSharedPtr kinematic_state_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr acceleration_; + + rclcpp::Subscription::SharedPtr sub_kinematic_state_; + rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_velocity_factors_; rclcpp::Subscription::SharedPtr sub_steering_factors_; void onVelocityFactors(const VelocityFactorArray::ConstSharedPtr msg); void onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg); + + double getMaxJerk() const; + double getMaxDeceleration() const; }; } // namespace rviz_plugins 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 b7ea26e2c6895..cf434bd03f2af 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 @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -46,7 +47,8 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() auto vertical_header = new QHeaderView(Qt::Vertical); vertical_header->hide(); auto horizontal_header = new QHeaderView(Qt::Horizontal); - horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + horizontal_header->setSectionResizeMode(QHeaderView::ResizeToContents); + horizontal_header->setStretchLastSection(true); auto header_labels = QStringList({"Type", "Status", "Distance [m]", "Detail"}); velocity_factors_table_ = new QTableWidget(); @@ -54,7 +56,47 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() velocity_factors_table_->setHorizontalHeaderLabels(header_labels); velocity_factors_table_->setVerticalHeader(vertical_header); velocity_factors_table_->setHorizontalHeader(horizontal_header); - grid->addWidget(velocity_factors_table_, 0, 0); + grid->addWidget(velocity_factors_table_, 0, 0, 1, 3); + + auto * max_jerk_slider_label = new QLabel("Max jerk"); + grid->addWidget(max_jerk_slider_label, 1, 0); + + max_jerk_slider_ = new QSlider(Qt::Horizontal); + max_jerk_slider_->setMinimum(0); + max_jerk_slider_->setMaximum(100); + grid->addWidget(max_jerk_slider_, 1, 1); + + max_jerk_label_ = new QLabel; + grid->addWidget(max_jerk_label_, 1, 2); + + auto * max_deceleration_slider_label = new QLabel("Max deceleration"); + grid->addWidget(max_deceleration_slider_label, 2, 0); + + max_deceleration_slider_ = new QSlider(Qt::Horizontal); + max_deceleration_slider_->setMinimum(0); + max_deceleration_slider_->setMaximum(100); + grid->addWidget(max_deceleration_slider_, 2, 1); + + max_deceleration_label_ = new QLabel; + grid->addWidget(max_deceleration_label_, 2, 2); + + connect(max_jerk_slider_, &QSlider::valueChanged, this, [this](int) { + max_jerk_label_->setText(QString("%1 [m/s3]").arg(getMaxJerk(), 4, 'f', 2)); + }); + connect(max_deceleration_slider_, &QSlider::valueChanged, this, [this](int) { + max_deceleration_label_->setText( + QString("%1 [m/s2]").arg(getMaxDeceleration(), 4, 'f', 2)); + }); + + max_jerk_slider_->setValue( + max_jerk_slider_->minimum() + (max_jerk_slider_->maximum() - max_jerk_slider_->minimum()) * + (MAX_JERK_DEFAULT - MAX_JERK_MIN) / + (MAX_JERK_MAX - MAX_JERK_MIN)); + max_deceleration_slider_->setValue( + max_deceleration_slider_->minimum() + + (max_deceleration_slider_->maximum() - max_deceleration_slider_->minimum()) * + (MAX_DECELERATION_DEFAULT - MAX_DECELERATION_MIN) / + (MAX_DECELERATION_MAX - MAX_DECELERATION_MIN)); group->setLayout(grid); return group; @@ -90,6 +132,17 @@ void VelocitySteeringFactorsPanel::onInitialize() raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); // Planning + sub_kinematic_state_ = raw_node_->create_subscription( + "/localization/kinematic_state", 10, + [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { kinematic_state_ = msg; }); + + sub_acceleration_ = + raw_node_->create_subscription( + "/localization/acceleration", 10, + [this](const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) { + acceleration_ = msg; + }); + sub_velocity_factors_ = raw_node_->create_subscription( "/api/planning/velocity_factors", 10, std::bind(&VelocitySteeringFactorsPanel::onVelocityFactors, this, _1)); @@ -155,6 +208,26 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: label->setAlignment(Qt::AlignCenter); velocity_factors_table_->setCellWidget(i, 3, label); } + + const auto row_background = [&]() -> QBrush { + if (!kinematic_state_ || !acceleration_) { + return {}; + } + const auto & current_vel = kinematic_state_->twist.twist.linear.x; + const auto & current_acc = acceleration_->accel.accel.linear.x; + const auto acc_min = -getMaxDeceleration(); + const auto jerk_acc = getMaxJerk(); + const auto decel_dist = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + current_vel, 0., current_acc, acc_min, jerk_acc, -jerk_acc); + if (decel_dist > e.distance && e.distance >= 0 && e.status == VelocityFactor::APPROACHING) { + return QColor{255, 0, 0, 127}; + } + return {}; + }(); + for (int j = 0; j < velocity_factors_table_->columnCount(); j++) { + velocity_factors_table_->setItem(i, j, new QTableWidgetItem); + velocity_factors_table_->item(i, j)->setBackground(row_background); + } } velocity_factors_table_->update(); } @@ -249,6 +322,21 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: } steering_factors_table_->update(); } + +double VelocitySteeringFactorsPanel::getMaxJerk() const +{ + return MAX_JERK_MIN + (MAX_JERK_MAX - MAX_JERK_MIN) * + (max_jerk_slider_->value() - max_jerk_slider_->minimum()) / + (max_jerk_slider_->maximum() - max_jerk_slider_->minimum()); +} + +double VelocitySteeringFactorsPanel::getMaxDeceleration() const +{ + return MAX_DECELERATION_MIN + + (MAX_DECELERATION_MAX - MAX_DECELERATION_MIN) * + (max_deceleration_slider_->value() - max_deceleration_slider_->minimum()) / + (max_deceleration_slider_->maximum() - max_deceleration_slider_->minimum()); +} } // namespace rviz_plugins #include From 6cfe599e2450fe131780b5c5218077bd62a1da0a Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Tue, 26 Nov 2024 20:13:01 +0900 Subject: [PATCH 2/4] use spin box instead of slider & change layout Signed-off-by: mitukou1109 --- .../velocity_steering_factors_panel.hpp | 24 ++---- .../src/velocity_steering_factors_panel.cpp | 83 ++++++------------- 2 files changed, 34 insertions(+), 73 deletions(-) diff --git a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp index d0391e8a8a697..a22a1d55fcc15 100644 --- a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp @@ -17,11 +17,10 @@ #ifndef VELOCITY_STEERING_FACTORS_PANEL_HPP_ #define VELOCITY_STEERING_FACTORS_PANEL_HPP_ +#include #include #include #include -#include -#include #include #include #include @@ -51,13 +50,13 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel void onInitialize() override; protected: - static constexpr double MAX_JERK_MIN = 0.0; - static constexpr double MAX_JERK_DEFAULT = 1.0; - static constexpr double MAX_JERK_MAX = 2.0; + static constexpr double JERK_MIN = 0.0; + static constexpr double JERK_DEFAULT = 1.0; + static constexpr double JERK_MAX = 2.0; - static constexpr double MAX_DECELERATION_MIN = 0.0; - static constexpr double MAX_DECELERATION_DEFAULT = 1.0; - static constexpr double MAX_DECELERATION_MAX = 2.0; + static constexpr double DECEL_LIMIT_MIN = 0.0; + static constexpr double DECEL_LIMIT_DEFAULT = 1.0; + static constexpr double DECEL_LIMIT_MAX = 2.0; // Layout QGroupBox * makeVelocityFactorsGroup(); @@ -68,10 +67,8 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel // Planning QTableWidget * velocity_factors_table_{nullptr}; QTableWidget * steering_factors_table_{nullptr}; - QSlider * max_jerk_slider_{nullptr}; - QLabel * max_jerk_label_{nullptr}; - QSlider * max_deceleration_slider_{nullptr}; - QLabel * max_deceleration_label_{nullptr}; + QDoubleSpinBox * jerk_input_{nullptr}; + QDoubleSpinBox * decel_limit_input_{nullptr}; nav_msgs::msg::Odometry::ConstSharedPtr kinematic_state_; geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr acceleration_; @@ -83,9 +80,6 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel void onVelocityFactors(const VelocityFactorArray::ConstSharedPtr msg); void onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg); - - double getMaxJerk() const; - double getMaxDeceleration() const; }; } // namespace rviz_plugins 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 cf434bd03f2af..3ccdfa3e3269b 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 @@ -56,47 +56,29 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() velocity_factors_table_->setHorizontalHeaderLabels(header_labels); velocity_factors_table_->setVerticalHeader(vertical_header); velocity_factors_table_->setHorizontalHeader(horizontal_header); - grid->addWidget(velocity_factors_table_, 0, 0, 1, 3); - - auto * max_jerk_slider_label = new QLabel("Max jerk"); - grid->addWidget(max_jerk_slider_label, 1, 0); - - max_jerk_slider_ = new QSlider(Qt::Horizontal); - max_jerk_slider_->setMinimum(0); - max_jerk_slider_->setMaximum(100); - grid->addWidget(max_jerk_slider_, 1, 1); - - max_jerk_label_ = new QLabel; - grid->addWidget(max_jerk_label_, 1, 2); - - auto * max_deceleration_slider_label = new QLabel("Max deceleration"); - grid->addWidget(max_deceleration_slider_label, 2, 0); - - max_deceleration_slider_ = new QSlider(Qt::Horizontal); - max_deceleration_slider_->setMinimum(0); - max_deceleration_slider_->setMaximum(100); - grid->addWidget(max_deceleration_slider_, 2, 1); - - max_deceleration_label_ = new QLabel; - grid->addWidget(max_deceleration_label_, 2, 2); - - connect(max_jerk_slider_, &QSlider::valueChanged, this, [this](int) { - max_jerk_label_->setText(QString("%1 [m/s3]").arg(getMaxJerk(), 4, 'f', 2)); - }); - connect(max_deceleration_slider_, &QSlider::valueChanged, this, [this](int) { - max_deceleration_label_->setText( - QString("%1 [m/s2]").arg(getMaxDeceleration(), 4, 'f', 2)); - }); - - max_jerk_slider_->setValue( - max_jerk_slider_->minimum() + (max_jerk_slider_->maximum() - max_jerk_slider_->minimum()) * - (MAX_JERK_DEFAULT - MAX_JERK_MIN) / - (MAX_JERK_MAX - MAX_JERK_MIN)); - max_deceleration_slider_->setValue( - max_deceleration_slider_->minimum() + - (max_deceleration_slider_->maximum() - max_deceleration_slider_->minimum()) * - (MAX_DECELERATION_DEFAULT - MAX_DECELERATION_MIN) / - (MAX_DECELERATION_MAX - MAX_DECELERATION_MIN)); + grid->addWidget(velocity_factors_table_, 0, 0, 4, 1); + + auto * jerk_label = new QLabel("Jerk"); + grid->addWidget(jerk_label, 0, 1); + + jerk_input_ = new QDoubleSpinBox; + jerk_input_->setMinimum(JERK_MIN); + jerk_input_->setMaximum(JERK_MAX); + jerk_input_->setValue(JERK_DEFAULT); + jerk_input_->setSingleStep(0.1); + jerk_input_->setSuffix(" [m/s\u00B3]"); + grid->addWidget(jerk_input_, 1, 1); + + auto * decel_limit_label = new QLabel("Decel limit"); + grid->addWidget(decel_limit_label, 2, 1); + + decel_limit_input_ = new QDoubleSpinBox; + decel_limit_input_->setMinimum(DECEL_LIMIT_MIN); + decel_limit_input_->setMaximum(DECEL_LIMIT_MAX); + decel_limit_input_->setValue(DECEL_LIMIT_DEFAULT); + decel_limit_input_->setSingleStep(0.1); + decel_limit_input_->setSuffix(" [m/s\u00B2]"); + grid->addWidget(decel_limit_input_, 3, 1); group->setLayout(grid); return group; @@ -215,8 +197,8 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: } const auto & current_vel = kinematic_state_->twist.twist.linear.x; const auto & current_acc = acceleration_->accel.accel.linear.x; - const auto acc_min = -getMaxDeceleration(); - const auto jerk_acc = getMaxJerk(); + const auto acc_min = -decel_limit_input_->value(); + const auto jerk_acc = jerk_input_->value(); const auto decel_dist = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( current_vel, 0., current_acc, acc_min, jerk_acc, -jerk_acc); if (decel_dist > e.distance && e.distance >= 0 && e.status == VelocityFactor::APPROACHING) { @@ -322,21 +304,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: } steering_factors_table_->update(); } - -double VelocitySteeringFactorsPanel::getMaxJerk() const -{ - return MAX_JERK_MIN + (MAX_JERK_MAX - MAX_JERK_MIN) * - (max_jerk_slider_->value() - max_jerk_slider_->minimum()) / - (max_jerk_slider_->maximum() - max_jerk_slider_->minimum()); -} - -double VelocitySteeringFactorsPanel::getMaxDeceleration() const -{ - return MAX_DECELERATION_MIN + - (MAX_DECELERATION_MAX - MAX_DECELERATION_MIN) * - (max_deceleration_slider_->value() - max_deceleration_slider_->minimum()) / - (max_deceleration_slider_->maximum() - max_deceleration_slider_->minimum()); -} } // namespace rviz_plugins #include From 128cce4b0967fb0158036123ed1e85a8dc426f5c Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 29 Nov 2024 19:29:18 +0900 Subject: [PATCH 3/4] set minimum input to 0.0 Signed-off-by: mitukou1109 --- .../src/include/velocity_steering_factors_panel.hpp | 5 ----- .../src/velocity_steering_factors_panel.cpp | 6 ++---- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp index a22a1d55fcc15..c94ea9d968583 100644 --- a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp @@ -50,13 +50,8 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel void onInitialize() override; protected: - static constexpr double JERK_MIN = 0.0; static constexpr double JERK_DEFAULT = 1.0; - static constexpr double JERK_MAX = 2.0; - - static constexpr double DECEL_LIMIT_MIN = 0.0; static constexpr double DECEL_LIMIT_DEFAULT = 1.0; - static constexpr double DECEL_LIMIT_MAX = 2.0; // Layout QGroupBox * makeVelocityFactorsGroup(); 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 3ccdfa3e3269b..7a890086858ba 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 @@ -62,8 +62,7 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() grid->addWidget(jerk_label, 0, 1); jerk_input_ = new QDoubleSpinBox; - jerk_input_->setMinimum(JERK_MIN); - jerk_input_->setMaximum(JERK_MAX); + jerk_input_->setMinimum(0.0); jerk_input_->setValue(JERK_DEFAULT); jerk_input_->setSingleStep(0.1); jerk_input_->setSuffix(" [m/s\u00B3]"); @@ -73,8 +72,7 @@ QGroupBox * VelocitySteeringFactorsPanel::makeVelocityFactorsGroup() grid->addWidget(decel_limit_label, 2, 1); decel_limit_input_ = new QDoubleSpinBox; - decel_limit_input_->setMinimum(DECEL_LIMIT_MIN); - decel_limit_input_->setMaximum(DECEL_LIMIT_MAX); + decel_limit_input_->setMinimum(0.0); decel_limit_input_->setValue(DECEL_LIMIT_DEFAULT); decel_limit_input_->setSingleStep(0.1); decel_limit_input_->setSuffix(" [m/s\u00B2]"); From 406cef1fc174d816deab4572fbbabc0202d33a06 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 29 Nov 2024 19:39:44 +0900 Subject: [PATCH 4/4] change accent color to Freak Pink Signed-off-by: mitukou1109 --- .../src/include/velocity_steering_factors_panel.hpp | 2 ++ .../src/velocity_steering_factors_panel.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp index c94ea9d968583..81677a69eb66f 100644 --- a/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp @@ -53,6 +53,8 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel static constexpr double JERK_DEFAULT = 1.0; static constexpr double DECEL_LIMIT_DEFAULT = 1.0; + static constexpr QColor COLOR_FREAK_PINK = {255, 0, 108}; + // Layout QGroupBox * makeVelocityFactorsGroup(); QGroupBox * makeSteeringFactorsGroup(); 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 7a890086858ba..fab40fe5a2214 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 @@ -200,7 +200,7 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: const auto decel_dist = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( current_vel, 0., current_acc, acc_min, jerk_acc, -jerk_acc); if (decel_dist > e.distance && e.distance >= 0 && e.status == VelocityFactor::APPROACHING) { - return QColor{255, 0, 0, 127}; + return COLOR_FREAK_PINK; } return {}; }();