|
| 1 | +// Copyright 2024 The Autoware Contributors |
| 2 | +// SPDX-License-Identifier: Apache-2.0 |
| 3 | + |
| 4 | +#include "route_selector_panel.hpp" |
| 5 | + |
| 6 | +#include <QGridLayout> |
| 7 | +#include <rviz_common/display_context.hpp> |
| 8 | + |
| 9 | +#include <memory> |
| 10 | +#include <string> |
| 11 | + |
| 12 | +namespace rviz_plugins |
| 13 | +{ |
| 14 | + |
| 15 | +QString to_string_state(const RouteState & state) |
| 16 | +{ |
| 17 | + // clang-format off |
| 18 | + switch (state.state) { |
| 19 | + case RouteState::UNKNOWN: return "unknown"; |
| 20 | + case RouteState::INITIALIZING: return "initializing"; |
| 21 | + case RouteState::UNSET: return "unset"; |
| 22 | + case RouteState::ROUTING: return "routing"; |
| 23 | + case RouteState::SET: return "set"; |
| 24 | + case RouteState::REROUTING: return "rerouting"; |
| 25 | + case RouteState::ARRIVED: return "arrived"; |
| 26 | + case RouteState::ABORTED: return "aborted"; |
| 27 | + case RouteState::INTERRUPTED: return "interrupted"; |
| 28 | + default: return "-----"; |
| 29 | + } |
| 30 | + // clang-format on |
| 31 | +} |
| 32 | + |
| 33 | +RouteSelectorPanel::RouteSelectorPanel(QWidget * parent) : rviz_common::Panel(parent) |
| 34 | +{ |
| 35 | + main_state_ = new QLabel("unknown"); |
| 36 | + main_clear_ = new QPushButton("clear"); |
| 37 | + mrm_state_ = new QLabel("unknown"); |
| 38 | + mrm_clear_ = new QPushButton("clear"); |
| 39 | + planner_state_ = new QLabel("unknown"); |
| 40 | + |
| 41 | + connect(main_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMainClear); |
| 42 | + connect(mrm_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMrmClear); |
| 43 | + |
| 44 | + const auto layout = new QGridLayout(); |
| 45 | + setLayout(layout); |
| 46 | + layout->addWidget(new QLabel("main"), 0, 0); |
| 47 | + layout->addWidget(main_state_, 0, 1); |
| 48 | + layout->addWidget(main_clear_, 0, 2); |
| 49 | + layout->addWidget(new QLabel("mrm"), 1, 0); |
| 50 | + layout->addWidget(mrm_state_, 1, 1); |
| 51 | + layout->addWidget(mrm_clear_, 1, 2); |
| 52 | + layout->addWidget(new QLabel("planner"), 2, 0); |
| 53 | + layout->addWidget(planner_state_, 2, 1); |
| 54 | +} |
| 55 | + |
| 56 | +void RouteSelectorPanel::onInitialize() |
| 57 | +{ |
| 58 | + auto lock = getDisplayContext()->getRosNodeAbstraction().lock(); |
| 59 | + auto node = lock->get_raw_node(); |
| 60 | + |
| 61 | + const auto durable_qos = rclcpp::QoS(1).transient_local(); |
| 62 | + |
| 63 | + sub_main_goal_ = node->create_subscription<PoseStamped>( |
| 64 | + "/rviz/route_selector/main/goal", rclcpp::QoS(1), |
| 65 | + std::bind(&RouteSelectorPanel::onMainGoal, this, std::placeholders::_1)); |
| 66 | + sub_mrm_goal_ = node->create_subscription<PoseStamped>( |
| 67 | + "/rviz/route_selector/mrm/goal", rclcpp::QoS(1), |
| 68 | + std::bind(&RouteSelectorPanel::onMrmGoal, this, std::placeholders::_1)); |
| 69 | + sub_main_state_ = node->create_subscription<RouteState>( |
| 70 | + "/planning/mission_planning/route_selector/main/state", durable_qos, |
| 71 | + std::bind(&RouteSelectorPanel::onMainState, this, std::placeholders::_1)); |
| 72 | + sub_mrm_state_ = node->create_subscription<RouteState>( |
| 73 | + "/planning/mission_planning/route_selector/mrm/state", durable_qos, |
| 74 | + std::bind(&RouteSelectorPanel::onMrmState, this, std::placeholders::_1)); |
| 75 | + sub_planner_state_ = node->create_subscription<RouteState>( |
| 76 | + "/planning/mission_planning/state", durable_qos, |
| 77 | + std::bind(&RouteSelectorPanel::onPlannerState, this, std::placeholders::_1)); |
| 78 | + |
| 79 | + cli_main_clear_ = |
| 80 | + node->create_client<ClearRoute>("/planning/mission_planning/route_selector/main/clear_route"); |
| 81 | + cli_mrm_clear_ = |
| 82 | + node->create_client<ClearRoute>("/planning/mission_planning/route_selector/mrm/clear_route"); |
| 83 | + cli_main_set_waypoint_ = node->create_client<SetWaypointRoute>( |
| 84 | + "/planning/mission_planning/route_selector/main/set_waypoint_route"); |
| 85 | + cli_mrm_set_waypoint_ = node->create_client<SetWaypointRoute>( |
| 86 | + "/planning/mission_planning/route_selector/mrm/set_waypoint_route"); |
| 87 | +} |
| 88 | + |
| 89 | +void RouteSelectorPanel::onMainGoal(const PoseStamped::ConstSharedPtr msg) |
| 90 | +{ |
| 91 | + const auto req = std::make_shared<SetWaypointRoute::Request>(); |
| 92 | + req->header = msg->header; |
| 93 | + req->goal_pose = msg->pose; |
| 94 | + req->allow_modification = true; |
| 95 | + cli_main_set_waypoint_->async_send_request(req); |
| 96 | +} |
| 97 | + |
| 98 | +void RouteSelectorPanel::onMrmGoal(const PoseStamped::ConstSharedPtr msg) |
| 99 | +{ |
| 100 | + const auto req = std::make_shared<SetWaypointRoute::Request>(); |
| 101 | + req->header = msg->header; |
| 102 | + req->goal_pose = msg->pose; |
| 103 | + req->allow_modification = true; |
| 104 | + cli_mrm_set_waypoint_->async_send_request(req); |
| 105 | +} |
| 106 | + |
| 107 | +void RouteSelectorPanel::onMainState(RouteState::ConstSharedPtr msg) |
| 108 | +{ |
| 109 | + main_state_->setText(to_string_state(*msg)); |
| 110 | +} |
| 111 | + |
| 112 | +void RouteSelectorPanel::onMrmState(RouteState::ConstSharedPtr msg) |
| 113 | +{ |
| 114 | + mrm_state_->setText(to_string_state(*msg)); |
| 115 | +} |
| 116 | + |
| 117 | +void RouteSelectorPanel::onPlannerState(RouteState::ConstSharedPtr msg) |
| 118 | +{ |
| 119 | + planner_state_->setText(to_string_state(*msg)); |
| 120 | +} |
| 121 | + |
| 122 | +void RouteSelectorPanel::onMainClear() |
| 123 | +{ |
| 124 | + const auto req = std::make_shared<ClearRoute::Request>(); |
| 125 | + cli_main_clear_->async_send_request(req); |
| 126 | +} |
| 127 | + |
| 128 | +void RouteSelectorPanel::onMrmClear() |
| 129 | +{ |
| 130 | + const auto req = std::make_shared<ClearRoute::Request>(); |
| 131 | + cli_mrm_clear_->async_send_request(req); |
| 132 | +} |
| 133 | + |
| 134 | +} // namespace rviz_plugins |
| 135 | + |
| 136 | +#include <pluginlib/class_list_macros.hpp> |
| 137 | +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RouteSelectorPanel, rviz_common::Panel) |
0 commit comments