fix(accel_brake_calibrator): fix to set service name and exception fa…

…ilure (autowarefoundation#6973) * add service * fix exception * fix style
zulfaqar-azmi-t4 · May 13, 2024 · bffbcce · bffbcce
1 parent fd6306d
commit bffbcce
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 7 deletions.
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
@@ -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

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