14
14
15
15
#include " route_panel.hpp"
16
16
17
+ #include < QGridLayout>
17
18
#include < QHBoxLayout>
18
19
#include < QVBoxLayout>
19
20
#include < rviz_common/display_context.hpp>
20
21
21
22
#include < memory>
23
+ #include < string>
22
24
23
25
namespace tier4_adapi_rviz_plugins
24
26
{
@@ -28,6 +30,11 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
28
30
waypoints_mode_ = new QPushButton (" mode" );
29
31
waypoints_reset_ = new QPushButton (" reset" );
30
32
waypoints_apply_ = new QPushButton (" apply" );
33
+ adapi_clear_ = new QPushButton (" clear" );
34
+ adapi_set_ = new QPushButton (" set" );
35
+ adapi_change_ = new QPushButton (" change" );
36
+ adapi_response_ = new QLabel (" the response will be displayed here" );
37
+ adapi_auto_clear_ = new QCheckBox (" auto clear" );
31
38
allow_goal_modification_ = new QCheckBox (" allow goal modification" );
32
39
33
40
waypoints_mode_->setCheckable (true );
@@ -37,6 +44,22 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
37
44
connect (waypoints_reset_, &QPushButton::clicked, this , &RoutePanel::onWaypointsReset);
38
45
connect (waypoints_apply_, &QPushButton::clicked, this , &RoutePanel::onWaypointsApply);
39
46
47
+ const auto buttons = new QButtonGroup (this );
48
+ buttons->addButton (adapi_set_);
49
+ buttons->addButton (adapi_change_);
50
+ buttons->setExclusive (true );
51
+ adapi_set_->setCheckable (true );
52
+ adapi_change_->setCheckable (true );
53
+ adapi_response_->setAlignment (Qt::AlignCenter);
54
+
55
+ connect (adapi_clear_, &QPushButton::clicked, this , &RoutePanel::clearRoute);
56
+ connect (adapi_set_, &QPushButton::clicked, [this ] { adapi_mode_ = AdapiMode::Set; });
57
+ connect (adapi_change_, &QPushButton::clicked, [this ] { adapi_mode_ = AdapiMode::Change; });
58
+
59
+ adapi_auto_clear_->setChecked (true );
60
+ adapi_set_->setChecked (true );
61
+ adapi_mode_ = AdapiMode::Set;
62
+
40
63
const auto layout = new QVBoxLayout ();
41
64
setLayout (layout);
42
65
@@ -52,6 +75,19 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
52
75
waypoints_group_ = group;
53
76
}
54
77
78
+ // adapi group
79
+ {
80
+ const auto group = new QGroupBox (" adapi" );
81
+ const auto local = new QGridLayout ();
82
+ local->addWidget (adapi_clear_, 0 , 0 );
83
+ local->addWidget (adapi_set_, 0 , 1 );
84
+ local->addWidget (adapi_change_, 0 , 2 );
85
+ local->addWidget (adapi_auto_clear_, 1 , 0 );
86
+ local->addWidget (adapi_response_, 1 , 1 , 1 , 2 );
87
+ group->setLayout (local);
88
+ layout->addWidget (group);
89
+ }
90
+
55
91
// options group
56
92
{
57
93
const auto group = new QGroupBox (" options" );
@@ -73,25 +109,14 @@ void RoutePanel::onInitialize()
73
109
74
110
const auto adaptor = component_interface_utils::NodeAdaptor (node.get ());
75
111
adaptor.init_cli (cli_clear_);
76
- adaptor.init_cli (cli_route_);
77
- }
78
-
79
- void RoutePanel::setRoute (const PoseStamped & pose)
80
- {
81
- const auto req = std::make_shared<ClearRoute::Service::Request>();
82
- cli_clear_->async_send_request (req, [this , pose](auto ) {
83
- const auto req = std::make_shared<SetRoutePoints::Service::Request>();
84
- req->header = pose.header ;
85
- req->goal = pose.pose ;
86
- req->option .allow_goal_modification = allow_goal_modification_->isChecked ();
87
- cli_route_->async_send_request (req);
88
- });
112
+ adaptor.init_cli (cli_set_);
113
+ adaptor.init_cli (cli_change_);
89
114
}
90
115
91
116
void RoutePanel::onPose (const PoseStamped::ConstSharedPtr msg)
92
117
{
93
118
if (!waypoints_mode_->isChecked ()) {
94
- setRoute (*msg);
119
+ requestRoute (*msg);
95
120
} else {
96
121
waypoints_.push_back (*msg);
97
122
waypoints_group_->setTitle (QString (" waypoints (count: %1)" ).arg (waypoints_.size ()));
@@ -120,18 +145,74 @@ void RoutePanel::onWaypointsApply()
120
145
{
121
146
if (waypoints_.empty ()) return ;
122
147
123
- const auto req = std::make_shared<ClearRoute::Service::Request>();
124
- cli_clear_->async_send_request (req, [this ](auto ) {
148
+ const auto call = [this ] {
125
149
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
126
150
req->header = waypoints_.back ().header ;
127
151
req->goal = waypoints_.back ().pose ;
128
152
for (size_t i = 0 ; i + 1 < waypoints_.size (); ++i) {
129
153
req->waypoints .push_back (waypoints_[i].pose );
130
154
}
131
155
req->option .allow_goal_modification = allow_goal_modification_->isChecked ();
132
- cli_route_-> async_send_request (req);
156
+ asyncSendRequest (req);
133
157
onWaypointsReset ();
134
- });
158
+ };
159
+
160
+ if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked ()) {
161
+ const auto req = std::make_shared<ClearRoute::Service::Request>();
162
+ cli_clear_->async_send_request (req, [call](auto ) { call (); });
163
+ } else {
164
+ call ();
165
+ }
166
+ }
167
+
168
+ void RoutePanel::requestRoute (const PoseStamped & pose)
169
+ {
170
+ const auto call = [this , pose] {
171
+ const auto req = std::make_shared<SetRoutePoints::Service::Request>();
172
+ req->header = pose.header ;
173
+ req->goal = pose.pose ;
174
+ req->option .allow_goal_modification = allow_goal_modification_->isChecked ();
175
+ asyncSendRequest (req);
176
+ };
177
+
178
+ if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked ()) {
179
+ const auto req = std::make_shared<ClearRoute::Service::Request>();
180
+ cli_clear_->async_send_request (req, [call](auto ) { call (); });
181
+ } else {
182
+ call ();
183
+ }
184
+ }
185
+
186
+ void RoutePanel::clearRoute ()
187
+ {
188
+ const auto callback = [this ](auto future) {
189
+ const auto status = future.get ()->status ;
190
+ std::string text = status.success ? " OK" : " Error" ;
191
+ text += " [" + std::to_string (status.code ) + " ]" ;
192
+ text += " " + status.message ;
193
+ adapi_response_->setText (QString::fromStdString (text));
194
+ };
195
+
196
+ const auto req = std::make_shared<ClearRoute::Service::Request>();
197
+ cli_clear_->async_send_request (req, callback);
198
+ }
199
+
200
+ void RoutePanel::asyncSendRequest (SetRoutePoints::Service::Request::SharedPtr req)
201
+ {
202
+ const auto callback = [this ](auto future) {
203
+ const auto status = future.get ()->status ;
204
+ std::string text = status.success ? " OK" : " Error" ;
205
+ text += " [" + std::to_string (status.code ) + " ]" ;
206
+ text += " " + status.message ;
207
+ adapi_response_->setText (QString::fromStdString (text));
208
+ };
209
+
210
+ if (adapi_mode_ == AdapiMode::Set) {
211
+ cli_set_->async_send_request (req, callback);
212
+ }
213
+ if (adapi_mode_ == AdapiMode::Change) {
214
+ cli_change_->async_send_request (req, callback);
215
+ }
135
216
}
136
217
137
218
} // namespace tier4_adapi_rviz_plugins
0 commit comments