Skip to content

Commit c6354d1

Browse files
committed
fix(accel_brake_calibrator): fix to set service name and exception failure (autowarefoundation#6973)
* add service * fix exception * fix style
1 parent bd81d4c commit c6354d1

File tree

2 files changed

+44
-7
lines changed

2 files changed

+44
-7
lines changed

common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp

+41-7
Original file line numberDiff line numberDiff line change
@@ -34,13 +34,19 @@ namespace tier4_calibration_rviz_plugin
3434
AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * parent)
3535
: rviz_common::Panel(parent)
3636
{
37-
topic_label_ = new QLabel("Topic name of update suggest ");
37+
topic_label_ = new QLabel("topic: ");
3838
topic_label_->setAlignment(Qt::AlignCenter);
3939

4040
topic_edit_ =
4141
new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/output/update_suggest");
4242
connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic()));
4343

44+
service_label_ = new QLabel("service: ");
45+
service_label_->setAlignment(Qt::AlignCenter);
46+
47+
service_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/update_map_dir");
48+
connect(service_edit_, SIGNAL(textEdited(QString)), SLOT(editService()));
49+
4450
calibration_button_ = new QPushButton("Wait for subscribe topic");
4551
calibration_button_->setEnabled(false);
4652
connect(calibration_button_, SIGNAL(clicked(bool)), SLOT(pushCalibrationButton()));
@@ -56,8 +62,13 @@ AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget *
5662
topic_layout->addWidget(topic_label_);
5763
topic_layout->addWidget(topic_edit_);
5864

65+
auto * service_layout = new QHBoxLayout;
66+
service_layout->addWidget(service_label_);
67+
service_layout->addWidget(service_edit_);
68+
5969
auto * v_layout = new QVBoxLayout;
6070
v_layout->addLayout(topic_layout);
71+
v_layout->addLayout(service_layout);
6172
v_layout->addWidget(calibration_button_);
6273
v_layout->addWidget(status_label_);
6374

@@ -75,7 +86,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize()
7586
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
7687

7788
client_ = raw_node->create_client<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
78-
"/vehicle/calibration/accel_brake_map_calibrator/update_map_dir");
89+
service_edit_->text().toStdString());
7990
}
8091

8192
void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
@@ -85,6 +96,12 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
8596
return;
8697
}
8798

99+
if (!client_ || !client_->service_is_ready()) {
100+
calibration_button_->setText("wait for service");
101+
calibration_button_->setEnabled(false);
102+
return;
103+
}
104+
88105
if (msg->data) {
89106
status_label_->setText("Ready");
90107
status_label_->setStyleSheet("QLabel { background-color : white;}");
@@ -98,17 +115,34 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
98115

99116
void AccelBrakeMapCalibratorButtonPanel::editTopic()
100117
{
101-
update_suggest_sub_.reset();
102118
rclcpp::Node::SharedPtr raw_node =
103119
this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
104-
update_suggest_sub_ = raw_node->create_subscription<std_msgs::msg::Bool>(
105-
topic_edit_->text().toStdString(), 10,
106-
std::bind(
107-
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
120+
try {
121+
update_suggest_sub_.reset();
122+
update_suggest_sub_ = raw_node->create_subscription<std_msgs::msg::Bool>(
123+
topic_edit_->text().toStdString(), 10,
124+
std::bind(
125+
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
126+
} catch (const rclcpp::exceptions::InvalidTopicNameError & e) {
127+
RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
128+
}
108129
calibration_button_->setText("Wait for subscribe topic");
109130
calibration_button_->setEnabled(false);
110131
}
111132

133+
void AccelBrakeMapCalibratorButtonPanel::editService()
134+
{
135+
rclcpp::Node::SharedPtr raw_node =
136+
this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
137+
try {
138+
client_.reset();
139+
client_ = raw_node->create_client<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
140+
service_edit_->text().toStdString());
141+
} catch (const rclcpp::exceptions::InvalidServiceNameError & e) {
142+
RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
143+
}
144+
}
145+
112146
void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton()
113147
{
114148
// lock button

common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class AccelBrakeMapCalibratorButtonPanel : public rviz_common::Panel
4646

4747
public Q_SLOTS: // NOLINT for Qt
4848
void editTopic();
49+
void editService();
4950
void pushCalibrationButton();
5051

5152
protected:
@@ -56,6 +57,8 @@ public Q_SLOTS: // NOLINT for Qt
5657

5758
QLabel * topic_label_;
5859
QLineEdit * topic_edit_;
60+
QLabel * service_label_;
61+
QLineEdit * service_edit_;
5962
QPushButton * calibration_button_;
6063
QLabel * status_label_;
6164
};

0 commit comments

Comments
 (0)