feat(rviz_plugins): add velocity limit to autoware state panel (tier4…

…#879) * feat(rviz_plugins): add velocity limit to autoware state panel Signed-off-by: tanaka3 <[email protected]> * chore(rviz_plugin): change ms to kmh Signed-off-by: tanaka3 <[email protected]>
boyali · Oct 19, 2022 · 149243e · 149243e
1 parent e4b4857
commit 149243e
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 0 deletions.
diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml
@@ -21,6 +21,7 @@
   <depend>rviz_common</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp
@@ -80,13 +80,26 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
   engage_button_ptr_ = new QPushButton("Engage");
   connect(engage_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareEngage()));
 
+  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()));
+
   auto * v_layout = new QVBoxLayout;
+  auto * velocity_limit_layout = new QHBoxLayout();
   v_layout->addLayout(gate_layout);
   v_layout->addLayout(selector_layout);
   v_layout->addLayout(state_layout);
   v_layout->addLayout(gear_layout);
   v_layout->addLayout(engage_status_layout);
   v_layout->addWidget(engage_button_ptr_);
+  v_layout->addLayout(engage_status_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]"));
+  v_layout->addLayout(velocity_limit_layout);
   setLayout(v_layout);
 }
 
@@ -114,6 +127,9 @@ void AutowareStatePanel::onInitialize()
 
   client_engage_ = raw_node_->create_client<tier4_external_api_msgs::srv::Engage>(
     "/api/autoware/set/engage", rmw_qos_profile_services_default);
+
+  pub_velocity_limit_ = raw_node_->create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
+    "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1));
 }
 
 void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
@@ -215,6 +231,13 @@ void AutowareStatePanel::onEngageStatus(
   engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_)));
 }
 
+void AutowareStatePanel::onClickVelocityLimit()
+{
+  auto velocity_limit = std::make_shared<tier4_planning_msgs::msg::VelocityLimit>();
+  velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6;
+  pub_velocity_limit_->publish(*velocity_limit);
+}
+
 void AutowareStatePanel::onClickAutowareEngage()
 {
   auto req = std::make_shared<tier4_external_api_msgs::srv::Engage::Request>();

diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp
@@ -19,6 +19,7 @@
 
 #include <QLabel>
 #include <QPushButton>
+#include <QSpinBox>
 #include <rclcpp/rclcpp.hpp>
 #include <rviz_common/panel.hpp>
 
@@ -28,6 +29,7 @@
 #include <tier4_control_msgs/msg/external_command_selector_mode.hpp>
 #include <tier4_control_msgs/msg/gate_mode.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
+#include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
 namespace rviz_plugins
 {
@@ -41,6 +43,7 @@ class AutowareStatePanel : public rviz_common::Panel
 
 public Q_SLOTS:
   void onClickAutowareEngage();
+  void onClickVelocityLimit();
 
 protected:
   void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
@@ -61,12 +64,16 @@ public Q_SLOTS:
 
   rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedPtr client_engage_;
 
+  rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
+
   QLabel * gate_mode_label_ptr_;
   QLabel * selector_mode_label_ptr_;
   QLabel * autoware_state_label_ptr_;
   QLabel * gear_label_ptr_;
   QLabel * engage_status_label_ptr_;
   QPushButton * engage_button_ptr_;
+  QPushButton * velocity_limit_button_ptr_;
+  QSpinBox * pub_velocity_limit_input_;
 
   bool current_engage_;
 };