Skip to content

Commit 13bd153

Browse files
isamu-takagiisamu-takagi
authored
feat(mission_planner_rviz_plugin): create mission planner tool (#6362)
* feat(mission_planner_rviz_plugin): create package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix copyright Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * add interrupted state Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * use full license text instead of spdx Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 46612e3 commit 13bd153

File tree

8 files changed

+369
-0
lines changed

8 files changed

+369
-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

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
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 "mrm_goal.hpp"
16+
17+
namespace rviz_plugins
18+
{
19+
20+
MrmGoalTool::MrmGoalTool()
21+
{
22+
shortcut_key_ = 'm';
23+
}
24+
25+
void MrmGoalTool::onInitialize()
26+
{
27+
GoalTool::onInitialize();
28+
setName("MRM Goal Pose");
29+
}
30+
31+
} // namespace rviz_plugins
32+
33+
#include <pluginlib/class_list_macros.hpp>
34+
PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool)

‎common/mission_planner_rviz_plugin/src/mrm_goal.hpp

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
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+
#ifndef MRM_GOAL_HPP_
16+
#define MRM_GOAL_HPP_
17+
18+
#include <rviz_default_plugins/tools/goal_pose/goal_tool.hpp>
19+
20+
namespace rviz_plugins
21+
{
22+
23+
class MrmGoalTool : public rviz_default_plugins::tools::GoalTool
24+
{
25+
Q_OBJECT
26+
27+
public:
28+
MrmGoalTool();
29+
void onInitialize() override;
30+
};
31+
32+
} // namespace rviz_plugins
33+
34+
#endif // MRM_GOAL_HPP_

‎common/mission_planner_rviz_plugin/src/route_selector_panel.cpp

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

‎common/mission_planner_rviz_plugin/src/route_selector_panel.hpp

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
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+
#ifndef ROUTE_SELECTOR_PANEL_HPP_
16+
#define ROUTE_SELECTOR_PANEL_HPP_
17+
18+
#include <QLabel>
19+
#include <QPushButton>
20+
#include <rclcpp/rclcpp.hpp>
21+
#include <rviz_common/panel.hpp>
22+
23+
#include <geometry_msgs/msg/pose_stamped.hpp>
24+
#include <tier4_planning_msgs/msg/route_state.hpp>
25+
#include <tier4_planning_msgs/srv/clear_route.hpp>
26+
#include <tier4_planning_msgs/srv/set_waypoint_route.hpp>
27+
28+
namespace rviz_plugins
29+
{
30+
31+
using geometry_msgs::msg::PoseStamped;
32+
using tier4_planning_msgs::msg::RouteState;
33+
using tier4_planning_msgs::srv::ClearRoute;
34+
using tier4_planning_msgs::srv::SetWaypointRoute;
35+
36+
class RouteSelectorPanel : public rviz_common::Panel
37+
{
38+
Q_OBJECT
39+
40+
public:
41+
explicit RouteSelectorPanel(QWidget * parent = nullptr);
42+
void onInitialize() override;
43+
44+
private:
45+
QLabel * main_state_;
46+
QLabel * mrm_state_;
47+
QLabel * planner_state_;
48+
QPushButton * main_clear_;
49+
QPushButton * mrm_clear_;
50+
51+
rclcpp::Subscription<PoseStamped>::SharedPtr sub_main_goal_;
52+
rclcpp::Subscription<PoseStamped>::SharedPtr sub_mrm_goal_;
53+
void onMainGoal(const PoseStamped::ConstSharedPtr msg);
54+
void onMrmGoal(const PoseStamped::ConstSharedPtr msg);
55+
56+
rclcpp::Subscription<RouteState>::SharedPtr sub_main_state_;
57+
rclcpp::Subscription<RouteState>::SharedPtr sub_mrm_state_;
58+
rclcpp::Subscription<RouteState>::SharedPtr sub_planner_state_;
59+
void onMainState(RouteState::ConstSharedPtr msg);
60+
void onMrmState(RouteState::ConstSharedPtr msg);
61+
void onPlannerState(RouteState::ConstSharedPtr msg);
62+
63+
rclcpp::Client<ClearRoute>::SharedPtr cli_main_clear_;
64+
rclcpp::Client<ClearRoute>::SharedPtr cli_mrm_clear_;
65+
rclcpp::Client<SetWaypointRoute>::SharedPtr cli_main_set_waypoint_;
66+
rclcpp::Client<SetWaypointRoute>::SharedPtr cli_mrm_set_waypoint_;
67+
68+
private Q_SLOTS:
69+
void onMainClear();
70+
void onMrmClear();
71+
};
72+
73+
} // namespace rviz_plugins
74+
75+
#endif // ROUTE_SELECTOR_PANEL_HPP_

0 commit comments

Comments
 (0)