remove function for debug

Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com>
autowarefoundation · Nov 8, 2022 · fea383a · fea383a
1 parent 6fefc7c
commit fea383a
Show file tree

Hide file tree

Showing 4 changed files with 0 additions and 73 deletions.
diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp
@@ -100,20 +100,11 @@ 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);
@@ -129,11 +120,6 @@ 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);
 }
 
@@ -179,21 +165,6 @@ void AutowareStatePanel::onInitialize()
     "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)

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,7 +19,6 @@
 
 #include <QLabel>
 #include <QPushButton>
-#include <QSlider>
 #include <QSpinBox>
 #include <rclcpp/rclcpp.hpp>
 #include <rviz_common/panel.hpp>
@@ -33,12 +32,10 @@
 #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
@@ -53,7 +50,6 @@ 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);
@@ -80,7 +76,6 @@ 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_;
@@ -96,11 +91,6 @@ 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

diff --git a/..._path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/..._path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
@@ -57,13 +57,6 @@ struct SideShiftParameters
   double drivable_area_left_bound_offset;
 };
 
-struct SideShiftDebugData
-{
-  std::shared_ptr<PathShifter> path_shifter;
-  ShiftLineArray shift_points;
-  double current_request;
-};
-
 class SideShiftModule : public SceneModuleInterface
 {
 public:
@@ -139,9 +132,6 @@ class SideShiftModule : public SceneModuleInterface
 
   mutable rclcpp::Time last_requested_shift_change_time_{clock_->now()};
 
-  // debug
-  mutable SideShiftDebugData debug_data_;
-  void setDebugMarkersVisualization() const;
 };
 }  // namespace behavior_path_planner
 

diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
@@ -14,7 +14,6 @@
 
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 
-#include "behavior_path_planner/debug_utilities.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/side_shift/util.hpp"
 #include "behavior_path_planner/utilities.hpp"
@@ -46,7 +45,6 @@ SideShiftModule::SideShiftModule(
 void SideShiftModule::initVariables()
 {
   reference_path_ = std::make_shared<PathWithLaneId>();
-  debug_data_.path_shifter = nullptr;
   start_pose_reset_request_ = false;
   requested_lateral_offset_ = 0.0;
   inserted_lateral_offset_ = 0.0;
@@ -254,8 +252,6 @@ BehaviorModuleOutput SideShiftModule::plan()
   output.path = std::make_shared<PathWithLaneId>(shifted_path.path);
 
   prev_output_ = shifted_path;
-  debug_data_.path_shifter = std::make_shared<PathShifter>(path_shifter_);
-  setDebugMarkersVisualization();
 
   return output;
 }
@@ -442,24 +438,4 @@ PathWithLaneId SideShiftModule::calcCenterLinePath(
   return centerline_path;
 }
 
-void SideShiftModule::setDebugMarkersVisualization() const
-{
-  using marker_utils::createShiftLineMarkerArray;
-
-  debug_marker_.markers.clear();
-
-  const auto add = [this](const MarkerArray & added) {
-    tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
-  };
-
-  const auto addShiftPoint = [this, add](const auto & ns, auto r, auto g, auto b, double w = 0.1) {
-    add(createShiftLineMarkerArray(
-      debug_data_.path_shifter->getShiftLines(), debug_data_.path_shifter->getBaseOffset(), ns, r,
-      g, b, w));
-  };
-
-  if (debug_data_.path_shifter) {
-    addShiftPoint("side_shift_shift_points", 0.7, 0.7, 0.7, 0.4);
-  }
-}
 }  // namespace behavior_path_planner