Merge pull request #1293 from tier4/backport/6992

chore: backport autowarefoundation#6973 and autowarefoundation#6992

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<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
-    "/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<std_msgs::msg::Bool>(
-    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<std_msgs::msg::Bool>(
+      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<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
+      service_edit_->text().toStdString());
+  } catch (const rclcpp::exceptions::InvalidServiceNameError & e) {
+    RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
+  }
+}
+
 void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton()
 {
   // lock button

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_;
 };

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]

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;
       }