cherry picked side shift controller

Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com>

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -100,11 +100,20 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
   // Emergency Button
   emergency_button_ptr_ = new QPushButton("Set Emergency");
   connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton()));
+  // for side shift
+  lateral_offset_slider_ptr_ = new QSlider(Qt::Horizontal, this);
+  lateral_offset_slider_ptr_->setRange(-8.0, 8.0);
+  lateral_offset_slider_ptr_->setValue(0.0);
+  pub_velocity_limit_input_->setSingleStep(1.0);
+  lateral_offset_value_ptr_ = new QLabel();
+  connect(
+    lateral_offset_slider_ptr_, SIGNAL(valueChanged(int)), this, SLOT(onClickLateralOffset()));
 
   // Layout
   auto * v_layout = new QVBoxLayout;
   auto * gate_mode_path_change_approval_layout = new QHBoxLayout;
   auto * velocity_limit_layout = new QHBoxLayout();
+  auto * lateral_offset_slider_layout = new QHBoxLayout;
   v_layout->addLayout(gate_layout);
   v_layout->addLayout(selector_layout);
   v_layout->addLayout(state_layout);
@@ -120,6 +129,11 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
   velocity_limit_layout->addWidget(new QLabel("  [km/h]"));
   velocity_limit_layout->addWidget(emergency_button_ptr_);
   v_layout->addLayout(velocity_limit_layout);
+  lateral_offset_slider_layout->addWidget(new QLabel("Side Shift "));
+  lateral_offset_slider_layout->addWidget(lateral_offset_slider_ptr_);
+  lateral_offset_slider_layout->addWidget(lateral_offset_value_ptr_);
+  lateral_offset_slider_layout->addWidget(new QLabel("  [m]"));
+  v_layout->addLayout(lateral_offset_slider_layout);
   setLayout(v_layout);
 }
 
@@ -164,6 +178,22 @@ void AutowareStatePanel::onInitialize()
     "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/"
     "path_change_approval",
     rclcpp::QoS{1}.transient_local());
+
+  pub_lateral_offset_ = raw_node_->create_publisher<LateralOffset>(
+    "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/input/"
+    "lateral_offset",
+    rclcpp::QoS(1));
+}
+
+void AutowareStatePanel::onClickLateralOffset()
+{
+  const double shift_step = -0.5;
+  lateral_offset_ = lateral_offset_slider_ptr_->sliderPosition() * shift_step;
+  const QString lateral_offset_string = QString::fromStdString(std::to_string(lateral_offset_));
+  lateral_offset_value_ptr_->setText(lateral_offset_string);
+  pub_lateral_offset_->publish(tier4_planning_msgs::build<LateralOffset>()
+                                 .stamp(raw_node_->now())
+                                 .lateral_offset(lateral_offset_));
 }
 
 void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -19,6 +19,7 @@
 
 #include <QLabel>
 #include <QPushButton>
+#include <QSlider>
 #include <QSpinBox>
 #include <rclcpp/rclcpp.hpp>
 #include <rviz_common/panel.hpp>
@@ -32,10 +33,12 @@
 #include <tier4_external_api_msgs/srv/engage.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
+#include <tier4_planning_msgs/msg/lateral_offset.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
 namespace rviz_plugins
 {
+using tier4_planning_msgs::msg::LateralOffset;
 class AutowareStatePanel : public rviz_common::Panel
 {
   Q_OBJECT
@@ -50,6 +53,7 @@ public Q_SLOTS:  // NOLINT for Qt
   void onClickGateMode();
   void onClickPathChangeApproval();
   void onClickEmergencyButton();
+  void onClickLateralOffset();
 
 protected:
   void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
@@ -76,6 +80,7 @@ public Q_SLOTS:  // NOLINT for Qt
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
   rclcpp::Publisher<tier4_planning_msgs::msg::Approval>::SharedPtr pub_path_change_approval_;
+  rclcpp::Publisher<LateralOffset>::SharedPtr pub_lateral_offset_;
 
   QLabel * gate_mode_label_ptr_;
   QLabel * selector_mode_label_ptr_;
@@ -91,6 +96,11 @@ public Q_SLOTS:  // NOLINT for Qt
 
   bool current_engage_{false};
   bool current_emergency_{false};
+  // for side shift
+  QSlider * lateral_offset_slider_ptr_;
+  QLabel * lateral_offset_value_ptr_;
+
+  double lateral_offset_ = {0.0};
 };
 
 }  // namespace rviz_plugins