diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index a3f3413e..1932e856 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -129,6 +129,7 @@ void ManualController::update() } GearCommand gear_cmd; { + gear_cmd.stamp = raw_node_->get_clock()->now(); const double eps = 0.001; if (control_cmd.longitudinal.velocity > eps && current_velocity > -eps) { gear_cmd.command = GearCommand::DRIVE; diff --git a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt b/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt index 5b6e205b..29a72304 100644 --- a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt +++ b/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt @@ -13,17 +13,30 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) -ament_auto_add_library(${PROJECT_NAME} SHARED +ament_auto_find_build_dependencies() +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/Capture.srv" + DEPENDENCIES + std_msgs +) + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/screen_capture_panel.hpp src/screen_capture_panel.cpp ) -target_link_libraries(${PROJECT_NAME} + +rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") + +target_link_libraries(${PROJECT_NAME}_lib ${QT_LIBRARIES} ${OpenCV_LIBRARIES} ) + pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) ament_auto_package( INSTALL_TO_SHARE - plugins + plugins ) diff --git a/common/tier4_screen_capture_rviz_plugin/package.xml b/common/tier4_screen_capture_rviz_plugin/package.xml index 180bf9ee..63b20c4e 100644 --- a/common/tier4_screen_capture_rviz_plugin/package.xml +++ b/common/tier4_screen_capture_rviz_plugin/package.xml @@ -12,6 +12,8 @@ ament_cmake_auto autoware_cmake + rosidl_default_generators + libopencv-dev libqt5-core libqt5-gui @@ -24,6 +26,10 @@ ament_lint_auto autoware_lint_common + rosidl_default_runtime + + rosidl_interface_packages + ament_cmake diff --git a/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml b/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml index fdf105d6..109487b1 100644 --- a/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml @@ -1,7 +1,8 @@ - + AutowareScreenCapturePanel diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp index a64d71a3..e780678a 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp @@ -20,6 +20,9 @@ #include #include +namespace rviz_plugins +{ + using std::placeholders::_1; using std::placeholders::_2; @@ -41,11 +44,11 @@ AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) { ros_time_label_ = new QLabel; screen_capture_button_ptr_ = new QPushButton("Capture Screen Shot"); - connect(screen_capture_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickScreenCapture())); - file_name_prefix_ = new QLineEdit("cap"); - connect(file_name_prefix_, SIGNAL(valueChanged(std::string)), this, SLOT(onPrefixChanged())); + connect(screen_capture_button_ptr_, SIGNAL(clicked()), this, SLOT(on_click_screen_capture())); + file_prefix_ = new QLineEdit("cap"); + connect(file_prefix_, SIGNAL(editingFinished()), this, SLOT(on_prefix_change())); cap_layout->addWidget(screen_capture_button_ptr_); - cap_layout->addWidget(file_name_prefix_); + cap_layout->addWidget(file_prefix_); cap_layout->addWidget(ros_time_label_); // initialize file name system clock is better for identification. setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); @@ -55,15 +58,15 @@ AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) auto * video_cap_layout = new QHBoxLayout; { capture_to_mp4_button_ptr_ = new QPushButton("Capture Screen"); - connect(capture_to_mp4_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVideoCapture())); - capture_hz_ = new QSpinBox(); - capture_hz_->setRange(1, 10); - capture_hz_->setValue(10); - capture_hz_->setSingleStep(1); - connect(capture_hz_, SIGNAL(valueChanged(int)), this, SLOT(onRateChanged())); + connect(capture_to_mp4_button_ptr_, SIGNAL(clicked()), this, SLOT(on_click_video_capture())); + rate_ = new QSpinBox(); + rate_->setRange(1, 10); + rate_->setValue(10); + rate_->setSingleStep(1); + connect(rate_, SIGNAL(valueChanged(const int)), this, SLOT(on_rate_change(const int))); // video cap layout video_cap_layout->addWidget(capture_to_mp4_button_ptr_); - video_cap_layout->addWidget(capture_hz_); + video_cap_layout->addWidget(rate_); video_cap_layout->addWidget(new QLabel(" [Hz]")); } @@ -73,74 +76,125 @@ AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) v_layout->addLayout(video_cap_layout); setLayout(v_layout); } - auto * timer = new QTimer(this); - connect(timer, &QTimer::timeout, this, &AutowareScreenCapturePanel::update); - timer->start(1000); - capture_timer_ = new QTimer(this); - connect(capture_timer_, &QTimer::timeout, this, &AutowareScreenCapturePanel::onTimer); - state_ = State::WAITING_FOR_CAPTURE; } -void AutowareScreenCapturePanel::onCaptureVideoTrigger( - [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res) +void AutowareScreenCapturePanel::onInitialize() { - onClickVideoCapture(false); - res->success = true; - res->message = stateToString(state_); + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + srv_ = raw_node_->create_service( + "/debug/capture", std::bind(&AutowareScreenCapturePanel::callback, this, _1, _2)); + + create_timer(); } -void AutowareScreenCapturePanel::onCaptureVideoWithBufferTrigger( - [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res) +void AutowareScreenCapturePanel::create_timer() { - onClickVideoCapture(true); - res->success = true; - res->message = stateToString(state_); + const auto period = std::chrono::milliseconds(static_cast(1e3 / rate_->value())); + timer_ = raw_node_->create_wall_timer(period, [&]() { on_timer(); }); } -void AutowareScreenCapturePanel::onCaptureScreenShotTrigger( - [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res) +void AutowareScreenCapturePanel::on_rate_change([[maybe_unused]] const int rate) { - onClickScreenCapture(); - res->success = true; - res->message = stateToString(state_); + timer_->cancel(); + create_timer(); + RCLCPP_INFO_STREAM(raw_node_->get_logger(), "RATE:" << rate_->value()); } -void AutowareScreenCapturePanel::onInitialize() +void AutowareScreenCapturePanel::on_timer() { - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - capture_video_srv_ = raw_node_->create_service( - "/debug/capture/video", - std::bind(&AutowareScreenCapturePanel::onCaptureVideoTrigger, this, _1, _2)); - capture_video_with_buffer_srv_ = raw_node_->create_service( - "/debug/capture/video_with_buffer", - std::bind(&AutowareScreenCapturePanel::onCaptureVideoWithBufferTrigger, this, _1, _2)); - capture_screen_shot_srv_ = raw_node_->create_service( - "/debug/capture/screen_shot", - std::bind(&AutowareScreenCapturePanel::onCaptureScreenShotTrigger, this, _1, _2)); + setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); + + if (!main_window_) return; + + // this is deprecated but only way to capture nicely + QScreen * screen = QGuiApplication::primaryScreen(); + QPixmap original_pixmap = screen->grabWindow(main_window_->winId()); + const auto q_image = + original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); + const int h = q_image.height(); + const int w = q_image.width(); + cv::Size size = cv::Size(w, h); + cv::Mat image( + size, CV_8UC3, const_cast(q_image.bits()), + static_cast(q_image.bytesPerLine())); + + size_ = size; + + if (is_buffering_) { + buffer_.push_back(image.clone()); + if (buffer_.size() > buffer_size_) { + buffer_.pop_front(); + } + } + + if (is_recording_) { + movie_.push_back(image.clone()); + } + + cv::waitKey(0); } -void onPrefixChanged() +void AutowareScreenCapturePanel::callback( + const Capture::Request::SharedPtr req, const Capture::Response::SharedPtr res) { + if (req->action == Capture::Request::SCREEN_SHOT) { + res->success = save_screen_shot(req->file_name); + return; + } + + if (req->action == Capture::Request::START_BUFFERING) { + res->success = start_buffering(); + return; + } + + if (req->action == Capture::Request::STOP_BUFFERING) { + res->success = stop_buffering(); + return; + } + + if (req->action == Capture::Request::RECORD) { + res->success = start_recording(); + return; + } + + if (req->action == Capture::Request::SAVE) { + res->success = save_movie(req->file_name); + return; + } + + if (req->action == Capture::Request::SAVE_BUFFER) { + res->success = save_buffer(req->file_name); + return; + } + + res->success = false; +} + +void AutowareScreenCapturePanel::on_click_screen_capture() +{ + save_screen_shot(file_prefix_->text().toStdString()); } -void AutowareScreenCapturePanel::onRateChanged() +void AutowareScreenCapturePanel::on_click_video_capture() { + if (is_recording_) { + save_movie(file_prefix_->text().toStdString()); + } else { + start_recording(); + } } -void AutowareScreenCapturePanel::onClickScreenCapture() +bool AutowareScreenCapturePanel::save_screen_shot(const std::string & file_name) { - const std::string time_text = - "capture/" + file_name_prefix_->text().toStdString() + ros_time_label_->text().toStdString(); + const std::string time_text = "capture/" + file_name + ros_time_label_->text().toStdString(); getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot( time_text + ".png"); + + return true; } -void AutowareScreenCapturePanel::onClickVideoCapture(bool use_buffer) +bool AutowareScreenCapturePanel::start_recording() { - const int clock = static_cast(1e3 / capture_hz_->value()); try { const QWidgetList top_level_widgets = QApplication::topLevelWidgets(); for (QWidget * widget : top_level_widgets) { @@ -150,125 +204,104 @@ void AutowareScreenCapturePanel::onClickVideoCapture(bool use_buffer) } } } catch (...) { - return; + return false; } - if (!main_window_) return; - switch (state_) { - case State::WAITING_FOR_CAPTURE: - // initialize setting - { - capture_file_name_ = ros_time_label_->text().toStdString(); - } - capture_to_mp4_button_ptr_->setText("capturing rviz screen"); - capture_to_mp4_button_ptr_->setStyleSheet("background-color: #FF0000;"); - { - int fourcc = cv::VideoWriter::fourcc('h', '2', '6', '4'); // mp4 - QScreen * screen = QGuiApplication::primaryScreen(); - const auto q_size = screen->grabWindow(main_window_->winId()) - .toImage() - .convertToFormat(QImage::Format_RGB888) - .rgbSwapped() - .size(); - current_movie_size_ = cv::Size(q_size.width(), q_size.height()); - is_buffering_ = use_buffer; - if (!is_buffering_) { - writer_.open( - "capture/" + file_name_prefix_->text().toStdString() + capture_file_name_ + ".mp4", - fourcc, capture_hz_->value(), current_movie_size_); - } else { - frame_buffer_.clear(); - } - } - capture_timer_->start(clock); - state_ = State::CAPTURING; - break; - case State::CAPTURING: - if (is_buffering_) { - saveBufferedVideo(); - } else { - writer_.release(); + + if (!main_window_) return false; + + RCLCPP_INFO_STREAM(raw_node_->get_logger(), "START RECORDING."); + + capture_to_mp4_button_ptr_->setText("capturing rviz screen"); + capture_to_mp4_button_ptr_->setStyleSheet("background-color: #FF0000;"); + + is_recording_ = true; + + return true; +} + +bool AutowareScreenCapturePanel::start_buffering() +{ + try { + const QWidgetList top_level_widgets = QApplication::topLevelWidgets(); + for (QWidget * widget : top_level_widgets) { + auto * main_window_candidate = qobject_cast(widget); + if (main_window_candidate) { + main_window_ = main_window_candidate; } - capture_timer_->stop(); - capture_to_mp4_button_ptr_->setText("waiting for capture"); - capture_to_mp4_button_ptr_->setStyleSheet("background-color: #00FF00;"); - state_ = State::WAITING_FOR_CAPTURE; - break; + } + } catch (...) { + return false; } + + if (!main_window_) return false; + + RCLCPP_INFO_STREAM(raw_node_->get_logger(), "START BUFFERING."); + + buffer_.clear(); + is_buffering_ = true; + + return true; } -void AutowareScreenCapturePanel::onTimer() +bool AutowareScreenCapturePanel::stop_buffering() { - if (!main_window_) return; - // this is deprecated but only way to capture nicely - QScreen * screen = QGuiApplication::primaryScreen(); - QPixmap original_pixmap = screen->grabWindow(main_window_->winId()); - const auto q_image = - original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); - const int h = q_image.height(); - const int w = q_image.width(); - cv::Size size = cv::Size(w, h); - cv::Mat image( - size, CV_8UC3, const_cast(q_image.bits()), - static_cast(q_image.bytesPerLine())); + if (!is_buffering_) return false; - if (is_buffering_) { - // Buffering mode: Store frame in buffer - frame_buffer_.push_back({image.clone(), rclcpp::Clock().now()}); - if (frame_buffer_.size() > buffer_size_) { - frame_buffer_.pop_front(); - } - } else { - // Normal mode: Write frame to video file - if (size != current_movie_size_) { - cv::Mat new_image; - cv::resize(image, new_image, current_movie_size_); - writer_.write(new_image); - } else { - writer_.write(image); - } - } + buffer_.clear(); + is_buffering_ = false; - cv::waitKey(0); + return true; } -void AutowareScreenCapturePanel::captureFrameToBuffer() +bool AutowareScreenCapturePanel::save_movie(const std::string & file_name) { - QScreen * screen = QGuiApplication::primaryScreen(); - QPixmap original_pixmap = screen->grabWindow(main_window_->winId()); - const auto q_image = - original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); - cv::Mat image( - q_image.height(), q_image.width(), CV_8UC3, const_cast(q_image.bits()), - static_cast(q_image.bytesPerLine())); + if (!is_recording_) return false; - frame_buffer_.push_back({image, rclcpp::Clock().now()}); - if (frame_buffer_.size() > buffer_size_) { - frame_buffer_.pop_front(); - } + RCLCPP_INFO_STREAM(raw_node_->get_logger(), "SAVE RECORDED MOVIE."); + + save(movie_, file_name); + + capture_to_mp4_button_ptr_->setText("waiting for capture"); + capture_to_mp4_button_ptr_->setStyleSheet("background-color: #00FF00;"); + + is_recording_ = false; + + movie_.clear(); + + return true; } -void AutowareScreenCapturePanel::saveBufferedVideo() +bool AutowareScreenCapturePanel::save_buffer(const std::string & file_name) { - if (frame_buffer_.empty()) return; + if (buffer_.empty()) return false; - int fourcc = cv::VideoWriter::fourcc('h', '2', '6', '4'); // mp4 - cv::VideoWriter buffered_writer; - buffered_writer.open( - "capture/" + file_name_prefix_->text().toStdString() + capture_file_name_ + "_buffered.mp4", - fourcc, capture_hz_->value(), current_movie_size_); + if (!is_buffering_) return false; - for (const auto & [frame, _] : frame_buffer_) { - cv::Mat resized_frame; - cv::resize(frame, resized_frame, current_movie_size_); - buffered_writer.write(resized_frame); - } + RCLCPP_INFO_STREAM(raw_node_->get_logger(), "SAVE BUFFERED MOVIE."); + + save(buffer_, file_name + "_buffered"); - buffered_writer.release(); + return true; } -void AutowareScreenCapturePanel::update() +void AutowareScreenCapturePanel::save( + const std::deque & images, const std::string & file_name) { - setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); + int fourcc = cv::VideoWriter::fourcc('h', '2', '6', '4'); // mp4 + + cv::VideoWriter writer; + + writer.open( + "capture/" + file_name + ros_time_label_->text().toStdString() + ".mp4", fourcc, rate_->value(), + size_); + + for (const auto & frame : images) { + cv::Mat resized_frame; + cv::resize(frame, resized_frame, size_); + writer.write(resized_frame); + } + + writer.release(); } void AutowareScreenCapturePanel::save(rviz_common::Config config) const @@ -283,5 +316,7 @@ void AutowareScreenCapturePanel::load(const rviz_common::Config & config) AutowareScreenCapturePanel::~AutowareScreenCapturePanel() = default; +} // namespace rviz_plugins + #include -PLUGINLIB_EXPORT_CLASS(AutowareScreenCapturePanel, rviz_common::Panel) +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareScreenCapturePanel, rviz_common::Panel) diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp index 78b90d4c..4d8b78d5 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp @@ -39,6 +39,8 @@ #include // ros +#include + #include #include @@ -47,7 +49,10 @@ #include #include -class QLineEdit; +namespace rviz_plugins +{ + +using tier4_screen_capture_rviz_plugin::srv::Capture; class AutowareScreenCapturePanel : public rviz_common::Panel { @@ -56,67 +61,68 @@ class AutowareScreenCapturePanel : public rviz_common::Panel public: explicit AutowareScreenCapturePanel(QWidget * parent = nullptr); ~AutowareScreenCapturePanel() override; - void update(); + void onInitialize() override; - void createWallTimer(); - void onTimer(); - void captureFrameToBuffer(); - void saveBufferedVideo(); + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; - void onCaptureVideoTrigger( - const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res); - void onCaptureVideoWithBufferTrigger( - const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res); - void onCaptureScreenShotTrigger( - const std_srvs::srv::Trigger::Request::SharedPtr req, - const std_srvs::srv::Trigger::Response::SharedPtr res); public Q_SLOTS: - void onClickScreenCapture(); - void onClickVideoCapture(bool use_buffer = false); - void onPrefixChanged(); - void onRateChanged(); + void on_click_screen_capture(); + + void on_click_video_capture(); + + void on_prefix_change() {} + + void on_rate_change(const int rate); private: + void create_timer(); + + void callback(const Capture::Request::SharedPtr req, const Capture::Response::SharedPtr res); + + bool save_screen_shot(const std::string & file_name); + + bool start_buffering(); + + bool start_recording(); + + bool save_movie(const std::string & file_name); + + bool save_buffer(const std::string & file_name); + + bool stop_buffering(); + + void on_timer(); + + void save(const std::deque & images, const std::string & file_name); + QLabel * ros_time_label_; QPushButton * screen_capture_button_ptr_; QPushButton * capture_to_mp4_button_ptr_; - QLineEdit * file_name_prefix_; - QSpinBox * capture_hz_; - QTimer * capture_timer_; + QLineEdit * file_prefix_; + QSpinBox * rate_; QMainWindow * main_window_{nullptr}; - enum class State { WAITING_FOR_CAPTURE, CAPTURING }; - State state_; - std::string capture_file_name_; - bool is_capture_{false}; - bool is_buffering_{false}; - cv::VideoWriter writer_; - cv::Size current_movie_size_; - std::vector image_vec_; - std::deque> frame_buffer_; + + cv::Size size_; + + std::deque movie_; + + std::deque buffer_; + // Size of the frame buffer (number of frames to keep in memory) // At 10 Hz capture rate, 100 frames correspond to approximately 10 seconds of video const size_t buffer_size_ = 100; - static std::string stateToString(const State & state) - { - if (state == State::WAITING_FOR_CAPTURE) { - return "waiting for capture"; - } - if (state == State::CAPTURING) { - return "capturing"; - } - return ""; - } - -protected: - rclcpp::Service::SharedPtr capture_video_srv_; - rclcpp::Service::SharedPtr capture_video_with_buffer_srv_; - rclcpp::Service::SharedPtr capture_screen_shot_srv_; + bool is_buffering_{false}; + bool is_recording_{false}; + + rclcpp::Service::SharedPtr srv_; rclcpp::Node::SharedPtr raw_node_; + rclcpp::TimerBase::SharedPtr timer_; }; +} // namespace rviz_plugins + #endif // SCREEN_CAPTURE_PANEL_HPP_ diff --git a/common/tier4_screen_capture_rviz_plugin/srv/Capture.srv b/common/tier4_screen_capture_rviz_plugin/srv/Capture.srv new file mode 100644 index 00000000..33f2de6f --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/srv/Capture.srv @@ -0,0 +1,13 @@ +uint8 SCREEN_SHOT = 0 +uint8 START_BUFFERING = 1 +uint8 STOP_BUFFERING = 2 +uint8 RECORD = 3 +uint8 SAVE = 4 +uint8 SAVE_BUFFER = 5 + +uint8 action +string file_name + +--- + +bool success diff --git a/control_data_collecting_tool/CMakeLists.txt b/control_data_collecting_tool/CMakeLists.txt index 331366c5..6d817fcd 100644 --- a/control_data_collecting_tool/CMakeLists.txt +++ b/control_data_collecting_tool/CMakeLists.txt @@ -7,6 +7,7 @@ autoware_package() install(PROGRAMS scripts/data_collecting_pure_pursuit_trajectory_follower.py scripts/data_collecting_trajectory_publisher.py + scripts/data_collecting_plotter.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/control_data_collecting_tool/README.md b/control_data_collecting_tool/README.md index f8e7d14e..4f853eec 100644 --- a/control_data_collecting_tool/README.md +++ b/control_data_collecting_tool/README.md @@ -144,3 +144,19 @@ ROS 2 params in `/data_collecting_pure_pursuit_trajectory_follower` node: | `steer_noise_amp` | `double` | Steer command additional sine noise amplitude [rad] | 0.01 | | `steer_noise_max_period` | `double` | Steer command additional sine noise maximum period [s] | 5.0 | | `steer_noise_min_period` | `double` | Steer command additional sine noise minimum period [s] | 20.0 | + +ROS 2 params in `/data_collecting_plotter` node: + +| Name | Type | Description | Default value | +| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------- | :------------- | +| `COURSE_NAME` | `string` | Course name [`eight_course`, `u_shaped_return`, `straight_line_positive`, `straight_line_negative`] | `eight_course` | +| `NUM_BINS_V` | `int` | Number of bins of velocity in heatmap | 10 | +| `NUM_BINS_STEER` | `int` | Number of bins of steer in heatmap | 10 | +| `NUM_BINS_ACCELERATION` | `int` | Number of bins of acceleration in heatmap | 10 | +| `V_MIN` | `double` | Minimum velocity in heatmap [m/s] | 0.0 | +| `V_MAX` | `double` | Maximum velocity in heatmap [m/s] | 11.5 | +| `STEER_MIN` | `double` | Minimum steer in heatmap [rad] | -1.0 | +| `STEER_MAX` | `double` | Maximum steer in heatmap [rad] | 1.0 | +| `A_MIN` | `double` | Minimum acceleration in heatmap [m/ss] | -1.0 | +| `A_MAX` | `double` | Maximum acceleration in heatmap [m/ss] | 1.0 | +| `wheel_base` | `double` | Wheel base [m] | 2.79 | diff --git a/control_data_collecting_tool/config/param.yaml b/control_data_collecting_tool/config/param.yaml index cc6177f6..d96553ac 100644 --- a/control_data_collecting_tool/config/param.yaml +++ b/control_data_collecting_tool/config/param.yaml @@ -1,7 +1,16 @@ -common_parameters: +data_collecting_plotter: ros__parameters: + NUM_BINS_V: 10 + NUM_BINS_STEER: 10 + NUM_BINS_A: 10 + V_MIN: 0.0 + V_MAX: 11.5 + STEER_MIN: -1.0 + STEER_MAX: 1.0 + A_MIN: -1.0 + A_MAX: 1.0 + wheel_base: 2.79 - acc_kp: 1.0 data_collecting_trajectory_publisher: ros__parameters: @@ -20,6 +29,8 @@ data_collecting_trajectory_publisher: A_MIN: -1.0 A_MAX: 1.0 + wheel_base: 2.79 + acc_kp: 1.0 max_lateral_accel: 0.5 lateral_error_threshold: 2.0 yaw_error_threshold: 0.50 @@ -33,6 +44,8 @@ data_collecting_trajectory_publisher: data_collecting_pure_pursuit_trajectory_follower: ros__parameters: pure_pursuit_type: linearized + wheel_base: 2.79 + acc_kp: 1.0 lookahead_time: 2.0 min_lookahead: 2.0 linearized_pure_pursuit_steer_kp_param: 2.0 diff --git a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py index f1b15476..ba3ea314 100644 --- a/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py +++ b/control_data_collecting_tool/launch/control_data_collecting_tool.launch.py @@ -39,6 +39,12 @@ def generate_launch_description(): name="data_collecting_trajectory_publisher", parameters=[param_file_path], ), + Node( + package="control_data_collecting_tool", + executable="data_collecting_plotter.py", + name="data_collecting_plotter", + parameters=[param_file_path], + ), ] ) diff --git a/control_data_collecting_tool/scripts/data_collecting_plotter.py b/control_data_collecting_tool/scripts/data_collecting_plotter.py new file mode 100755 index 00000000..30f1f2cc --- /dev/null +++ b/control_data_collecting_tool/scripts/data_collecting_plotter.py @@ -0,0 +1,310 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Proxima Technology Inc, TIER IV +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import threading + +from geometry_msgs.msg import AccelWithCovarianceStamped +import matplotlib.pyplot as plt +from nav_msgs.msg import Odometry +import numpy as np +from numpy import arctan2 +from rcl_interfaces.msg import ParameterDescriptor +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +import seaborn as sns + + +class DataCollectingPlotter(Node): + def __init__(self): + super().__init__("data_collecting_plotter") + + self.declare_parameter( + "NUM_BINS_V", + 10, + ParameterDescriptor(description="Number of bins of velocity in heatmap"), + ) + + self.declare_parameter( + "NUM_BINS_STEER", + 10, + ParameterDescriptor(description="Number of bins of steer in heatmap"), + ) + + self.declare_parameter( + "NUM_BINS_A", + 10, + ParameterDescriptor(description="Number of bins of acceleration in heatmap"), + ) + + self.declare_parameter( + "V_MIN", + 0.0, + ParameterDescriptor(description="Maximum velocity in heatmap [m/s]"), + ) + + self.declare_parameter( + "V_MAX", + 1.0, + ParameterDescriptor(description="Minimum steer in heatmap [m/s]"), + ) + + self.declare_parameter( + "STEER_MIN", + -1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) + + self.declare_parameter( + "STEER_MAX", + 1.0, + ParameterDescriptor(description="Maximum steer in heatmap [rad]"), + ) + + self.declare_parameter( + "A_MIN", + -1.0, + ParameterDescriptor(description="Minimum acceleration in heatmap [m/ss]"), + ) + + self.declare_parameter( + "A_MAX", + 1.0, + ParameterDescriptor(description="Maximum acceleration in heatmap [m/ss]"), + ) + + self.declare_parameter( + "wheel_base", + 2.79, # sample_vehicle_launch/sample_vehicle_description/config/vehicle_info.param.yaml + ParameterDescriptor(description="Wheel base [m]"), + ) + + self.sub_odometry_ = self.create_subscription( + Odometry, + "/localization/kinematic_state", + self.onOdometry, + 1, + ) + + self.sub_acceleration_ = self.create_subscription( + AccelWithCovarianceStamped, + "/localization/acceleration", + self.onAcceleration, + 1, + ) + + self._present_kinematic_state = None + self._present_acceleration = None + + # velocity and acceleration grid + # velocity and steer grid + self.num_bins_v = self.get_parameter("NUM_BINS_V").get_parameter_value().integer_value + self.num_bins_steer = ( + self.get_parameter("NUM_BINS_STEER").get_parameter_value().integer_value + ) + self.num_bins_a = self.get_parameter("NUM_BINS_A").get_parameter_value().integer_value + self.v_min, self.v_max = ( + self.get_parameter("V_MIN").get_parameter_value().double_value, + self.get_parameter("V_MAX").get_parameter_value().double_value, + ) + self.steer_min, self.steer_max = ( + self.get_parameter("STEER_MIN").get_parameter_value().double_value, + self.get_parameter("STEER_MAX").get_parameter_value().double_value, + ) + self.a_min, self.a_max = ( + self.get_parameter("A_MIN").get_parameter_value().double_value, + self.get_parameter("A_MAX").get_parameter_value().double_value, + ) + + self.collected_data_counts_of_vel_acc = np.zeros((self.num_bins_v, self.num_bins_a)) + self.collected_data_counts_of_vel_steer = np.zeros((self.num_bins_v, self.num_bins_steer)) + + self.v_bins = np.linspace(self.v_min, self.v_max, self.num_bins_v + 1) + self.steer_bins = np.linspace(self.steer_min, self.steer_max, self.num_bins_steer + 1) + self.a_bins = np.linspace(self.a_min, self.a_max, self.num_bins_a + 1) + + self.v_bin_centers = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + self.vel_hist = np.zeros(200) + self.acc_hist = np.zeros(200) + + self.vel_hist_for_plot = np.zeros(200) + self.acc_hist_for_plot = np.zeros(200) + + # create callback groups + self.callback_group = ReentrantCallbackGroup() + self.lock = threading.Lock() + + # callback for data count + self.timer_period_callback = 0.033 # 30ms + self.timer_counter = self.create_timer( + self.timer_period_callback, + self.timer_callback_counter, + callback_group=self.callback_group, + ) + + # callback for plot + self.grid_update_time_interval = 5.0 + self.timer_plotter = self.create_timer( + self.grid_update_time_interval, + self.timer_callback_plotter, + callback_group=self.callback_group, + ) + + self.fig, self.axs = plt.subplots(3, 1, figsize=(12, 20)) + plt.ion() + + self.collected_data_counts_of_vel_acc_for_plot = np.zeros( + (self.num_bins_v, self.num_bins_a) + ) + self.collected_data_counts_of_vel_steer_for_plot = np.zeros( + (self.num_bins_v, self.num_bins_steer) + ) + + self.v_bin_centers_for_plot = (self.v_bins[:-1] + self.v_bins[1:]) / 2 + self.steer_bin_centers_for_plot = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 + self.a_bin_centers_for_plot = (self.a_bins[:-1] + self.a_bins[1:]) / 2 + + def onOdometry(self, msg): + self._present_kinematic_state = msg + + def onAcceleration(self, msg): + self._present_acceleration = msg + + def count_observations(self, v, a, steer): + v_bin = np.digitize(v, self.v_bins) - 1 + steer_bin = np.digitize(steer, self.steer_bins) - 1 + a_bin = np.digitize(a, self.a_bins) - 1 + + if 0 <= v_bin < self.num_bins_v and 0 <= a_bin < self.num_bins_a: + self.collected_data_counts_of_vel_acc[v_bin, a_bin] += 1 + + if 0 <= v_bin < self.num_bins_v and 0 <= steer_bin < self.num_bins_steer: + self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 + + def timer_callback_plotter(self): + with self.lock: + self.collected_data_counts_of_vel_acc_for_plot = ( + self.collected_data_counts_of_vel_acc.copy() + ) + self.collected_data_counts_of_vel_steer_for_plot = ( + self.collected_data_counts_of_vel_steer.copy() + ) + + self.acc_hist_for_plot = self.acc_hist.copy() + self.vel_hist_for_plot = self.vel_hist.copy() + + self.v_bin_centers_for_plot = self.v_bin_centers + self.steer_bin_centers_for_plot = self.steer_bin_centers + self.a_bin_centers_for_plot = self.a_bin_centers + + self.plot_data_collection_grid() + plt.pause(0.01) + + def timer_callback_counter(self): + if self._present_kinematic_state is not None and self._present_acceleration is not None: + # calculate steer + angular_z = self._present_kinematic_state.twist.twist.angular.z + wheel_base = self.get_parameter("wheel_base").get_parameter_value().double_value + current_steer = arctan2( + wheel_base * angular_z, self._present_kinematic_state.twist.twist.linear.x + ) + + current_vel = self._present_kinematic_state.twist.twist.linear.x + current_acc = self._present_acceleration.accel.accel.linear.x + + # update velocity and acceleration bin if ego vehicle is moving + if self._present_kinematic_state.twist.twist.linear.x > 1e-3: + with self.lock: + self.count_observations( + current_vel, + current_acc, + current_steer, + ) + + self.acc_hist[:-1] = 1.0 * self.acc_hist[1:] + self.acc_hist[-1] = current_acc + self.vel_hist[:-1] = 1.0 * self.vel_hist[1:] + self.vel_hist[-1] = current_vel + + def plot_data_collection_grid(self): + self.axs[0].cla() + self.axs[0].scatter(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].plot(self.acc_hist_for_plot, self.vel_hist_for_plot) + self.axs[0].set_xlim([-2.0, 2.0]) + self.axs[0].set_ylim([0.0, self.v_max + 1.0]) + self.axs[0].set_xlabel("Acceleration") + self.axs[0].set_ylabel("Velocity") + + # update collected acceleration and velocity grid + for collection in self.axs[1].collections: + if collection.colorbar is not None: + collection.colorbar.remove() + self.axs[1].cla() + + self.heatmap = sns.heatmap( + self.collected_data_counts_of_vel_acc_for_plot.T, + annot=True, + cmap="coolwarm", + xticklabels=np.round(self.v_bin_centers_for_plot, 2), + yticklabels=np.round(self.a_bin_centers_for_plot, 2), + ax=self.axs[1], + linewidths=0.1, + linecolor="gray", + ) + + self.axs[1].set_xlabel("Velocity bins") + self.axs[1].set_ylabel("Acceleration bins") + + for collection in self.axs[2].collections: + if collection.colorbar is not None: + collection.colorbar.remove() + self.axs[2].cla() + + self.heatmap = sns.heatmap( + self.collected_data_counts_of_vel_steer_for_plot.T, + annot=True, + cmap="coolwarm", + xticklabels=np.round(self.v_bin_centers_for_plot, 2), + yticklabels=np.round(self.steer_bin_centers_for_plot, 2), + ax=self.axs[2], + linewidths=0.1, + linecolor="gray", + ) + + # update collected steer and velocity grid + self.axs[2].set_xlabel("Velocity bins") + self.axs[2].set_ylabel("Steer bins") + + self.fig.canvas.draw() + + plt.pause(0.01) + + +def main(args=None): + rclpy.init(args=args) + + data_collecting_plot = DataCollectingPlotter() + rclpy.spin(data_collecting_plot) + + data_collecting_plot.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py index 00af7fe8..d8c396ac 100755 --- a/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py +++ b/control_data_collecting_tool/scripts/data_collecting_trajectory_publisher.py @@ -14,9 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - -import threading - from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from geometry_msgs.msg import AccelWithCovarianceStamped @@ -31,7 +28,6 @@ from numpy import sin from rcl_interfaces.msg import ParameterDescriptor import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from scipy.spatial.transform import Rotation as R import seaborn as sns @@ -39,7 +35,6 @@ from visualization_msgs.msg import MarkerArray debug_matplotlib_plot_flag = False -data_counts_matplotlib_plot_flag = True Differential_Smoothing_Flag = True USE_CURVATURE_RADIUS_FLAG = False @@ -509,42 +504,12 @@ def __init__(self): self.vel_hist = np.zeros(200) self.acc_hist = np.zeros(200) - self.vel_hist_for_plot = np.zeros(200) - self.acc_hist_for_plot = np.zeros(200) - - self.callback_group = ReentrantCallbackGroup() - self.lock = threading.Lock() - self.timer_period_callback = 0.03 # 30ms self.traj_step = 0.1 - self.timer_traj = self.create_timer( - self.timer_period_callback, self.timer_callback_traj, callback_group=self.callback_group - ) - - if data_counts_matplotlib_plot_flag: - self.collected_data_counts_of_vel_acc_for_plot = np.zeros( - (self.num_bins_v, self.num_bins_a) - ) - self.collected_data_counts_of_vel_steer_for_plot = np.zeros( - (self.num_bins_v, self.num_bins_steer) - ) - - self.v_bin_centers_for_plot = (self.v_bins[:-1] + self.v_bins[1:]) / 2 - self.steer_bin_centers_for_plot = (self.steer_bins[:-1] + self.steer_bins[1:]) / 2 - self.a_bin_centers_for_plot = (self.a_bins[:-1] + self.a_bins[1:]) / 2 - - self.fig, self.axs = plt.subplots(3, 1, figsize=(12, 20)) - self.grid_update_time_interval = 2.0 - plt.ion() - - self.timer_plot = self.create_timer( - self.grid_update_time_interval, - self.timer_callback_plot, - callback_group=self.callback_group, - ) + self.timer_traj = self.create_timer(self.timer_period_callback, self.timer_callback_traj) if debug_matplotlib_plot_flag: - self.fig_debug, self.axs_debug = plt.subplots(figsize=(12, 6)) + self.fig, self.axs = plt.subplots(4, 1, figsize=(12, 20)) plt.ion() self._present_kinematic_state = None @@ -962,76 +927,52 @@ def count_observations(self, v, a, steer): self.collected_data_counts_of_vel_steer[v_bin, steer_bin] += 1 def plot_data_collection_grid(self): - self.axs[0].cla() - self.axs[0].scatter(self.acc_hist_for_plot, self.vel_hist_for_plot) - self.axs[0].plot(self.acc_hist_for_plot, self.vel_hist_for_plot) - self.axs[0].set_xlim([-2.0, 2.0]) - self.axs[0].set_ylim([0.0, self.v_max + 1.0]) - self.axs[0].set_xlabel("Acceleration") - self.axs[0].set_ylabel("Velocity") + self.axs[1].cla() + self.axs[1].scatter(self.acc_hist, self.vel_hist) + self.axs[1].plot(self.acc_hist, self.vel_hist) + self.axs[1].set_xlim([-2.0, 2.0]) + self.axs[1].set_ylim([0.0, self.v_max + 1.0]) + self.axs[1].set_xlabel("Acceleration") + self.axs[1].set_ylabel("Velocity") # update collected acceleration and velocity grid - for collection in self.axs[1].collections: + for collection in self.axs[2].collections: if collection.colorbar is not None: collection.colorbar.remove() - self.axs[1].cla() + self.axs[2].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_acc_for_plot.T, + self.collected_data_counts_of_vel_acc.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers_for_plot, 2), - yticklabels=np.round(self.a_bin_centers_for_plot, 2), - ax=self.axs[1], + xticklabels=np.round(self.v_bin_centers, 2), + yticklabels=np.round(self.a_bin_centers, 2), + ax=self.axs[2], linewidths=0.1, linecolor="gray", ) - self.axs[1].set_xlabel("Velocity bins") - self.axs[1].set_ylabel("Acceleration bins") + self.axs[2].set_xlabel("Velocity bins") + self.axs[2].set_ylabel("Acceleration bins") - for collection in self.axs[2].collections: + for collection in self.axs[3].collections: if collection.colorbar is not None: collection.colorbar.remove() - self.axs[2].cla() + self.axs[3].cla() self.heatmap = sns.heatmap( - self.collected_data_counts_of_vel_steer_for_plot.T, + self.collected_data_counts_of_vel_steer.T, annot=True, cmap="coolwarm", - xticklabels=np.round(self.v_bin_centers_for_plot, 2), - yticklabels=np.round(self.steer_bin_centers_for_plot, 2), - ax=self.axs[2], + xticklabels=np.round(self.v_bin_centers, 2), + yticklabels=np.round(self.steer_bin_centers, 2), + ax=self.axs[3], linewidths=0.1, linecolor="gray", ) - # update collected steer and velocity grid - self.axs[2].set_xlabel("Velocity bins") - self.axs[2].set_ylabel("Steer bins") - - self.fig.canvas.draw() - - plt.pause(0.005) - - def timer_callback_plot(self): - with self.lock: - self.collected_data_counts_of_vel_acc_for_plot = ( - self.collected_data_counts_of_vel_acc.copy() - ) - self.collected_data_counts_of_vel_steer_for_plot = ( - self.collected_data_counts_of_vel_steer.copy() - ) - - self.acc_hist_for_plot = self.acc_hist.copy() - self.vel_hist_for_plot = self.vel_hist.copy() - - self.v_bin_centers_for_plot = self.v_bin_centers - self.steer_bin_centers_for_plot = self.steer_bin_centers - self.a_bin_centers_for_plot = self.a_bin_centers - - self.plot_data_collection_grid() - plt.pause(0.01) + self.axs[3].set_xlabel("Velocity bins") + self.axs[3].set_ylabel("Steer bins") def timer_callback_traj(self): if ( @@ -1048,12 +989,11 @@ def timer_callback_traj(self): # update velocity and acceleration bin if ego vehicle is moving if self._present_kinematic_state.twist.twist.linear.x > 1e-3: - with self.lock: - self.count_observations( - self._present_kinematic_state.twist.twist.linear.x, - self._present_acceleration.accel.accel.linear.x, - steer, - ) + self.count_observations( + self._present_kinematic_state.twist.twist.linear.x, + self._present_acceleration.accel.accel.linear.x, + steer, + ) # [0] update nominal target trajectory if changing related ros2 params target_longitudinal_velocity = ( @@ -1165,8 +1105,7 @@ def timer_callback_traj(self): self.one_round_progress_rate = 1.0 * nearestIndex / len(trajectory_position_data) # set target velocity - with self.lock: - target_vel = self.get_target_velocity(nearestIndex) + target_vel = self.get_target_velocity(nearestIndex) trajectory_longitudinal_velocity_data = np.array( [target_vel for _ in range(len(trajectory_longitudinal_velocity_data))] @@ -1345,10 +1284,57 @@ def timer_callback_traj(self): marker_array.markers.append(marker_traj2) + # [6-2c] add arrow + marker_arrow = Marker() + marker_arrow.type = marker_arrow.ARROW + marker_arrow.id = 2 + marker_arrow.header.frame_id = "map" + + marker_arrow.action = marker_arrow.ADD + + marker_arrow.scale.x = 0.5 + marker_arrow.scale.y = 2.5 + marker_arrow.scale.z = 0.0 + + marker_arrow.color.a = 1.0 + marker_arrow.color.r = 1.0 + marker_arrow.color.g = 0.0 + marker_arrow.color.b = 1.0 + + marker_arrow.lifetime.nanosec = 500000000 + marker_arrow.frame_locked = True + + tangent_vec = np.array( + [ + tmp_traj.points[1].pose.position.x - tmp_traj.points[0].pose.position.x, + tmp_traj.points[1].pose.position.y - tmp_traj.points[0].pose.position.y, + ] + ) + + marker_arrow.points = [] + + start_marker_point = Point() + start_marker_point.x = tmp_traj.points[0].pose.position.x + start_marker_point.y = tmp_traj.points[0].pose.position.y + start_marker_point.z = 0.0 + marker_arrow.points.append(start_marker_point) + + end_marker_point = Point() + end_marker_point.x = tmp_traj.points[0].pose.position.x + 5.0 * tangent_vec[ + 0 + ] / np.linalg.norm(tangent_vec) + end_marker_point.y = tmp_traj.points[0].pose.position.y + 5.0 * tangent_vec[ + 1 + ] / np.linalg.norm(tangent_vec) + end_marker_point.z = 0.0 + marker_arrow.points.append(end_marker_point) + + marker_array.markers.append(marker_arrow) + self.data_collecting_trajectory_marker_array_pub_.publish(marker_array) if debug_matplotlib_plot_flag: - self.axs_debug.cla() + self.axs[0].cla() step_size_array = np.sqrt( ((trajectory_position_data[1:] - trajectory_position_data[:-1]) ** 2).sum( axis=1 @@ -1366,9 +1352,9 @@ def timer_callback_traj(self): timestamp[i] = timestamp[i - 1] + time_width_array[i - 1] timestamp -= timestamp[nearestIndex] - self.axs_debug.plot(0, present_linear_velocity[0], "o", label="current vel") + self.axs[0].plot(0, present_linear_velocity[0], "o", label="current vel") - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], trajectory_longitudinal_velocity_data_without_limit[ nearestIndex : nearestIndex + pub_traj_len @@ -1376,31 +1362,35 @@ def timer_callback_traj(self): "--", label="target vel before applying limit", ) - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], lateral_acc_limit[nearestIndex : nearestIndex + pub_traj_len], "--", label="lateral acc limit (always)", ) - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], velocity_limit_by_tracking_error * np.ones(pub_traj_len), "--", label="vel limit by tracking error (only when exceeding threshold)", ) - self.axs_debug.plot( + self.axs[0].plot( timestamp[nearestIndex : nearestIndex + pub_traj_len], trajectory_longitudinal_velocity_data[ nearestIndex : nearestIndex + pub_traj_len ], label="actual target vel", ) - self.axs_debug.set_xlim([-0.5, 10.5]) - self.axs_debug.set_ylim([-0.5, 12.5]) + self.axs[0].set_xlim([-0.5, 10.5]) + self.axs[0].set_ylim([-0.5, 12.5]) + + self.axs[0].set_xlabel("future timestamp [s]") + self.axs[0].set_ylabel("longitudinal_velocity [m/s]") + self.axs[0].legend(fontsize=8) + + self.plot_data_collection_grid() - self.axs_debug.set_xlabel("future timestamp [s]") - self.axs_debug.set_ylabel("longitudinal_velocity [m/s]") - self.axs_debug.legend(fontsize=8) + self.fig.canvas.draw() plt.pause(0.01)