Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(tier4_adapi_rviz_plugin): add change button to the route panel #6326

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
117 changes: 99 additions & 18 deletions common/tier4_adapi_rviz_plugin/src/route_panel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -28,6 +30,11 @@
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");

Check warning on line 37 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L33-L37

Added lines #L33 - L37 were not covered by tests
allow_goal_modification_ = new QCheckBox("allow goal modification");

waypoints_mode_->setCheckable(true);
Expand All @@ -37,6 +44,22 @@
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);

Check warning on line 53 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L47-L53

Added lines #L47 - L53 were not covered by tests

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; });

Check warning on line 57 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L55-L57

Added lines #L55 - L57 were not covered by tests

adapi_auto_clear_->setChecked(true);
adapi_set_->setChecked(true);
adapi_mode_ = AdapiMode::Set;

Check warning on line 61 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L59-L61

Added lines #L59 - L61 were not covered by tests

const auto layout = new QVBoxLayout();
setLayout(layout);

Expand All @@ -52,6 +75,19 @@
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);

Check warning on line 88 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L80-L88

Added lines #L80 - L88 were not covered by tests
}

// options group
{
const auto group = new QGroupBox("options");
Expand All @@ -73,25 +109,14 @@

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_);

Check warning on line 113 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L112-L113

Added lines #L112 - L113 were not covered by tests
}

void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg)
{
if (!waypoints_mode_->isChecked()) {
setRoute(*msg);
requestRoute(*msg);

Check warning on line 119 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L119

Added line #L119 was not covered by tests
} else {
waypoints_.push_back(*msg);
waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
Expand Down Expand Up @@ -120,18 +145,74 @@
{
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] {

Check warning on line 148 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L148

Added line #L148 was not covered by tests
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
req->header = waypoints_.back().header;
req->goal = waypoints_.back().pose;
for (size_t i = 0; i + 1 < waypoints_.size(); ++i) {
req->waypoints.push_back(waypoints_[i].pose);
}
req->option.allow_goal_modification = allow_goal_modification_->isChecked();
cli_route_->async_send_request(req);
asyncSendRequest(req);

Check warning on line 156 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L156

Added line #L156 was not covered by tests
onWaypointsReset();
});
};

Check warning on line 158 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L158

Added line #L158 was not covered by tests

if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) {

Check warning on line 160 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L160

Added line #L160 was not covered by tests
const auto req = std::make_shared<ClearRoute::Service::Request>();
cli_clear_->async_send_request(req, [call](auto) { call(); });

Check warning on line 162 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L162

Added line #L162 was not covered by tests
} else {
call();

Check warning on line 164 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L164

Added line #L164 was not covered by tests
}
}

void RoutePanel::requestRoute(const PoseStamped & pose)

Check warning on line 168 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L168

Added line #L168 was not covered by tests
{
const auto call = [this, pose] {

Check warning on line 170 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L170

Added line #L170 was not covered by tests
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);
};

Check warning on line 176 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L173-L176

Added lines #L173 - L176 were not covered by tests

if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) {

Check warning on line 178 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L178

Added line #L178 was not covered by tests
const auto req = std::make_shared<ClearRoute::Service::Request>();
cli_clear_->async_send_request(req, [call](auto) { call(); });

Check warning on line 180 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L180

Added line #L180 was not covered by tests
} else {
call();

Check warning on line 182 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L182

Added line #L182 was not covered by tests
}
}

Check warning on line 184 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L184

Added line #L184 was not covered by tests

void RoutePanel::clearRoute()

Check warning on line 186 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L186

Added line #L186 was not covered by tests
{
const auto callback = [this](auto future) {

Check warning on line 188 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L188

Added line #L188 was not covered by tests
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));
};

Check warning on line 194 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L190-L194

Added lines #L190 - L194 were not covered by tests

const auto req = std::make_shared<ClearRoute::Service::Request>();
cli_clear_->async_send_request(req, callback);
}

Check warning on line 198 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L197-L198

Added lines #L197 - L198 were not covered by tests

void RoutePanel::asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req)

Check warning on line 200 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L200

Added line #L200 was not covered by tests
{
const auto callback = [this](auto future) {

Check warning on line 202 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L202

Added line #L202 was not covered by tests
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));
};

Check warning on line 208 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L204-L208

Added lines #L204 - L208 were not covered by tests

if (adapi_mode_ == AdapiMode::Set) {
cli_set_->async_send_request(req, callback);

Check warning on line 211 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L210-L211

Added lines #L210 - L211 were not covered by tests
}
if (adapi_mode_ == AdapiMode::Change) {
cli_change_->async_send_request(req, callback);

Check warning on line 214 in common/tier4_adapi_rviz_plugin/src/route_panel.cpp

View check run for this annotation

Codecov / codecov/patch

common/tier4_adapi_rviz_plugin/src/route_panel.cpp#L213-L214

Added lines #L213 - L214 were not covered by tests
}
}

} // namespace tier4_adapi_rviz_plugins
Expand Down
18 changes: 16 additions & 2 deletions common/tier4_adapi_rviz_plugin/src/route_panel.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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:
Expand All @@ -45,18 +48,29 @@ 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_;

rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_;
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();
Expand Down
Loading