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 <QGridLayout>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <rviz_common/display_context.hpp>
#include <memory>
+#include <string>
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<ClearRoute::Service::Request>();
- cli_clear_->async_send_request(req, [this, pose](auto) {
- const auto req = std::make_shared<SetRoutePoints::Service::Request>();
- 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<ClearRoute::Service::Request>();
- cli_clear_->async_send_request(req, [this](auto) {
+ const auto call = [this] {
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
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<ClearRoute::Service::Request>();
+ 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<SetRoutePoints::Service::Request>();
+ 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<ClearRoute::Service::Request>();
+ 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<ClearRoute::Service::Request>();
+ 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 <QButtonGroup>
#include <QCheckBox>
#include <QGroupBox>
+#include <QLabel>
#include <QPushButton>
#include <autoware_ad_api_specs/routing.hpp>
#include <component_interface_utils/rclcpp.hpp>
@@ -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<PoseStamped> waypoints_;
void onPose(const PoseStamped::ConstSharedPtr msg);
+ enum AdapiMode { Set, Change };
+ AdapiMode adapi_mode_;
+
component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
- component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
- void setRoute(const PoseStamped & pose);
+ component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_set_;
+ component_interface_utils::Client<ChangeRoutePoints>::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();