Merge branch 'main' into autoware_msgs

Signed-off-by: Yukihito Saito <[email protected]>

.gitignore

@@ -0,0 +1 @@
+*.py[cod]

.pre-commit-config.yaml

@@ -34,7 +34,7 @@ repos:
       - id: yamllint
 
   - repo: https://github.com/tier4/pre-commit-hooks-ros
-    rev: v0.8.0
+    rev: v0.9.0
     hooks:
       - id: flake8-ros
       - id: prettier-xacro

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()

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`  |

common/mission_planner_rviz_plugin/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mission_planner_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>The mission_planner_rviz_plugin package</description>
+  <maintainer email="[email protected]">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>tier4_planning_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

common/mission_planner_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,8 @@
+<library path="mission_planner_rviz_plugin">
+  <class type="rviz_plugins::MrmGoalTool" base_class_type="rviz_common::Tool">
+    <description>MrmGoalTool</description>
+  </class>
+  <class type="rviz_plugins::RouteSelectorPanel" base_class_type="rviz_common::Panel">
+    <description>RouteSelectorPanel</description>
+  </class>
+</library>

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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool)

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 <rviz_default_plugins/tools/goal_pose/goal_tool.hpp>
+
+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_

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 <QGridLayout>
+#include <rviz_common/display_context.hpp>
+
+#include <memory>
+#include <string>
+
+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<PoseStamped>(
+    "/rviz/route_selector/main/goal", rclcpp::QoS(1),
+    std::bind(&RouteSelectorPanel::onMainGoal, this, std::placeholders::_1));
+  sub_mrm_goal_ = node->create_subscription<PoseStamped>(
+    "/rviz/route_selector/mrm/goal", rclcpp::QoS(1),
+    std::bind(&RouteSelectorPanel::onMrmGoal, this, std::placeholders::_1));
+  sub_main_state_ = node->create_subscription<RouteState>(
+    "/planning/mission_planning/route_selector/main/state", durable_qos,
+    std::bind(&RouteSelectorPanel::onMainState, this, std::placeholders::_1));
+  sub_mrm_state_ = node->create_subscription<RouteState>(
+    "/planning/mission_planning/route_selector/mrm/state", durable_qos,
+    std::bind(&RouteSelectorPanel::onMrmState, this, std::placeholders::_1));
+  sub_planner_state_ = node->create_subscription<RouteState>(
+    "/planning/mission_planning/state", durable_qos,
+    std::bind(&RouteSelectorPanel::onPlannerState, this, std::placeholders::_1));
+
+  cli_main_clear_ =
+    node->create_client<ClearRoute>("/planning/mission_planning/route_selector/main/clear_route");
+  cli_mrm_clear_ =
+    node->create_client<ClearRoute>("/planning/mission_planning/route_selector/mrm/clear_route");
+  cli_main_set_waypoint_ = node->create_client<SetWaypointRoute>(
+    "/planning/mission_planning/route_selector/main/set_waypoint_route");
+  cli_mrm_set_waypoint_ = node->create_client<SetWaypointRoute>(
+    "/planning/mission_planning/route_selector/mrm/set_waypoint_route");
+}
+
+void RouteSelectorPanel::onMainGoal(const PoseStamped::ConstSharedPtr msg)
+{
+  const auto req = std::make_shared<SetWaypointRoute::Request>();
+  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<SetWaypointRoute::Request>();
+  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<ClearRoute::Request>();
+  cli_main_clear_->async_send_request(req);
+}
+
+void RouteSelectorPanel::onMrmClear()
+{
+  const auto req = std::make_shared<ClearRoute::Request>();
+  cli_mrm_clear_->async_send_request(req);
+}
+
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::RouteSelectorPanel, rviz_common::Panel)

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 <QLabel>
+#include <QPushButton>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tier4_planning_msgs/msg/route_state.hpp>
+#include <tier4_planning_msgs/srv/clear_route.hpp>
+#include <tier4_planning_msgs/srv/set_waypoint_route.hpp>
+
+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<PoseStamped>::SharedPtr sub_main_goal_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_mrm_goal_;
+  void onMainGoal(const PoseStamped::ConstSharedPtr msg);
+  void onMrmGoal(const PoseStamped::ConstSharedPtr msg);
+
+  rclcpp::Subscription<RouteState>::SharedPtr sub_main_state_;
+  rclcpp::Subscription<RouteState>::SharedPtr sub_mrm_state_;
+  rclcpp::Subscription<RouteState>::SharedPtr sub_planner_state_;
+  void onMainState(RouteState::ConstSharedPtr msg);
+  void onMrmState(RouteState::ConstSharedPtr msg);
+  void onPlannerState(RouteState::ConstSharedPtr msg);
+
+  rclcpp::Client<ClearRoute>::SharedPtr cli_main_clear_;
+  rclcpp::Client<ClearRoute>::SharedPtr cli_mrm_clear_;
+  rclcpp::Client<SetWaypointRoute>::SharedPtr cli_main_set_waypoint_;
+  rclcpp::Client<SetWaypointRoute>::SharedPtr cli_mrm_set_waypoint_;
+
+private Q_SLOTS:
+  void onMainClear();
+  void onMrmClear();
+};
+
+}  // namespace rviz_plugins
+
+#endif  // ROUTE_SELECTOR_PANEL_HPP_