Skip to content

Commit 9fabcd1

Browse files
isamu-takagiisamu-takagi
committed
WIP
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 16370ca commit 9fabcd1

File tree

2 files changed

+207
-0
lines changed

2 files changed

+207
-0
lines changed

‎common/tier4_adapi_rviz_plugin/src/door_panel.cpp

+140
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
// Copyright 2023 The Autoware Contributors
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "door_panel.hpp"
16+
17+
#include <QHBoxLayout>
18+
#include <QVBoxLayout>
19+
#include <rviz_common/display_context.hpp>
20+
21+
#include <memory>
22+
23+
namespace tier4_adapi_rviz_plugins
24+
{
25+
26+
DoorPanel::DoorPanel(QWidget * parent) : rviz_common::Panel(parent)
27+
{
28+
waypoints_mode_ = new QPushButton("mode");
29+
waypoints_reset_ = new QPushButton("reset");
30+
waypoints_apply_ = new QPushButton("apply");
31+
allow_goal_modification_ = new QCheckBox("allow goal modification");
32+
33+
waypoints_mode_->setCheckable(true);
34+
waypoints_reset_->setDisabled(true);
35+
waypoints_apply_->setDisabled(true);
36+
connect(waypoints_mode_, &QPushButton::clicked, this, &DoorPanel::onWaypointsMode);
37+
connect(waypoints_reset_, &QPushButton::clicked, this, &DoorPanel::onWaypointsReset);
38+
connect(waypoints_apply_, &QPushButton::clicked, this, &DoorPanel::onWaypointsApply);
39+
40+
const auto layout = new QVBoxLayout();
41+
setLayout(layout);
42+
43+
// waypoints group
44+
{
45+
const auto group = new QGroupBox("waypoints");
46+
const auto local = new QHBoxLayout();
47+
local->addWidget(waypoints_mode_);
48+
local->addWidget(waypoints_reset_);
49+
local->addWidget(waypoints_apply_);
50+
group->setLayout(local);
51+
layout->addWidget(group);
52+
waypoints_group_ = group;
53+
}
54+
55+
// options group
56+
{
57+
const auto group = new QGroupBox("options");
58+
const auto local = new QHBoxLayout();
59+
local->addWidget(allow_goal_modification_);
60+
group->setLayout(local);
61+
layout->addWidget(group);
62+
}
63+
}
64+
65+
void DoorPanel::onInitialize()
66+
{
67+
auto lock = getDisplayContext()->getRosNodeAbstraction().lock();
68+
auto node = lock->get_raw_node();
69+
70+
sub_pose_ = node->create_subscription<PoseStamped>(
71+
"/rviz/routing/pose", rclcpp::QoS(1),
72+
std::bind(&DoorPanel::onPose, this, std::placeholders::_1));
73+
74+
const auto adaptor = component_interface_utils::NodeAdaptor(node.get());
75+
adaptor.init_cli(cli_clear_);
76+
adaptor.init_cli(cli_route_);
77+
}
78+
79+
void DoorPanel::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+
});
89+
}
90+
91+
void DoorPanel::onPose(const PoseStamped::ConstSharedPtr msg)
92+
{
93+
if (!waypoints_mode_->isChecked()) {
94+
setRoute(*msg);
95+
} else {
96+
waypoints_.push_back(*msg);
97+
waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
98+
}
99+
}
100+
101+
void DoorPanel::onWaypointsMode(bool clicked)
102+
{
103+
waypoints_reset_->setEnabled(clicked);
104+
waypoints_apply_->setEnabled(clicked);
105+
106+
if (clicked) {
107+
onWaypointsReset();
108+
} else {
109+
waypoints_group_->setTitle("waypoints");
110+
}
111+
}
112+
113+
void DoorPanel::onWaypointsReset()
114+
{
115+
waypoints_.clear();
116+
waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
117+
}
118+
119+
void DoorPanel::onWaypointsApply()
120+
{
121+
if (waypoints_.empty()) return;
122+
123+
const auto req = std::make_shared<ClearRoute::Service::Request>();
124+
cli_clear_->async_send_request(req, [this](auto) {
125+
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
126+
req->header = waypoints_.back().header;
127+
req->goal = waypoints_.back().pose;
128+
for (size_t i = 0; i + 1 < waypoints_.size(); ++i) {
129+
req->waypoints.push_back(waypoints_[i].pose);
130+
}
131+
req->option.allow_goal_modification = allow_goal_modification_->isChecked();
132+
cli_route_->async_send_request(req);
133+
onWaypointsReset();
134+
});
135+
}
136+
137+
} // namespace tier4_adapi_rviz_plugins
138+
139+
#include <pluginlib/class_list_macros.hpp>
140+
PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::DoorPanel, rviz_common::Panel)

‎common/tier4_adapi_rviz_plugin/src/door_panel.hpp

+67
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
// Copyright 2023 The Autoware Contributors
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef DOOR_PANEL_HPP_
16+
#define DOOR_PANEL_HPP_
17+
18+
#include <QCheckBox>
19+
#include <QGroupBox>
20+
#include <QPushButton>
21+
#include <autoware_ad_api_specs/routing.hpp>
22+
#include <component_interface_utils/rclcpp.hpp>
23+
#include <rclcpp/rclcpp.hpp>
24+
#include <rviz_common/panel.hpp>
25+
26+
#include <geometry_msgs/msg/pose_stamped.hpp>
27+
28+
#include <vector>
29+
30+
namespace tier4_adapi_rviz_plugins
31+
{
32+
33+
class DoorPanel : public rviz_common::Panel
34+
{
35+
Q_OBJECT
36+
using ClearRoute = autoware_ad_api::routing::ClearRoute;
37+
using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints;
38+
using PoseStamped = geometry_msgs::msg::PoseStamped;
39+
40+
public:
41+
explicit DoorPanel(QWidget * parent = nullptr);
42+
void onInitialize() override;
43+
44+
private:
45+
QPushButton * waypoints_mode_;
46+
QPushButton * waypoints_reset_;
47+
QPushButton * waypoints_apply_;
48+
QGroupBox * waypoints_group_;
49+
QCheckBox * allow_goal_modification_;
50+
51+
rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_;
52+
std::vector<PoseStamped> waypoints_;
53+
void onPose(const PoseStamped::ConstSharedPtr msg);
54+
55+
component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
56+
component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
57+
void setRoute(const PoseStamped & pose);
58+
59+
private slots:
60+
void onWaypointsMode(bool clicked);
61+
void onWaypointsReset();
62+
void onWaypointsApply();
63+
};
64+
65+
} // namespace tier4_adapi_rviz_plugins
66+
67+
#endif // DOOR_PANEL_HPP_

0 commit comments

Comments
 (0)