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..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
@@ -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
@@ -29,6 +28,8 @@
#include
#include
#include
+#include
+#include
#include
@@ -49,6 +50,11 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel
void onInitialize() override;
protected:
+ 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();
@@ -58,7 +64,14 @@ class VelocitySteeringFactorsPanel : public rviz_common::Panel
// Planning
QTableWidget * velocity_factors_table_{nullptr};
QTableWidget * steering_factors_table_{nullptr};
+ QDoubleSpinBox * jerk_input_{nullptr};
+ QDoubleSpinBox * decel_limit_input_{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_;
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..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
@@ -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,27 @@ 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, 4, 1);
+
+ auto * jerk_label = new QLabel("Jerk");
+ grid->addWidget(jerk_label, 0, 1);
+
+ jerk_input_ = new QDoubleSpinBox;
+ jerk_input_->setMinimum(0.0);
+ 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(0.0);
+ 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;
@@ -90,6 +112,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 +188,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 = -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) {
+ return COLOR_FREAK_PINK;
+ }
+ 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();
}