Skip to content

Commit 4e45188

Browse files
isamu-takagiisamu-takagi
authored
feat(tier4_adapi_rviz_plugin): add route panel (#3840)
* feat: add panel Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: set route Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: set waypoints Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: add readme Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: copyright Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 802c392 commit 4e45188

File tree

7 files changed

+226
-1
lines changed

7 files changed

+226
-1
lines changed

‎common/tier4_adapi_rviz_plugin/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@ find_package(Qt5 REQUIRED Core Widgets)
88
set(QT_LIBRARIES Qt5::Widgets)
99
set(CMAKE_AUTOMOC ON)
1010
set(CMAKE_INCLUDE_CURRENT_DIR ON)
11-
add_definitions(-DQT_NO_KEYWORDS)
1211

1312
ament_auto_add_library(${PROJECT_NAME} SHARED
1413
src/route_tool.cpp
14+
src/route_panel.cpp
1515
)
1616

1717
target_link_libraries(${PROJECT_NAME}

‎common/tier4_adapi_rviz_plugin/README.md

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
# tier4_adapi_rviz_plugin
2+
3+
## RoutePanel
4+
5+
To use the panel, set the topic name from 2D Goal Pose Tool to `/rviz/routing/pose`.
6+
By default, when a tool publish a pose, the panel immediately sets a route with that as the goal.
7+
Enable or disable of allow_goal_modification option can be set with the check box.
8+
9+
Push the mode button in the waypoint to enter waypoint mode. In this mode, the pose is added to waypoints.
10+
Press the apply button to set the route using the saved waypoints (the last one is a goal).
11+
Reset the saved waypoints with the reset button.

‎common/tier4_adapi_rviz_plugin/icons/classes/RoutePanel.png

18.4 KB
Loading

‎common/tier4_adapi_rviz_plugin/package.xml

+3
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@
1212
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1313
<buildtool_depend>autoware_cmake</buildtool_depend>
1414

15+
<depend>autoware_ad_api_specs</depend>
16+
<depend>component_interface_utils</depend>
17+
<depend>geometry_msgs</depend>
1518
<depend>libqt5-core</depend>
1619
<depend>libqt5-gui</depend>
1720
<depend>libqt5-widgets</depend>

‎common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml

+4
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,8 @@
44
<description>RouteTool</description>
55
</class>
66

7+
<class type="tier4_adapi_rviz_plugins::RoutePanel" base_class_type="rviz_common::Panel">
8+
<description>RoutePanel</description>
9+
</class>
10+
711
</library>

‎common/tier4_adapi_rviz_plugin/src/route_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 "route_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+
RoutePanel::RoutePanel(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, &RoutePanel::onWaypointsMode);
37+
connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset);
38+
connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::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 RoutePanel::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(&RoutePanel::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 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+
});
89+
}
90+
91+
void RoutePanel::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 RoutePanel::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 RoutePanel::onWaypointsReset()
114+
{
115+
waypoints_.clear();
116+
waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
117+
}
118+
119+
void RoutePanel::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::RoutePanel, rviz_common::Panel)

‎common/tier4_adapi_rviz_plugin/src/route_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 ROUTE_PANEL_HPP_
16+
#define ROUTE_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 RoutePanel : 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 RoutePanel(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 // ROUTE_PANEL_HPP_

0 commit comments

Comments
 (0)