diff --git a/common/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..2b06e6db3e70d --- /dev/null +++ b/common/mission_planner_rviz_plugin/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(mission_planner_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/mrm_goal.cpp + src/route_selector_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package() diff --git a/common/mission_planner_rviz_plugin/README.md b/common/mission_planner_rviz_plugin/README.md new file mode 100644 index 0000000000000..59d36a0a2f840 --- /dev/null +++ b/common/mission_planner_rviz_plugin/README.md @@ -0,0 +1,18 @@ +# mission_planner_rviz_plugin + +## MrmGoalTool + +This is a copy of `rviz_default_plugins::tools::GoalTool`. Used together with the RouteSelectorPanel to set the MRM route. +After adding the tool, change the topic name to `/rviz/route_selector/mrm/goal` from the topic property panel in rviz. + +## RouteSelectorPanel + +This panel shows the main and mrm route state in the route_selector and the route states in the mission_planner. +Additionally, it provides clear and set functions for each main route and mrm route. + +| Trigger | Action | +| -------------------------------------- | ------------------------------------------------------------------------ | +| main route clear button | call `/planning/mission_planning/route_selector/main/clear_route` | +| mrm route clear button | call `/planning/mission_planning/route_selector/mrm/clear_route` | +| `/rviz/route_selector/main/goal` topic | call `/planning/mission_planning/route_selector/main/set_waypoint_route` | +| `/rviz/route_selector/mrm/goal` topic | call `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | diff --git a/common/mission_planner_rviz_plugin/package.xml b/common/mission_planner_rviz_plugin/package.xml new file mode 100644 index 0000000000000..e45cf2739f260 --- /dev/null +++ b/common/mission_planner_rviz_plugin/package.xml @@ -0,0 +1,29 @@ + + + + mission_planner_rviz_plugin + 0.0.0 + The mission_planner_rviz_plugin package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + rclcpp + rviz_common + rviz_default_plugins + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/mission_planner_rviz_plugin/plugins/plugin_description.xml b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..c8285fcc604d7 --- /dev/null +++ b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,8 @@ + + + MrmGoalTool + + + RouteSelectorPanel + + diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp new file mode 100644 index 0000000000000..ef5529abfb4a7 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp @@ -0,0 +1,34 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mrm_goal.hpp" + +namespace rviz_plugins +{ + +MrmGoalTool::MrmGoalTool() +{ + shortcut_key_ = 'm'; +} + +void MrmGoalTool::onInitialize() +{ + GoalTool::onInitialize(); + setName("MRM Goal Pose"); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool) diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp new file mode 100644 index 0000000000000..e16b0abf0bab3 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MRM_GOAL_HPP_ +#define MRM_GOAL_HPP_ + +#include + +namespace rviz_plugins +{ + +class MrmGoalTool : public rviz_default_plugins::tools::GoalTool +{ + Q_OBJECT + +public: + MrmGoalTool(); + void onInitialize() override; +}; + +} // namespace rviz_plugins + +#endif // MRM_GOAL_HPP_ diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp new file mode 100644 index 0000000000000..b4b376b1e76ec --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp @@ -0,0 +1,148 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "route_selector_panel.hpp" + +#include +#include + +#include +#include + +namespace rviz_plugins +{ + +QString to_string_state(const RouteState & state) +{ + // clang-format off + switch (state.state) { + case RouteState::UNKNOWN: return "unknown"; + case RouteState::INITIALIZING: return "initializing"; + case RouteState::UNSET: return "unset"; + case RouteState::ROUTING: return "routing"; + case RouteState::SET: return "set"; + case RouteState::REROUTING: return "rerouting"; + case RouteState::ARRIVED: return "arrived"; + case RouteState::ABORTED: return "aborted"; + case RouteState::INTERRUPTED: return "interrupted"; + default: return "-----"; + } + // clang-format on +} + +RouteSelectorPanel::RouteSelectorPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + main_state_ = new QLabel("unknown"); + main_clear_ = new QPushButton("clear"); + mrm_state_ = new QLabel("unknown"); + mrm_clear_ = new QPushButton("clear"); + planner_state_ = new QLabel("unknown"); + + connect(main_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMainClear); + connect(mrm_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMrmClear); + + const auto layout = new QGridLayout(); + setLayout(layout); + layout->addWidget(new QLabel("main"), 0, 0); + layout->addWidget(main_state_, 0, 1); + layout->addWidget(main_clear_, 0, 2); + layout->addWidget(new QLabel("mrm"), 1, 0); + layout->addWidget(mrm_state_, 1, 1); + layout->addWidget(mrm_clear_, 1, 2); + layout->addWidget(new QLabel("planner"), 2, 0); + layout->addWidget(planner_state_, 2, 1); +} + +void RouteSelectorPanel::onInitialize() +{ + auto lock = getDisplayContext()->getRosNodeAbstraction().lock(); + auto node = lock->get_raw_node(); + + const auto durable_qos = rclcpp::QoS(1).transient_local(); + + sub_main_goal_ = node->create_subscription( + "/rviz/route_selector/main/goal", rclcpp::QoS(1), + std::bind(&RouteSelectorPanel::onMainGoal, this, std::placeholders::_1)); + sub_mrm_goal_ = node->create_subscription( + "/rviz/route_selector/mrm/goal", rclcpp::QoS(1), + std::bind(&RouteSelectorPanel::onMrmGoal, this, std::placeholders::_1)); + sub_main_state_ = node->create_subscription( + "/planning/mission_planning/route_selector/main/state", durable_qos, + std::bind(&RouteSelectorPanel::onMainState, this, std::placeholders::_1)); + sub_mrm_state_ = node->create_subscription( + "/planning/mission_planning/route_selector/mrm/state", durable_qos, + std::bind(&RouteSelectorPanel::onMrmState, this, std::placeholders::_1)); + sub_planner_state_ = node->create_subscription( + "/planning/mission_planning/state", durable_qos, + std::bind(&RouteSelectorPanel::onPlannerState, this, std::placeholders::_1)); + + cli_main_clear_ = + node->create_client("/planning/mission_planning/route_selector/main/clear_route"); + cli_mrm_clear_ = + node->create_client("/planning/mission_planning/route_selector/mrm/clear_route"); + cli_main_set_waypoint_ = node->create_client( + "/planning/mission_planning/route_selector/main/set_waypoint_route"); + cli_mrm_set_waypoint_ = node->create_client( + "/planning/mission_planning/route_selector/mrm/set_waypoint_route"); +} + +void RouteSelectorPanel::onMainGoal(const PoseStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->header = msg->header; + req->goal_pose = msg->pose; + req->allow_modification = true; + cli_main_set_waypoint_->async_send_request(req); +} + +void RouteSelectorPanel::onMrmGoal(const PoseStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->header = msg->header; + req->goal_pose = msg->pose; + req->allow_modification = true; + cli_mrm_set_waypoint_->async_send_request(req); +} + +void RouteSelectorPanel::onMainState(RouteState::ConstSharedPtr msg) +{ + main_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onMrmState(RouteState::ConstSharedPtr msg) +{ + mrm_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onPlannerState(RouteState::ConstSharedPtr msg) +{ + planner_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onMainClear() +{ + const auto req = std::make_shared(); + cli_main_clear_->async_send_request(req); +} + +void RouteSelectorPanel::onMrmClear() +{ + const auto req = std::make_shared(); + cli_mrm_clear_->async_send_request(req); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RouteSelectorPanel, rviz_common::Panel) diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp new file mode 100644 index 0000000000000..99101730661e0 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROUTE_SELECTOR_PANEL_HPP_ +#define ROUTE_SELECTOR_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ + +using geometry_msgs::msg::PoseStamped; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; + +class RouteSelectorPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit RouteSelectorPanel(QWidget * parent = nullptr); + void onInitialize() override; + +private: + QLabel * main_state_; + QLabel * mrm_state_; + QLabel * planner_state_; + QPushButton * main_clear_; + QPushButton * mrm_clear_; + + rclcpp::Subscription::SharedPtr sub_main_goal_; + rclcpp::Subscription::SharedPtr sub_mrm_goal_; + void onMainGoal(const PoseStamped::ConstSharedPtr msg); + void onMrmGoal(const PoseStamped::ConstSharedPtr msg); + + rclcpp::Subscription::SharedPtr sub_main_state_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Subscription::SharedPtr sub_planner_state_; + void onMainState(RouteState::ConstSharedPtr msg); + void onMrmState(RouteState::ConstSharedPtr msg); + void onPlannerState(RouteState::ConstSharedPtr msg); + + rclcpp::Client::SharedPtr cli_main_clear_; + rclcpp::Client::SharedPtr cli_mrm_clear_; + rclcpp::Client::SharedPtr cli_main_set_waypoint_; + rclcpp::Client::SharedPtr cli_mrm_set_waypoint_; + +private Q_SLOTS: + void onMainClear(); + void onMrmClear(); +}; + +} // namespace rviz_plugins + +#endif // ROUTE_SELECTOR_PANEL_HPP_