diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp index 5c1c98488b0f6..d89f13ce74d02 100644 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp +++ b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp @@ -34,13 +34,19 @@ namespace tier4_calibration_rviz_plugin AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * parent) : rviz_common::Panel(parent) { - topic_label_ = new QLabel("Topic name of update suggest "); + topic_label_ = new QLabel("topic: "); topic_label_->setAlignment(Qt::AlignCenter); topic_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/output/update_suggest"); connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); + service_label_ = new QLabel("service: "); + service_label_->setAlignment(Qt::AlignCenter); + + service_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/update_map_dir"); + connect(service_edit_, SIGNAL(textEdited(QString)), SLOT(editService())); + calibration_button_ = new QPushButton("Wait for subscribe topic"); calibration_button_->setEnabled(false); connect(calibration_button_, SIGNAL(clicked(bool)), SLOT(pushCalibrationButton())); @@ -56,8 +62,13 @@ AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * topic_layout->addWidget(topic_label_); topic_layout->addWidget(topic_edit_); + auto * service_layout = new QHBoxLayout; + service_layout->addWidget(service_label_); + service_layout->addWidget(service_edit_); + auto * v_layout = new QVBoxLayout; v_layout->addLayout(topic_layout); + v_layout->addLayout(service_layout); v_layout->addWidget(calibration_button_); v_layout->addWidget(status_label_); @@ -75,7 +86,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize() &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); client_ = raw_node->create_client( - "/vehicle/calibration/accel_brake_map_calibrator/update_map_dir"); + service_edit_->text().toStdString()); } void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( @@ -85,6 +96,12 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( return; } + if (!client_ || !client_->service_is_ready()) { + calibration_button_->setText("wait for service"); + calibration_button_->setEnabled(false); + return; + } + if (msg->data) { status_label_->setText("Ready"); status_label_->setStyleSheet("QLabel { background-color : white;}"); @@ -98,17 +115,34 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( void AccelBrakeMapCalibratorButtonPanel::editTopic() { - update_suggest_sub_.reset(); rclcpp::Node::SharedPtr raw_node = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - update_suggest_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind( - &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); + try { + update_suggest_sub_.reset(); + update_suggest_sub_ = raw_node->create_subscription( + topic_edit_->text().toStdString(), 10, + std::bind( + &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); + } catch (const rclcpp::exceptions::InvalidTopicNameError & e) { + RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what()); + } calibration_button_->setText("Wait for subscribe topic"); calibration_button_->setEnabled(false); } +void AccelBrakeMapCalibratorButtonPanel::editService() +{ + rclcpp::Node::SharedPtr raw_node = + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + try { + client_.reset(); + client_ = raw_node->create_client( + service_edit_->text().toStdString()); + } catch (const rclcpp::exceptions::InvalidServiceNameError & e) { + RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what()); + } +} + void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton() { // lock button diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp index e26789c9f120f..70ebe0631fa21 100644 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp +++ b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp @@ -46,6 +46,7 @@ class AccelBrakeMapCalibratorButtonPanel : public rviz_common::Panel public Q_SLOTS: // NOLINT for Qt void editTopic(); + void editService(); void pushCalibrationButton(); protected: @@ -56,6 +57,8 @@ public Q_SLOTS: // NOLINT for Qt QLabel * topic_label_; QLineEdit * topic_edit_; + QLabel * service_label_; + QLineEdit * service_edit_; QPushButton * calibration_button_; QLabel * status_label_; }; diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index 5ded8d2ea409c..7e8e0a18b4b7c 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -146,7 +146,7 @@ def toc(self, name): time.perf_counter() - self.start_times[name] ) * 1000 # Convert to milliseconds if self.verbose: - print(f"Time for {name}: {elapsed_time:.2f} ms") + print(f"Time for {name}: {elapsed_time: .2f} ms") # Reset the starting time for the name del self.start_times[name] diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index ecfc312d2bdfd..37f0a11165df3 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -703,17 +703,19 @@ void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() { const double bit = std::pow(1e-01, precision_); for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { + for (std::size_t vel_idx = update_accel_map_value_.at(0).size() - 1;; vel_idx--) { + if (vel_idx == 0) break; + const double current_acc = update_accel_map_value_.at(ped_idx).at(vel_idx); const double next_ped_acc = update_accel_map_value_.at(ped_idx + 1).at(vel_idx); - const double next_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx + 1); + const double prev_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx - 1); - if (current_acc <= next_vel_acc) { + if (current_acc + bit >= prev_vel_acc) { // the higher the velocity, the lower the acceleration - update_accel_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; + update_accel_map_value_.at(ped_idx).at(vel_idx - 1) = current_acc + bit; } - if (current_acc >= next_ped_acc) { + if (current_acc + bit >= next_ped_acc) { // the higher the accel pedal, the higher the acceleration update_accel_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc + bit; }