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