diff --git a/common/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..2b06e6db3 --- /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 000000000..59d36a0a2 --- /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 000000000..e45cf2739 --- /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 000000000..c8285fcc6 --- /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 000000000..ef5529abf --- /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 000000000..e16b0abf0 --- /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 000000000..b4b376b1e --- /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 000000000..991017306 --- /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_ diff --git a/common/rtc_manager_rviz_plugin/CMakeLists.txt b/common/rtc_manager_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..f2fad9e24 --- /dev/null +++ b/common/rtc_manager_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(rtc_manager_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) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/rtc_manager_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/rtc_manager_rviz_plugin/README.md b/common/rtc_manager_rviz_plugin/README.md new file mode 100644 index 000000000..7a15d56fe --- /dev/null +++ b/common/rtc_manager_rviz_plugin/README.md @@ -0,0 +1,36 @@ +# rtc_manager_rviz_plugin + +## Purpose + +The purpose of this Rviz plugin is + +1. To display each content of RTC status. + +2. To switch each module of RTC auto mode. + +3. To change RTC cooperate commands by button. + +![rtc_manager_panel](./images/rtc_manager_panel.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------ | ------------------------------------------- | --------------------------------------- | +| `/api/external/get/rtc_status` | `tier4_rtc_msgs::msg::CooperateStatusArray` | The statuses of each Cooperate Commands | + +### Output + +| Name | Type | Description | +| -------------------------------- | ---------------------------------------- | ---------------------------------------------------- | +| `/api/external/set/rtc_commands` | `tier4_rtc_msgs::src::CooperateCommands` | The Cooperate Commands for each planning | +| `/planning/enable_auto_mode/*` | `tier4_rtc_msgs::src::AutoMode` | The Cooperate Commands mode for each planning module | + +## HowToUse + +1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) + +2. tier4_state_rviz_plugin/RTCManagerPanel and press OK. + ![select_rtc_panel](./images/rtc_selection.png) diff --git a/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png b/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png new file mode 100644 index 000000000..6a6757371 Binary files /dev/null and b/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png differ diff --git a/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png b/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png new file mode 100644 index 000000000..36c1b4760 Binary files /dev/null and b/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png differ diff --git a/common/rtc_manager_rviz_plugin/images/rtc_selection.png b/common/rtc_manager_rviz_plugin/images/rtc_selection.png new file mode 100644 index 000000000..f9c5d120b Binary files /dev/null and b/common/rtc_manager_rviz_plugin/images/rtc_selection.png differ diff --git a/common/rtc_manager_rviz_plugin/images/select_panels.png b/common/rtc_manager_rviz_plugin/images/select_panels.png new file mode 100644 index 000000000..a691602c4 Binary files /dev/null and b/common/rtc_manager_rviz_plugin/images/select_panels.png differ diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml new file mode 100644 index 000000000..53f00386b --- /dev/null +++ b/common/rtc_manager_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + rtc_manager_rviz_plugin + 0.0.0 + The rtc manager rviz plugin package + Taiki Tanaka + Tomoya Kimura + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + tier4_external_api_msgs + tier4_planning_msgs + tier4_rtc_msgs + unique_identifier_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..001ae153e --- /dev/null +++ b/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + RTCManagerPanel + + + diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp new file mode 100644 index 000000000..058a5d5de --- /dev/null +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -0,0 +1,474 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "rtc_manager_panel.hpp" + +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +inline std::string Bool2String(const bool var) +{ + return var ? "True" : "False"; +} +inline bool uint2bool(uint8_t var) +{ + return var == static_cast(0) ? false : true; +} +using std::placeholders::_1; +using std::placeholders::_2; + +std::string getModuleName(const uint8_t module_type) +{ + switch (module_type) { + case Module::LANE_CHANGE_LEFT: { + return "lane_change_left"; + } + case Module::LANE_CHANGE_RIGHT: { + return "lane_change_right"; + } + case Module::EXT_REQUEST_LANE_CHANGE_LEFT: { + return "external_request_lane_change_left"; + } + case Module::EXT_REQUEST_LANE_CHANGE_RIGHT: { + return "external_request_lane_change_right"; + } + case Module::AVOIDANCE_BY_LC_LEFT: { + return "avoidance_by_lane_change_left"; + } + case Module::AVOIDANCE_BY_LC_RIGHT: { + return "avoidance_by_lane_change_right"; + } + case Module::AVOIDANCE_LEFT: { + return "avoidance_left"; + } + case Module::AVOIDANCE_RIGHT: { + return "avoidance_right"; + } + case Module::GOAL_PLANNER: { + return "goal_planner"; + } + case Module::START_PLANNER: { + return "start_planner"; + } + case Module::TRAFFIC_LIGHT: { + return "traffic_light"; + } + case Module::INTERSECTION: { + return "intersection"; + } + case Module::CROSSWALK: { + return "crosswalk"; + } + case Module::BLIND_SPOT: { + return "blind_spot"; + } + case Module::DETECTION_AREA: { + return "detection_area"; + } + case Module::NO_STOPPING_AREA: { + return "no_stopping_area"; + } + case Module::OCCLUSION_SPOT: { + return "occlusion_spot"; + } + case Module::INTERSECTION_OCCLUSION: { + return "intersection_occlusion"; + } + } + return "NONE"; +} + +bool isPathChangeModule(const uint8_t module_type) +{ + if ( + module_type == Module::LANE_CHANGE_LEFT || module_type == Module::LANE_CHANGE_RIGHT || + module_type == Module::EXT_REQUEST_LANE_CHANGE_LEFT || + module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || + module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT || + module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || + module_type == Module::GOAL_PLANNER || module_type == Module::START_PLANNER) { + return true; + } + return false; +} + +RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // TODO(tanaka): replace this magic number to Module::SIZE + const size_t module_size = 19; + auto_modes_.reserve(module_size); + auto * v_layout = new QVBoxLayout; + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + auto_mode_table_ = new QTableWidget(); + auto_mode_table_->setColumnCount(4); + auto_mode_table_->setHorizontalHeaderLabels( + {"Module", "ToAutoMode", "ToManualMode", "ApprovalMode"}); + auto_mode_table_->setVerticalHeader(vertical_header); + auto_mode_table_->setHorizontalHeader(horizontal_header); + const size_t num_modules = module_size; + auto_mode_table_->setRowCount(num_modules); + for (size_t i = 0; i < num_modules; i++) { + auto * rtc_auto_mode = new RTCAutoMode(); + rtc_auto_mode->setParent(this); + // module + { + const uint8_t module_type = static_cast(i); + rtc_auto_mode->module_name = getModuleName(module_type); + std::string module_name = rtc_auto_mode->module_name; + auto label = new QLabel(QString::fromStdString(module_name)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(module_name)); + if (isPathChangeModule(module_type)) + label->setStyleSheet(BG_PURPLE); + else + label->setStyleSheet(BG_ORANGE); + auto_mode_table_->setCellWidget(i, 0, label); + } + // mode button + { + // auto mode button + rtc_auto_mode->auto_module_button_ptr = new QPushButton("auto mode"); + rtc_auto_mode->auto_module_button_ptr->setCheckable(true); + connect( + rtc_auto_mode->auto_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, + &RTCAutoMode::onChangeToAutoMode); + auto_mode_table_->setCellWidget(i, 1, rtc_auto_mode->auto_module_button_ptr); + // manual mode button + rtc_auto_mode->manual_module_button_ptr = new QPushButton("manual mode"); + rtc_auto_mode->manual_module_button_ptr->setCheckable(true); + connect( + rtc_auto_mode->manual_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, + &RTCAutoMode::onChangeToManualMode); + auto_mode_table_->setCellWidget(i, 2, rtc_auto_mode->manual_module_button_ptr); + } + // current mode + { + QString mode = QString::fromStdString("INIT"); + rtc_auto_mode->auto_manual_mode_label = new QLabel(mode); + rtc_auto_mode->auto_manual_mode_label->setAlignment(Qt::AlignCenter); + rtc_auto_mode->auto_manual_mode_label->setText(mode); + auto_mode_table_->setCellWidget(i, 3, rtc_auto_mode->auto_manual_mode_label); + } + auto_modes_.emplace_back(rtc_auto_mode); + } + v_layout->addWidget(auto_mode_table_); + + num_rtc_status_ptr_ = new QLabel("Init"); + v_layout->addWidget(num_rtc_status_ptr_); + + // lateral execution + auto * exe_path_change_layout = new QHBoxLayout; + { + exec_path_change_button_ptr_ = new QPushButton("Execute Path Change"); + exec_path_change_button_ptr_->setCheckable(false); + exec_path_change_button_ptr_->setStyleSheet(BG_PURPLE); + connect( + exec_path_change_button_ptr_, &QPushButton::clicked, this, + &RTCManagerPanel::onClickExecutePathChange); + exe_path_change_layout->addWidget(exec_path_change_button_ptr_); + wait_path_change_button_ptr_ = new QPushButton("Wait Path Change"); + wait_path_change_button_ptr_->setCheckable(false); + wait_path_change_button_ptr_->setStyleSheet(BG_PURPLE); + connect( + wait_path_change_button_ptr_, &QPushButton::clicked, this, + &RTCManagerPanel::onClickWaitPathChange); + exe_path_change_layout->addWidget(wait_path_change_button_ptr_); + } + v_layout->addLayout(exe_path_change_layout); + + // longitudinal execution + auto * exe_vel_change_layout = new QHBoxLayout; + { + exec_vel_change_button_ptr_ = new QPushButton("Execute Velocity Change"); + exec_vel_change_button_ptr_->setCheckable(false); + exec_vel_change_button_ptr_->setStyleSheet(BG_ORANGE); + connect( + exec_vel_change_button_ptr_, &QPushButton::clicked, this, + &RTCManagerPanel::onClickExecuteVelChange); + exe_vel_change_layout->addWidget(exec_vel_change_button_ptr_); + wait_vel_change_button_ptr_ = new QPushButton("Wait Velocity Change"); + wait_vel_change_button_ptr_->setCheckable(false); + wait_vel_change_button_ptr_->setStyleSheet(BG_ORANGE); + connect( + wait_vel_change_button_ptr_, &QPushButton::clicked, this, + &RTCManagerPanel::onClickWaitVelChange); + exe_vel_change_layout->addWidget(wait_vel_change_button_ptr_); + } + v_layout->addLayout(exe_vel_change_layout); + + // execution + auto * rtc_exe_layout = new QHBoxLayout; + { + exec_button_ptr_ = new QPushButton("Execute All"); + exec_button_ptr_->setCheckable(false); + connect(exec_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickExecution); + rtc_exe_layout->addWidget(exec_button_ptr_); + wait_button_ptr_ = new QPushButton("Wait All"); + wait_button_ptr_->setCheckable(false); + connect(wait_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickWait); + rtc_exe_layout->addWidget(wait_button_ptr_); + } + v_layout->addLayout(rtc_exe_layout); + + // statuses + auto * rtc_table_layout = new QHBoxLayout; + { + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + rtc_table_ = new QTableWidget(); + rtc_table_->setColumnCount(column_size_); + rtc_table_->setHorizontalHeaderLabels( + {"ID", "Module", "AW Safe", "Received Cmd", "AutoMode", "StartDistance", "FinishDistance"}); + rtc_table_->setVerticalHeader(vertical_header); + rtc_table_->setHorizontalHeader(horizontal_header); + rtc_table_layout->addWidget(rtc_table_); + v_layout->addLayout(rtc_table_layout); + } + setLayout(v_layout); +} + +void RTCManagerPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + client_rtc_commands_ = + raw_node_->create_client("/api/external/set/rtc_commands"); + + for (size_t i = 0; i < auto_modes_.size(); i++) { + auto & a = auto_modes_.at(i); + // auto mode + a->enable_auto_mode_cli = + raw_node_->create_client(enable_auto_mode_namespace_ + "/" + a->module_name); + } + + sub_rtc_status_ = raw_node_->create_subscription( + "/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1)); +} + +void RTCAutoMode::onChangeToAutoMode() +{ + AutoMode::Request::SharedPtr request = std::make_shared(); + request->enable = true; + enable_auto_mode_cli->async_send_request(request); + auto_manual_mode_label->setText("AutoMode"); + auto_manual_mode_label->setStyleSheet(BG_BLUE); + auto_module_button_ptr->setChecked(true); + manual_module_button_ptr->setChecked(false); +} + +void RTCAutoMode::onChangeToManualMode() +{ + AutoMode::Request::SharedPtr request = std::make_shared(); + request->enable = false; + enable_auto_mode_cli->async_send_request(request); + auto_manual_mode_label->setText("ManualMode"); + auto_manual_mode_label->setStyleSheet(BG_YELLOW); + manual_module_button_ptr->setChecked(true); + auto_module_button_ptr->setChecked(false); +} + +CooperateCommand setRTCCommandFromStatus(CooperateStatus & status) +{ + CooperateCommand cooperate_command; + cooperate_command.uuid = status.uuid; + cooperate_command.module = status.module; + cooperate_command.command = status.command_status; + return cooperate_command; +} + +void RTCManagerPanel::onClickChangeRequest(const bool is_path_change, const uint8_t command) +{ + if (!cooperate_statuses_ptr_) return; + if (cooperate_statuses_ptr_->statuses.empty()) return; + auto executable_cooperate_commands_request = std::make_shared(); + executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp; + // send coop request + for (auto status : cooperate_statuses_ptr_->statuses) { + if (is_path_change ^ isPathChangeModule(status.module.type)) continue; + CooperateCommand cooperate_command = setRTCCommandFromStatus(status); + cooperate_command.command.type = command; + executable_cooperate_commands_request->commands.emplace_back(cooperate_command); + // To consider needs to change path step by step + if (is_path_change && !status.auto_mode && status.command_status.type ^ command) { + break; + } + } + client_rtc_commands_->async_send_request(executable_cooperate_commands_request); +} + +void RTCManagerPanel::onClickCommandRequest(const uint8_t command) +{ + if (!cooperate_statuses_ptr_) return; + if (cooperate_statuses_ptr_->statuses.empty()) return; + auto executable_cooperate_commands_request = std::make_shared(); + executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp; + // send coop request + for (auto status : cooperate_statuses_ptr_->statuses) { + CooperateCommand cooperate_command = setRTCCommandFromStatus(status); + cooperate_command.command.type = command; + executable_cooperate_commands_request->commands.emplace_back(cooperate_command); + } + client_rtc_commands_->async_send_request(executable_cooperate_commands_request); +} + +void RTCManagerPanel::onClickExecuteVelChange() +{ + onClickChangeRequest(false, Command::ACTIVATE); +} +void RTCManagerPanel::onClickWaitVelChange() +{ + onClickChangeRequest(false, Command::DEACTIVATE); +} +void RTCManagerPanel::onClickExecutePathChange() +{ + onClickChangeRequest(true, Command::ACTIVATE); +} +void RTCManagerPanel::onClickWaitPathChange() +{ + onClickChangeRequest(true, Command::DEACTIVATE); +} +void RTCManagerPanel::onClickExecution() +{ + onClickCommandRequest(Command::ACTIVATE); +} +void RTCManagerPanel::onClickWait() +{ + onClickCommandRequest(Command::DEACTIVATE); +} + +void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg) +{ + cooperate_statuses_ptr_ = std::make_shared(*msg); + rtc_table_->clearContents(); + num_rtc_status_ptr_->setText( + QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); + if (msg->statuses.empty()) { + rtc_table_->update(); + return; + } + // this is to stable rtc display not to occupy too much + size_t min_display_size{5}; + size_t max_display_size{10}; + // rtc messages are already sorted by distance + rtc_table_->setRowCount( + std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); + int cnt = 0; + + auto sorted_statuses = msg->statuses; + std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { + return !status.auto_mode && !uint2bool(status.command_status.type); + }); + + for (auto status : sorted_statuses) { + if (static_cast(cnt) >= max_display_size) { + rtc_table_->update(); + return; + } + // uuid + { + std::stringstream uuid; + uuid << std::setw(4) << std::setfill('0') << static_cast(status.uuid.uuid.at(0)); + auto label = new QLabel(QString::fromStdString(uuid.str())); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(uuid.str())); + rtc_table_->setCellWidget(cnt, 0, label); + } + + // module name + { + std::string module_name = getModuleName(status.module.type); + auto label = new QLabel(QString::fromStdString(module_name)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(module_name)); + rtc_table_->setCellWidget(cnt, 1, label); + } + + // is aw safe + bool is_aw_safe = status.safe; + { + std::string is_safe = Bool2String(is_aw_safe); + auto label = new QLabel(QString::fromStdString(is_safe)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(is_safe)); + rtc_table_->setCellWidget(cnt, 2, label); + } + + // is operator safe + const bool is_execute = uint2bool(status.command_status.type); + { + std::string text = is_execute ? "EXECUTE" : "WAIT"; + if (status.auto_mode) text = "NONE"; + auto label = new QLabel(QString::fromStdString(text)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(text)); + rtc_table_->setCellWidget(cnt, 3, label); + } + + // is auto mode + const bool is_rtc_auto_mode = status.auto_mode; + { + std::string is_auto_mode = Bool2String(is_rtc_auto_mode); + auto label = new QLabel(QString::fromStdString(is_auto_mode)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(is_auto_mode)); + rtc_table_->setCellWidget(cnt, 4, label); + } + + // start distance + { + std::string start_distance = std::to_string(status.start_distance); + auto label = new QLabel(QString::fromStdString(start_distance)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(start_distance)); + rtc_table_->setCellWidget(cnt, 5, label); + } + + // finish distance + { + std::string finish_distance = std::to_string(status.finish_distance); + auto label = new QLabel(QString::fromStdString(finish_distance)); + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(finish_distance)); + rtc_table_->setCellWidget(cnt, 6, label); + } + + // add color for recognition + if (is_rtc_auto_mode || (is_aw_safe && is_execute)) { + rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_GREEN); + } else if (is_aw_safe || is_execute) { + rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_YELLOW); + } else { + rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_RED); + } + cnt++; + } + rtc_table_->update(); +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RTCManagerPanel, rviz_common::Panel) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp new file mode 100644 index 000000000..8bdaef94b --- /dev/null +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp @@ -0,0 +1,132 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 RTC_MANAGER_PANEL_HPP_ +#define RTC_MANAGER_PANEL_HPP_ + +#include +#include +#include +#include +#include + +#ifndef Q_MOC_RUN +// cpp +#include +#include +#include +#include +// ros +#include +#include + +#include +// autoware +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +namespace rviz_plugins +{ +using tier4_rtc_msgs::msg::Command; +using tier4_rtc_msgs::msg::CooperateCommand; +using tier4_rtc_msgs::msg::CooperateResponse; +using tier4_rtc_msgs::msg::CooperateStatus; +using tier4_rtc_msgs::msg::CooperateStatusArray; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::srv::AutoMode; +using tier4_rtc_msgs::srv::CooperateCommands; +using unique_identifier_msgs::msg::UUID; + +static const QString BG_BLUE = "background-color: #3dffff;"; +static const QString BG_YELLOW = "background-color: #ffff3d;"; +static const QString BG_PURPLE = "background-color: #9e3dff;"; +static const QString BG_ORANGE = "background-color: #ff7f00;"; +static const QString BG_GREEN = "background-color: #3dff3d;"; +static const QString BG_RED = "background-color: #ff3d3d;"; + +struct RTCAutoMode : public QObject +{ + Q_OBJECT + +public Q_SLOTS: + void onChangeToAutoMode(); + void onChangeToManualMode(); + +public: + std::string module_name; + QPushButton * auto_module_button_ptr; + QPushButton * manual_module_button_ptr; + QLabel * auto_manual_mode_label; + rclcpp::Client::SharedPtr enable_auto_mode_cli; +}; + +class RTCManagerPanel : public rviz_common::Panel +{ + Q_OBJECT +public Q_SLOTS: + void onClickExecution(); + void onClickWait(); + void onClickVelocityChangeRequest(); + void onClickExecutePathChange(); + void onClickWaitPathChange(); + void onClickExecuteVelChange(); + void onClickWaitVelChange(); + +public: + explicit RTCManagerPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected: + void onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg); + void onEnableService( + const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const; + void onClickCommandRequest(const uint8_t command); + void onClickChangeRequest(const bool is_path_change, const uint8_t command); + +private: + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_rtc_status_; + rclcpp::Client::SharedPtr client_rtc_commands_; + rclcpp::Client::SharedPtr enable_auto_mode_cli_; + std::vector auto_modes_; + + std::shared_ptr cooperate_statuses_ptr_; + QTableWidget * rtc_table_; + QTableWidget * auto_mode_table_; + QPushButton * path_change_button_ptr_ = {nullptr}; + QPushButton * velocity_change_button_ptr_ = {nullptr}; + QPushButton * exec_path_change_button_ptr_ = {nullptr}; + QPushButton * wait_path_change_button_ptr_ = {nullptr}; + QPushButton * exec_vel_change_button_ptr_ = {nullptr}; + QPushButton * wait_vel_change_button_ptr_ = {nullptr}; + QPushButton * exec_button_ptr_ = {nullptr}; + QPushButton * wait_button_ptr_ = {nullptr}; + QLabel * num_rtc_status_ptr_ = {nullptr}; + + size_t column_size_ = {7}; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; +}; + +} // namespace rviz_plugins + +#endif // RTC_MANAGER_PANEL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..cdc57743a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_automatic_goal_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) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/automatic_checkpoint_append_tool.cpp + src/automatic_goal_sender.cpp + src/automatic_goal_append_tool.cpp + src/automatic_goal_panel.cpp +) + +ament_auto_add_executable(automatic_goal_sender + src/automatic_goal_sender.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + launch + icons + plugins +) diff --git a/common/tier4_automatic_goal_rviz_plugin/README.md b/common/tier4_automatic_goal_rviz_plugin/README.md new file mode 100644 index 000000000..6fd626d5a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/README.md @@ -0,0 +1,98 @@ +# tier4_automatic_goal_rviz_plugin + +## Purpose + +1. Defining a `GoalsList` by adding goals using `RvizTool` (Pose on the map). + +2. Automatic execution of the created `GoalsList` from the selected goal - it can be stopped and restarted. + +3. Looping the current `GoalsList`. + +4. Saving achieved goals to a file. + +5. Plan the route to one (single) selected goal and starting that route - it can be stopped and restarted. + +6. Remove any goal from the list or clear the current route. + +7. Save the current `GoalsList` to a file and load the list from the file. + +8. The application enables/disables access to options depending on the current state. + +9. The saved `GoalsList` can be executed without using a plugin - using a node `automatic_goal_sender`. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------- | ------------------------------------------------ | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode | +| `/api/routing/state` | `autoware_adapi_v1_msgs::msg::RouteState` | The topic represents the state of route | +| `/rviz2/automatic_goal/goal` | `geometry_msgs::msgs::PoseStamped` | The topic for adding goals to GoalsList | + +### Output + +| Name | Type | Description | +| ------------------------------------------ | -------------------------------------------------- | -------------------------------------------------- | +| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous | +| `/api/operation_mode/change_to_stop` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop | +| `/api/routing/set_route_points` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | The service to set route | +| `/api/routing/clear_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | The service to clear route state | +| `/rviz2/automatic_goal/markers` | `visualization_msgs::msg::MarkerArray` | The topic to visualize goals as rviz markers | + +## HowToUse + +1. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +2. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalPanel` and press OK. + +3. Select Add a new tool. + + ![select_tool](./images/select_tool.png) + +4. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalTool` and press OK. + +5. Add goals visualization as markers to `Displays`. + + ![markers](./images/markers.png) + +6. Append goals to the `GoalsList` to be achieved using `2D Append Goal` - in such a way that routes can be planned. + +7. Start sequential planning and goal achievement by clicking `Send goals automatically` + + ![panel](./images/panel.png) + +8. You can save `GoalsList` by clicking `Save to file`. + +9. After saving, you can run the `GoalsList` without using a plugin also: + - example: `ros2 launch tier4_automatic_goal_rviz_plugin automatic_goal_sender.launch.xml goals_list_file_path:="/tmp/goals_list.yaml" goals_achieved_dir_path:="/tmp/"` + - `goals_list_file_path` - is the path to the saved `GoalsList` file to be loaded + - `goals_achieved_dir_path` - is the path to the directory where the file `goals_achieved.log` will be created and the achieved goals will be written to it + +### Hints + +If the application (Engagement) goes into `ERROR` mode (usually returns to `EDITING` later), it means that one of the services returned a calling error (`code!=0`). +In this situation, check the terminal output for more information. + +- Often it is enough to try again. +- Sometimes a clearing of the current route is required before retrying. + +## Material Design Icons + +This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0. + +Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products. + +### License + +The Material Design Icons are licensed under the Apache License, Version 2.0. You may obtain a copy of the License at: + + + +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. + +### Acknowledgments + +We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects. diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png new file mode 100644 index 000000000..4f5b4961f Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png new file mode 100644 index 000000000..6a6757371 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png new file mode 100644 index 000000000..58d12f95e Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/markers.png b/common/tier4_automatic_goal_rviz_plugin/images/markers.png new file mode 100644 index 000000000..8dd4d9d02 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/markers.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/panel.png b/common/tier4_automatic_goal_rviz_plugin/images/panel.png new file mode 100644 index 000000000..1800202ea Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/panel.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png new file mode 100644 index 000000000..61dd9e1d7 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png new file mode 100644 index 000000000..a6ddc6d24 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml new file mode 100644 index 000000000..a9af89c07 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml new file mode 100644 index 000000000..fb5331379 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -0,0 +1,38 @@ + + + + tier4_automatic_goal_rviz_plugin + 0.0.0 + The autoware automatic goal rviz plugin package + Shumpei Wakabayashi + Dawid Moszyński + Kyoichi Sugahara + Satoshi Ota + Apache License 2.0 + Dawid Moszyński + + ament_cmake_auto + autoware_cmake + autoware_adapi_v1_msgs + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + tf2 + tf2_geometry_msgs + tier4_autoware_utils + visualization_msgs + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..e9d774919 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,17 @@ + + + AutowareAutomaticGoalPanel + + + AutowareAutomaticGoalTool + + + AutowareAutomaticCheckpointTool + + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp new file mode 100644 index 000000000..4efa6fedb --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_checkpoint_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticCheckpointTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendCheckpoint"); + updateTopic(); +} + +void AutowareAutomaticCheckpointTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticCheckpointTool"), + "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta, + fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp new file mode 100644 index 000000000..4ea3fa4bd --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ +#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticCheckpointTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp new file mode 100644 index 000000000..43fc0edcc --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_goal_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticGoalTool::AutowareAutomaticGoalTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/goal", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticGoalTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendGoal"); + updateTopic(); +} + +void AutowareAutomaticGoalTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticGoalTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticGoalTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", + x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp new file mode 100644 index 000000000..6fc98cee9 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_GOAL_APPEND_TOOL_HPP_ +#define AUTOMATIC_GOAL_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticGoalTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticGoalTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_GOAL_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp new file mode 100644 index 000000000..6b8d7765a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -0,0 +1,532 @@ +// +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 "automatic_goal_panel.hpp" + +#include + +namespace rviz_plugins +{ +AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + qt_timer_ = new QTimer(this); + connect( + qt_timer_, &QTimer::timeout, this, &AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick); + + auto * h_layout = new QHBoxLayout(this); + auto * v_layout = new QVBoxLayout(this); + h_layout->addWidget(makeGoalsListGroup()); + v_layout->addWidget(makeEngagementGroup()); + v_layout->addWidget(makeRoutingGroup()); + h_layout->addLayout(v_layout); + setLayout(h_layout); +} + +// Layouts +QGroupBox * AutowareAutomaticGoalPanel::makeGoalsListGroup() +{ + auto * group = new QGroupBox("GoalsList", this); + auto * grid = new QGridLayout(group); + + load_file_btn_ptr_ = new QPushButton("Load from file", group); + connect(load_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickLoadListFromFile())); + grid->addWidget(load_file_btn_ptr_, 0, 0); + + save_file_btn_ptr_ = new QPushButton("Save to file", group); + connect(save_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickSaveListToFile())); + grid->addWidget(save_file_btn_ptr_, 1, 0); + + goals_list_widget_ptr_ = new QListWidget(group); + goals_list_widget_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(goals_list_widget_ptr_, 2, 0); + + remove_selected_goal_btn_ptr_ = new QPushButton("Remove selected", group); + connect(remove_selected_goal_btn_ptr_, SIGNAL(clicked()), SLOT(onClickRemove())); + grid->addWidget(remove_selected_goal_btn_ptr_, 3, 0); + + loop_list_btn_ptr_ = new QPushButton("Loop list", group); + loop_list_btn_ptr_->setCheckable(true); + connect(loop_list_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleLoopList(bool))); + grid->addWidget(loop_list_btn_ptr_, 4, 0); + + goals_achieved_btn_ptr_ = new QPushButton("Saving achieved goals to file", group); + goals_achieved_btn_ptr_->setCheckable(true); + connect(goals_achieved_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleSaveGoalsAchievement(bool))); + grid->addWidget(goals_achieved_btn_ptr_, 5, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareAutomaticGoalPanel::makeRoutingGroup() +{ + auto * group = new QGroupBox("Routing", this); + auto * grid = new QGridLayout(group); + + routing_label_ptr_ = new QLabel("INIT", group); + routing_label_ptr_->setMinimumSize(100, 25); + routing_label_ptr_->setAlignment(Qt::AlignCenter); + routing_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(routing_label_ptr_, 0, 0); + + clear_route_btn_ptr_ = new QPushButton("Clear planned route", group); + connect(clear_route_btn_ptr_, &QPushButton::clicked, [this]() { onClickClearRoute(); }); + grid->addWidget(clear_route_btn_ptr_, 1, 0); + group->setLayout(grid); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareAutomaticGoalPanel::makeEngagementGroup() +{ + auto * group = new QGroupBox("Engagement", this); + auto * grid = new QGridLayout(group); + + engagement_label_ptr_ = new QLabel("INITIALIZING", group); + engagement_label_ptr_->setMinimumSize(100, 25); + engagement_label_ptr_->setAlignment(Qt::AlignCenter); + engagement_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(engagement_label_ptr_, 0, 0); + + automatic_mode_btn_ptr_ = new QPushButton("Send goals automatically", group); + automatic_mode_btn_ptr_->setCheckable(true); + + connect(automatic_mode_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleAutoMode(bool))); + grid->addWidget(automatic_mode_btn_ptr_, 1, 0); + + plan_btn_ptr_ = new QPushButton("Plan to selected goal", group); + connect(plan_btn_ptr_, &QPushButton::clicked, [this] { onClickPlan(); }); + grid->addWidget(plan_btn_ptr_, 2, 0); + + start_btn_ptr_ = new QPushButton("Start current plan", group); + connect(start_btn_ptr_, &QPushButton::clicked, [this] { onClickStart(); }); + grid->addWidget(start_btn_ptr_, 3, 0); + + stop_btn_ptr_ = new QPushButton("Stop current plan", group); + connect(stop_btn_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + grid->addWidget(stop_btn_ptr_, 4, 0); + group->setLayout(grid); + + group->setLayout(grid); + return group; +} + +void AutowareAutomaticGoalPanel::showMessageBox(const QString & string) +{ + QMessageBox msg_box(this); + msg_box.setText(string); + msg_box.exec(); +} + +// Slots +void AutowareAutomaticGoalPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + pub_marker_ = raw_node_->create_publisher("~/automatic_goal/markers", 0); + sub_append_goal_ = raw_node_->create_subscription( + "~/automatic_goal/goal", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + sub_append_checkpoint_ = raw_node_->create_subscription( + "~/automatic_goal/checkpoint", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendCheckpoint, this, std::placeholders::_1)); + initCommunication(raw_node_.get()); +} + +void AutowareAutomaticGoalPanel::onToggleLoopList(bool checked) +{ + is_loop_list_on_ = checked; + updateGUI(); +} + +void AutowareAutomaticGoalPanel::onToggleSaveGoalsAchievement(bool checked) +{ + if (checked) { + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save File with GoalsList"), "/tmp/goals_achieved.log", + tr("Achieved goals (*.log)")); + goals_achieved_file_path_ = file_name.toStdString(); + } else { + goals_achieved_file_path_ = ""; + } + updateGUI(); +} + +void AutowareAutomaticGoalPanel::onToggleAutoMode(bool checked) +{ + if (checked && goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select the first goal in GoalsList"); + automatic_mode_btn_ptr_->setChecked(false); + } else { + if (checked) current_goal_ = goals_list_widget_ptr_->currentRow(); + is_automatic_mode_on_ = checked; + is_automatic_mode_on_ ? qt_timer_->start(1000) : qt_timer_->stop(); + onClickClearRoute(); // here will be set State::AUTO_NEXT or State::EDITING; + } +} + +void AutowareAutomaticGoalPanel::onClickPlan() +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList"); + return; + } + + if (callPlanToGoalIndex(cli_set_route_, goals_list_widget_ptr_->currentRow())) { + state_ = State::PLANNING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickStart() +{ + if (callService(cli_change_to_autonomous_)) { + state_ = State::STARTING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickStop() +{ + // if ERROR is set then the ego is already stopped + if (state_ == State::ERROR) { + state_ = State::STOPPED; + updateGUI(); + } else if (callService(cli_change_to_stop_)) { + state_ = State::STOPPING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickClearRoute() +{ + if (callService(cli_clear_route_)) { + state_ = State::CLEARING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickRemove() +{ + if (static_cast(goals_list_widget_ptr_->currentRow()) < goals_list_.size()) + goals_list_.erase(goals_list_.begin() + goals_list_widget_ptr_->currentRow()); + resetAchievedGoals(); + updateGUI(); + updateGoalsList(); +} + +void AutowareAutomaticGoalPanel::onClickLoadListFromFile() +{ + QString file_name = QFileDialog::getOpenFileName( + this, tr("Open File with GoalsList"), "/tmp", tr("Goal lists (*.yaml)")); + if (file_name.count() > 0) loadGoalsList(file_name.toStdString()); +} + +void AutowareAutomaticGoalPanel::onClickSaveListToFile() +{ + if (!goals_list_.empty()) { + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save File with GoalsList"), "/tmp/goals_list.yaml", tr("Goal lists (*.yaml)")); + if (file_name.count() > 0) saveGoalsList(file_name.toStdString()); + } +} + +// Inputs +void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) +{ + if (state_ == State::EDITING) { + goals_list_.emplace_back(pose); + updateGoalsList(); + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose) +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList before set checkpoint"); + return; + } + + current_goal_ = goals_list_widget_ptr_->currentRow(); + if (current_goal_ >= goals_list_.size()) { + return; + } + + goals_list_.at(current_goal_).checkpoint_pose_ptrs.push_back(pose); + publishMarkers(); +} + +// Override +void AutowareAutomaticGoalPanel::onCallResult() +{ + updateGUI(); +} +void AutowareAutomaticGoalPanel::onGoalListUpdated() +{ + goals_list_widget_ptr_->clear(); + for (auto const & goal : goals_achieved_) { + auto * item = + new QListWidgetItem(QString::fromStdString(goal.second.first), goals_list_widget_ptr_); + goals_list_widget_ptr_->addItem(item); + updateGoalIcon(goals_list_widget_ptr_->count() - 1, QColor("lightGray")); + } + publishMarkers(); +} +void AutowareAutomaticGoalPanel::onOperationModeUpdated(const OperationModeState::ConstSharedPtr) +{ + updateGUI(); +} +void AutowareAutomaticGoalPanel::onRouteUpdated(const RouteState::ConstSharedPtr msg) +{ + std::pair style; + if (msg->state == RouteState::UNSET) + style = std::make_pair("UNSET", "background-color: #FFFF00;"); // yellow + else if (msg->state == RouteState::SET) + style = std::make_pair("SET", "background-color: #00FF00;"); // green + else if (msg->state == RouteState::ARRIVED) + style = std::make_pair("ARRIVED", "background-color: #FFA500;"); // orange + else if (msg->state == RouteState::CHANGING) + style = std::make_pair("CHANGING", "background-color: #FFFF00;"); // yellow + else + style = std::make_pair("UNKNOWN", "background-color: #FF0000;"); // red + + updateLabel(routing_label_ptr_, style.first, style.second); + updateGUI(); +} + +void AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick() +{ + if (is_automatic_mode_on_) { + if (state_ == State::AUTO_NEXT) { + // end if loop is off + if (current_goal_ >= goals_list_.size() && !is_loop_list_on_) { + disableAutomaticMode(); + return; + } + // plan to next goal + current_goal_ = current_goal_ % goals_list_.size(); + if (callPlanToGoalIndex(cli_set_route_, current_goal_)) { + state_ = State::PLANNING; + updateGUI(); + } + } else if (state_ == State::PLANNED) { + updateGoalIcon(current_goal_, QColor("yellow")); + onClickStart(); + } else if (state_ == State::ARRIVED) { + goals_achieved_[current_goal_].second++; + updateAchievedGoalsFile(current_goal_); + updateGoalIcon(current_goal_++, QColor("green")); + onClickClearRoute(); // will be set AUTO_NEXT as next state_ + } else if (state_ == State::STOPPED || state_ == State::ERROR) { + disableAutomaticMode(); + } + } +} + +// Visual updates +void AutowareAutomaticGoalPanel::updateGUI() +{ + deactivateButton(automatic_mode_btn_ptr_); + deactivateButton(remove_selected_goal_btn_ptr_); + deactivateButton(clear_route_btn_ptr_); + deactivateButton(plan_btn_ptr_); + deactivateButton(start_btn_ptr_); + deactivateButton(stop_btn_ptr_); + deactivateButton(load_file_btn_ptr_); + deactivateButton(save_file_btn_ptr_); + deactivateButton(loop_list_btn_ptr_); + deactivateButton(goals_achieved_btn_ptr_); + + std::pair style; + switch (state_) { + case State::EDITING: + activateButton(load_file_btn_ptr_); + if (!goals_list_.empty()) { + activateButton(goals_achieved_btn_ptr_); + activateButton(plan_btn_ptr_); + activateButton(remove_selected_goal_btn_ptr_); + activateButton(automatic_mode_btn_ptr_); + activateButton(save_file_btn_ptr_); + activateButton(loop_list_btn_ptr_); + } + style = std::make_pair("EDITING", "background-color: #FFFF00;"); + break; + case State::PLANNED: + activateButton(start_btn_ptr_); + activateButton(clear_route_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("PLANNED", "background-color: #00FF00;"); + break; + case State::STARTED: + activateButton(stop_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("STARTED", "background-color: #00FF00;"); + break; + case State::STOPPED: + activateButton(start_btn_ptr_); + activateButton(automatic_mode_btn_ptr_); + activateButton(clear_route_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("STOPPED", "background-color: #00FF00;"); + break; + case State::ARRIVED: + if (!is_automatic_mode_on_) onClickClearRoute(); // will be set EDITING as next state_ + break; + case State::CLEARED: + is_automatic_mode_on_ ? state_ = State::AUTO_NEXT : state_ = State::EDITING; + updateGUI(); + break; + case State::ERROR: + activateButton(stop_btn_ptr_); + if (!goals_list_.empty()) activateButton(save_file_btn_ptr_); + style = std::make_pair("ERROR", "background-color: #FF0000;"); + break; + case State::PLANNING: + activateButton(clear_route_btn_ptr_); + style = std::make_pair("PLANNING", "background-color: #FFA500;"); + break; + case State::STARTING: + style = std::make_pair("STARTING", "background-color: #FFA500;"); + break; + case State::STOPPING: + style = std::make_pair("STOPPING", "background-color: #FFA500;"); + break; + case State::CLEARING: + style = std::make_pair("CLEARING", "background-color: #FFA500;"); + break; + default: + break; + } + + automatic_mode_btn_ptr_->setStyleSheet(""); + loop_list_btn_ptr_->setStyleSheet(""); + goals_achieved_btn_ptr_->setStyleSheet(""); + if (is_automatic_mode_on_) automatic_mode_btn_ptr_->setStyleSheet("background-color: green"); + if (is_loop_list_on_) loop_list_btn_ptr_->setStyleSheet("background-color: green"); + if (!goals_achieved_file_path_.empty()) + goals_achieved_btn_ptr_->setStyleSheet("background-color: green"); + + updateLabel(engagement_label_ptr_, style.first, style.second); +} + +void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const QColor & color) +{ + QPixmap pixmap(24, 24); + pixmap.fill(color); + QPainter painter(&pixmap); + painter.setPen(QColor("black")); + painter.setFont(QFont("fixed-width", 10)); + QString text = QString::number(goals_achieved_[goal_index].second); + painter.drawText(QRect(0, 0, 24, 24), Qt::AlignCenter, text); + QIcon icon(pixmap); + goals_list_widget_ptr_->item(static_cast(goal_index))->setIcon(icon); +} + +void AutowareAutomaticGoalPanel::publishMarkers() +{ + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + + MarkerArray text_array; + MarkerArray arrow_array; + // Clear existing + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + pub_marker_->publish(text_array); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + arrow_array.markers.push_back(marker); + pub_marker_->publish(arrow_array); + } + + text_array.markers.clear(); + arrow_array.markers.clear(); + + const auto push_arrow_marker = [&](const auto & pose, const auto & color, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", id, Marker::ARROW, + createMarkerScale(1.6, 0.5, 0.5), color); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = false; + arrow_array.markers.push_back(marker); + }; + + const auto push_text_marker = [&](const auto & pose, const auto & text, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(1.6, 1.6, 1.6), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = false; + marker.text = text; + text_array.markers.push_back(marker); + }; + + // Publish current + size_t id = 0; + for (size_t i = 0; i < goals_list_.size(); ++i) { + { + const auto pose = goals_list_.at(i).goal_pose_ptr->pose; + push_arrow_marker(pose, createMarkerColor(0.0, 1.0, 0.0, 0.999), id++); + push_text_marker(pose, "Goal:" + std::to_string(i), id++); + } + + for (size_t j = 0; j < goals_list_.at(i).checkpoint_pose_ptrs.size(); ++j) { + const auto pose = goals_list_.at(i).checkpoint_pose_ptrs.at(j)->pose; + push_arrow_marker(pose, createMarkerColor(1.0, 1.0, 0.0, 0.999), id++); + push_text_marker( + pose, "Checkpoint:" + std::to_string(i) + "[Goal:" + std::to_string(j) + "]", id++); + } + } + pub_marker_->publish(text_array); + pub_marker_->publish(arrow_array); +} + +// File +void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) +{ + YAML::Node node; + for (unsigned i = 0; i < goals_list_.size(); ++i) { + node[i]["position_x"] = goals_list_[i].goal_pose_ptr->pose.position.x; + node[i]["position_y"] = goals_list_[i].goal_pose_ptr->pose.position.y; + node[i]["position_z"] = goals_list_[i].goal_pose_ptr->pose.position.z; + node[i]["orientation_x"] = goals_list_[i].goal_pose_ptr->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i].goal_pose_ptr->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i].goal_pose_ptr->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i].goal_pose_ptr->pose.orientation.w; + } + std::ofstream file_out(file_path); + file_out << node; + file_out.close(); +} + +} // namespace rviz_plugins +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalPanel, rviz_common::Panel) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp new file mode 100644 index 000000000..72a5bd4fb --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -0,0 +1,151 @@ +// +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 AUTOMATIC_GOAL_PANEL_HPP_ +#define AUTOMATIC_GOAL_PANEL_HPP_ + +#include "automatic_goal_sender.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +class AutowareAutomaticGoalPanel : public rviz_common::Panel, + public automatic_goal::AutowareAutomaticGoalSender +{ + using State = automatic_goal::AutomaticGoalState; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + Q_OBJECT + +public: + explicit AutowareAutomaticGoalPanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onToggleLoopList(bool checked); + void onToggleAutoMode(bool checked); + void onToggleSaveGoalsAchievement(bool checked); + void onClickPlan(); + void onClickStart(); + void onClickStop(); + void onClickClearRoute(); + void onClickRemove(); + void onClickLoadListFromFile(); + void onClickSaveListToFile(); + +private: + // Override + void updateAutoExecutionTimerTick() override; + void onRouteUpdated(const RouteState::ConstSharedPtr msg) override; + void onOperationModeUpdated(const OperationModeState::ConstSharedPtr msg) override; + void onCallResult() override; + void onGoalListUpdated() override; + + // Inputs + void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + void onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose); + + // Visual updates + void updateGUI(); + void updateGoalIcon(const unsigned goal_index, const QColor & color); + void publishMarkers(); + void showMessageBox(const QString & string); + void disableAutomaticMode() { automatic_mode_btn_ptr_->setChecked(false); } + static void activateButton(QAbstractButton * btn) { btn->setEnabled(true); } + static void deactivateButton(QAbstractButton * btn) { btn->setEnabled(false); } + static void updateLabel(QLabel * label, const QString & text, const QString & style_sheet) + { + label->setText(text); + label->setStyleSheet(style_sheet); + } + // File + void saveGoalsList(const std::string & file); + + // Pub/Sub + rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_checkpoint_{nullptr}; + + // Containers + rclcpp::Node::SharedPtr raw_node_{nullptr}; + bool is_automatic_mode_on_{false}; + bool is_loop_list_on_{false}; + + // QT Containers + QGroupBox * makeGoalsListGroup(); + QGroupBox * makeRoutingGroup(); + QGroupBox * makeEngagementGroup(); + QTimer * qt_timer_{nullptr}; + QListWidget * goals_list_widget_ptr_{nullptr}; + QLabel * routing_label_ptr_{nullptr}; + QLabel * operation_mode_label_ptr_{nullptr}; + QLabel * engagement_label_ptr_{nullptr}; + QPushButton * loop_list_btn_ptr_{nullptr}; + QPushButton * goals_achieved_btn_ptr_{nullptr}; + QPushButton * load_file_btn_ptr_{nullptr}; + QPushButton * save_file_btn_ptr_{nullptr}; + QPushButton * automatic_mode_btn_ptr_{nullptr}; + QPushButton * remove_selected_goal_btn_ptr_{nullptr}; + QPushButton * clear_route_btn_ptr_{nullptr}; + QPushButton * plan_btn_ptr_{nullptr}; + QPushButton * start_btn_ptr_{nullptr}; + QPushButton * stop_btn_ptr_{nullptr}; +}; +} // namespace rviz_plugins + +#endif // AUTOMATIC_GOAL_PANEL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp new file mode 100644 index 000000000..3ca368a3b --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -0,0 +1,227 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 "automatic_goal_sender.hpp" + +namespace automatic_goal +{ +AutowareAutomaticGoalSender::AutowareAutomaticGoalSender() : Node("automatic_goal_sender") +{ +} + +void AutowareAutomaticGoalSender::init() +{ + loadParams(this); + initCommunication(this); + loadGoalsList(goals_list_file_path_); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&AutowareAutomaticGoalSender::updateAutoExecutionTimerTick, this)); + + // Print info + RCLCPP_INFO_STREAM(get_logger(), "GoalsList has been loaded from: " << goals_list_file_path_); + for (auto const & goal : goals_achieved_) + RCLCPP_INFO_STREAM(get_logger(), "Loaded: " << goal.second.first); + RCLCPP_INFO_STREAM( + get_logger(), "Achieved goals will be saved in: " << goals_achieved_file_path_); +} + +void AutowareAutomaticGoalSender::loadParams(rclcpp::Node * node) +{ + namespace fs = std::filesystem; + node->declare_parameter("goals_list_file_path", ""); + node->declare_parameter("goals_achieved_dir_path", ""); + // goals_list + goals_list_file_path_ = node->get_parameter("goals_list_file_path").as_string(); + if (!fs::exists(goals_list_file_path_) || !fs::is_regular_file(goals_list_file_path_)) + throw std::invalid_argument( + "goals_list_file_path parameter - file path is invalid: " + goals_list_file_path_); + // goals_achieved + goals_achieved_file_path_ = node->get_parameter("goals_achieved_dir_path").as_string(); + if (!fs::exists(goals_achieved_file_path_) || fs::is_regular_file(goals_achieved_file_path_)) + throw std::invalid_argument( + "goals_achieved_dir_path - directory path is invalid: " + goals_achieved_file_path_); + goals_achieved_file_path_ += "goals_achieved.log"; +} + +void AutowareAutomaticGoalSender::initCommunication(rclcpp::Node * node) +{ + // Executing + sub_operation_mode_ = node->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareAutomaticGoalSender::onOperationMode, this, std::placeholders::_1)); + + cli_change_to_autonomous_ = + node->create_client("/api/operation_mode/change_to_autonomous"); + + cli_change_to_stop_ = + node->create_client("/api/operation_mode/change_to_stop"); + + // Planning + sub_route_ = node->create_subscription( + "/api/routing/state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareAutomaticGoalSender::onRoute, this, std::placeholders::_1)); + + cli_clear_route_ = node->create_client("/api/routing/clear_route"); + + cli_set_route_ = node->create_client("/api/routing/set_route_points"); +} + +// Sub +void AutowareAutomaticGoalSender::onRoute(const RouteState::ConstSharedPtr msg) +{ + if (msg->state == RouteState::UNSET && state_ == State::CLEARING) + state_ = State::CLEARED; + else if (msg->state == RouteState::SET && state_ == State::PLANNING) + state_ = State::PLANNED; + else if (msg->state == RouteState::ARRIVED && state_ == State::STARTED) + state_ = State::ARRIVED; + onRouteUpdated(msg); +} + +void AutowareAutomaticGoalSender::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + if (msg->mode == OperationModeState::STOP && state_ == State::INITIALIZING) + state_ = State::EDITING; + else if (msg->mode == OperationModeState::STOP && state_ == State::STOPPING) + state_ = State::STOPPED; + else if (msg->mode == OperationModeState::AUTONOMOUS && state_ == State::STARTING) + state_ = State::STARTED; + onOperationModeUpdated(msg); +} + +// Update +void AutowareAutomaticGoalSender::updateGoalsList() +{ + unsigned i = 0; + for (const auto & goal : goals_list_) { + std::stringstream ss; + ss << std::fixed << std::setprecision(2); + tf2::Quaternion tf2_quat; + tf2::convert(goal.goal_pose_ptr->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal.goal_pose_ptr->pose.position.x << ", "; + ss << goal.goal_pose_ptr->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); + } + onGoalListUpdated(); +} + +void AutowareAutomaticGoalSender::updateAutoExecutionTimerTick() +{ + auto goal = goals_achieved_[current_goal_].first; + + if (state_ == State::INITIALIZING) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, "Initializing... waiting for OperationModeState::STOP"); + + } else if (state_ == State::EDITING) { // skip the editing step by default + state_ = State::AUTO_NEXT; + + } else if (state_ == State::AUTO_NEXT) { // plan to next goal + RCLCPP_INFO_STREAM(get_logger(), goal << ": Goal set as the next. Planning in progress..."); + if (callPlanToGoalIndex(cli_set_route_, current_goal_)) state_ = State::PLANNING; + + } else if (state_ == State::PLANNED) { // start plan to next goal + RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been planned. Route starting..."); + if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; + + } else if (state_ == State::STARTED) { + RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, goal << ": Driving to the goal."); + + } else if (state_ == State::ARRIVED) { // clear plan after achieving next goal, goal++ + RCLCPP_INFO_STREAM( + get_logger(), goal << ": Goal has been ACHIEVED " << ++goals_achieved_[current_goal_].second + << " times. Route clearing..."); + updateAchievedGoalsFile(current_goal_); + if (callService(cli_clear_route_)) state_ = State::CLEARING; + + } else if (state_ == State::CLEARED) { + RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been cleared."); + current_goal_++; + current_goal_ = current_goal_ % goals_list_.size(); + state_ = State::AUTO_NEXT; + + } else if (state_ == State::STOPPED) { + RCLCPP_WARN_STREAM( + get_logger(), goal << ": The execution has been stopped unexpectedly! Restarting..."); + if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; + + } else if (state_ == State::ERROR) { + timer_->cancel(); + throw std::runtime_error(goal + ": Error encountered during execution!"); + } +} + +// File +void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) +{ + YAML::Node node = YAML::LoadFile(file_path); + goals_list_.clear(); + for (auto && goal : node) { + std::shared_ptr pose = std::make_shared(); + pose->header.frame_id = "map"; + pose->header.stamp = rclcpp::Time(); + pose->pose.position.x = goal["position_x"].as(); + pose->pose.position.y = goal["position_y"].as(); + pose->pose.position.z = goal["position_z"].as(); + pose->pose.orientation.x = goal["orientation_x"].as(); + pose->pose.orientation.y = goal["orientation_y"].as(); + pose->pose.orientation.z = goal["orientation_z"].as(); + pose->pose.orientation.w = goal["orientation_w"].as(); + goals_list_.emplace_back(pose); + } + resetAchievedGoals(); + updateGoalsList(); +} + +void AutowareAutomaticGoalSender::updateAchievedGoalsFile(const unsigned goal_index) +{ + if (!goals_achieved_file_path_.empty()) { + std::ofstream out(goals_achieved_file_path_, std::fstream::app); + std::stringstream ss; + ss << "[" << getTimestamp() << "] Achieved: " << goals_achieved_[goal_index].first; + ss << ", Current number of achievements: " << goals_achieved_[goal_index].second << "\n"; + out << ss.str(); + out.close(); + } +} + +void AutowareAutomaticGoalSender::resetAchievedGoals() +{ + goals_achieved_.clear(); + if (!goals_achieved_file_path_.empty()) { + std::ofstream out(goals_achieved_file_path_, std::fstream::app); + out << "[" << getTimestamp() + << "] GoalsList was loaded from a file or a goal was removed - counters have been reset\n"; + out.close(); + } +} +} // namespace automatic_goal + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node{nullptr}; + try { + node = std::make_shared(); + node->init(); + } catch (const std::exception & e) { + fprintf(stderr, "%s Exiting...\n", e.what()); + return 1; + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp new file mode 100644 index 000000000..88b7b5e7d --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -0,0 +1,184 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 AUTOMATIC_GOAL_SENDER_HPP_ +#define AUTOMATIC_GOAL_SENDER_HPP_ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace automatic_goal +{ +enum class AutomaticGoalState { + INITIALIZING, + EDITING, + PLANNING, + PLANNED, + STARTING, + STARTED, + ARRIVED, + AUTO_NEXT, + STOPPING, + STOPPED, + CLEARING, + CLEARED, + ERROR, +}; + +class AutowareAutomaticGoalSender : public rclcpp::Node +{ + using State = AutomaticGoalState; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + +public: + AutowareAutomaticGoalSender(); + void init(); + +protected: + void initCommunication(rclcpp::Node * node); + // Calls + bool callPlanToGoalIndex( + const rclcpp::Client::SharedPtr client, const unsigned goal_index) + { + if (!client->service_is_ready()) { + RCLCPP_WARN(get_logger(), "SetRoutePoints client is unavailable"); + return false; + } + + auto req = std::make_shared(); + req->header = goals_list_.at(goal_index).goal_pose_ptr->header; + req->goal = goals_list_.at(goal_index).goal_pose_ptr->pose; + for (const auto & checkpoint : goals_list_.at(goal_index).checkpoint_pose_ptrs) { + req->waypoints.push_back(checkpoint->pose); + } + client->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + if (result.get()->status.code != 0) state_ = State::ERROR; + printCallResult(result); + onCallResult(); + }); + return true; + } + template + bool callService(const typename rclcpp::Client::SharedPtr client) + { + if (!client->service_is_ready()) { + RCLCPP_WARN(get_logger(), "Client is unavailable"); + return false; + } + + auto req = std::make_shared(); + client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { + if (result.get()->status.code != 0) state_ = State::ERROR; + printCallResult(result); + onCallResult(); + }); + return true; + } + template + void printCallResult(typename rclcpp::Client::SharedFuture result) + { + if (result.get()->status.code != 0) { + RCLCPP_ERROR( + get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), + result.get()->status.code, result.get()->status.message.c_str()); + } else { + RCLCPP_DEBUG( + get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), + result.get()->status.code, result.get()->status.message.c_str()); + } + } + + struct Route + { + explicit Route(const PoseStamped::ConstSharedPtr & goal) : goal_pose_ptr{goal} {} + PoseStamped::ConstSharedPtr goal_pose_ptr{}; + std::vector checkpoint_pose_ptrs{}; + }; + + // Update + void updateGoalsList(); + virtual void updateAutoExecutionTimerTick(); + + // File + void loadGoalsList(const std::string & file_path); + void updateAchievedGoalsFile(const unsigned goal_index); + void resetAchievedGoals(); + static std::string getTimestamp() + { + char buffer[128]; + std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::strftime(&buffer[0], sizeof(buffer), "%Y-%m-%d %H:%M:%S", std::localtime(&now)); + return std::string{buffer}; + } + + // Sub + void onRoute(const RouteState::ConstSharedPtr msg); + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + + // Interface + virtual void onRouteUpdated(const RouteState::ConstSharedPtr) {} + virtual void onOperationModeUpdated(const OperationModeState::ConstSharedPtr) {} + virtual void onCallResult() {} + virtual void onGoalListUpdated() {} + + // Cli + rclcpp::Client::SharedPtr cli_change_to_autonomous_{nullptr}; + rclcpp::Client::SharedPtr cli_change_to_stop_{nullptr}; + rclcpp::Client::SharedPtr cli_clear_route_{nullptr}; + rclcpp::Client::SharedPtr cli_set_route_{nullptr}; + + // Containers + unsigned current_goal_{0}; + State state_{State::INITIALIZING}; + std::vector goals_list_{}; + std::map> goals_achieved_{}; + std::string goals_achieved_file_path_{}; + +private: + void loadParams(rclcpp::Node * node); + + // Sub + rclcpp::Subscription::SharedPtr sub_route_{nullptr}; + rclcpp::Subscription::SharedPtr sub_operation_mode_{nullptr}; + + // Containers + std::string goals_list_file_path_{}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; +}; +} // namespace automatic_goal +#endif // AUTOMATIC_GOAL_SENDER_HPP_ diff --git a/common/tier4_calibration_rviz_plugin/CMakeLists.txt b/common/tier4_calibration_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..6b03fe92d --- /dev/null +++ b/common/tier4_calibration_rviz_plugin/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_calibration_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) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/accel_brake_map_calibrator_button_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_calibration_rviz_plugin/package.xml b/common/tier4_calibration_rviz_plugin/package.xml new file mode 100644 index 000000000..f422847d8 --- /dev/null +++ b/common/tier4_calibration_rviz_plugin/package.xml @@ -0,0 +1,29 @@ + + + + tier4_calibration_rviz_plugin + 0.1.0 + The accel_brake_map_calibrator_button_panel package + Tomoya Kimura + Apache License 2.0 + + Tomoya Kimura + + ament_cmake_auto + autoware_cmake + + libqt5-core + libqt5-widgets + qtbase5-dev + rviz_common + std_msgs + tier4_vehicle_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml b/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..82f35b934 --- /dev/null +++ b/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + execution button of accel brake map calibration. + + + diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp new file mode 100644 index 000000000..d89f13ce7 --- /dev/null +++ b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp @@ -0,0 +1,185 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 "accel_brake_map_calibrator_button_panel.hpp" + +#include "QFileDialog" +#include "QHBoxLayout" +#include "QLineEdit" +#include "QPainter" +#include "QPushButton" +#include "pluginlib/class_list_macros.hpp" +#include "rviz_common/display_context.hpp" + +#include +#include +#include +#include + +namespace tier4_calibration_rviz_plugin +{ +AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + topic_label_ = new QLabel("topic: "); + topic_label_->setAlignment(Qt::AlignCenter); + + topic_edit_ = + new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/output/update_suggest"); + connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); + + service_label_ = new QLabel("service: "); + service_label_->setAlignment(Qt::AlignCenter); + + service_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/update_map_dir"); + connect(service_edit_, SIGNAL(textEdited(QString)), SLOT(editService())); + + calibration_button_ = new QPushButton("Wait for subscribe topic"); + calibration_button_->setEnabled(false); + connect(calibration_button_, SIGNAL(clicked(bool)), SLOT(pushCalibrationButton())); + + status_label_ = new QLabel("Not Ready"); + status_label_->setAlignment(Qt::AlignCenter); + status_label_->setStyleSheet("QLabel { background-color : darkgray;}"); + + QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding); + calibration_button_->setSizePolicy(q_size_policy); + + auto * topic_layout = new QHBoxLayout; + topic_layout->addWidget(topic_label_); + topic_layout->addWidget(topic_edit_); + + auto * service_layout = new QHBoxLayout; + service_layout->addWidget(service_label_); + service_layout->addWidget(service_edit_); + + auto * v_layout = new QVBoxLayout; + v_layout->addLayout(topic_layout); + v_layout->addLayout(service_layout); + v_layout->addWidget(calibration_button_); + v_layout->addWidget(status_label_); + + setLayout(v_layout); +} + +void AccelBrakeMapCalibratorButtonPanel::onInitialize() +{ + rclcpp::Node::SharedPtr raw_node = + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + update_suggest_sub_ = raw_node->create_subscription( + topic_edit_->text().toStdString(), 10, + std::bind( + &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); + + client_ = raw_node->create_client( + service_edit_->text().toStdString()); +} + +void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( + const std_msgs::msg::Bool::ConstSharedPtr msg) +{ + if (after_calib_) { + return; + } + + if (!client_ || !client_->service_is_ready()) { + calibration_button_->setText("wait for service"); + calibration_button_->setEnabled(false); + return; + } + + if (msg->data) { + status_label_->setText("Ready"); + status_label_->setStyleSheet("QLabel { background-color : white;}"); + } else { + status_label_->setText("Ready (not recommended)"); + status_label_->setStyleSheet("QLabel { background-color : darkgray;}"); + } + calibration_button_->setText("push: start to accel/brake map calibration"); + calibration_button_->setEnabled(true); +} + +void AccelBrakeMapCalibratorButtonPanel::editTopic() +{ + rclcpp::Node::SharedPtr raw_node = + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + try { + update_suggest_sub_.reset(); + update_suggest_sub_ = raw_node->create_subscription( + topic_edit_->text().toStdString(), 10, + std::bind( + &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); + } catch (const rclcpp::exceptions::InvalidTopicNameError & e) { + RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what()); + } + calibration_button_->setText("Wait for subscribe topic"); + calibration_button_->setEnabled(false); +} + +void AccelBrakeMapCalibratorButtonPanel::editService() +{ + rclcpp::Node::SharedPtr raw_node = + this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + try { + client_.reset(); + client_ = raw_node->create_client( + service_edit_->text().toStdString()); + } catch (const rclcpp::exceptions::InvalidServiceNameError & e) { + RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what()); + } +} + +void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton() +{ + // lock button + calibration_button_->setEnabled(false); + + status_label_->setStyleSheet("QLabel { background-color : blue;}"); + status_label_->setText("executing calibration..."); + + std::thread thread([this] { + if (!client_->wait_for_service(std::chrono::seconds(1))) { + status_label_->setStyleSheet("QLabel { background-color : red;}"); + status_label_->setText("service server not found"); + + } else { + auto req = std::make_shared(); + req->path = ""; + client_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client< + tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>::SharedFuture result) {}); + + status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); + status_label_->setText("OK!!!"); + } + + // wait 3 second + after_calib_ = true; + rclcpp::Rate(1.0 / 3.0).sleep(); + after_calib_ = false; + + // unlock button + calibration_button_->setEnabled(true); + }); + + thread.detach(); +} + +} // namespace tier4_calibration_rviz_plugin + +PLUGINLIB_EXPORT_CLASS( + tier4_calibration_rviz_plugin::AccelBrakeMapCalibratorButtonPanel, rviz_common::Panel) diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp new file mode 100644 index 000000000..70ebe0631 --- /dev/null +++ b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp @@ -0,0 +1,68 @@ +// +// Copyright 2020 Tier IV, Inc. All rights reserved. +// +// 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 ACCEL_BRAKE_MAP_CALIBRATOR_BUTTON_PANEL_HPP_ +#define ACCEL_BRAKE_MAP_CALIBRATOR_BUTTON_PANEL_HPP_ + +#include "QLabel" +#include "QLineEdit" +#include "QPushButton" +#include "QSettings" + +#include +#ifndef Q_MOC_RUN + +#include "rclcpp/rclcpp.hpp" +#include "rviz_common/panel.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" +#endif + +#include "std_msgs/msg/bool.hpp" +#include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" + +namespace tier4_calibration_rviz_plugin +{ +class AccelBrakeMapCalibratorButtonPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit AccelBrakeMapCalibratorButtonPanel(QWidget * parent = nullptr); + void onInitialize() override; + void callbackUpdateSuggest(const std_msgs::msg::Bool::ConstSharedPtr msg); + +public Q_SLOTS: // NOLINT for Qt + void editTopic(); + void editService(); + void pushCalibrationButton(); + +protected: + rclcpp::Subscription::SharedPtr update_suggest_sub_; + rclcpp::Client::SharedPtr client_; + + bool after_calib_ = false; + + QLabel * topic_label_; + QLineEdit * topic_edit_; + QLabel * service_label_; + QLineEdit * service_edit_; + QPushButton * calibration_button_; + QLabel * status_label_; +}; + +} // end namespace tier4_calibration_rviz_plugin + +#endif // ACCEL_BRAKE_MAP_CALIBRATOR_BUTTON_PANEL_HPP_ diff --git a/common/tier4_control_rviz_plugin/CMakeLists.txt b/common/tier4_control_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..95fff4559 --- /dev/null +++ b/common/tier4_control_rviz_plugin/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_control_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) +add_definitions(-DQT_NO_KEYWORDS) + +set(HEADERS + src/tools/manual_controller.hpp +) + +## Declare a C++ library +ament_auto_add_library(tier4_control_rviz_plugin SHARED + src/tools/manual_controller.cpp + ${HEADERS} +) + +target_link_libraries(tier4_control_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_control_rviz_plugin/README.md b/common/tier4_control_rviz_plugin/README.md new file mode 100644 index 000000000..8bca77771 --- /dev/null +++ b/common/tier4_control_rviz_plugin/README.md @@ -0,0 +1,40 @@ +# tier4_control_rviz_plugin + +This package is to mimic external control for simulation. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------- | ----------------------- | +| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Current GATE mode | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Current velocity status | +| `/api/autoware/get/engage` | `tier4_external_api_msgs::srv::Engage` | Getting Engage | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The state of GEAR | + +### Output + +| Name | Type | Description | +| -------------------------------- | ---------------------------------------------------------- | ----------------------- | +| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode | +| `/external/selected/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | AckermannControlCommand | +| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR | + +## Usage + +1. Start rviz and select Panels. + + ![select_panels](./images/select_panels.png) + +2. Select tier4_control_rviz_plugin/ManualController and press OK. + + ![select_manual_controller](./images/select_manual_controller.png) + +3. Enter velocity in "Set Cruise Velocity" and Press the button to confirm. You can notice that GEAR shows D (DRIVE). + + ![manual_controller_not_ready](./images/manual_controller_not_ready.png) + +4. Press "Enable Manual Control" and you can notice that "GATE" and "Engage" turn "Ready" and the vehicle starts! + + ![manual_controller_ready](./images/manual_controller_ready.png) diff --git a/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png b/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png new file mode 100644 index 000000000..e4a8ddb0b Binary files /dev/null and b/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png differ diff --git a/common/tier4_control_rviz_plugin/images/manual_controller_ready.png b/common/tier4_control_rviz_plugin/images/manual_controller_ready.png new file mode 100644 index 000000000..d5da7f064 Binary files /dev/null and b/common/tier4_control_rviz_plugin/images/manual_controller_ready.png differ diff --git a/common/tier4_control_rviz_plugin/images/select_manual_controller.png b/common/tier4_control_rviz_plugin/images/select_manual_controller.png new file mode 100644 index 000000000..5027ec571 Binary files /dev/null and b/common/tier4_control_rviz_plugin/images/select_manual_controller.png differ diff --git a/common/tier4_control_rviz_plugin/images/select_panels.png b/common/tier4_control_rviz_plugin/images/select_panels.png new file mode 100644 index 000000000..a691602c4 Binary files /dev/null and b/common/tier4_control_rviz_plugin/images/select_panels.png differ diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml new file mode 100644 index 000000000..73562a766 --- /dev/null +++ b/common/tier4_control_rviz_plugin/package.xml @@ -0,0 +1,32 @@ + + + + tier4_control_rviz_plugin + 0.1.0 + The tier4_vehicle_rviz_plugin package + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + rviz_ogre_vendor + tier4_autoware_utils + tier4_control_msgs + tier4_external_api_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/tier4_control_rviz_plugin/plugins/plugin_description.xml b/common/tier4_control_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..068bbcd73 --- /dev/null +++ b/common/tier4_control_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,7 @@ + + + ManualController + + diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp new file mode 100644 index 000000000..8bbb096f7 --- /dev/null +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -0,0 +1,261 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// 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 "manual_controller.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +using std::placeholders::_1; + +namespace rviz_plugins +{ + +ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent) +{ + auto * state_layout = new QHBoxLayout; + { + // Enable Button + enable_button_ptr_ = new QPushButton("Enable Manual Control"); + connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickEnableButton())); + state_layout->addWidget(enable_button_ptr_); + + // Gate Mode + auto * gate_prefix_label_ptr = new QLabel("GATE: "); + gate_prefix_label_ptr->setAlignment(Qt::AlignRight); + gate_mode_label_ptr_ = new QLabel("INIT"); + gate_mode_label_ptr_->setAlignment(Qt::AlignCenter); + state_layout->addWidget(gate_prefix_label_ptr); + state_layout->addWidget(gate_mode_label_ptr_); + + // Engage Status + auto * engage_prefix_label_ptr = new QLabel("Engage: "); + engage_prefix_label_ptr->setAlignment(Qt::AlignRight); + engage_status_label_ptr_ = new QLabel("INIT"); + engage_status_label_ptr_->setAlignment(Qt::AlignCenter); + state_layout->addWidget(engage_prefix_label_ptr); + state_layout->addWidget(engage_status_label_ptr_); + + // Gear + auto * gear_prefix_label_ptr = new QLabel("GEAR: "); + gear_prefix_label_ptr->setAlignment(Qt::AlignRight); + gear_label_ptr_ = new QLabel("INIT"); + gear_label_ptr_->setAlignment(Qt::AlignCenter); + state_layout->addWidget(gear_prefix_label_ptr); + state_layout->addWidget(gear_label_ptr_); + } + + auto * cruise_velocity_layout = new QHBoxLayout(); + // Velocity Limit + { + cruise_velocity_button_ptr_ = new QPushButton("Set Cruise Velocity"); + cruise_velocity_input_ = new QSpinBox(); + cruise_velocity_input_->setRange(-100.0, 100.0); + cruise_velocity_input_->setValue(0.0); + cruise_velocity_input_->setSingleStep(5.0); + connect(cruise_velocity_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickCruiseVelocity())); + cruise_velocity_layout->addWidget(cruise_velocity_button_ptr_); + cruise_velocity_layout->addWidget(cruise_velocity_input_); + cruise_velocity_layout->addWidget(new QLabel(" [km/h]")); + } + + steering_slider_ptr_ = new QDial(); + steering_slider_ptr_->setRange(-90, 90); + steering_slider_ptr_->setValue(0.0); + connect(steering_slider_ptr_, SIGNAL(valueChanged(int)), this, SLOT(onManualSteering())); + steering_angle_ptr_ = new QLabel(); + cruise_velocity_layout->addWidget(new QLabel("steering ")); + cruise_velocity_layout->addWidget(steering_slider_ptr_); + cruise_velocity_layout->addWidget(steering_angle_ptr_); + cruise_velocity_layout->addWidget(new QLabel(" [deg]")); + + // Layout + auto * v_layout = new QVBoxLayout; + v_layout->addLayout(state_layout); + v_layout->addLayout(cruise_velocity_layout); + setLayout(v_layout); + + auto * timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &ManualController::update); + timer->start(30); +} + +void ManualController::update() +{ + if (!raw_node_) return; + AckermannControlCommand ackermann; + { + ackermann.stamp = raw_node_->get_clock()->now(); + ackermann.lateral.steering_tire_angle = steering_angle_; + ackermann.longitudinal.speed = cruise_velocity_; + /** + * @brief Calculate desired acceleration by simple BackSteppingControl + * V = 0.5*(v-v_des)^2 >= 0 + * D[V] = (D[v] - a_des)*(v-v_des) <=0 + * a_des = k_const *(v - v_des) + a (k < 0 ) + */ + const double k = -0.5; + const double v = current_velocity_; + const double v_des = cruise_velocity_; + const double a = current_acceleration_; + const double a_des = k * (v - v_des) + a; + ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); + } + GearCommand gear_cmd; + { + const double eps = 0.001; + if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) { + gear_cmd.command = GearCommand::DRIVE; + } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) { + gear_cmd.command = GearCommand::REVERSE; + ackermann.longitudinal.acceleration *= -1.0; + } else { + gear_cmd.command = GearCommand::PARK; + } + } + pub_control_command_->publish(ackermann); + pub_gear_cmd_->publish(gear_cmd); +} + +void ManualController::onManualSteering() +{ + const double scale_factor = -0.25; + steering_angle_ = scale_factor * steering_slider_ptr_->sliderPosition() * M_PI / 180.0; + const QString steering_string = + QString::fromStdString(std::to_string(steering_angle_ * 180.0 / M_PI)); + steering_angle_ptr_->setText(steering_string); +} + +void ManualController::onClickCruiseVelocity() +{ + cruise_velocity_ = cruise_velocity_input_->value() / 3.6; +} + +void ManualController::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_gate_mode_ = raw_node_->create_subscription( + "/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1)); + + sub_velocity_ = raw_node_->create_subscription( + "/vehicle/status/velocity_status", 1, std::bind(&ManualController::onVelocity, this, _1)); + + sub_engage_ = raw_node_->create_subscription( + "/api/autoware/get/engage", 10, std::bind(&ManualController::onEngageStatus, this, _1)); + + sub_gear_ = raw_node_->create_subscription( + "/vehicle/status/gear_status", 10, std::bind(&ManualController::onGear, this, _1)); + + client_engage_ = raw_node_->create_client("/api/autoware/set/engage"); + + pub_gate_mode_ = raw_node_->create_publisher("/control/gate_mode_cmd", rclcpp::QoS(1)); + + pub_control_command_ = raw_node_->create_publisher( + "/external/selected/control_cmd", rclcpp::QoS(1)); + + pub_gear_cmd_ = raw_node_->create_publisher("/external/selected/gear_cmd", 1); +} + +void ManualController::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) +{ + switch (msg->data) { + case tier4_control_msgs::msg::GateMode::AUTO: + gate_mode_label_ptr_->setText("Not Ready"); + gate_mode_label_ptr_->setStyleSheet("background-color: #00FF00;"); + break; + + case tier4_control_msgs::msg::GateMode::EXTERNAL: + gate_mode_label_ptr_->setText("Ready"); + gate_mode_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + break; + + default: + gate_mode_label_ptr_->setText("UNKNOWN"); + gate_mode_label_ptr_->setStyleSheet("background-color: #FF0000;"); + break; + } +} +void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg) +{ + current_engage_ = msg->engage; + if (current_engage_) { + engage_status_label_ptr_->setText(QString::fromStdString("Ready")); + engage_status_label_ptr_->setStyleSheet("background-color: #FFFF00;"); + } else { + engage_status_label_ptr_->setText(QString::fromStdString("Not Ready")); + engage_status_label_ptr_->setStyleSheet("background-color: #00FF00;"); + } +} + +void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg) +{ + current_velocity_ = msg->longitudinal_velocity; +} + +void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + current_acceleration_ = msg->accel.accel.linear.x; +} + +void ManualController::onGear(const GearReport::ConstSharedPtr msg) +{ + switch (msg->report) { + case GearReport::PARK: + gear_label_ptr_->setText("P"); + break; + case GearReport::REVERSE: + gear_label_ptr_->setText("R"); + break; + case GearReport::DRIVE: + gear_label_ptr_->setText("D"); + break; + case GearReport::LOW: + gear_label_ptr_->setText("L"); + break; + } +} + +void ManualController::onClickEnableButton() +{ + // gate mode + { + pub_gate_mode_->publish(tier4_control_msgs::build().data(GateMode::EXTERNAL)); + } + // engage + { + auto req = std::make_shared(); + req->engage = true; + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + if (!client_engage_->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + client_engage_->async_send_request( + req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::ManualController, rviz_common::Panel) diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp new file mode 100644 index 000000000..aaa625bff --- /dev/null +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -0,0 +1,104 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// 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 TOOLS__MANUAL_CONTROLLER_HPP_ +#define TOOLS__MANUAL_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Twist; +using tier4_control_msgs::msg::GateMode; +using EngageSrv = tier4_external_api_msgs::srv::Engage; +using autoware_auto_vehicle_msgs::msg::Engage; +using autoware_auto_vehicle_msgs::msg::GearReport; + +class ManualController : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit ManualController(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onClickCruiseVelocity(); + void onClickEnableButton(); + void onManualSteering(); + void update(); + +protected: + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void onTimer(); + void onPublishControlCommand(); + void onGateMode(const GateMode::ConstSharedPtr msg); + void onVelocity(const VelocityReport::ConstSharedPtr msg); + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); + void onEngageStatus(const Engage::ConstSharedPtr msg); + void onGear(const GearReport::ConstSharedPtr msg); + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_engage_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Publisher::SharedPtr pub_control_command_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Client::SharedPtr client_engage_; + rclcpp::Subscription::SharedPtr sub_gear_; + + double cruise_velocity_{0.0}; + double steering_angle_{0.0}; + double current_velocity_{0.0}; + double current_acceleration_{0.0}; + + QLabel * gate_mode_label_ptr_; + QLabel * gear_label_ptr_; + QLabel * engage_status_label_ptr_; + QPushButton * enable_button_ptr_; + QPushButton * cruise_velocity_button_ptr_; + QSpinBox * cruise_velocity_input_; + QDial * steering_slider_ptr_; + QLabel * steering_angle_ptr_; + + bool current_engage_{false}; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__MANUAL_CONTROLLER_HPP_ diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..02e65ad75 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_debug_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(tier4_debug_rviz_plugin SHARED + include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp + include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp + include/tier4_debug_rviz_plugin/string_stamped.hpp + src/float32_multi_array_stamped_pie_chart.cpp + src/string_stamped.cpp + src/jsk_overlay_utils.cpp +) + +target_link_libraries(tier4_debug_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/tier4_debug_rviz_plugin/README.md b/common/tier4_debug_rviz_plugin/README.md new file mode 100644 index 000000000..918981614 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/README.md @@ -0,0 +1,12 @@ +# tier4_debug_rviz_plugin + +This package is including jsk code. +Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. + +## Plugins + +### Float32MultiArrayStampedPieChart + +Pie chart from `tier4_debug_msgs::msg::Float32MultiArrayStamped`. + +![float32_multi_array_stamped_pie_chart](./images/float32_multi_array_stamped_pie_chart.png) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png b/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png new file mode 100644 index 000000000..6a6757371 Binary files /dev/null and b/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png differ diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png new file mode 100644 index 000000000..6a6757371 Binary files /dev/null and b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png differ diff --git a/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png b/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png new file mode 100644 index 000000000..7cd7ca753 Binary files /dev/null and b/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png differ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp new file mode 100644 index 000000000..c8267c705 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp @@ -0,0 +1,172 @@ +// Copyright 2022 Tier IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace rviz_plugins +{ +class Float32MultiArrayStampedPieChartDisplay : public rviz_common::Display +{ + Q_OBJECT +public: + Float32MultiArrayStampedPieChartDisplay(); + virtual ~Float32MultiArrayStampedPieChartDisplay(); + + // methods for OverlayPickerTool + virtual bool isInRegion(int x, int y); + virtual void movePosition(int x, int y); + virtual void setPosition(int x, int y); + virtual int getX() { return left_; } + virtual int getY() { return top_; } + +protected: + virtual void subscribe(); + virtual void unsubscribe(); + virtual void onEnable(); + virtual void onDisable(); + virtual void onInitialize(); + virtual void processMessage( + const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg); + virtual void drawPlot(double val); + virtual void update(float wall_dt, float ros_dt); + // properties + rviz_common::properties::StringProperty * update_topic_property_; + rviz_common::properties::IntProperty * data_index_property_; + rviz_common::properties::IntProperty * size_property_; + rviz_common::properties::IntProperty * left_property_; + rviz_common::properties::IntProperty * top_property_; + rviz_common::properties::ColorProperty * fg_color_property_; + rviz_common::properties::ColorProperty * bg_color_property_; + rviz_common::properties::ColorProperty * text_color_property_; + rviz_common::properties::FloatProperty * fg_alpha_property_; + rviz_common::properties::FloatProperty * fg_alpha2_property_; + rviz_common::properties::FloatProperty * bg_alpha_property_; + rviz_common::properties::FloatProperty * text_alpha_property_; + rviz_common::properties::IntProperty * text_size_property_; + rviz_common::properties::FloatProperty * max_value_property_; + rviz_common::properties::FloatProperty * min_value_property_; + rviz_common::properties::BoolProperty * show_caption_property_; + rviz_common::properties::BoolProperty * auto_color_change_property_; + rviz_common::properties::ColorProperty * max_color_property_; + rviz_common::properties::ColorProperty * med_color_property_; + rviz_common::properties::FloatProperty * max_color_threshold_property_; + rviz_common::properties::FloatProperty * med_color_threshold_property_; + rviz_common::properties::BoolProperty * clockwise_rotate_property_; + + rclcpp::Subscription::SharedPtr sub_; + int left_; + int top_; + uint16_t texture_size_; + QColor fg_color_; + QColor bg_color_; + QColor max_color_; + QColor med_color_; + int text_size_; + bool show_caption_; + bool auto_color_change_; + int caption_offset_; + double fg_alpha_; + double fg_alpha2_; + double bg_alpha_; + double max_value_; + double min_value_; + double max_color_threshold_; + double med_color_threshold_; + bool update_required_; + bool first_time_; + float data_; + int data_index_{0}; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + bool clockwise_rotate_; + + std::mutex mutex_; + +protected Q_SLOTS: + void updateTopic(); + void updateDataIndex(); + void updateSize(); + void updateTop(); + void updateLeft(); + void updateBGColor(); + void updateTextSize(); + void updateFGColor(); + void updateFGAlpha(); + void updateFGAlpha2(); + void updateBGAlpha(); + void updateMinValue(); + void updateMaxValue(); + void updateShowCaption(); + void updateAutoColorChange(); + void updateMaxColor(); + void updateMedColor(); + void updateMaxColorThreshold(); + void updateMedColorThreshold(); + void updateClockwiseRotate(); + +private: +}; + +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__FLOAT32_MULTI_ARRAY_STAMPED_PIE_CHART_HPP_ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp new file mode 100644 index 000000000..37bf743e3 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp @@ -0,0 +1,143 @@ +// Copyright 2022 Tier IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__JSK_OVERLAY_UTILS_HPP_ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp new file mode 100644 index 000000000..0960875d7 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp @@ -0,0 +1,107 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace rviz_plugins +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml new file mode 100644 index 000000000..45b73d5b9 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_debug_rviz_plugin + 0.1.0 + The tier4_debug_rviz_plugin package + Takayuki Murooka + Apache License 2.0 + + ament_cmake + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + tier4_debug_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..e18900afc --- /dev/null +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,12 @@ + + + Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + + + Display drivable area of tier4_debug_msgs::msg::StringStamped + + diff --git a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp new file mode 100644 index 000000000..0187cc3e2 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp @@ -0,0 +1,479 @@ +// Copyright 2022 Tier IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include + +namespace rviz_plugins +{ + +Float32MultiArrayStampedPieChartDisplay::Float32MultiArrayStampedPieChartDisplay() +: rviz_common::Display(), update_required_(false), first_time_(true), data_(0.0) +{ + update_topic_property_ = new rviz_common::properties::StringProperty( + "Topic", "", "tier4_debug_msgs::msg::Float32MultiArrayStamped topic to subscribe to.", this, + SLOT(updateTopic()), this); + data_index_property_ = new rviz_common::properties::IntProperty( + "data index", 0, "data index in message to visualize", this, SLOT(updateDataIndex())); + size_property_ = new rviz_common::properties::IntProperty( + "size", 128, "size of the plotter window", this, SLOT(updateSize())); + left_property_ = new rviz_common::properties::IntProperty( + "left", 128, "left of the plotter window", this, SLOT(updateLeft())); + top_property_ = new rviz_common::properties::IntProperty( + "top", 128, "top of the plotter window", this, SLOT(updateTop())); + fg_color_property_ = new rviz_common::properties::ColorProperty( + "foreground color", QColor(25, 255, 240), "color to draw line", this, SLOT(updateFGColor())); + fg_alpha_property_ = new rviz_common::properties::FloatProperty( + "foreground alpha", 0.7, "alpha blending value for foreground", this, SLOT(updateFGAlpha())); + fg_alpha2_property_ = new rviz_common::properties::FloatProperty( + "foreground alpha 2", 0.4, "alpha blending value for foreground for indicator", this, + SLOT(updateFGAlpha2())); + bg_color_property_ = new rviz_common::properties::ColorProperty( + "background color", QColor(0, 0, 0), "background color", this, SLOT(updateBGColor())); + bg_alpha_property_ = new rviz_common::properties::FloatProperty( + "background alpha", 0.0, "alpha blending value for background", this, SLOT(updateBGAlpha())); + text_size_property_ = new rviz_common::properties::IntProperty( + "text size", 14, "text size", this, SLOT(updateTextSize())); + show_caption_property_ = new rviz_common::properties::BoolProperty( + "show caption", false, "show caption", this, SLOT(updateShowCaption())); + max_value_property_ = new rviz_common::properties::FloatProperty( + "max value", 1.0, "max value of pie chart", this, SLOT(updateMaxValue())); + min_value_property_ = new rviz_common::properties::FloatProperty( + "min value", 0.0, "min value of pie chart", this, SLOT(updateMinValue())); + auto_color_change_property_ = new rviz_common::properties::BoolProperty( + "auto color change", false, "change the color automatically", this, + SLOT(updateAutoColorChange())); + max_color_property_ = new rviz_common::properties::ColorProperty( + "max color", QColor(255, 0, 0), "only used if auto color change is set to True.", this, + SLOT(updateMaxColor())); + + med_color_property_ = new rviz_common::properties::ColorProperty( + "med color", QColor(255, 0, 0), "only used if auto color change is set to True.", this, + SLOT(updateMedColor())); + + max_color_threshold_property_ = new rviz_common::properties::FloatProperty( + "max color change threshold", 0, "change the max color at threshold", this, + SLOT(updateMaxColorThreshold())); + + med_color_threshold_property_ = new rviz_common::properties::FloatProperty( + "med color change threshold", 0, "change the med color at threshold ", this, + SLOT(updateMedColorThreshold())); + + clockwise_rotate_property_ = new rviz_common::properties::BoolProperty( + "clockwise rotate direction", false, "change the rotate direction", this, + SLOT(updateClockwiseRotate())); +} + +Float32MultiArrayStampedPieChartDisplay::~Float32MultiArrayStampedPieChartDisplay() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } + delete update_topic_property_; + delete fg_color_property_; + delete bg_color_property_; + delete fg_alpha_property_; + delete fg_alpha2_property_; + delete bg_alpha_property_; + delete top_property_; + delete left_property_; + delete size_property_; + delete min_value_property_; + delete max_value_property_; + delete max_color_property_; + delete med_color_property_; + delete text_size_property_; + delete show_caption_property_; +} + +void Float32MultiArrayStampedPieChartDisplay::onInitialize() +{ + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "Float32MultiArrayStampedPieChart" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + onEnable(); + updateSize(); + updateLeft(); + updateTop(); + updateFGColor(); + updateBGColor(); + updateFGAlpha(); + updateFGAlpha2(); + updateBGAlpha(); + updateMinValue(); + updateMaxValue(); + updateTextSize(); + updateShowCaption(); + updateAutoColorChange(); + updateMaxColor(); + updateMedColor(); + updateMaxColorThreshold(); + updateMedColorThreshold(); + updateClockwiseRotate(); + overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_); + overlay_->hide(); +} + +void Float32MultiArrayStampedPieChartDisplay::update( + [[maybe_unused]] float wall_dt, [[maybe_unused]] float ros_dt) +{ + if (update_required_) { + update_required_ = false; + overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_); + overlay_->setPosition(left_, top_); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + drawPlot(data_); + } +} + +void Float32MultiArrayStampedPieChartDisplay::processMessage( + const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + if (!overlay_->isVisible()) { + return; + } + if (data_index_ < 0 || static_cast(msg->data.size()) <= data_index_) { + return; + } + + if (data_ != msg->data.at(data_index_) || first_time_) { + first_time_ = false; + data_ = msg->data.at(data_index_); + update_required_ = true; + } +} + +void Float32MultiArrayStampedPieChartDisplay::drawPlot(double val) +{ + QColor fg_color(fg_color_); + + if (auto_color_change_) { + double r = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_))); + if (r > 0.6) { + double r2 = (r - 0.6) / 0.4; + fg_color.setRed((max_color_.red() - fg_color_.red()) * r2 + fg_color_.red()); + fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2 + fg_color_.green()); + fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2 + fg_color_.blue()); + } + if (max_color_threshold_ != 0) { + if (r > max_color_threshold_) { + fg_color.setRed(max_color_.red()); + fg_color.setGreen(max_color_.green()); + fg_color.setBlue(max_color_.blue()); + } + } + if (med_color_threshold_ != 0) { + if (max_color_threshold_ > r && r > med_color_threshold_) { + fg_color.setRed(med_color_.red()); + fg_color.setGreen(med_color_.green()); + fg_color.setBlue(med_color_.blue()); + } + } + } + + QColor fg_color2(fg_color); + QColor bg_color(bg_color_); + fg_color.setAlpha(fg_alpha_); + fg_color2.setAlpha(fg_alpha2_); + bg_color.setAlpha(bg_alpha_); + int width = overlay_->getTextureWidth(); + int height = overlay_->getTextureHeight(); + { + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage Hud = buffer.getQImage(*overlay_, bg_color); + QPainter painter(&Hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int outer_line_width = 5; + const int value_line_width = 10; + const int value_indicator_line_width = 2; + const int value_padding = 5; + + const int value_offset = outer_line_width + value_padding + value_line_width / 2; + + painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine)); + + painter.drawEllipse( + outer_line_width / 2, outer_line_width / 2, width - outer_line_width, + height - outer_line_width - caption_offset_); + + painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine)); + painter.drawEllipse( + value_offset, value_offset, width - value_offset * 2, + height - value_offset * 2 - caption_offset_); + + const double ratio = (val - min_value_) / (max_value_ - min_value_); + const double rotate_direction = clockwise_rotate_ ? -1.0 : 1.0; + const double ratio_angle = ratio * 360.0 * rotate_direction; + const double start_angle_offset = -90; + painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine)); + painter.drawArc( + QRectF( + value_offset, value_offset, width - value_offset * 2, + height - value_offset * 2 - caption_offset_), + start_angle_offset * 16, ratio_angle * 16); + QFont font = painter.font(); + font.setPointSize(text_size_); + font.setBold(true); + painter.setFont(font); + painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine)); + std::ostringstream s; + s << std::fixed << std::setprecision(2) << val; + painter.drawText( + 0, 0, width, height - caption_offset_, Qt::AlignCenter | Qt::AlignVCenter, s.str().c_str()); + + // caption + if (show_caption_) { + painter.drawText( + 0, height - caption_offset_, width, caption_offset_, Qt::AlignCenter | Qt::AlignVCenter, + getName()); + } + + // done + painter.end(); + // Unlock the pixel buffer + } +} + +void Float32MultiArrayStampedPieChartDisplay::subscribe() +{ + std::string topic_name = update_topic_property_->getStdString(); + + // NOTE: Remove all spaces since topic name filled with only spaces will crash + topic_name.erase(std::remove(topic_name.begin(), topic_name.end(), ' '), topic_name.end()); + + if (topic_name.length() > 0 && topic_name != "/") { + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + sub_ = raw_node->create_subscription( + topic_name, 1, + std::bind( + &Float32MultiArrayStampedPieChartDisplay::processMessage, this, std::placeholders::_1)); + } +} + +void Float32MultiArrayStampedPieChartDisplay::unsubscribe() +{ + sub_.reset(); +} + +void Float32MultiArrayStampedPieChartDisplay::onEnable() +{ + subscribe(); + overlay_->show(); + first_time_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::onDisable() +{ + unsubscribe(); + overlay_->hide(); +} + +void Float32MultiArrayStampedPieChartDisplay::updateSize() +{ + std::lock_guard lock(mutex_); + + texture_size_ = size_property_->getInt(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateTop() +{ + top_ = top_property_->getInt(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateLeft() +{ + left_ = left_property_->getInt(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateBGColor() +{ + bg_color_ = bg_color_property_->getColor(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateFGColor() +{ + fg_color_ = fg_color_property_->getColor(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateFGAlpha() +{ + fg_alpha_ = fg_alpha_property_->getFloat() * 255.0; + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateFGAlpha2() +{ + fg_alpha2_ = fg_alpha2_property_->getFloat() * 255.0; + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateBGAlpha() +{ + bg_alpha_ = bg_alpha_property_->getFloat() * 255.0; + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateMinValue() +{ + min_value_ = min_value_property_->getFloat(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateMaxValue() +{ + max_value_ = max_value_property_->getFloat(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateTextSize() +{ + std::lock_guard lock(mutex_); + + text_size_ = text_size_property_->getInt(); + QFont font; + font.setPointSize(text_size_); + caption_offset_ = QFontMetrics(font).height(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateShowCaption() +{ + show_caption_ = show_caption_property_->getBool(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateTopic() +{ + unsubscribe(); + subscribe(); +} + +void Float32MultiArrayStampedPieChartDisplay::updateDataIndex() +{ + data_index_ = data_index_property_->getInt(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateAutoColorChange() +{ + auto_color_change_ = auto_color_change_property_->getBool(); + if (auto_color_change_) { + max_color_property_->show(); + med_color_property_->show(); + max_color_threshold_property_->show(); + med_color_threshold_property_->show(); + } else { + max_color_property_->hide(); + med_color_property_->hide(); + max_color_threshold_property_->hide(); + med_color_threshold_property_->hide(); + } + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateMaxColor() +{ + max_color_ = max_color_property_->getColor(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateMedColor() +{ + med_color_ = med_color_property_->getColor(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateMaxColorThreshold() +{ + max_color_threshold_ = max_color_threshold_property_->getFloat(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateMedColorThreshold() +{ + med_color_threshold_ = med_color_threshold_property_->getFloat(); + update_required_ = true; +} + +void Float32MultiArrayStampedPieChartDisplay::updateClockwiseRotate() +{ + clockwise_rotate_ = clockwise_rotate_property_->getBool(); + update_required_ = true; +} + +bool Float32MultiArrayStampedPieChartDisplay::isInRegion(int x, int y) +{ + return (top_ < y && top_ + texture_size_ > y && left_ < x && left_ + texture_size_ > x); +} + +void Float32MultiArrayStampedPieChartDisplay::movePosition(int x, int y) +{ + top_ = y; + left_ = x; +} + +void Float32MultiArrayStampedPieChartDisplay::setPosition(int x, int y) +{ + top_property_->setValue(y); + left_property_->setValue(x); +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::Float32MultiArrayStampedPieChartDisplay, rviz_common::Display) diff --git a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 000000000..b1933ebb3 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,225 @@ +// Copyright 2022 Tier IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp new file mode 100644 index 000000000..538dc0cbe --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp @@ -0,0 +1,198 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "tier4_debug_rviz_plugin/string_stamped.hpp" + +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + last_msg_ptr_->data.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..cc7da7e32 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_logging_level_configure_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(tier4_logging_level_configure_rviz_plugin SHARED + include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp + src/logging_level_configure.cpp +) + +target_link_libraries(tier4_logging_level_configure_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins + config +) diff --git a/common/tier4_logging_level_configure_rviz_plugin/README.md b/common/tier4_logging_level_configure_rviz_plugin/README.md new file mode 100644 index 000000000..ed400e521 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/README.md @@ -0,0 +1,72 @@ +# tier4_logging_level_configure_rviz_plugin + +This package provides an rviz_plugin that can easily change the logger level of each node. + +![tier4_logging_level_configure_rviz_plugin](tier4_logging_level_configure_rviz_plugin.png) + +This plugin dispatches services to the "logger name" associated with "nodes" specified in YAML, adjusting the logger level. + +!!! Warning + + It is highly recommended to use this plugin when you're attempting to print any debug information. Furthermore, it is strongly advised to avoid using the logging level INFO, as it might flood the terminal with your information, potentially causing other useful information to be missed. + +!!! note + + To add your logger to the list, simply include the `node_name` and `logger_name` in the [logger_config.yaml](https://github.com/autowarefoundation/autoware.universe/blob/main/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml) under the corresponding component or module. If the relevant component or module is not listed, you may add them yourself. + +!!! note + + As of November 2023, in ROS 2 Humble, users are required to initiate a service server in the node to use this feature. (This might be integrated into ROS standards in the future.) For easy service server generation, you can use the [LoggerLevelConfigure](https://github.com/autowarefoundation/autoware.universe/blob/main/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp) utility. + +## How to use the plugin + +In RVIZ2, go to Panels and add LoggingLevelConfigureRVizPlugin. Then, search for the node you're interested in and select the corresponding logging level to print the logs. + +## How to add or find your logger name + +Because there are no available ROS 2 CLI commands to list loggers, there isn't a straightforward way to check your logger name. Additionally, the following assumes that you already know which node you're working with. + +### For logger as a class member variable + +If your class doesn't have an `rclcpp::Logger` member variable, you can start by including one yourself: + +```c++ +mutable rclcpp::Logger logger_; +``` + +If your node already has a logger, it should, under normal circumstances, be similar to the node's name. + +For instance, if the node name is `/some_component/some_node/node_child`, the `logger_name` would be `some_component.some_node.node_child`. + +Should your log not print as expected, one approach is to initially set your logging level in the code to info, like so: + +```c++ +RCLCPP_INFO(logger_, "Print something here."); +``` + +This will result in something like the following being printed in the terminal: + +```shell +[component_container_mt-36] [INFO 1711949149.735437551] [logger_name]: Print something here. (func() at /path/to/code:line_number) +``` + +Afterward, you can simply copy the `logger_name`. + +!!! warning + + Remember to revert your code to the appropriate logging level after testing. + ```c++ + RCLCPP_DEBUG(logger_, "Print something here."); + ``` + +### For libraries + +When dealing with libraries, such as utility functions, you may need to add the logger manually. Here's an example: + +```c++ +RCLCPP_WARN( + rclcpp::get_logger("some_component").get_child("some_child").get_child("some_child2"), + "Print something here."); +``` + +In this scenario, the `logger_name` would be `some_component.some_child.some_child2`. diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml new file mode 100644 index 000000000..e8d2f775e --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -0,0 +1,144 @@ +# logger_config.yaml + +# ============================================================ +# localization +# ============================================================ +Localization: + ndt_scan_matcher: + - node_name: /localization/pose_estimator/ndt_scan_matcher + logger_name: localization.pose_estimator.ndt_scan_matcher + + gyro_odometer: + - node_name: /localization/twist_estimator/gyro_odometer + logger_name: localization.twist_estimator.gyro_odometer + + pose_initializer: + - node_name: /localization/util/pose_initializer_node + logger_name: localization.util.pose_initializer_node + + ekf_localizer: + - node_name: /localization/pose_twist_fusion_filter/ekf_localizer + logger_name: localization.pose_twist_fusion_filter.ekf_localizer + + localization_error_monitor: + - node_name: /localization/localization_error_monitor + logger_name: localization.localization_error_monitor + +# ============================================================ +# planning +# ============================================================ +Planning: + behavior_path_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: behavior_path_planner.path_shifter + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: motion_utils + + behavior_path_planner_avoidance: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils + + behavior_path_planner_goal_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner + + behavior_path_planner_start_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner + + behavior_path_avoidance_by_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.utils + + behavior_path_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.NORMAL + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.utils + + behavior_velocity_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: motion_utils + + behavior_velocity_planner_intersection: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection + + motion_obstacle_avoidance: + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: motion_utils + + motion_velocity_smoother: + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: planning.scenario_planning.motion_velocity_smoother + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: motion_utils + + route_handler: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: route_handler + +# ============================================================ +# control +# ============================================================ +Control: + lateral_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.lateral_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: motion_utils + + longitudinal_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: motion_utils + + vehicle_cmd_gate: + - node_name: /control/vehicle_cmd_gate + logger_name: control.vehicle_cmd_gate + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils + +# ============================================================ +# common +# ============================================================ + +Common: + tier4_autoware_utils: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/motion_planning/path_smoother + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp new file mode 100644 index 000000000..37d70b494 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ +#define TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ + +#include "logging_demo/srv/config_logger.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace rviz_plugin +{ +struct LoggerInfo +{ + QString node_name; + QString logger_name; +}; +struct ButtonInfo +{ + QString button_name; + std::vector logger_info_vec; +}; +struct LoggerNamespaceInfo +{ + QString ns; // group namespace + std::vector button_info_vec; +}; +class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel +{ + Q_OBJECT // This macro is needed for Qt to handle slots and signals + + public : LoggingLevelConfigureRvizPlugin(QWidget * parent = nullptr); + void onInitialize() override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; + +private: + QMap buttonGroups_; + rclcpp::Node::SharedPtr raw_node_; + + std::vector display_info_vec_; + + // client_map_[node_name] = service_client + std::unordered_map::SharedPtr> + client_map_; + + // button_map_[button_name][logging_level] = Q_button_pointer + std::unordered_map> button_map_; + + QStringList getNodeList(); + int getMaxModuleNameWidth(QLabel * containerLabel); + void setLoggerNodeMap(); + + ButtonInfo getButtonInfoFromNamespace(const QString & ns); + std::vector getNodeLoggerNameFromButtonName(const QString button_name); + +private Q_SLOTS: + void onButtonClick(QPushButton * button, const QString & name, const QString & level); + void updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level); + void changeLogLevel(const QString & container, const QString & level); +}; + +} // namespace rviz_plugin + +#endif // TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml new file mode 100644 index 000000000..7849e6049 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + tier4_logging_level_configure_rviz_plugin + 0.1.0 + The tier4_logging_level_configure_rviz_plugin package + Takamasa Horibe + Satoshi Ota + Kosuke Takeuchi + Apache License 2.0 + + ament_cmake + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + logging_demo + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..ce5cbd13f --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,7 @@ + + + tier4_logging_level_configure_rviz_plugin + + diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp new file mode 100644 index 000000000..72ecf361c --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -0,0 +1,258 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "yaml-cpp/yaml.h" + +#include +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugin +{ + +LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * parent) +: rviz_common::Panel(parent) +{ +} + +void LoggingLevelConfigureRvizPlugin::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + setLoggerNodeMap(); + + QVBoxLayout * mainLayout = new QVBoxLayout; + + QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; + + constexpr int height = 20; + + // Iterate over the namespaces + for (const auto & ns_group_info : display_info_vec_) { + // Create a group box for each namespace + QGroupBox * groupBox = new QGroupBox(ns_group_info.ns); + QVBoxLayout * groupLayout = new QVBoxLayout; + + // Iterate over the node/logger pairs within this namespace + for (const auto & button_info : ns_group_info.button_info_vec) { + const auto & button_name = button_info.button_name; + + QHBoxLayout * hLayout = new QHBoxLayout; + + // Create a QLabel to display the node name + QLabel * label = new QLabel(button_name); + label->setFixedHeight(height); + label->setFixedWidth(getMaxModuleNameWidth(label)); + + hLayout->addWidget(label); + + // Create a button group for each node + QButtonGroup * buttonGroup = new QButtonGroup(this); + + // Create buttons for each logging level + for (const QString & level : levels) { + QPushButton * button = new QPushButton(level); + button->setFixedHeight(height); + hLayout->addWidget(button); + buttonGroup->addButton(button); + button_map_[button_name][level] = button; + connect(button, &QPushButton::clicked, this, [this, button, button_name, level]() { + this->onButtonClick(button, button_name, level); + }); + } + + // Set the "INFO" button as checked by default and change its color + updateButtonColors(button_name, button_map_[button_name]["INFO"], "INFO"); + + buttonGroups_[button_name] = buttonGroup; + groupLayout->addLayout(hLayout); // Add the horizontal layout to the group layout + } + + groupBox->setLayout(groupLayout); // Set the group layout to the group box + mainLayout->addWidget(groupBox); // Add the group box to the main layout + } + + // Create a QWidget to hold the main layout + QWidget * containerWidget = new QWidget; + containerWidget->setLayout(mainLayout); + + // Create a QScrollArea to make the layout scrollable + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setWidget(containerWidget); + scrollArea->setWidgetResizable(true); + + // Set the QScrollArea as the layout of the main widget + QVBoxLayout * scrollLayout = new QVBoxLayout; + scrollLayout->addWidget(scrollArea); + setLayout(scrollLayout); + + // Setup service clients + const auto & nodes = getNodeList(); + for (const QString & node : nodes) { + const auto client = raw_node_->create_client( + node.toStdString() + "/config_logger"); + client_map_[node] = client; + } +} + +// Calculate the maximum width among all target_module_name. +int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) +{ + int max_width = 0; + QFontMetrics metrics(label->font()); + for (const auto & ns_info : display_info_vec_) { + for (const auto & b : ns_info.button_info_vec) { + max_width = std::max(metrics.horizontalAdvance(b.button_name), max_width); + } + } + return max_width; +} + +// create node list in node_logger_map_ without +QStringList LoggingLevelConfigureRvizPlugin::getNodeList() +{ + QStringList nodes; + for (const auto & d : display_info_vec_) { + for (const auto & b : d.button_info_vec) { + for (const auto & info : b.logger_info_vec) { + if (!nodes.contains(info.node_name)) { + nodes.append(info.node_name); + } + } + } + } + return nodes; +} + +// Modify the signature of the onButtonClick function: +void LoggingLevelConfigureRvizPlugin::onButtonClick( + QPushButton * button, const QString & target_module_name, const QString & level) +{ + if (button) { + const auto callback = + [&](rclcpp::Client::SharedFuture future) { + std::cerr << "change logging level: " + << std::string(future.get()->success ? "success!" : "failed...") << std::endl; + }; + + const auto node_logger_vec = getNodeLoggerNameFromButtonName(target_module_name); + for (const auto & data : node_logger_vec) { + const auto req = std::make_shared(); + + req->logger_name = data.logger_name.toStdString(); + req->level = level.toStdString(); + std::cerr << "logger level of " << req->logger_name << " is set to " << req->level + << std::endl; + client_map_[data.node_name]->async_send_request(req, callback); + } + + updateButtonColors( + target_module_name, button, level); // Modify updateButtonColors to accept QPushButton only. + } +} + +void LoggingLevelConfigureRvizPlugin::updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level) +{ + std::unordered_map colorMap = { + {"DEBUG", "rgb(181, 255, 20)"}, /* green */ + {"INFO", "rgb(200, 255, 255)"}, /* light blue */ + {"WARN", "rgb(255, 255, 0)"}, /* yellow */ + {"ERROR", "rgb(255, 0, 0)"}, /* red */ + {"FATAL", "rgb(139, 0, 0)"}, /* dark red */ + {"OFF", "rgb(211, 211, 211)"} /* gray */ + }; + + const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; + + const QString color = colorMap.count(level) ? colorMap[level] : colorMap["OFF"]; + + for (const auto & button : button_map_[target_module_name]) { + if (button.second == active_button) { + button.second->setStyleSheet("background-color: " + color + "; color: black"); + } else { + button.second->setStyleSheet( + "background-color: " + colorMap["OFF"] + "; color: " + LIGHT_GRAY_TEXT); + } + } +} +void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const +{ + Panel::save(config); +} + +void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) +{ + Panel::load(config); +} + +void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() +{ + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("tier4_logging_level_configure_rviz_plugin"); + const std::string default_config_path = package_share_directory + "/config/logger_config.yaml"; + + const auto filename = + raw_node_->declare_parameter("config_filename", default_config_path); + RCLCPP_INFO(raw_node_->get_logger(), "load config file: %s", filename.c_str()); + + YAML::Node config = YAML::LoadFile(filename); + + for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { + const auto ns = QString::fromStdString(it->first.as()); + const YAML::Node ns_config = it->second; + + LoggerNamespaceInfo display_data; + display_data.ns = ns; + + for (YAML::const_iterator ns_it = ns_config.begin(); ns_it != ns_config.end(); ++ns_it) { + const auto key = QString::fromStdString(ns_it->first.as()); + ButtonInfo button_data; + button_data.button_name = key; + const YAML::Node values = ns_it->second; + for (size_t i = 0; i < values.size(); i++) { + LoggerInfo data; + data.node_name = QString::fromStdString(values[i]["node_name"].as()); + data.logger_name = QString::fromStdString(values[i]["logger_name"].as()); + button_data.logger_info_vec.push_back(data); + } + display_data.button_info_vec.push_back(button_data); + } + display_info_vec_.push_back(display_data); + } +} + +std::vector LoggingLevelConfigureRvizPlugin::getNodeLoggerNameFromButtonName( + const QString button_name) +{ + for (const auto & ns_level : display_info_vec_) { + for (const auto & button : ns_level.button_info_vec) { + if (button.button_name == button_name) { + return button.logger_info_vec; + } + } + } + RCLCPP_ERROR( + raw_node_->get_logger(), "Failed to find target name: %s", button_name.toStdString().c_str()); + return {}; +} + +} // namespace rviz_plugin +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugin::LoggingLevelConfigureRvizPlugin, rviz_common::Panel) diff --git a/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png new file mode 100644 index 000000000..a93aff724 Binary files /dev/null and b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png differ diff --git a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt b/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..5b6e205ba --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_screen_capture_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(OpenCV REQUIRED) +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +include_directories( + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/screen_capture_panel.hpp + src/screen_capture_panel.cpp +) +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${OpenCV_LIBRARIES} +) +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_screen_capture_rviz_plugin/README.md b/common/tier4_screen_capture_rviz_plugin/README.md new file mode 100644 index 000000000..d16c19c34 --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/README.md @@ -0,0 +1,23 @@ +# tier4_screen_capture_rviz_plugin + +## Purpose + +This plugin captures the screen of rviz. + +## Interface + +| Name | Type | Description | +| ---------------------------- | ------------------------ | ---------------------------------- | +| `/debug/capture/video` | `std_srvs::srv::Trigger` | Trigger to start screen capturing. | +| `/debug/capture/screen_shot` | `std_srvs::srv::Trigger` | Trigger to capture screen shot. | + +## Assumptions / Known limits + +This is only for debug or analyze. +The `capture screen` button is still beta version which can slow frame rate. +set lower frame rate according to PC spec. + +## Usage + +1. Start rviz and select panels/Add new panel. + ![select_panel](./images/select_panels.png) diff --git a/common/tier4_screen_capture_rviz_plugin/images/select_panels.png b/common/tier4_screen_capture_rviz_plugin/images/select_panels.png new file mode 100644 index 000000000..a691602c4 Binary files /dev/null and b/common/tier4_screen_capture_rviz_plugin/images/select_panels.png differ diff --git a/common/tier4_screen_capture_rviz_plugin/package.xml b/common/tier4_screen_capture_rviz_plugin/package.xml new file mode 100644 index 000000000..180bf9eed --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/package.xml @@ -0,0 +1,31 @@ + + + + tier4_screen_capture_rviz_plugin + 0.0.0 + The screen capture package + Taiki, Tanaka + Satoshi Ota + Kyoichi Sugahara + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + libopencv-dev + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_rendering + std_srvs + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml b/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..fdf105d64 --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + AutowareScreenCapturePanel + + + diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp new file mode 100644 index 000000000..cad828903 --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp @@ -0,0 +1,220 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "screen_capture_panel.hpp" + +#include + +#include +#include +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +void setFormatDate(QLabel * line, double time) +{ + char buffer[128]; + auto seconds = static_cast(time); + strftime(buffer, sizeof(buffer), "%Y-%m-%d-%H-%M-%S", localtime(&seconds)); + line->setText(QString("- ") + QString(buffer)); +} + +AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + std::filesystem::create_directory("capture"); + auto * v_layout = new QVBoxLayout; + // screen capture + auto * cap_layout = new QHBoxLayout; + { + ros_time_label_ = new QLabel; + screen_capture_button_ptr_ = new QPushButton("Capture Screen Shot"); + connect(screen_capture_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickScreenCapture())); + file_name_prefix_ = new QLineEdit("cap"); + connect(file_name_prefix_, SIGNAL(valueChanged(std::string)), this, SLOT(onPrefixChanged())); + cap_layout->addWidget(screen_capture_button_ptr_); + cap_layout->addWidget(file_name_prefix_); + cap_layout->addWidget(ros_time_label_); + // initialize file name system clock is better for identification. + setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); + } + + // video capture + auto * video_cap_layout = new QHBoxLayout; + { + capture_to_mp4_button_ptr_ = new QPushButton("Capture Screen"); + connect(capture_to_mp4_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVideoCapture())); + capture_hz_ = new QSpinBox(); + capture_hz_->setRange(1, 10); + capture_hz_->setValue(10); + capture_hz_->setSingleStep(1); + connect(capture_hz_, SIGNAL(valueChanged(int)), this, SLOT(onRateChanged())); + // video cap layout + video_cap_layout->addWidget(capture_to_mp4_button_ptr_); + video_cap_layout->addWidget(capture_hz_); + video_cap_layout->addWidget(new QLabel(" [Hz]")); + } + + // consider layout + { + v_layout->addLayout(cap_layout); + v_layout->addLayout(video_cap_layout); + setLayout(v_layout); + } + auto * timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &AutowareScreenCapturePanel::update); + timer->start(1000); + capture_timer_ = new QTimer(this); + connect(capture_timer_, &QTimer::timeout, this, &AutowareScreenCapturePanel::onTimer); + state_ = State::WAITING_FOR_CAPTURE; +} + +void AutowareScreenCapturePanel::onCaptureVideoTrigger( + [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) +{ + onClickVideoCapture(); + res->success = true; + res->message = stateToString(state_); +} + +void AutowareScreenCapturePanel::onCaptureScreenShotTrigger( + [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) +{ + onClickScreenCapture(); + res->success = true; + res->message = stateToString(state_); +} + +void AutowareScreenCapturePanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + capture_video_srv_ = raw_node_->create_service( + "/debug/capture/video", + std::bind(&AutowareScreenCapturePanel::onCaptureVideoTrigger, this, _1, _2)); + capture_screen_shot_srv_ = raw_node_->create_service( + "/debug/capture/screen_shot", + std::bind(&AutowareScreenCapturePanel::onCaptureScreenShotTrigger, this, _1, _2)); +} + +void onPrefixChanged() +{ +} + +void AutowareScreenCapturePanel::onRateChanged() +{ +} + +void AutowareScreenCapturePanel::onClickScreenCapture() +{ + const std::string time_text = + "capture/" + file_name_prefix_->text().toStdString() + ros_time_label_->text().toStdString(); + getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot( + time_text + ".png"); +} + +void AutowareScreenCapturePanel::onClickVideoCapture() +{ + const int clock = static_cast(1e3 / capture_hz_->value()); + try { + const QWidgetList top_level_widgets = QApplication::topLevelWidgets(); + for (QWidget * widget : top_level_widgets) { + auto * main_window_candidate = qobject_cast(widget); + if (main_window_candidate) { + main_window_ = main_window_candidate; + } + } + } catch (...) { + return; + } + if (!main_window_) return; + switch (state_) { + case State::WAITING_FOR_CAPTURE: + // initialize setting + { + capture_file_name_ = ros_time_label_->text().toStdString(); + } + capture_to_mp4_button_ptr_->setText("capturing rviz screen"); + capture_to_mp4_button_ptr_->setStyleSheet("background-color: #FF0000;"); + { + int fourcc = cv::VideoWriter::fourcc('h', '2', '6', '4'); // mp4 + QScreen * screen = QGuiApplication::primaryScreen(); + const auto q_size = screen->grabWindow(main_window_->winId()) + .toImage() + .convertToFormat(QImage::Format_RGB888) + .rgbSwapped() + .size(); + current_movie_size_ = cv::Size(q_size.width(), q_size.height()); + writer_.open( + "capture/" + file_name_prefix_->text().toStdString() + capture_file_name_ + ".mp4", + fourcc, capture_hz_->value(), current_movie_size_); + } + capture_timer_->start(clock); + state_ = State::CAPTURING; + break; + case State::CAPTURING: + writer_.release(); + capture_timer_->stop(); + capture_to_mp4_button_ptr_->setText("waiting for capture"); + capture_to_mp4_button_ptr_->setStyleSheet("background-color: #00FF00;"); + state_ = State::WAITING_FOR_CAPTURE; + break; + } +} + +void AutowareScreenCapturePanel::onTimer() +{ + if (!main_window_) return; + // this is deprecated but only way to capture nicely + QScreen * screen = QGuiApplication::primaryScreen(); + QPixmap original_pixmap = screen->grabWindow(main_window_->winId()); + const auto q_image = + original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); + const int h = q_image.height(); + const int w = q_image.width(); + cv::Size size = cv::Size(w, h); + cv::Mat image( + size, CV_8UC3, const_cast(q_image.bits()), + static_cast(q_image.bytesPerLine())); + if (size != current_movie_size_) { + cv::Mat new_image; + cv::resize(image, new_image, current_movie_size_); + writer_.write(new_image); + } else { + writer_.write(image); + } + cv::waitKey(0); +} + +void AutowareScreenCapturePanel::update() +{ + setFormatDate(ros_time_label_, rclcpp::Clock().now().seconds()); +} + +void AutowareScreenCapturePanel::save(rviz_common::Config config) const +{ + Panel::save(config); +} + +void AutowareScreenCapturePanel::load(const rviz_common::Config & config) +{ + Panel::load(config); +} + +AutowareScreenCapturePanel::~AutowareScreenCapturePanel() = default; + +#include +PLUGINLIB_EXPORT_CLASS(AutowareScreenCapturePanel, rviz_common::Panel) diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp new file mode 100644 index 000000000..5c4d16f57 --- /dev/null +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp @@ -0,0 +1,109 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 SCREEN_CAPTURE_PANEL_HPP_ +#define SCREEN_CAPTURE_PANEL_HPP_ + +// Qt +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// rviz +#include +#include +#include +#include +#include +#include +#include + +// ros +#include + +#include +#include +#include + +class QLineEdit; + +class AutowareScreenCapturePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit AutowareScreenCapturePanel(QWidget * parent = nullptr); + ~AutowareScreenCapturePanel() override; + void update(); + void onInitialize() override; + void createWallTimer(); + void onTimer(); + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; + void onCaptureVideoTrigger( + const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res); + void onCaptureScreenShotTrigger( + const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res); + +public Q_SLOTS: + void onClickScreenCapture(); + void onClickVideoCapture(); + void onPrefixChanged(); + void onRateChanged(); + +private: + QLabel * ros_time_label_; + QPushButton * screen_capture_button_ptr_; + QPushButton * capture_to_mp4_button_ptr_; + QLineEdit * file_name_prefix_; + QSpinBox * capture_hz_; + QTimer * capture_timer_; + QMainWindow * main_window_{nullptr}; + enum class State { WAITING_FOR_CAPTURE, CAPTURING }; + State state_; + std::string capture_file_name_; + bool is_capture_{false}; + cv::VideoWriter writer_; + cv::Size current_movie_size_; + std::vector image_vec_; + + static std::string stateToString(const State & state) + { + if (state == State::WAITING_FOR_CAPTURE) { + return "waiting for capture"; + } + if (state == State::CAPTURING) { + return "capturing"; + } + return ""; + } + +protected: + rclcpp::Service::SharedPtr capture_video_srv_; + rclcpp::Service::SharedPtr capture_screen_shot_srv_; + rclcpp::Node::SharedPtr raw_node_; +}; + +#endif // SCREEN_CAPTURE_PANEL_HPP_ diff --git a/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt b/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..020766340 --- /dev/null +++ b/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_simulated_clock_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) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/simulated_clock_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/tier4_simulated_clock_rviz_plugin/README.md b/common/tier4_simulated_clock_rviz_plugin/README.md new file mode 100644 index 000000000..a6218f32b --- /dev/null +++ b/common/tier4_simulated_clock_rviz_plugin/README.md @@ -0,0 +1,46 @@ +# tier4_simulated_clock_rviz_plugin + +## Purpose + +This plugin allows publishing and controlling the simulated ROS time. + +## Output + +| Name | Type | Description | +| -------- | --------------------------- | -------------------------- | +| `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time | + +## How to use the plugin + +1. Launch [planning simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#1-launch-autoware) with `use_sim_time:=true`. + + ```shell + ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit use_sim_time:=true + ``` + + > Warning + > If you launch the planning simulator without adding the `tier4_simulated_clock_rviz_plugin`, your simulation will not be running. You'll not even be able to place the initial and the goal poses. + +2. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +3. Select tier4_clock_rviz_plugin/SimulatedClock and press OK. + + ![select_clock_plugin](./images/select_clock_plugin.png) + +4. Use the added panel to control how the simulated clock is published. + + ![use_clock_plugin](./images/use_clock_plugin.png) + +
    +
  1. Pause button: pause/resume the clock.
    2. +
  2. Speed: speed of the clock relative to the system clock.
    3. +
  3. Rate: publishing rate of the clock.
    4. +
  4. Step button: advance the clock by the specified time step.
    5. +
  5. Time step: value used to advance the clock when pressing the step button d).
    6. +
  6. Time unit: time unit associated with the value from e).
    7. +
+ + > Warning + > If you set the time step too large, your simulation will go haywire. diff --git a/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png b/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png new file mode 100644 index 000000000..9431c2d42 Binary files /dev/null and b/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png differ diff --git a/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png b/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png new file mode 100644 index 000000000..8bf5e3a90 Binary files /dev/null and b/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png differ diff --git a/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png b/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png new file mode 100644 index 000000000..a691602c4 Binary files /dev/null and b/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png differ diff --git a/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png b/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png new file mode 100644 index 000000000..39c3669c2 Binary files /dev/null and b/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png differ diff --git a/common/tier4_simulated_clock_rviz_plugin/package.xml b/common/tier4_simulated_clock_rviz_plugin/package.xml new file mode 100644 index 000000000..d80b18a58 --- /dev/null +++ b/common/tier4_simulated_clock_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + tier4_simulated_clock_rviz_plugin + 0.0.1 + Rviz plugin to publish and control the /clock topic + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rosgraph_msgs + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml b/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..00caf2e5d --- /dev/null +++ b/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + Panel that publishes a simulated clock to the /clock topic and provides an interface to pause the clock and modify its speed. + + + diff --git a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp b/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp new file mode 100644 index 000000000..ad698d925 --- /dev/null +++ b/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp @@ -0,0 +1,153 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// 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 "simulated_clock_panel.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rviz_plugins +{ +SimulatedClockPanel::SimulatedClockPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + pause_button_ = new QPushButton("Pause"); + pause_button_->setToolTip("Freeze ROS time."); + pause_button_->setCheckable(true); + + publishing_rate_input_ = new QSpinBox(); + publishing_rate_input_->setRange(1, 1000); + publishing_rate_input_->setSingleStep(1); + publishing_rate_input_->setValue(100); + publishing_rate_input_->setSuffix("Hz"); + + clock_speed_input_ = new QDoubleSpinBox(); + clock_speed_input_->setRange(0.0, 10.0); + clock_speed_input_->setSingleStep(0.1); + clock_speed_input_->setValue(1.0); + clock_speed_input_->setSuffix(" X real time"); + + step_button_ = new QPushButton("Step"); + step_button_->setToolTip("Pause and steps the simulation clock"); + step_time_input_ = new QSpinBox(); + step_time_input_->setRange(1, 999); + step_time_input_->setValue(1); + step_unit_combo_ = new QComboBox(); + step_unit_combo_->addItems({"s", "ms", "µs", "ns"}); + + auto * layout = new QGridLayout(this); + auto * step_layout = new QHBoxLayout(); + auto * clock_layout = new QHBoxLayout(); + auto * clock_box = new QWidget(); + auto * step_box = new QWidget(); + clock_box->setLayout(clock_layout); + step_box->setLayout(step_layout); + layout->addWidget(pause_button_, 0, 0); + layout->addWidget(step_button_, 1, 0); + clock_layout->addWidget(new QLabel("Speed:")); + clock_layout->addWidget(clock_speed_input_); + clock_layout->addWidget(new QLabel("Rate:")); + clock_layout->addWidget(publishing_rate_input_); + step_layout->addWidget(step_time_input_); + step_layout->addWidget(step_unit_combo_); + layout->addWidget(clock_box, 0, 1, 1, 2); + layout->addWidget(step_box, 1, 1, 1, 2); + layout->setContentsMargins(0, 0, 20, 0); + prev_published_time_ = std::chrono::system_clock::now(); + + connect(publishing_rate_input_, SIGNAL(valueChanged(int)), this, SLOT(onRateChanged(int))); + connect(step_button_, SIGNAL(clicked()), this, SLOT(onStepClicked())); +} + +void SimulatedClockPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + clock_pub_ = raw_node_->create_publisher("/clock", rclcpp::QoS(1)); + createWallTimer(); +} + +void SimulatedClockPanel::onRateChanged(int new_rate) +{ + (void)new_rate; + pub_timer_->cancel(); + createWallTimer(); +} + +void SimulatedClockPanel::onStepClicked() +{ + using std::chrono::duration_cast, std::chrono::seconds, std::chrono::milliseconds, + std::chrono::microseconds, std::chrono::nanoseconds; + pause_button_->setChecked(true); + const auto step_time = step_time_input_->value(); + const auto unit = step_unit_combo_->currentText(); + nanoseconds step_duration_ns{}; + if (unit == "s") { + step_duration_ns += duration_cast(seconds(step_time)); + } else if (unit == "ms") { + step_duration_ns += duration_cast(milliseconds(step_time)); + } else if (unit == "µs") { + step_duration_ns += duration_cast(microseconds(step_time)); + } else if (unit == "ns") { + step_duration_ns += duration_cast(nanoseconds(step_time)); + } + addTimeToClock(step_duration_ns); +} + +void SimulatedClockPanel::createWallTimer() +{ + // convert rate from Hz to milliseconds + const auto period = + std::chrono::milliseconds(static_cast(1e3 / publishing_rate_input_->value())); + pub_timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); +} + +void SimulatedClockPanel::onTimer() +{ + if (!pause_button_->isChecked()) { + const auto duration_since_prev_clock = std::chrono::system_clock::now() - prev_published_time_; + const auto speed_adjusted_duration = duration_since_prev_clock * clock_speed_input_->value(); + addTimeToClock(std::chrono::duration_cast(speed_adjusted_duration)); + } + clock_pub_->publish(clock_msg_); + prev_published_time_ = std::chrono::system_clock::now(); +} + +void SimulatedClockPanel::addTimeToClock(std::chrono::nanoseconds time_to_add_ns) +{ + constexpr auto one_sec = std::chrono::seconds(1); + constexpr auto one_sec_ns = std::chrono::nanoseconds(one_sec); + while (time_to_add_ns >= one_sec) { + time_to_add_ns -= one_sec; + clock_msg_.clock.sec += 1; + } + clock_msg_.clock.nanosec += time_to_add_ns.count(); + if (clock_msg_.clock.nanosec >= one_sec_ns.count()) { + clock_msg_.clock.sec += 1; + clock_msg_.clock.nanosec = clock_msg_.clock.nanosec - one_sec_ns.count(); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::SimulatedClockPanel, rviz_common::Panel) diff --git a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp b/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp new file mode 100644 index 000000000..b2ac33210 --- /dev/null +++ b/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp @@ -0,0 +1,76 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// 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 SIMULATED_CLOCK_PANEL_HPP_ +#define SIMULATED_CLOCK_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace rviz_plugins +{ +class SimulatedClockPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit SimulatedClockPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected Q_SLOTS: + /// @brief callback for when the publishing rate is changed + void onRateChanged(int new_rate); + /// @brief callback for when the step button is clicked + void onStepClicked(); + +protected: + /// @brief creates ROS wall timer to periodically call onTimer() + void createWallTimer(); + void onTimer(); + /// @brief add some time to the clock + /// @input ns time to add in nanoseconds + void addTimeToClock(std::chrono::nanoseconds ns); + + // ROS + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Publisher::SharedPtr clock_pub_; + rclcpp::TimerBase::SharedPtr pub_timer_; + + // GUI + QPushButton * pause_button_; + QPushButton * step_button_; + QSpinBox * publishing_rate_input_; + QDoubleSpinBox * clock_speed_input_; + QSpinBox * step_time_input_; + QComboBox * step_unit_combo_; + + // Clocks + std::chrono::time_point prev_published_time_; + rosgraph_msgs::msg::Clock clock_msg_; +}; + +} // namespace rviz_plugins + +#endif // SIMULATED_CLOCK_PANEL_HPP_ diff --git a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt new file mode 100644 index 000000000..ed458cf92 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_target_object_type_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(OpenCV REQUIRED) +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/target_object_type_panel.hpp + src/target_object_type_panel.cpp +) +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${OpenCV_LIBRARIES} +) +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_target_object_type_rviz_plugin/README.md b/common/tier4_target_object_type_rviz_plugin/README.md new file mode 100644 index 000000000..1296bd344 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/README.md @@ -0,0 +1,9 @@ +# tier4_target_object_type_rviz_plugin + +This plugin allows you to check which types of the dynamic object is being used by each planner. + +![window](./image/window.png) + +## Limitations + +Currently, which parameters of which module to check are hardcoded. In the future, this will be parameterized using YAML. diff --git a/common/tier4_target_object_type_rviz_plugin/image/window.png b/common/tier4_target_object_type_rviz_plugin/image/window.png new file mode 100644 index 000000000..33aa9a1e5 Binary files /dev/null and b/common/tier4_target_object_type_rviz_plugin/image/window.png differ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml new file mode 100644 index 000000000..04d2f28d9 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + tier4_target_object_type_rviz_plugin + 0.0.1 + The tier4_target_object_type_rviz_plugin package + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + cv_bridge + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_rendering + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 000000000..eb3350e43 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + TargetObjectTypePanel + + + diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp new file mode 100644 index 000000000..e01430794 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp @@ -0,0 +1,316 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 "target_object_type_panel.hpp" + +#include +#include +#include +#include +#include + +TargetObjectTypePanel::TargetObjectTypePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + node_ = std::make_shared("matrix_display_node"); + + setParameters(); + + matrix_widget_ = new QTableWidget(modules_.size(), targets_.size(), this); + for (size_t i = 0; i < modules_.size(); i++) { + matrix_widget_->setVerticalHeaderItem( + i, new QTableWidgetItem(QString::fromStdString(modules_[i]))); + } + for (size_t j = 0; j < targets_.size(); j++) { + matrix_widget_->setHorizontalHeaderItem( + j, new QTableWidgetItem(QString::fromStdString(targets_[j]))); + } + + updateMatrix(); + + reload_button_ = new QPushButton("Reload", this); + connect( + reload_button_, &QPushButton::clicked, this, &TargetObjectTypePanel::onReloadButtonClicked); + + QVBoxLayout * layout = new QVBoxLayout; + layout->addWidget(matrix_widget_); + layout->addWidget(reload_button_); + setLayout(layout); +} + +void TargetObjectTypePanel::onReloadButtonClicked() +{ + RCLCPP_INFO(node_->get_logger(), "Reload button clicked. Update parameter data."); + updateMatrix(); +} + +void TargetObjectTypePanel::setParameters() +{ + // Parameter will be investigated for these modules: + modules_ = { + "avoidance", + "avoidance_by_lane_change", + "dynamic_avoidance", + "lane_change", + "start_planner", + "goal_planner", + "crosswalk", + "surround_obstacle_checker", + "obstacle_cruise (inside)", + "obstacle_cruise (outside)", + "obstacle_stop", + "obstacle_slowdown"}; + + // Parameter will be investigated for targets in each module + targets_ = {"car", "truck", "bus", "trailer", "unknown", "bicycle", "motorcycle", "pedestrian"}; + + // TODO(Horibe): If the param naming strategy is aligned, this should be done automatically based + // on the modules_ and targets_. + + // default + ParamNameEnableObject default_param_name; + default_param_name.name.emplace("car", "car"); + default_param_name.name.emplace("truck", "truck"); + default_param_name.name.emplace("bus", "bus"); + default_param_name.name.emplace("trailer", "trailer"); + default_param_name.name.emplace("unknown", "unknown"); + default_param_name.name.emplace("bicycle", "bicycle"); + default_param_name.name.emplace("motorcycle", "motorcycle"); + default_param_name.name.emplace("pedestrian", "pedestrian"); + + // avoidance + { + const auto module = "avoidance"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance.target_filtering.target_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // avoidance_by_lane_change + { + const auto module = "avoidance_by_lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "avoidance_by_lane_change.target_filtering.target_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // lane_change + { + const auto module = "lane_change"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "lane_change.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // start_planner + { + const auto module = "start_planner"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "start_planner.path_safety_check.target_filtering.object_types_to_check"; + param_name.name.emplace("car", "check_car"); + param_name.name.emplace("truck", "check_truck"); + param_name.name.emplace("bus", "check_bus"); + param_name.name.emplace("trailer", "check_trailer"); + param_name.name.emplace("unknown", "check_unknown"); + param_name.name.emplace("bicycle", "check_bicycle"); + param_name.name.emplace("motorcycle", "check_motorcycle"); + param_name.name.emplace("pedestrian", "check_pedestrian"); + param_names_.emplace(module, param_name); + } + + // goal_planner + { + const auto module = "goal_planner"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "goal_planner.path_safety_check.target_filtering.object_types_to_check"; + param_name.name.emplace("car", "check_car"); + param_name.name.emplace("truck", "check_truck"); + param_name.name.emplace("bus", "check_bus"); + param_name.name.emplace("trailer", "check_trailer"); + param_name.name.emplace("unknown", "check_unknown"); + param_name.name.emplace("bicycle", "check_bicycle"); + param_name.name.emplace("motorcycle", "check_motorcycle"); + param_name.name.emplace("pedestrian", "check_pedestrian"); + param_names_.emplace(module, param_name); + } + + // dynamic_avoidance + { + const auto module = "dynamic_avoidance"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner"; + param_name.ns = "dynamic_avoidance.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // crosswalk + { + const auto module = "crosswalk"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner"; + param_name.ns = "crosswalk.object_filtering.target_object"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (inside) + { + const auto module = "obstacle_cruise (inside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.inside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle cruise (outside) + { + const auto module = "obstacle_cruise (outside)"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.cruise_obstacle_type.outside"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // surround_obstacle_check + { + const auto module = "surround_obstacle_checker"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker"; + param_name.ns = ""; + param_name.name.emplace("car", "car.enable_check"); + param_name.name.emplace("truck", "truck.enable_check"); + param_name.name.emplace("bus", "bus.enable_check"); + param_name.name.emplace("trailer", "trailer.enable_check"); + param_name.name.emplace("unknown", "unknown.enable_check"); + param_name.name.emplace("bicycle", "bicycle.enable_check"); + param_name.name.emplace("motorcycle", "motorcycle.enable_check"); + param_name.name.emplace("pedestrian", "pedestrian.enable_check"); + param_names_.emplace(module, param_name); + } + + // obstacle stop + { + const auto module = "obstacle_stop"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.stop_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } + + // obstacle slowdown + { + const auto module = "obstacle_slowdown"; + ParamNameEnableObject param_name; + param_name.node = + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner"; + param_name.ns = "common.slow_down_obstacle_type"; + param_name.name = default_param_name.name; + param_names_.emplace(module, param_name); + } +} + +void TargetObjectTypePanel::updateMatrix() +{ + // blue base + // const QColor color_in_use("#6eb6cc"); + // const QColor color_no_use("#1d3e48"); + // const QColor color_undefined("#9e9e9e"); + + // green base + const QColor color_in_use("#afff70"); + const QColor color_no_use("#44642b"); + const QColor color_undefined("#9e9e9e"); + + const auto set_undefined = [&](const auto i, const auto j) { + QTableWidgetItem * item = new QTableWidgetItem("N/A"); + item->setForeground(QBrush(Qt::black)); // set the text color to black + item->setBackground(color_undefined); + matrix_widget_->setItem(i, j, item); + }; + + for (size_t i = 0; i < modules_.size(); i++) { + const auto & module = modules_[i]; + + // Check if module exists in param_names_ + if (param_names_.find(module) == param_names_.end()) { + RCLCPP_WARN_STREAM(node_->get_logger(), module << " is not in the param names"); + continue; + } + + const auto & module_params = param_names_.at(module); + auto parameters_client = + std::make_shared(node_, module_params.node); + if (!parameters_client->wait_for_service(std::chrono::microseconds(500))) { + RCLCPP_WARN_STREAM( + node_->get_logger(), "Failed to find parameter service for node: " << module_params.node); + for (size_t j = 0; j < targets_.size(); j++) { + set_undefined(i, j); + } + continue; + } + + for (size_t j = 0; j < targets_.size(); j++) { + const auto & target = targets_[j]; + + // Check if target exists in module's name map + if (module_params.name.find(target) == module_params.name.end()) { + RCLCPP_WARN_STREAM( + node_->get_logger(), target << " parameter is not set in the " << module); + continue; + } + + std::string param_name = + (module_params.ns.empty() ? "" : module_params.ns + ".") + module_params.name.at(target); + auto parameter_result = parameters_client->get_parameters({param_name}); + + if (!parameter_result.empty()) { + bool value = parameter_result[0].as_bool(); + QTableWidgetItem * item = new QTableWidgetItem(value ? "O" : "X"); + item->setForeground(QBrush(value ? Qt::black : Qt::black)); // set the text color to black + item->setBackground(QBrush(value ? color_in_use : color_no_use)); + matrix_widget_->setItem(i, j, item); + } else { + RCLCPP_WARN_STREAM( + node_->get_logger(), + "Failed to get parameter " << module_params.node << " " << param_name); + + set_undefined(i, j); + } + } + } +} + +PLUGINLIB_EXPORT_CLASS(TargetObjectTypePanel, rviz_common::Panel) diff --git a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp new file mode 100644 index 000000000..1f3934369 --- /dev/null +++ b/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// 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 TARGET_OBJECT_TYPE_PANEL_HPP_ +#define TARGET_OBJECT_TYPE_PANEL_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +class TargetObjectTypePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit TargetObjectTypePanel(QWidget * parent = 0); + +protected: + QTableWidget * matrix_widget_; + std::shared_ptr node_; + std::vector modules_; + std::vector targets_; + + struct ParamNameEnableObject + { + std::string node; + std::string ns; + std::unordered_map name; + }; + std::unordered_map param_names_; + +private slots: + void onReloadButtonClicked(); + +private: + QPushButton * reload_button_; + + void updateMatrix(); + void setParameters(); +}; + +#endif // TARGET_OBJECT_TYPE_PANEL_HPP_