Skip to content

Commit 28e39da

Browse files
soblinsoblin
committed
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 89318e7 commit 28e39da

File tree

8 files changed

+325
-0
lines changed

8 files changed

+325
-0
lines changed

‎common/mission_planner_rviz_plugin/CMakeLists.txt

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(mission_planner_rviz_plugin)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(Qt5 REQUIRED Core Widgets)
8+
set(QT_LIBRARIES Qt5::Widgets)
9+
set(CMAKE_AUTOMOC ON)
10+
set(CMAKE_INCLUDE_CURRENT_DIR ON)
11+
12+
ament_auto_add_library(${PROJECT_NAME} SHARED
13+
src/mrm_goal.cpp
14+
src/route_selector_panel.cpp
15+
)
16+
17+
target_link_libraries(${PROJECT_NAME}
18+
${QT_LIBRARIES}
19+
)
20+
21+
pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
22+
23+
ament_auto_package()

‎common/mission_planner_rviz_plugin/README.md

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
# mission_planner_rviz_plugin
2+
3+
## MrmGoalTool
4+
5+
This is a copy of `rviz_default_plugins::tools::GoalTool`. Used together with the RouteSelectorPanel to set the MRM route.
6+
After adding the tool, change the topic name to `/rviz/route_selector/mrm/goal` from the topic property panel in rviz.
7+
8+
## RouteSelectorPanel
9+
10+
This panel shows the main and mrm route state in the route_selector and the route states in the mission_planner.
11+
Additionally, it provides clear and set functions for each main route and mrm route.
12+
13+
| Trigger | Action |
14+
| -------------------------------------- | ------------------------------------------------------------------------ |
15+
| main route clear button | call `/planning/mission_planning/route_selector/main/clear_route` |
16+
| mrm route clear button | call `/planning/mission_planning/route_selector/mrm/clear_route` |
17+
| `/rviz/route_selector/main/goal` topic | call `/planning/mission_planning/route_selector/main/set_waypoint_route` |
18+
| `/rviz/route_selector/mrm/goal` topic | call `/planning/mission_planning/route_selector/mrm/set_waypoint_route` |

‎common/mission_planner_rviz_plugin/package.xml

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>mission_planner_rviz_plugin</name>
5+
<version>0.0.0</version>
6+
<description>The mission_planner_rviz_plugin package</description>
7+
<maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
11+
<buildtool_depend>autoware_cmake</buildtool_depend>
12+
13+
<depend>geometry_msgs</depend>
14+
<depend>libqt5-core</depend>
15+
<depend>libqt5-gui</depend>
16+
<depend>libqt5-widgets</depend>
17+
<depend>rclcpp</depend>
18+
<depend>rviz_common</depend>
19+
<depend>rviz_default_plugins</depend>
20+
<depend>tier4_planning_msgs</depend>
21+
22+
<test_depend>ament_lint_auto</test_depend>
23+
<test_depend>autoware_lint_common</test_depend>
24+
25+
<export>
26+
<build_type>ament_cmake</build_type>
27+
<rviz plugin="${prefix}/plugins/plugin_description.xml"/>
28+
</export>
29+
</package>

‎common/mission_planner_rviz_plugin/plugins/plugin_description.xml

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
<library path="mission_planner_rviz_plugin">
2+
<class type="rviz_plugins::MrmGoalTool" base_class_type="rviz_common::Tool">
3+
<description>MrmGoalTool</description>
4+
</class>
5+
<class type="rviz_plugins::RouteSelectorPanel" base_class_type="rviz_common::Panel">
6+
<description>RouteSelectorPanel</description>
7+
</class>
8+
</library>

‎common/mission_planner_rviz_plugin/src/mrm_goal.cpp

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
// Copyright 2024 The Autoware Contributors
2+
// SPDX-License-Identifier: Apache-2.0
3+
4+
#include "mrm_goal.hpp"
5+
6+
namespace rviz_plugins
7+
{
8+
9+
MrmGoalTool::MrmGoalTool()
10+
{
11+
shortcut_key_ = 'm';
12+
}
13+
14+
void MrmGoalTool::onInitialize()
15+
{
16+
GoalTool::onInitialize();
17+
setName("MRM Goal Pose");
18+
}
19+
20+
} // namespace rviz_plugins
21+
22+
#include <pluginlib/class_list_macros.hpp>
23+
PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool)

‎common/mission_planner_rviz_plugin/src/mrm_goal.hpp

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
// Copyright 2024 The Autoware Contributors
2+
// SPDX-License-Identifier: Apache-2.0
3+
4+
#ifndef MRM_GOAL_HPP_
5+
#define MRM_GOAL_HPP_
6+
7+
#include <rviz_default_plugins/tools/goal_pose/goal_tool.hpp>
8+
9+
namespace rviz_plugins
10+
{
11+
12+
class MrmGoalTool : public rviz_default_plugins::tools::GoalTool
13+
{
14+
Q_OBJECT
15+
16+
public:
17+
MrmGoalTool();
18+
void onInitialize() override;
19+
};
20+
21+
} // namespace rviz_plugins
22+
23+
#endif // MRM_GOAL_HPP_

‎common/mission_planner_rviz_plugin/src/route_selector_panel.cpp

+137
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
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)

‎common/mission_planner_rviz_plugin/src/route_selector_panel.hpp

+64
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
// Copyright 2024 The Autoware Contributors
2+
// SPDX-License-Identifier: Apache-2.0
3+
4+
#ifndef ROUTE_SELECTOR_PANEL_HPP_
5+
#define ROUTE_SELECTOR_PANEL_HPP_
6+
7+
#include <QLabel>
8+
#include <QPushButton>
9+
#include <rclcpp/rclcpp.hpp>
10+
#include <rviz_common/panel.hpp>
11+
12+
#include <geometry_msgs/msg/pose_stamped.hpp>
13+
#include <tier4_planning_msgs/msg/route_state.hpp>
14+
#include <tier4_planning_msgs/srv/clear_route.hpp>
15+
#include <tier4_planning_msgs/srv/set_waypoint_route.hpp>
16+
17+
namespace rviz_plugins
18+
{
19+
20+
using geometry_msgs::msg::PoseStamped;
21+
using tier4_planning_msgs::msg::RouteState;
22+
using tier4_planning_msgs::srv::ClearRoute;
23+
using tier4_planning_msgs::srv::SetWaypointRoute;
24+
25+
class RouteSelectorPanel : public rviz_common::Panel
26+
{
27+
Q_OBJECT
28+
29+
public:
30+
explicit RouteSelectorPanel(QWidget * parent = nullptr);
31+
void onInitialize() override;
32+
33+
private:
34+
QLabel * main_state_;
35+
QLabel * mrm_state_;
36+
QLabel * planner_state_;
37+
QPushButton * main_clear_;
38+
QPushButton * mrm_clear_;
39+
40+
rclcpp::Subscription<PoseStamped>::SharedPtr sub_main_goal_;
41+
rclcpp::Subscription<PoseStamped>::SharedPtr sub_mrm_goal_;
42+
void onMainGoal(const PoseStamped::ConstSharedPtr msg);
43+
void onMrmGoal(const PoseStamped::ConstSharedPtr msg);
44+
45+
rclcpp::Subscription<RouteState>::SharedPtr sub_main_state_;
46+
rclcpp::Subscription<RouteState>::SharedPtr sub_mrm_state_;
47+
rclcpp::Subscription<RouteState>::SharedPtr sub_planner_state_;
48+
void onMainState(RouteState::ConstSharedPtr msg);
49+
void onMrmState(RouteState::ConstSharedPtr msg);
50+
void onPlannerState(RouteState::ConstSharedPtr msg);
51+
52+
rclcpp::Client<ClearRoute>::SharedPtr cli_main_clear_;
53+
rclcpp::Client<ClearRoute>::SharedPtr cli_mrm_clear_;
54+
rclcpp::Client<SetWaypointRoute>::SharedPtr cli_main_set_waypoint_;
55+
rclcpp::Client<SetWaypointRoute>::SharedPtr cli_mrm_set_waypoint_;
56+
57+
private Q_SLOTS:
58+
void onMainClear();
59+
void onMrmClear();
60+
};
61+
62+
} // namespace rviz_plugins
63+
64+
#endif // ROUTE_SELECTOR_PANEL_HPP_

0 commit comments

Comments
 (0)