Skip to content

Commit 7eac7f4

Browse files
isamu-takagiisamu-takagi
committed
add door panel
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent bc06079 commit 7eac7f4

File tree

5 files changed

+102
-122
lines changed

5 files changed

+102
-122
lines changed

‎common/tier4_adapi_rviz_plugin/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
1212
ament_auto_add_library(${PROJECT_NAME} SHARED
1313
src/route_tool.cpp
1414
src/route_panel.cpp
15+
src/door_panel.cpp
1516
)
1617

1718
target_link_libraries(${PROJECT_NAME}

‎common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml

+4
Original file line numberDiff line numberDiff line change
@@ -8,4 +8,8 @@
88
<description>RoutePanel</description>
99
</class>
1010

11+
<class type="tier4_adapi_rviz_plugins::DoorPanel" base_class_type="rviz_common::Panel">
12+
<description>DoorPanel</description>
13+
</class>
14+
1115
</library>

‎common/tier4_adapi_rviz_plugin/src/door_panel.cpp

+75-97
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,6 @@
1414

1515
#include "door_panel.hpp"
1616

17-
#include <QHBoxLayout>
18-
#include <QVBoxLayout>
1917
#include <rviz_common/display_context.hpp>
2018

2119
#include <memory>
@@ -25,113 +23,93 @@ namespace tier4_adapi_rviz_plugins
2523

2624
DoorPanel::DoorPanel(QWidget * parent) : rviz_common::Panel(parent)
2725
{
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-
}
26+
status_ = new QLabel("Trying to get door layout.");
27+
layout_ = new QGridLayout();
28+
layout_->addWidget(status_, 0, 0, 1, 4);
29+
setLayout(layout_);
6330
}
6431

6532
void DoorPanel::onInitialize()
6633
{
34+
const auto on_layout = [this](const rclcpp::Client<DoorLayout::Service>::SharedFuture future) {
35+
const auto res = future.get();
36+
if (!res->status.success) {
37+
status_->setText(QString::fromStdString("failed to get layout: " + res->status.message));
38+
return;
39+
}
40+
const auto & layouts = res->doors;
41+
for (size_t index = 0; index < layouts.size(); ++index) {
42+
const auto & layout = layouts[index];
43+
DoorUI door;
44+
door.description = new QLabel(QString::fromStdString(layout.description));
45+
door.status = new QLabel("unknown");
46+
door.open = new QPushButton("open");
47+
door.close = new QPushButton("close");
48+
doors_.push_back(door);
49+
50+
layout_->addWidget(door.description, index + 1, 0);
51+
layout_->addWidget(door.status, index + 1, 1);
52+
layout_->addWidget(door.open, index + 1, 2);
53+
layout_->addWidget(door.close, index + 1, 3);
54+
55+
using Command = autoware_adapi_v1_msgs::msg::DoorCommand;
56+
const auto on_open = [this, index] { on_button(index, Command::OPEN); };
57+
const auto on_close = [this, index] { on_button(index, Command::CLOSE); };
58+
connect(door.open, &QPushButton::clicked, on_open);
59+
connect(door.close, &QPushButton::clicked, on_close);
60+
}
61+
status_->hide();
62+
};
63+
64+
const auto on_status = [this](const DoorStatus::Message::ConstSharedPtr msg) {
65+
using Status = autoware_adapi_v1_msgs::msg::DoorStatus;
66+
if (doors_.size() != msg->doors.size()) {
67+
return;
68+
}
69+
for (size_t index = 0; index < doors_.size(); ++index) {
70+
const auto & label = doors_[index].status;
71+
switch (msg->doors[index].status) {
72+
case Status::NOT_AVAILABLE:
73+
label->setText("not available");
74+
break;
75+
case Status::OPENED:
76+
label->setText("opened");
77+
break;
78+
case Status::CLOSED:
79+
label->setText("closed");
80+
break;
81+
case Status::OPENING:
82+
label->setText("opening");
83+
break;
84+
case Status::CLOSING:
85+
label->setText("closing");
86+
break;
87+
default:
88+
label->setText("unknown");
89+
break;
90+
}
91+
}
92+
};
93+
6794
auto lock = getDisplayContext()->getRosNodeAbstraction().lock();
6895
auto node = lock->get_raw_node();
6996

70-
sub_pose_ = node->create_subscription<PoseStamped>(
71-
"/rviz/routing/pose", rclcpp::QoS(1),
72-
std::bind(&DoorPanel::onPose, this, std::placeholders::_1));
73-
7497
const auto adaptor = component_interface_utils::NodeAdaptor(node.get());
75-
adaptor.init_cli(cli_clear_);
76-
adaptor.init_cli(cli_route_);
77-
}
98+
adaptor.init_cli(cli_command_);
99+
adaptor.init_cli(cli_layout_);
100+
adaptor.init_sub(sub_status_, on_status);
78101

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-
});
102+
const auto req = std::make_shared<DoorLayout::Service::Request>();
103+
cli_layout_->async_send_request(req, on_layout);
89104
}
90105

91-
void DoorPanel::onPose(const PoseStamped::ConstSharedPtr msg)
106+
void DoorPanel::on_button(uint32_t index, uint8_t command)
92107
{
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-
});
108+
const auto req = std::make_shared<DoorCommand::Service::Request>();
109+
req->doors.resize(1);
110+
req->doors.back().index = index;
111+
req->doors.back().command = command;
112+
cli_command_->async_send_request(req);
135113
}
136114

137115
} // namespace tier4_adapi_rviz_plugins

‎common/tier4_adapi_rviz_plugin/src/door_panel.hpp

+20-23
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,14 @@
1515
#ifndef DOOR_PANEL_HPP_
1616
#define DOOR_PANEL_HPP_
1717

18-
#include <QCheckBox>
19-
#include <QGroupBox>
18+
#include <QGridLayout>
19+
#include <QLabel>
2020
#include <QPushButton>
21-
#include <autoware_ad_api_specs/routing.hpp>
21+
#include <autoware_ad_api_specs/vehicle.hpp>
2222
#include <component_interface_utils/rclcpp.hpp>
2323
#include <rclcpp/rclcpp.hpp>
2424
#include <rviz_common/panel.hpp>
2525

26-
#include <geometry_msgs/msg/pose_stamped.hpp>
27-
2826
#include <vector>
2927

3028
namespace tier4_adapi_rviz_plugins
@@ -33,33 +31,32 @@ namespace tier4_adapi_rviz_plugins
3331
class DoorPanel : public rviz_common::Panel
3432
{
3533
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;
34+
using DoorCommand = autoware_ad_api::vehicle::DoorCommand;
35+
using DoorLayout = autoware_ad_api::vehicle::DoorLayout;
36+
using DoorStatus = autoware_ad_api::vehicle::DoorStatus;
3937

4038
public:
4139
explicit DoorPanel(QWidget * parent = nullptr);
4240
void onInitialize() override;
4341

4442
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);
43+
component_interface_utils::Client<DoorCommand>::SharedPtr cli_command_;
44+
component_interface_utils::Client<DoorLayout>::SharedPtr cli_layout_;
45+
component_interface_utils::Subscription<DoorStatus>::SharedPtr sub_status_;
5446

55-
component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
56-
component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
57-
void setRoute(const PoseStamped & pose);
47+
struct DoorUI
48+
{
49+
QLabel * description;
50+
QLabel * status;
51+
QPushButton * open;
52+
QPushButton * close;
53+
};
54+
QGridLayout * layout_;
55+
QLabel * status_;
56+
std::vector<DoorUI> doors_;
5857

5958
private slots:
60-
void onWaypointsMode(bool clicked);
61-
void onWaypointsReset();
62-
void onWaypointsApply();
59+
void on_button(uint32_t index, uint8_t command);
6360
};
6461

6562
} // namespace tier4_adapi_rviz_plugins

‎system/default_ad_api/src/vehicle_door.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,8 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options)
2222
{
2323
const auto adaptor = component_interface_utils::NodeAdaptor(this);
2424
group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
25-
adaptor.relay_service(cli_command_, srv_command_, group_cli_);
26-
adaptor.relay_service(cli_layout_, srv_layout_, group_cli_);
25+
adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0);
26+
adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0);
2727
adaptor.relay_message(pub_status_, sub_status_);
2828
}
2929

0 commit comments

Comments
 (0)