Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(mission_planner_rviz_plugin): create mission planner tool #6362

Merged
Merged
Show file tree
Hide file tree
Changes from 4 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 23 additions & 0 deletions common/mission_planner_rviz_plugin/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
18 changes: 18 additions & 0 deletions common/mission_planner_rviz_plugin/README.md
Original file line number Diff line number Diff line change
@@ -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` |
29 changes: 29 additions & 0 deletions common/mission_planner_rviz_plugin/package.xml
Original file line number Diff line number Diff line change
@@ -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="isamu.takagi@tier4.jp">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>
Original file line number Diff line number Diff line change
@@ -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>
23 changes: 23 additions & 0 deletions common/mission_planner_rviz_plugin/src/mrm_goal.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
// Copyright 2024 The Autoware Contributors
// SPDX-License-Identifier: Apache-2.0

#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)
23 changes: 23 additions & 0 deletions common/mission_planner_rviz_plugin/src/mrm_goal.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
// Copyright 2024 The Autoware Contributors
// SPDX-License-Identifier: Apache-2.0

#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_
137 changes: 137 additions & 0 deletions common/mission_planner_rviz_plugin/src/route_selector_panel.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
// Copyright 2024 The Autoware Contributors
// SPDX-License-Identifier: Apache-2.0

#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
}

Check warning on line 31 in common/mission_planner_rviz_plugin/src/route_selector_panel.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

to_string_state has a cyclomatic complexity of 11, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

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)
64 changes: 64 additions & 0 deletions common/mission_planner_rviz_plugin/src/route_selector_panel.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
// Copyright 2024 The Autoware Contributors
// SPDX-License-Identifier: Apache-2.0

#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_
Loading