@@ -34,13 +34,19 @@ namespace tier4_calibration_rviz_plugin
34
34
AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel (QWidget * parent)
35
35
: rviz_common::Panel(parent)
36
36
{
37
- topic_label_ = new QLabel (" Topic name of update suggest " );
37
+ topic_label_ = new QLabel (" topic: " );
38
38
topic_label_->setAlignment (Qt::AlignCenter);
39
39
40
40
topic_edit_ =
41
41
new QLineEdit (" /vehicle/calibration/accel_brake_map_calibrator/output/update_suggest" );
42
42
connect (topic_edit_, SIGNAL (textEdited (QString)), SLOT (editTopic ()));
43
43
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
+
44
50
calibration_button_ = new QPushButton (" Wait for subscribe topic" );
45
51
calibration_button_->setEnabled (false );
46
52
connect (calibration_button_, SIGNAL (clicked (bool )), SLOT (pushCalibrationButton ()));
@@ -56,8 +62,13 @@ AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget *
56
62
topic_layout->addWidget (topic_label_);
57
63
topic_layout->addWidget (topic_edit_);
58
64
65
+ auto * service_layout = new QHBoxLayout;
66
+ service_layout->addWidget (service_label_);
67
+ service_layout->addWidget (service_edit_);
68
+
59
69
auto * v_layout = new QVBoxLayout;
60
70
v_layout->addLayout (topic_layout);
71
+ v_layout->addLayout (service_layout);
61
72
v_layout->addWidget (calibration_button_);
62
73
v_layout->addWidget (status_label_);
63
74
@@ -75,7 +86,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize()
75
86
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this , std::placeholders::_1));
76
87
77
88
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 () );
79
90
}
80
91
81
92
void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest (
@@ -85,6 +96,12 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
85
96
return ;
86
97
}
87
98
99
+ if (!client_ || !client_->service_is_ready ()) {
100
+ calibration_button_->setText (" wait for service" );
101
+ calibration_button_->setEnabled (false );
102
+ return ;
103
+ }
104
+
88
105
if (msg->data ) {
89
106
status_label_->setText (" Ready" );
90
107
status_label_->setStyleSheet (" QLabel { background-color : white;}" );
@@ -98,17 +115,34 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
98
115
99
116
void AccelBrakeMapCalibratorButtonPanel::editTopic ()
100
117
{
101
- update_suggest_sub_.reset ();
102
118
rclcpp::Node::SharedPtr raw_node =
103
119
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
+ }
108
129
calibration_button_->setText (" Wait for subscribe topic" );
109
130
calibration_button_->setEnabled (false );
110
131
}
111
132
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
+
112
146
void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton ()
113
147
{
114
148
// lock button
0 commit comments