From 7927e34fe1513b35a165d90b110be5a5830e2471 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 5 Feb 2024 19:13:38 +0900 Subject: [PATCH] feat(tier4_adapi_rviz_plugin): add route change button to the route panel Signed-off-by: Takagi, Isamu --- .../src/route_panel.cpp | 117 +++++++++++++++--- .../src/route_panel.hpp | 18 ++- 2 files changed, 115 insertions(+), 20 deletions(-) diff --git a/common/tier4_adapi_rviz_plugin/src/route_panel.cpp b/common/tier4_adapi_rviz_plugin/src/route_panel.cpp index 3e99761fd9fb3..5bdee028bf4b3 100644 --- a/common/tier4_adapi_rviz_plugin/src/route_panel.cpp +++ b/common/tier4_adapi_rviz_plugin/src/route_panel.cpp @@ -14,11 +14,13 @@ #include "route_panel.hpp" +#include #include #include #include #include +#include namespace tier4_adapi_rviz_plugins { @@ -28,6 +30,11 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) waypoints_mode_ = new QPushButton("mode"); waypoints_reset_ = new QPushButton("reset"); waypoints_apply_ = new QPushButton("apply"); + adapi_clear_ = new QPushButton("clear"); + adapi_set_ = new QPushButton("set"); + adapi_change_ = new QPushButton("change"); + adapi_response_ = new QLabel("the response will be displayed here"); + adapi_auto_clear_ = new QCheckBox("auto clear"); allow_goal_modification_ = new QCheckBox("allow goal modification"); waypoints_mode_->setCheckable(true); @@ -37,6 +44,22 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset); connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::onWaypointsApply); + const auto buttons = new QButtonGroup(this); + buttons->addButton(adapi_set_); + buttons->addButton(adapi_change_); + buttons->setExclusive(true); + adapi_set_->setCheckable(true); + adapi_change_->setCheckable(true); + adapi_response_->setAlignment(Qt::AlignCenter); + + connect(adapi_clear_, &QPushButton::clicked, this, &RoutePanel::clearRoute); + connect(adapi_set_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Set; }); + connect(adapi_change_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Change; }); + + adapi_auto_clear_->setChecked(true); + adapi_set_->setChecked(true); + adapi_mode_ = AdapiMode::Set; + const auto layout = new QVBoxLayout(); setLayout(layout); @@ -52,6 +75,19 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) waypoints_group_ = group; } + // adapi group + { + const auto group = new QGroupBox("adapi"); + const auto local = new QGridLayout(); + local->addWidget(adapi_clear_, 0, 0); + local->addWidget(adapi_set_, 0, 1); + local->addWidget(adapi_change_, 0, 2); + local->addWidget(adapi_auto_clear_, 1, 0); + local->addWidget(adapi_response_, 1, 1, 1, 2); + group->setLayout(local); + layout->addWidget(group); + } + // options group { const auto group = new QGroupBox("options"); @@ -73,25 +109,14 @@ void RoutePanel::onInitialize() const auto adaptor = component_interface_utils::NodeAdaptor(node.get()); adaptor.init_cli(cli_clear_); - adaptor.init_cli(cli_route_); -} - -void RoutePanel::setRoute(const PoseStamped & pose) -{ - const auto req = std::make_shared(); - cli_clear_->async_send_request(req, [this, pose](auto) { - const auto req = std::make_shared(); - req->header = pose.header; - req->goal = pose.pose; - req->option.allow_goal_modification = allow_goal_modification_->isChecked(); - cli_route_->async_send_request(req); - }); + adaptor.init_cli(cli_set_); + adaptor.init_cli(cli_change_); } void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg) { if (!waypoints_mode_->isChecked()) { - setRoute(*msg); + requestRoute(*msg); } else { waypoints_.push_back(*msg); waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size())); @@ -120,8 +145,7 @@ void RoutePanel::onWaypointsApply() { if (waypoints_.empty()) return; - const auto req = std::make_shared(); - cli_clear_->async_send_request(req, [this](auto) { + const auto call = [this] { const auto req = std::make_shared(); req->header = waypoints_.back().header; req->goal = waypoints_.back().pose; @@ -129,9 +153,66 @@ void RoutePanel::onWaypointsApply() req->waypoints.push_back(waypoints_[i].pose); } req->option.allow_goal_modification = allow_goal_modification_->isChecked(); - cli_route_->async_send_request(req); + asyncSendRequest(req); onWaypointsReset(); - }); + }; + + if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) { + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, [call](auto) { call(); }); + } else { + call(); + } +} + +void RoutePanel::requestRoute(const PoseStamped & pose) +{ + const auto call = [this, pose] { + const auto req = std::make_shared(); + req->header = pose.header; + req->goal = pose.pose; + req->option.allow_goal_modification = allow_goal_modification_->isChecked(); + asyncSendRequest(req); + }; + + if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) { + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, [call](auto) { call(); }); + } else { + call(); + } +} + +void RoutePanel::clearRoute() +{ + const auto callback = [this](auto future) { + const auto status = future.get()->status; + std::string text = status.success ? "OK" : "Error"; + text += "[" + std::to_string(status.code) + "]"; + text += " " + status.message; + adapi_response_->setText(QString::fromStdString(text)); + }; + + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, callback); +} + +void RoutePanel::asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req) +{ + const auto callback = [this](auto future) { + const auto status = future.get()->status; + std::string text = status.success ? "OK" : "Error"; + text += "[" + std::to_string(status.code) + "]"; + text += " " + status.message; + adapi_response_->setText(QString::fromStdString(text)); + }; + + if (adapi_mode_ == AdapiMode::Set) { + cli_set_->async_send_request(req, callback); + } + if (adapi_mode_ == AdapiMode::Change) { + cli_change_->async_send_request(req, callback); + } } } // namespace tier4_adapi_rviz_plugins diff --git a/common/tier4_adapi_rviz_plugin/src/route_panel.hpp b/common/tier4_adapi_rviz_plugin/src/route_panel.hpp index f7c953d65dd2d..47855d243a564 100644 --- a/common/tier4_adapi_rviz_plugin/src/route_panel.hpp +++ b/common/tier4_adapi_rviz_plugin/src/route_panel.hpp @@ -15,8 +15,10 @@ #ifndef ROUTE_PANEL_HPP_ #define ROUTE_PANEL_HPP_ +#include #include #include +#include #include #include #include @@ -35,6 +37,7 @@ class RoutePanel : public rviz_common::Panel Q_OBJECT using ClearRoute = autoware_ad_api::routing::ClearRoute; using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints; + using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints; using PoseStamped = geometry_msgs::msg::PoseStamped; public: @@ -45,6 +48,11 @@ class RoutePanel : public rviz_common::Panel QPushButton * waypoints_mode_; QPushButton * waypoints_reset_; QPushButton * waypoints_apply_; + QPushButton * adapi_clear_; + QPushButton * adapi_set_; + QPushButton * adapi_change_; + QLabel * adapi_response_; + QCheckBox * adapi_auto_clear_; QGroupBox * waypoints_group_; QCheckBox * allow_goal_modification_; @@ -52,11 +60,17 @@ class RoutePanel : public rviz_common::Panel std::vector waypoints_; void onPose(const PoseStamped::ConstSharedPtr msg); + enum AdapiMode { Set, Change }; + AdapiMode adapi_mode_; + component_interface_utils::Client::SharedPtr cli_clear_; - component_interface_utils::Client::SharedPtr cli_route_; - void setRoute(const PoseStamped & pose); + component_interface_utils::Client::SharedPtr cli_set_; + component_interface_utils::Client::SharedPtr cli_change_; + void requestRoute(const PoseStamped & pose); + void asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req); private slots: + void clearRoute(); void onWaypointsMode(bool clicked); void onWaypointsReset(); void onWaypointsApply();