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