diff --git a/.gitignore b/.gitignore index 09bf5d65..73a34844 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ *.pyc -__pycache__/ \ No newline at end of file +__pycache__/ + diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a5ca7b6c..1e61f5e7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -34,7 +34,7 @@ repos: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.8.0 + rev: v0.9.0 hooks: - id: flake8-ros - id: prettier-xacro diff --git a/build_depends.repos b/build_depends.repos index 69312a4f..825fbf65 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -16,10 +16,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_adapi_msgs.git version: main - core/external/autoware_auto_msgs: + core/autoware_internal_msgs: type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main + url: https://github.com/autowarefoundation/autoware_internal_msgs.git + version: main # universe universe/autoware.universe: type: git diff --git a/common/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt new file mode 100644 index 00000000..2b06e6db --- /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 00000000..59d36a0a --- /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 00000000..e45cf273 --- /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 00000000..c8285fcc --- /dev/null +++ b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,8 @@ + + + MrmGoalTool + + + RouteSelectorPanel + + diff --git a/vehicle/accel_brake_map_calibrator/src/main.cpp b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp similarity index 59% rename from vehicle/accel_brake_map_calibrator/src/main.cpp rename to common/mission_planner_rviz_plugin/src/mrm_goal.cpp index 01dfe203..ef5529ab 100644 --- a/vehicle/accel_brake_map_calibrator/src/main.cpp +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp @@ -1,5 +1,4 @@ -// -// Copyright 2020 Tier IV, Inc. All rights reserved. +// 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. @@ -12,17 +11,24 @@ // 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/accel_brake_map_calibrator_node.hpp" +#include "mrm_goal.hpp" + +namespace rviz_plugins +{ -#include +MrmGoalTool::MrmGoalTool() +{ + shortcut_key_ = 'm'; +} -int main(int argc, char ** argv) +void MrmGoalTool::onInitialize() { - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - rclcpp::spin(std::make_shared(node_options)); - rclcpp::shutdown(); - return 0; + 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 00000000..e16b0abf --- /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 00000000..b4b376b1 --- /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 00000000..99101730 --- /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 00000000..f2fad9e2 --- /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 00000000..7a15d56f --- /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 00000000..6a675737 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 00000000..36c1b476 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 00000000..f9c5d120 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 00000000..a691602c 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 00000000..53f00386 --- /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 00000000..001ae153 --- /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 00000000..0a32423e --- /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 "static_obstacle_avoidance_left"; + } + case Module::AVOIDANCE_RIGHT: { + return "static_obstacle_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 00000000..8bdaef94 --- /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 00000000..cdc57743 --- /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 00000000..6fd626d5 --- /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 00000000..4f5b4961 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 00000000..6a675737 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 00000000..58d12f95 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 00000000..8dd4d9d0 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 00000000..1800202e 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 00000000..61dd9e1d 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 00000000..a6ddc6d2 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 00000000..a9af89c0 --- /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 00000000..fb533137 --- /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 00000000..e9d77491 --- /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 00000000..4efa6fed --- /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 00000000..4ea3fa4b --- /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 00000000..43fc0edc --- /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 00000000..6fc98cee --- /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 00000000..6b8d7765 --- /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 00000000..72a5bd4f --- /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 00000000..3ca368a3 --- /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 00000000..88b7b5e7 --- /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 00000000..6b03fe92 --- /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 00000000..f422847d --- /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 00000000..82f35b93 --- /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 00000000..d89f13ce --- /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 00000000..70ebe063 --- /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 00000000..95fff455 --- /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 00000000..aed5a92b --- /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_vehicle_msgs::msg::VelocityReport` | Current velocity status | +| `/api/autoware/get/engage` | `tier4_external_api_msgs::srv::Engage` | Getting Engage | +| `/vehicle/status/gear_status` | `autoware_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_control_msgs::msg::ControlCommand` | Control command | +| `/external/selected/gear_cmd` | `autoware_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 00000000..e4a8ddb0 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 00000000..d5da7f06 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 00000000..5027ec57 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 00000000..a691602c 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 00000000..bec78de0 --- /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_control_msgs + autoware_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 00000000..068bbcd7 --- /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 00000000..7fd700b2 --- /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; + Control control_cmd; + { + control_cmd.stamp = raw_node_->get_clock()->now(); + control_cmd.lateral.steering_tire_angle = steering_angle_; + control_cmd.longitudinal.velocity = 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; + control_cmd.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); + } + GearCommand gear_cmd; + { + const double eps = 0.001; + if (control_cmd.longitudinal.velocity > eps && current_velocity_ > -eps) { + gear_cmd.command = GearCommand::DRIVE; + } else if (control_cmd.longitudinal.velocity < -eps && current_velocity_ < eps) { + gear_cmd.command = GearCommand::REVERSE; + control_cmd.longitudinal.acceleration *= -1.0; + } else { + gear_cmd.command = GearCommand::PARK; + } + } + pub_control_command_->publish(control_cmd); + 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 00000000..61b4f2a0 --- /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_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_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_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_vehicle_msgs::msg::Engage; +using autoware_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 00000000..02e65ad7 --- /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 00000000..91898161 --- /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 00000000..6a675737 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 00000000..6a675737 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 00000000..7cd7ca75 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 00000000..c8267c70 --- /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 00000000..37bf743e --- /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 00000000..0960875d --- /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 00000000..45b73d5b --- /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 00000000..e18900af --- /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 00000000..0187cc3e --- /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 00000000..b1933ebb --- /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 00000000..538dc0cb --- /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_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp index 962fee8a..163f4995 100644 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ class LateralErrorPublisher : public rclcpp::Node double yaw_threshold_to_search_closest_; /* States */ - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr + autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; //!< @brief reference trajectory geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_vehicle_pose_ptr_; //!< @brief current EKF pose @@ -44,7 +44,7 @@ class LateralErrorPublisher : public rclcpp::Node current_ground_truth_pose_ptr_; //!< @brief current GNSS pose /* Publishers and Subscribers */ - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscription for reference trajectory rclcpp::Subscription::SharedPtr sub_vehicle_pose_; //!< @brief subscription for vehicle pose @@ -60,7 +60,7 @@ class LateralErrorPublisher : public rclcpp::Node /** * @brief set current_trajectory_ with received message */ - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr); /** * @brief set current_vehicle_pose_ with received message */ diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index 8f03a3b5..3a028c90 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs geometry_msgs motion_utils rclcpp diff --git a/common/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp index 18c97a47..13c87223 100644 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -26,7 +26,7 @@ LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_op declare_parameter("yaw_threshold_to_search_closest", M_PI / 4.0); /* Publishers and Subscribers */ - sub_trajectory_ = create_subscription( + sub_trajectory_ = create_subscription( "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&LateralErrorPublisher::onTrajectory, this, _1)); sub_vehicle_pose_ = create_subscription( @@ -44,7 +44,7 @@ LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_op } void LateralErrorPublisher::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { current_trajectory_ptr_ = msg; } 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 00000000..cc7da7e3 --- /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 00000000..ed400e52 --- /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 00000000..e8d2f775 --- /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 00000000..37d70b49 --- /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 00000000..7849e604 --- /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 00000000..ce5cbd13 --- /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 00000000..72ecf361 --- /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 00000000..a93aff72 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 00000000..5b6e205b --- /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 00000000..d16c19c3 --- /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 00000000..a691602c 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 00000000..180bf9ee --- /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 00000000..fdf105d6 --- /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 00000000..cad82890 --- /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 00000000..5c4d16f5 --- /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 00000000..02076634 --- /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 00000000..a6218f32 --- /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 00000000..9431c2d4 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 00000000..8bf5e3a9 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 00000000..a691602c 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 00000000..39c3669c 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 00000000..d80b18a5 --- /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 00000000..00caf2e5 --- /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 00000000..ad698d92 --- /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 00000000..b2ac3321 --- /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 00000000..ed458cf9 --- /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 00000000..1296bd34 --- /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 00000000..33aa9a1e 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 00000000..04d2f28d --- /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 00000000..eb3350e4 --- /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 00000000..e0143079 --- /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 00000000..1f393436 --- /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_ diff --git a/control/control_debug_tools/CMakeLists.txt b/control/control_debug_tools/CMakeLists.txt new file mode 100644 index 00000000..91ab86d3 --- /dev/null +++ b/control/control_debug_tools/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.5) +project(control_debug_tools) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package( + INSTALL_TO_SHARE + scripts +) diff --git a/control/control_debug_tools/package.xml b/control/control_debug_tools/package.xml new file mode 100644 index 00000000..2d938f13 --- /dev/null +++ b/control/control_debug_tools/package.xml @@ -0,0 +1,22 @@ + + + + control_debug_tools + 0.1.0 + The control_debug_tools package + Zhe Shen + Takayuki Murooka + Apache License 2.0 + + Zhe Shen + + ament_cmake_auto + autoware_cmake + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/control_debug_tools/scripts/consistency_checker.py b/control/control_debug_tools/scripts/consistency_checker.py new file mode 100755 index 00000000..a8b964aa --- /dev/null +++ b/control/control_debug_tools/scripts/consistency_checker.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +# 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. + + +import argparse +import math + +from ament_index_python.packages import get_package_share_directory +import yaml + + +def read_yaml(file_path): + """Read YAML file and return the data.""" + with open(file_path, "r") as file: + return yaml.safe_load(file) + + +parser = argparse.ArgumentParser( + description="Process the parameters of the controllers and simulator." +) + +parser.add_argument( + "--vehicle_description", + help="Specify the vehicle description package name", + default="sample_vehicle_description", +) +parser.add_argument("--mpc_param_file", help="Override the default MPC parameter file path") +parser.add_argument("--pid_param_file", help="Override the default PID parameter file path") +parser.add_argument( + "--simulator_model_param_file", help="Override the default simulator model parameter file path" +) + + +def main(): + args = parser.parse_args() + + autoware_launch_path = get_package_share_directory("autoware_launch") + vehicle_description_path = get_package_share_directory(args.vehicle_description) + + mpc_param_file_path = ( + args.mpc_param_file + if args.mpc_param_file + else f"{autoware_launch_path}/config/control/trajectory_follower/lateral/mpc.param.yaml" + ) + pid_param_file_path = ( + args.pid_param_file + if args.pid_param_file + else f"{autoware_launch_path}/config/control/trajectory_follower/longitudinal/pid.param.yaml" + ) + simulator_model_param_file_path = ( + args.simulator_model_param_file + if args.simulator_model_param_file + else f"{vehicle_description_path}/config/simulator_model.param.yaml" + ) + + mpc_params = read_yaml(mpc_param_file_path)["/**"]["ros__parameters"] + pid_params = read_yaml(pid_param_file_path)["/**"]["ros__parameters"] + simulator_model_params = read_yaml(simulator_model_param_file_path)["/**"]["ros__parameters"] + + results = [ + ( + "[MPC] steer_delay_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_time_delay"] - mpc_params["input_delay"], + mpc_params["input_delay"], + simulator_model_params["steer_time_delay"], + ) + ), + ( + "[MPC] steer_time_constant_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_time_constant"] + - mpc_params["vehicle_model_steer_tau"], + mpc_params["vehicle_model_steer_tau"], + simulator_model_params["steer_time_constant"], + ) + ), + ( + "[MPC] acceleration_limit_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["vel_rate_lim"] - mpc_params["acceleration_limit"], + mpc_params["acceleration_limit"], + simulator_model_params["vel_rate_lim"], + ) + ), + ( + "[MPC] max_steer_rate_lim_difference_by_curvature: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_rate_lim"] + - max(mpc_params["steer_rate_lim_dps_list_by_curvature"]) * (math.pi / 180), + max(mpc_params["steer_rate_lim_dps_list_by_curvature"]) * (math.pi / 180), + simulator_model_params["steer_rate_lim"], + ) + ), + ( + "[MPC] max_steer_rate_lim_difference_by_velocity: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_rate_lim"] + - max(mpc_params["steer_rate_lim_dps_list_by_velocity"]) * (math.pi / 180), + max(mpc_params["steer_rate_lim_dps_list_by_velocity"]) * (math.pi / 180), + simulator_model_params["steer_rate_lim"], + ) + ), + ( + "[PID] max_acc_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["vel_rate_lim"] - pid_params["max_acc"], + pid_params["max_acc"], + simulator_model_params["vel_rate_lim"], + ) + ), + ( + "[PID] min_acc_difference: {}, (controller: {}, simulator: {})".format( + -simulator_model_params["vel_rate_lim"] - pid_params["min_acc"], + pid_params["min_acc"], + -simulator_model_params["vel_rate_lim"], + ) + ), + ( + "[PID] accelerate_delay_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["acc_time_delay"] - pid_params["delay_compensation_time"], + pid_params["delay_compensation_time"], + simulator_model_params["acc_time_delay"], + ) + ), + ] + + for item in results: + description = item.split(",")[0] + value = float(description.split(":")[1].strip()) + error_message = "" + if ( + "steer_delay_difference" in item + or "steer_time_constant_difference" in item + or "accelerate_delay_difference" in item + ): + if value != 0.0: + error_message = "[ERROR] The parameters of the controller and simulator should be identical.\033[0m" + if ( + "acceleration_limit_difference" in item + or "max_steer_rate_lim_difference_by_curvature" in item + or "max_steer_rate_lim_difference_by_velocity" in item + or "max_acc_difference" in item + ): + if value < 0: + error_message = "[ERROR] The parameter of the controller should be smaller than the parameter of the simulator.\033[0m" + if "min_acc_difference" in item and value > 0: + error_message = "[ERROR] The parameter of the controller should be bigger than the parameter of the simulator.\033[0m" + print(f"{item}") + if error_message: + print(f"\033[91m{error_message}\033[0m\n") + + +if __name__ == "__main__": + main() diff --git a/driving_environment_analyzer/CMakeLists.txt b/driving_environment_analyzer/CMakeLists.txt new file mode 100644 index 00000000..6641bcf8 --- /dev/null +++ b/driving_environment_analyzer/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.14) +project(driving_environment_analyzer) + +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 + include/${PROJECT_NAME}/driving_environment_analyzer_node.hpp + include/${PROJECT_NAME}/driving_environment_analyzer_rviz_plugin.hpp + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzerNode" + EXECUTABLE driving_environment_analyzer_node +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + launch + plugins +) diff --git a/driving_environment_analyzer/README.md b/driving_environment_analyzer/README.md new file mode 100644 index 00000000..28d3ef30 --- /dev/null +++ b/driving_environment_analyzer/README.md @@ -0,0 +1,141 @@ +# Driving Environment Analyzer + +このツールはROSBAGに含まれる走行履歴を元に走行環境のODDを解析するツールです。 + +## ROSBAGの特定時刻における周囲のODDを解析する場合 + +この場合にはRvizプラグインである`driving_environment_analyzer_rviz_panel`を使用することをおすすめします。 + +現在以下の情報が出力可能です。 + +- EGOの現在車速 +- 現在位置の勾配 +- EGOの挙動 +- 現在の車線情報 + +こちらのツールはautoware_launchに含まれる`logging_simulator`を使用します。まず以下のコマンドからシミュレータを起動してください。 + +`ros2 launch autoware_launch logging_simulator.launch.xml map_path:= vehicle_model:= sensor_model:= sensing:=false control:=false planning:=false perception:=false localization:=false system:=false` + +![fig1](./images/launch_rviz.png) + +シミュレータ起動時に地図を読み込むためROSBAGに地図情報が含まれていなくてもODDの解析が可能です。（ただし、その場合にはROSBAG取得の際に使用した地図を指定してシミュレータを起動するようにしてください。） + +次に本パッケージに含まれる解析ツールを起動します。Rviz画面左上部のAdd New PanelからDrivingEnvironmentAnalyzerPanelを選択してください。これでRviz左下に新しく操作パネルが追加されます。 + +![fig1](./images/launch_tool.png) +![fig1](./images/rviz_overview_1.png) + +本ツールはROSBAGファイル指定してロードできる他、複数のROSBAGファイルが格納されているディレクトリを指定することも可能です。ただし、その場合には事前に以下のコマンドで`metadata.yaml`の生成が必要になります。 + +`ros2 bag reindex sqlite3` + +![fig1](./images/select_directory.png) + +ROSBAGの読み込みが完了したらODDを解析したい時刻を指定します。時刻の指定にはUnix timeを直接指定するほかスライドバーも使用可能です。左に表示されている日時を参考に調整してください。 + +![fig1](./images/select_time .png) + +また、このときViewsのTarget Flameを`base_link`にしておくことで、指定した時刻のEGOの位置と周囲の状況をRvizで可視化可能です。 + +![fig1](./images/select_target_frame.png) + +時刻の指定が完了したら、`Set time stamp`ボタンを押し、最後に`Analyze dynamic ODD factor`を押すことで解析が始まります。 + +![fig1](./images/rviz_overview_2.png) + +```bash +[rviz2-11] *********************************************************** +[rviz2-11] ODD analysis result +[rviz2-11] *********************************************************** +[rviz2-11] Type: TIME SPECIFIED +[rviz2-11] Time: 2024-04-22 14:48:05 +[rviz2-11] +[rviz2-11] +[rviz2-11] - EGO INFO +[rviz2-11] [SPEED] : 0 [m/s] +[rviz2-11] [ELEVATION ANGLE] : 0.00963597 [rad] +[rviz2-11] +[rviz2-11] - EGO BEHAIOVR +[rviz2-11] [AVOIDANCE(R)] : NONE +[rviz2-11] [AVOIDANCE(L)] : NONE +[rviz2-11] [LANE_CHANGE(R)] : NONE +[rviz2-11] [LANE_CHANGE(L)] : NONE +[rviz2-11] [START_PLANNER] : SAFE: true COMMAND: deactivate +[rviz2-11] [GOAL_PLANNER] : NONE +[rviz2-11] [CROSSWALK] : NONE +[rviz2-11] [INTERSECTION] : NONE +[rviz2-11] +[rviz2-11] - LANE INFO +[rviz2-11] [ID] : 176126 +[rviz2-11] [WIDTH] : 4.24132 [m] +[rviz2-11] [SHAPE] : STRAIGHT +[rviz2-11] [RIGHT LANE NUM] : 0 +[rviz2-11] [LEFT LANE NUM] : 0 +[rviz2-11] [TOTAL LANE NUM] : 1 +[rviz2-11] [SAME DIRECTION LANE] : NONE +[rviz2-11] [OPPOSITE DIRECTION LANE] : NONE +[rviz2-11] [ROAD SHOULDER] : EXIST +[rviz2-11] +[rviz2-11] - SURROUND OBJECT NUM +[rviz2-11] [UNKNOWN] : 0 +[rviz2-11] [CAR] : 6 +[rviz2-11] [TRUCK] : 0 +[rviz2-11] [BUS] : 3 +[rviz2-11] [TRAILER] : 2 +[rviz2-11] [MOTORCYCLE] : 0 +[rviz2-11] [BICYCLE] : 0 +[rviz2-11] [PEDESTRIAN] : 7 +[rviz2-11] *********************************************************** +``` + +## ROSBAG全体に対して経路沿いのODDを解析する場合 + +現在以下の情報が出力可能です。 + +- 走行経路の長さ +- 走行経路の車線情報 +- 走行経路の最大・最小勾配 +- 走行経路の最大曲率 +- 走行経路の最大・最小車線幅 +- 交差点の有無 +- 信号機の有無 +- 横断歩道の有無 + +起動時に`bag_path`オプションで解析したいROSBAGを指定してください。（ディレクトリの指定も.db3ファイルの直接指定もサポートしています。） + +解析に必要なtopicは以下のとおりです。（今後増える可能性もあります。） + +- `/planning/mission_planning/route` +- `/map/vector_map` + +以下のようにlaunchすることでODDの解析結果が得られます。 + +`ros2 launch driving_environment_analyzer driving_environment_analyzer.launch.xml use_map_in_bag:=true bag_path:=` + +```bash +[component_container-1] [INFO 1708999777.768870564] [driving_environment_analyzer]: ====================================== +[component_container-1] [INFO 1708999777.768922452] [driving_environment_analyzer]: data is ready. start ODD analysis... +[component_container-1] [INFO 1708999777.768933574] [driving_environment_analyzer]: ====================================== +[component_container-1] [INFO 1708999777.768967412] [driving_environment_analyzer]: - Length of total lanes : 2357.50 [m] +[component_container-1] [INFO 1708999777.769031174] [driving_environment_analyzer]: - Length of lane that has adjacent lane : 2080.43 [m] +[component_container-1] [INFO 1708999777.769076141] [driving_environment_analyzer]: - Length of lane that has opposite lane : 0.00 [m] +[component_container-1] [INFO 1708999777.769101793] [driving_environment_analyzer]: - Length of lane that has no adjacent lane : 277.07 [m] +[component_container-1] [INFO 1708999777.769225729] [driving_environment_analyzer]: - Min lane width: 3.14 [m] Max lane width: 4.94 [m] +[component_container-1] [INFO 1708999777.769278698] [driving_environment_analyzer]: - Max curvature: 0.007967 [1/m] +[component_container-1] [INFO 1708999777.769293161] [driving_environment_analyzer]: - Min curve radius: 125.52 [m] +[component_container-1] [INFO 1708999777.769336094] [driving_environment_analyzer]: - Min elevation angle: -0.033037 [rad] Max elevation angle: 0.026073 [rad] +[component_container-1] [INFO 1708999777.769403870] [driving_environment_analyzer]: - Min speed limit: 13.89 [m/s] Max speed limit: 16.67 [m/s] +[component_container-1] [INFO 1708999777.769424648] [driving_environment_analyzer]: - Exist traffic light: true +[component_container-1] [INFO 1708999777.769435813] [driving_environment_analyzer]: - Exist intersection: true +[component_container-1] [INFO 1708999777.769620035] [driving_environment_analyzer]: - Exist crosswalk: true +[component_container-1] [INFO 1708999777.769634980] [driving_environment_analyzer]: ====================================== +[component_container-1] [INFO 1708999777.769642769] [driving_environment_analyzer]: complete ODD analysis. shutdown. +[component_container-1] [INFO 1708999777.769650034] [driving_environment_analyzer]: ====================================== +``` + +ただし、`map/vector_map`に関しては`use_map_in_bag`を`false`にすることでローカル環境に保存されている地図を使用してODD解析を行うことも可能です。その場合、`map_path`オプションで地図のパスを指定してください。 + +`ros2 launch driving_environment_analyzer driving_environment_analyzer.launch.xml use_map_in_bag:=false map_path:= bag_path:=` + +以上のようにオプションを指定することでROSBAGに地図情報が保存されていなくてもODD解析が可能です。 diff --git a/driving_environment_analyzer/images/launch_rviz.png b/driving_environment_analyzer/images/launch_rviz.png new file mode 100644 index 00000000..fa2adcf7 Binary files /dev/null and b/driving_environment_analyzer/images/launch_rviz.png differ diff --git a/driving_environment_analyzer/images/launch_tool.png b/driving_environment_analyzer/images/launch_tool.png new file mode 100644 index 00000000..5db44cdb Binary files /dev/null and b/driving_environment_analyzer/images/launch_tool.png differ diff --git a/driving_environment_analyzer/images/rviz_overview_1.png b/driving_environment_analyzer/images/rviz_overview_1.png new file mode 100644 index 00000000..b5b26992 Binary files /dev/null and b/driving_environment_analyzer/images/rviz_overview_1.png differ diff --git a/driving_environment_analyzer/images/rviz_overview_2.png b/driving_environment_analyzer/images/rviz_overview_2.png new file mode 100644 index 00000000..e626873d Binary files /dev/null and b/driving_environment_analyzer/images/rviz_overview_2.png differ diff --git a/driving_environment_analyzer/images/select_directory.png b/driving_environment_analyzer/images/select_directory.png new file mode 100644 index 00000000..6b40fe48 Binary files /dev/null and b/driving_environment_analyzer/images/select_directory.png differ diff --git a/driving_environment_analyzer/images/select_target_frame.png b/driving_environment_analyzer/images/select_target_frame.png new file mode 100644 index 00000000..97c1342a Binary files /dev/null and b/driving_environment_analyzer/images/select_target_frame.png differ diff --git a/driving_environment_analyzer/images/select_time.png b/driving_environment_analyzer/images/select_time.png new file mode 100644 index 00000000..8ff092d0 Binary files /dev/null and b/driving_environment_analyzer/images/select_time.png differ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp new file mode 100644 index 00000000..3227faa6 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp @@ -0,0 +1,103 @@ +// 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. + +#ifndef DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_ + +#include "driving_environment_analyzer/type_alias.hpp" +#include "rosbag2_cpp/reader.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace driving_environment_analyzer::analyzer_core +{ + +struct ODDRawData +{ + rcutils_time_point_value_t timestamp; + Odometry odometry; + PredictedObjects objects; + TFMessage tf; + TFMessage tf_static; + CooperateStatusArray rtc_status; +}; + +class AnalyzerCore +{ +public: + explicit AnalyzerCore(rclcpp::Node & node); + ~AnalyzerCore(); + + bool isDataReadyForStaticODDAnalysis() const; + bool isDataReadyForDynamicODDAnalysis() const { return odd_raw_data_.has_value(); } + + void analyzeStaticODDFactor() const; + void analyzeDynamicODDFactor(std::ofstream & ofs_csv_file) const; + + void addHeader(std::ofstream & ofs_csv_file) const; + + void setBagFile(const std::string & file_name); + + void setTimeStamp(const rcutils_time_point_value_t & timestamp) + { + odd_raw_data_ = getRawData(timestamp); + } + + void setMap(const LaneletMapBin & msg) { route_handler_.setMap(msg); } + + void clearData() { odd_raw_data_ = std::nullopt; } + + std::pair getBagStartEndTime() + { + const auto metadata = reader_.get_metadata(); + const auto start_time = + duration_cast(metadata.starting_time.time_since_epoch()).count(); + const auto duration_time = duration_cast(metadata.duration).count(); + return {start_time, start_time + duration_time}; + } + + Odometry getOdometry() const { return odd_raw_data_.value().odometry; } + PredictedObjects getObjects() const { return odd_raw_data_.value().objects; } + TFMessage getTF() const { return odd_raw_data_.value().tf; } + TFMessage getTFStatic() const { return odd_raw_data_.value().tf_static; } + +private: + Pose getEgoPose() const { return odd_raw_data_.value().odometry.pose.pose; } + + double getEgoSpeed() const { return odd_raw_data_.value().odometry.twist.twist.linear.x; } + + template + std::optional getLastTopic(const std::string & topic_name); + template + std::optional seekTopic( + const std::string & topic_name, const rcutils_time_point_value_t & timestamp); + std::optional getRawData(const rcutils_time_point_value_t & timestamp); + + std::optional odd_raw_data_{std::nullopt}; + + route_handler::RouteHandler route_handler_; + + rosbag2_cpp::Reader reader_; + + rclcpp::Logger logger_; +}; +} // namespace driving_environment_analyzer::analyzer_core + +#endif // DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp new file mode 100644 index 00000000..aed631cb --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp @@ -0,0 +1,45 @@ +// 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. + +#ifndef DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_ + +#include "driving_environment_analyzer/analyzer_core.hpp" +#include "driving_environment_analyzer/type_alias.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer +{ + +class DrivingEnvironmentAnalyzerNode : public rclcpp::Node +{ +public: + explicit DrivingEnvironmentAnalyzerNode(const rclcpp::NodeOptions & node_options); + +private: + void onMap(const LaneletMapBin::ConstSharedPtr map_msg); + void analyze(); + + std::shared_ptr analyzer_; + + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::TimerBase::SharedPtr timer_; + rosbag2_cpp::Reader reader_; +}; +} // namespace driving_environment_analyzer + +#endif // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp new file mode 100644 index 00000000..3d12c350 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp @@ -0,0 +1,86 @@ +// 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. + +#ifndef DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_ + +#include "driving_environment_analyzer/analyzer_core.hpp" +#include "driving_environment_analyzer/type_alias.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace driving_environment_analyzer +{ + +class DrivingEnvironmentAnalyzerPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit DrivingEnvironmentAnalyzerPanel(QWidget * parent = nullptr); + ~DrivingEnvironmentAnalyzerPanel() override; + void onInitialize() override; + +public Q_SLOTS: + void onBoxUpdate(); + void onSliderUpdate(); + void onClickSetTimeStamp(); + void onSelectBagFile(); + void onSelectDirectory(); + void onClickAnalyzeStaticODDFactor(); + void onClickAnalyzeDynamicODDFactor(); + +private: + void onMap(const LaneletMapBin::ConstSharedPtr map_msg); + + std::shared_ptr analyzer_; + + std::ofstream ofs_csv_file_; + + QSpinBox * bag_time_selector_; + QSlider * bag_time_slider_; + QLabel * bag_name_label_; + QLabel * bag_time_line_; + QPushButton * dir_button_ptr_; + QPushButton * file_button_ptr_; + QPushButton * analyze_static_odd_button_; + QPushButton * analyze_dynamic_odd_button_; + QPushButton * set_timestamp_btn_; + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_odometry_; + rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Publisher::SharedPtr pub_tf_; + rclcpp::Publisher::SharedPtr pub_tf_static_; +}; +} // namespace driving_environment_analyzer + +#endif // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp new file mode 100644 index 00000000..a0c4625d --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp @@ -0,0 +1,53 @@ +// 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. + +#ifndef DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_ + +#include +#include + +#include "tier4_rtc_msgs/msg/command.hpp" +#include "tier4_rtc_msgs/msg/cooperate_status_array.hpp" +#include "tier4_rtc_msgs/msg/module.hpp" +#include +#include +#include +#include +#include + +namespace driving_environment_analyzer +{ + +// std +using std::chrono::duration_cast; +using std::chrono::seconds; + +// ros2 +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using tf2_msgs::msg::TFMessage; + +// autoware +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_rtc_msgs::msg::Command; +using tier4_rtc_msgs::msg::CooperateStatusArray; +using tier4_rtc_msgs::msg::Module; + +} // namespace driving_environment_analyzer + +#endif // DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_ diff --git a/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp b/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp new file mode 100644 index 00000000..8bb57023 --- /dev/null +++ b/driving_environment_analyzer/include/driving_environment_analyzer/utils.hpp @@ -0,0 +1,96 @@ +// 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. + +#ifndef DRIVING_ENVIRONMENT_ANALYZER__UTILS_HPP_ +#define DRIVING_ENVIRONMENT_ANALYZER__UTILS_HPP_ + +#include "driving_environment_analyzer/type_alias.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer::utils +{ + +template +std::vector calcElevationAngle(const T & points); + +double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose); + +double getRouteLength(const lanelet::ConstLanelets & lanes); + +double getMaxCurvature(const lanelet::ConstLanelets & lanes); + +double getLaneWidth(const lanelet::ConstLanelet & lane); + +std::pair getLaneWidth(const lanelet::ConstLanelets & lanes); + +std::pair getElevation(const lanelet::ConstLanelets & lanes); + +std::pair getSpeedLimit( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +double getRouteLengthWithSameDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +double getRouteLengthWithOppositeDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +double getRouteLengthWithNoAdjacentLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +bool existSameDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existOppositeDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existRoadShoulderLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +int getLeftLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +int getRightLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +int getTotalLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existTrafficLight(const lanelet::ConstLanelets & lanes); + +bool existIntersection(const lanelet::ConstLanelets & lanes); + +bool existCrosswalk( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler); + +bool existCrosswalk( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler); + +bool isStraightLane(const lanelet::ConstLanelet & lane); + +size_t getObjectNumber(const PredictedObjects & objects, const std::uint8_t target_class); + +std::string getLaneShape(const lanelet::ConstLanelet & lane); + +std::string getModuleStatus(const CooperateStatusArray & status_array, const uint8_t module_type); + +std::string getEgoBehavior( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler, + const Pose & pose); + +} // namespace driving_environment_analyzer::utils + +#endif // DRIVING_ENVIRONMENT_ANALYZER__UTILS_HPP_ diff --git a/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml b/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml new file mode 100644 index 00000000..ff84bc98 --- /dev/null +++ b/driving_environment_analyzer/launch/driving_environment_analyzer.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/driving_environment_analyzer/package.xml b/driving_environment_analyzer/package.xml new file mode 100644 index 00000000..df2ea6b4 --- /dev/null +++ b/driving_environment_analyzer/package.xml @@ -0,0 +1,53 @@ + + + + driving_environment_analyzer + 0.1.0 + The driving_environment_analyzer package + + Satoshi Ota + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs + behavior_path_planner_common + geometry_msgs + interpolation + lane_departure_checker + lanelet2_extension + libboost-dev + libqt5-core + libqt5-gui + libqt5-widgets + motion_utils + qtbase5-dev + rclcpp + rclcpp_components + route_handler + rviz_common + rviz_default_plugins + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + + ament_cmake + + + diff --git a/driving_environment_analyzer/plugins/plugin_description.xml b/driving_environment_analyzer/plugins/plugin_description.xml new file mode 100644 index 00000000..fe19f819 --- /dev/null +++ b/driving_environment_analyzer/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + DrivingEnvironmentAnalyzerPanel + + + diff --git a/driving_environment_analyzer/src/analyzer_core.cpp b/driving_environment_analyzer/src/analyzer_core.cpp new file mode 100644 index 00000000..06d0fda9 --- /dev/null +++ b/driving_environment_analyzer/src/analyzer_core.cpp @@ -0,0 +1,347 @@ +// 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. + +#include "driving_environment_analyzer/analyzer_core.hpp" + +#include "driving_environment_analyzer/utils.hpp" + +#include +#include +#include +#include +#include + +namespace driving_environment_analyzer::analyzer_core +{ + +AnalyzerCore::AnalyzerCore(rclcpp::Node & node) : logger_{node.get_logger()} +{ +} + +bool AnalyzerCore::isDataReadyForStaticODDAnalysis() const +{ + if (!route_handler_.isMapMsgReady()) { + RCLCPP_WARN(logger_, "Map data is not ready."); + return false; + } + + if (!route_handler_.isHandlerReady()) { + RCLCPP_WARN(logger_, "Route data is not ready."); + return false; + } + + return true; +} + +void AnalyzerCore::setBagFile(const std::string & file_name) +{ + reader_.open(file_name); + + const auto opt_route = getLastTopic("/planning/mission_planning/route"); + if (opt_route.has_value()) { + route_handler_.setRoute(opt_route.value()); + } + + const auto opt_map = getLastTopic("/map/vector_map"); + if (opt_map.has_value()) { + route_handler_.setMap(opt_map.value()); + } +} + +template +std::optional AnalyzerCore::getLastTopic(const std::string & topic_name) +{ + rosbag2_storage::StorageFilter filter; + filter.topics.emplace_back(topic_name); + reader_.set_filter(filter); + + if (!reader_.has_next()) { + return std::nullopt; + } + + rclcpp::Serialization serializer; + + const auto deserialized_message = std::make_shared(); + while (reader_.has_next()) { + rclcpp::SerializedMessage serialized_msg(*reader_.read_next()->serialized_data); + serializer.deserialize_message(&serialized_msg, deserialized_message.get()); + } + + return *deserialized_message; +} + +template +std::optional AnalyzerCore::seekTopic( + const std::string & topic_name, const rcutils_time_point_value_t & timestamp) +{ + rosbag2_storage::StorageFilter filter; + filter.topics.emplace_back(topic_name); + reader_.set_filter(filter); + + rclcpp::Serialization serializer; + reader_.seek(timestamp); + + if (!reader_.has_next()) { + return std::nullopt; + } + + rclcpp::SerializedMessage serialized_msg(*reader_.read_next()->serialized_data); + const auto deserialized_message = std::make_shared(); + serializer.deserialize_message(&serialized_msg, deserialized_message.get()); + + return *deserialized_message; +} + +std::optional AnalyzerCore::getRawData(const rcutils_time_point_value_t & timestamp) +{ + ODDRawData odd_raw_data; + + odd_raw_data.timestamp = timestamp; + + const auto opt_odometry = seekTopic("/localization/kinematic_state", timestamp * 1e9); + if (!opt_odometry.has_value()) { + return std::nullopt; + } else { + odd_raw_data.odometry = opt_odometry.value(); + } + + const auto opt_objects = + seekTopic("/perception/object_recognition/objects", timestamp * 1e9); + if (!opt_objects.has_value()) { + return std::nullopt; + } else { + odd_raw_data.objects = opt_objects.value(); + } + + const auto opt_rtc_status = + seekTopic("/api/external/get/rtc_status", timestamp * 1e9); + if (!opt_rtc_status.has_value()) { + return std::nullopt; + } else { + odd_raw_data.rtc_status = opt_rtc_status.value(); + } + + const auto opt_tf = seekTopic("/tf", timestamp * 1e9); + if (!opt_tf.has_value()) { + return std::nullopt; + } else { + odd_raw_data.tf = opt_tf.value(); + } + + const auto [start_time, end_time] = getBagStartEndTime(); + const auto opt_tf_static = seekTopic("/tf_static", start_time * 1e9); + if (!opt_tf_static.has_value()) { + return std::nullopt; + } else { + odd_raw_data.tf_static = opt_tf_static.value(); + } + + return odd_raw_data; +} + +void AnalyzerCore::addHeader(std::ofstream & ofs_csv_file) const +{ + ofs_csv_file << "TIME" << ','; + ofs_csv_file << "EGO [SPEED]" << ','; + ofs_csv_file << "EGO [ELEVATION ANGLE]" << ','; + ofs_csv_file << "EGO BEHAVIOR [AVOIDANCE(R)]" << ','; + ofs_csv_file << "EGO BEHAVIOR [AVOIDANCE(L)]" << ','; + ofs_csv_file << "EGO BEHAVIOR [LANE_CHANGE(R)]" << ','; + ofs_csv_file << "EGO BEHAVIOR [LANE_CHANGE(L)]" << ','; + ofs_csv_file << "EGO BEHAVIOR [START_PLANNER]" << ','; + ofs_csv_file << "EGO BEHAVIOR [GOAL_PLANNER]" << ','; + ofs_csv_file << "EGO BEHAVIOR [CROSSWALK]" << ','; + ofs_csv_file << "EGO BEHAVIOR [INTERSECTION]" << ','; + ofs_csv_file << "LANE [ID]" << ','; + ofs_csv_file << "LANE [WIDTH]" << ','; + ofs_csv_file << "LANE [SHAPE]" << ','; + ofs_csv_file << "LANE [RIGHT LANE NUM]" << ','; + ofs_csv_file << "LANE [LEFT LANE NUM]" << ','; + ofs_csv_file << "LANE [TOTAL LANE NUM]" << ','; + ofs_csv_file << "LANE [SAME DIRECTION LANE]" << ','; + ofs_csv_file << "LANE [OPPOSITE DIRECTION LANE]" << ','; + ofs_csv_file << "LANE [ROAD SHOULDER]" << ','; + ofs_csv_file << "OBJECT [UNKNOWN]" << ','; + ofs_csv_file << "OBJECT [CAR]" << ','; + ofs_csv_file << "OBJECT [TRUCK]" << ','; + ofs_csv_file << "OBJECT [BUS]" << ','; + ofs_csv_file << "OBJECT [TRAILER]" << ','; + ofs_csv_file << "OBJECT [MOTORCYCLE]" << ','; + ofs_csv_file << "OBJECT [BICYCLE]" << ','; + ofs_csv_file << "OBJECT [PEDESTRIAN]" << ','; + ofs_csv_file << std::endl; +} + +void AnalyzerCore::analyzeDynamicODDFactor(std::ofstream & ofs_csv_file) const +{ + std::ostringstream ss; + ss << std::boolalpha << "\n"; + ss << "***********************************************************\n"; + ss << " ODD analysis result\n"; + ss << "***********************************************************\n"; + ss << "Type: TIME SPECIFIED\n"; + + const auto write = [&ofs_csv_file](const auto & data) { + ofs_csv_file << data << ','; + return data; + }; + + char buffer[128]; + auto seconds = static_cast(odd_raw_data_.value().timestamp); + strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", localtime(&seconds)); + ss << "Time: " << write(buffer) << "\n"; + ss << "\n"; + ss << "\n"; + + lanelet::ConstLanelet closest_lanelet; + if (!route_handler_.getClosestLaneletWithinRoute(getEgoPose(), &closest_lanelet)) { + return; + } + + const auto number = [this](const auto & target_class) { + return utils::getObjectNumber(odd_raw_data_.value().objects, target_class); + }; + + const auto status = [this](const auto & module_type) { + return utils::getModuleStatus(odd_raw_data_.value().rtc_status, module_type); + }; + + const auto exist_crosswalk = [this, &closest_lanelet]() { + return utils::existCrosswalk(closest_lanelet, route_handler_) ? "EXIST" : "NONE"; + }; + + const auto to_string = [](const bool exist) { return exist ? "EXIST" : "NONE"; }; + + ss << "- EGO INFO\n"; + ss << " [SPEED] : " << write(getEgoSpeed()) << " [m/s]\n"; + ss << " [ELEVATION ANGLE] : " + << write(utils::calcElevationAngle(closest_lanelet, getEgoPose())) << " [rad]\n"; + ss << "\n"; + + ss << "- EGO BEHAVIOR\n"; + ss << " [AVOIDANCE(R)] : " << write(status(Module::AVOIDANCE_RIGHT)) << "\n"; + ss << " [AVOIDANCE(L)] : " << write(status(Module::AVOIDANCE_LEFT)) << "\n"; + ss << " [LANE_CHANGE(R)] : " << write(status(Module::LANE_CHANGE_RIGHT)) << "\n"; + ss << " [LANE_CHANGE(L)] : " << write(status(Module::LANE_CHANGE_LEFT)) << "\n"; + ss << " [START_PLANNER] : " << write(status(Module::START_PLANNER)) << "\n"; + ss << " [GOAL_PLANNER] : " << write(status(Module::GOAL_PLANNER)) << "\n"; + ss << " [CROSSWALK] : " << write(exist_crosswalk()) << "\n"; + ss << " [INTERSECTION] : " + << write(utils::getEgoBehavior(closest_lanelet, route_handler_, getEgoPose())) << "\n"; + ss << "\n"; + + ss << "- LANE INFO\n"; + ss << " [ID] : " << write(closest_lanelet.id()) << "\n"; + ss << " [WIDTH] : " << write(utils::getLaneWidth(closest_lanelet)) + << " [m]\n"; + ss << " [SHAPE] : " << write(utils::getLaneShape(closest_lanelet)) << "\n"; + ss << " [RIGHT LANE NUM] : " + << write(utils::getRightLaneletNum(closest_lanelet, route_handler_)) << "\n"; + ss << " [LEFT LANE NUM] : " + << write(utils::getLeftLaneletNum(closest_lanelet, route_handler_)) << "\n"; + ss << " [TOTAL LANE NUM] : " + << write(utils::getTotalLaneletNum(closest_lanelet, route_handler_)) << "\n"; + ss << " [SAME DIRECTION LANE] : " + << write(to_string(utils::existSameDirectionLane(closest_lanelet, route_handler_))) << "\n"; + ss << " [OPPOSITE DIRECTION LANE] : " + << write(to_string(utils::existOppositeDirectionLane(closest_lanelet, route_handler_))) + << "\n"; + ss << " [ROAD SHOULDER] : " + << write(to_string(utils::existRoadShoulderLane(closest_lanelet, route_handler_))) << "\n"; + ss << "\n"; + + ss << "- SURROUND OBJECT NUM\n"; + ss << " [UNKNOWN] : " << write(number(ObjectClassification::UNKNOWN)) + << "\n"; + ss << " [CAR] : " << write(number(ObjectClassification::CAR)) << "\n"; + ss << " [TRUCK] : " << write(number(ObjectClassification::TRUCK)) << "\n"; + ss << " [BUS] : " << write(number(ObjectClassification::BUS)) << "\n"; + ss << " [TRAILER] : " << write(number(ObjectClassification::TRAILER)) + << "\n"; + ss << " [MOTORCYCLE] : " << write(number(ObjectClassification::MOTORCYCLE)) + << "\n"; + ss << " [BICYCLE] : " << write(number(ObjectClassification::BICYCLE)) + << "\n"; + ss << " [PEDESTRIAN] : " << write(number(ObjectClassification::PEDESTRIAN)) + << "\n"; + + ss << "***********************************************************\n"; + + ofs_csv_file << std::endl; + + RCLCPP_INFO_STREAM(logger_, ss.str()); +} + +void AnalyzerCore::analyzeStaticODDFactor() const +{ + std::ostringstream ss; + ss << std::boolalpha << "\n"; + ss << "***********************************************************\n"; + ss << " ODD analysis result\n"; + ss << "***********************************************************\n"; + ss << "Type: ROUTE SPECIFIED\n"; + ss << "\n"; + ss << "\n"; + + const auto preferred_lanes = route_handler_.getPreferredLanelets(); + ss << "- ROUTE INFO\n"; + ss << " total length : " << utils::getRouteLength(preferred_lanes) + << " [m]\n"; + ss << " exist same direction lane section : " + << utils::getRouteLengthWithSameDirectionLane(preferred_lanes, route_handler_) << " [m]\n"; + ss << " exist same opposite lane section : " + << utils::getRouteLengthWithOppositeDirectionLane(preferred_lanes, route_handler_) << " [m]\n"; + ss << " no adjacent lane section : " + << utils::getRouteLengthWithNoAdjacentLane(preferred_lanes, route_handler_) << " [m]\n"; + + ss << " exist traffic light : " << utils::existTrafficLight(preferred_lanes) + << "\n"; + ss << " exist intersection : " << utils::existIntersection(preferred_lanes) + << "\n"; + ss << " exist crosswalk : " + << utils::existCrosswalk(preferred_lanes, route_handler_) << "\n"; + ss << "\n"; + + const auto [min_width, max_width] = utils::getLaneWidth(preferred_lanes); + ss << "- LANE WIDTH\n"; + ss << " max : " << max_width << " [m]\n"; + ss << " min : " << min_width << " [m]\n"; + ss << "\n"; + + const auto max_curvature = utils::getMaxCurvature(preferred_lanes); + ss << "- LANE CURVATURE\n"; + ss << " max : " << max_curvature << " [1/m]\n"; + ss << " max : " << 1.0 / max_curvature << " [m]\n"; + ss << "\n"; + + const auto [min_elevation, max_elevation] = utils::getElevation(preferred_lanes); + ss << "- ELEVATION ANGLE\n"; + ss << " max : " << max_elevation << " [rad]\n"; + ss << " min : " << min_elevation << " [rad]\n"; + ss << "\n"; + + const auto [min_speed_limit, max_speed_limit] = + utils::getSpeedLimit(preferred_lanes, route_handler_); + ss << "- SPEED LIMIT\n"; + ss << " max : " << max_speed_limit << " [m/s]\n"; + ss << " min : " << min_speed_limit << " [m/s]\n"; + ss << "\n"; + + ss << "***********************************************************\n"; + + RCLCPP_INFO_STREAM(logger_, ss.str()); +} + +AnalyzerCore::~AnalyzerCore() = default; +} // namespace driving_environment_analyzer::analyzer_core diff --git a/driving_environment_analyzer/src/driving_environment_analyzer_node.cpp b/driving_environment_analyzer/src/driving_environment_analyzer_node.cpp new file mode 100644 index 00000000..6ec4b567 --- /dev/null +++ b/driving_environment_analyzer/src/driving_environment_analyzer_node.cpp @@ -0,0 +1,62 @@ +// 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. + +#include "driving_environment_analyzer/driving_environment_analyzer_node.hpp" + +#include "driving_environment_analyzer/analyzer_core.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer +{ + +DrivingEnvironmentAnalyzerNode::DrivingEnvironmentAnalyzerNode( + const rclcpp::NodeOptions & node_options) +: Node("driving_environment_analyzer", node_options) +{ + using std::placeholders::_1; + using std::chrono_literals::operator""ms; + + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&DrivingEnvironmentAnalyzerNode::analyze, this)); + + sub_map_ = create_subscription( + "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), + std::bind(&DrivingEnvironmentAnalyzerNode::onMap, this, _1)); + + analyzer_ = std::make_shared(*this); + + analyzer_->setBagFile(declare_parameter("bag_path")); +} + +void DrivingEnvironmentAnalyzerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) +{ + analyzer_->setMap(*msg); +} + +void DrivingEnvironmentAnalyzerNode::analyze() +{ + if (!analyzer_->isDataReadyForStaticODDAnalysis()) { + return; + } + + analyzer_->analyzeStaticODDFactor(); + rclcpp::shutdown(); +} +} // namespace driving_environment_analyzer + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(driving_environment_analyzer::DrivingEnvironmentAnalyzerNode) diff --git a/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp b/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp new file mode 100644 index 00000000..6045ef72 --- /dev/null +++ b/driving_environment_analyzer/src/driving_environment_analyzer_rviz_plugin.cpp @@ -0,0 +1,223 @@ +// 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. + +#include "driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp" + +#include "driving_environment_analyzer/analyzer_core.hpp" + +#include +#include +#include + +namespace driving_environment_analyzer +{ +namespace +{ +void set_format_time(QLabel * label, double time) +{ + char buffer[128]; + auto seconds = static_cast(time); + strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", localtime(&seconds)); + label->setText(QString(buffer) + QString::number((time - seconds), 'f', 3).rightRef(4)); +} +} // namespace + +DrivingEnvironmentAnalyzerPanel::DrivingEnvironmentAnalyzerPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + auto * v_layout = new QVBoxLayout(this); + { + auto * layout = new QHBoxLayout(this); + + // add button to load bag file. + file_button_ptr_ = new QPushButton("Select bag file"); + connect(file_button_ptr_, SIGNAL(clicked()), SLOT(onSelectBagFile())); + layout->addWidget(file_button_ptr_); + + // add button to load directory. + dir_button_ptr_ = new QPushButton("Select directory"); + connect(dir_button_ptr_, SIGNAL(clicked()), SLOT(onSelectDirectory())); + layout->addWidget(dir_button_ptr_); + + v_layout->addLayout(layout); + } + + { + auto * layout = new QHBoxLayout(this); + + // add label to show target time. + layout->addWidget(new QLabel("Bag Time:")); + bag_time_line_ = new QLabel; + layout->addWidget(bag_time_line_); + + // add spin box to select target time. + bag_time_selector_ = new QSpinBox(); + bag_time_selector_->setValue(0); + bag_time_selector_->setSingleStep(1); + connect(bag_time_selector_, SIGNAL(valueChanged(int)), SLOT(onBoxUpdate())); + layout->addWidget(bag_time_selector_); + + // add button to set target time. + set_timestamp_btn_ = new QPushButton("Set time stamp"); + connect(set_timestamp_btn_, SIGNAL(clicked()), SLOT(onClickSetTimeStamp())); + layout->addWidget(set_timestamp_btn_); + + // add button to start analyzing dynamic ODD. + analyze_static_odd_button_ = new QPushButton("Analyze dynamic ODD factor"); + connect(analyze_static_odd_button_, SIGNAL(clicked()), SLOT(onClickAnalyzeDynamicODDFactor())); + layout->addWidget(analyze_static_odd_button_); + + // add button to start analyzing static ODD. + analyze_dynamic_odd_button_ = new QPushButton("Analyze static ODD factor"); + connect(analyze_dynamic_odd_button_, SIGNAL(clicked()), SLOT(onClickAnalyzeStaticODDFactor())); + layout->addWidget(analyze_dynamic_odd_button_); + + v_layout->addLayout(layout); + } + + { + bag_time_slider_ = new QSlider(); + bag_time_slider_->setOrientation(Qt::Horizontal); + bag_time_slider_->setValue(0); + connect(bag_time_slider_, SIGNAL(valueChanged(int)), SLOT(onSliderUpdate())); + v_layout->addWidget(bag_time_slider_); + } + + { + bag_name_label_ = new QLabel(); + bag_name_label_->setAlignment(Qt::AlignLeft); + auto * layout = new QHBoxLayout(this); + layout->addWidget(new QLabel("BagFile:")); + layout->addWidget(bag_name_label_); + v_layout->addLayout(layout); + } + + setLayout(v_layout); +} + +void DrivingEnvironmentAnalyzerPanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_map_ = raw_node_->create_subscription( + "/map/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&DrivingEnvironmentAnalyzerPanel::onMap, this, _1)); + + pub_odometry_ = + raw_node_->create_publisher("/localization/kinematic_state", rclcpp::QoS(1)); + pub_objects_ = raw_node_->create_publisher( + "/perception/object_recognition/objects", rclcpp::QoS(1)); + pub_tf_ = raw_node_->create_publisher("/tf", rclcpp::QoS(1)); + pub_tf_static_ = raw_node_->create_publisher("/tf_static", rclcpp::QoS(1)); + + analyzer_ = std::make_shared(*raw_node_); +} + +void DrivingEnvironmentAnalyzerPanel::onMap(const LaneletMapBin::ConstSharedPtr msg) +{ + analyzer_->setMap(*msg); +} + +void DrivingEnvironmentAnalyzerPanel::onBoxUpdate() +{ + set_format_time(bag_time_line_, bag_time_selector_->value()); + bag_time_slider_->setValue(bag_time_selector_->value()); +} + +void DrivingEnvironmentAnalyzerPanel::onSliderUpdate() +{ + set_format_time(bag_time_line_, bag_time_slider_->value()); + bag_time_selector_->setValue(bag_time_slider_->value()); +} + +void DrivingEnvironmentAnalyzerPanel::onSelectDirectory() +{ + QString file_name = QFileDialog::getExistingDirectory(this, tr("Open ROSBAG file"), "/tmp"); + if (file_name.count() == 0) { + return; + } + + analyzer_->setBagFile(file_name.toStdString()); + + ofs_csv_file_ = std::ofstream( + file_name.toStdString() + "/" + file_name.split("/").back().toStdString() + "_odd.csv"); + analyzer_->addHeader(ofs_csv_file_); + + const auto [start_time, end_time] = analyzer_->getBagStartEndTime(); + bag_time_selector_->setRange(start_time, end_time); + bag_time_slider_->setRange(start_time, end_time); + bag_name_label_->setText(file_name); +} + +void DrivingEnvironmentAnalyzerPanel::onSelectBagFile() +{ + QString file_name = + QFileDialog::getOpenFileName(this, tr("Open ROSBAG file"), "/tmp", tr("ROSBAG (*.db3)")); + + if (file_name.count() == 0) { + return; + } + + analyzer_->setBagFile(file_name.toStdString()); + + ofs_csv_file_ = std::ofstream(file_name.toStdString() + "_odd.csv"); + analyzer_->addHeader(ofs_csv_file_); + + const auto [start_time, end_time] = analyzer_->getBagStartEndTime(); + bag_time_selector_->setRange(start_time, end_time); + bag_time_slider_->setRange(start_time, end_time); + bag_name_label_->setText(file_name); +} + +void DrivingEnvironmentAnalyzerPanel::onClickSetTimeStamp() +{ + analyzer_->clearData(); + analyzer_->setTimeStamp(bag_time_selector_->value()); + + if (!analyzer_->isDataReadyForDynamicODDAnalysis()) { + return; + } + + pub_odometry_->publish(analyzer_->getOdometry()); + pub_objects_->publish(analyzer_->getObjects()); + pub_tf_->publish(analyzer_->getTF()); + pub_tf_static_->publish(analyzer_->getTFStatic()); +} + +void DrivingEnvironmentAnalyzerPanel::onClickAnalyzeDynamicODDFactor() +{ + if (!analyzer_->isDataReadyForDynamicODDAnalysis()) { + return; + } + + analyzer_->analyzeDynamicODDFactor(ofs_csv_file_); +} + +void DrivingEnvironmentAnalyzerPanel::onClickAnalyzeStaticODDFactor() +{ + if (!analyzer_->isDataReadyForStaticODDAnalysis()) { + return; + } + + analyzer_->analyzeStaticODDFactor(); +} + +DrivingEnvironmentAnalyzerPanel::~DrivingEnvironmentAnalyzerPanel() = default; +} // namespace driving_environment_analyzer + +#include +PLUGINLIB_EXPORT_CLASS( + driving_environment_analyzer::DrivingEnvironmentAnalyzerPanel, rviz_common::Panel) diff --git a/driving_environment_analyzer/src/utils.cpp b/driving_environment_analyzer/src/utils.cpp new file mode 100644 index 00000000..b42dd3d4 --- /dev/null +++ b/driving_environment_analyzer/src/utils.cpp @@ -0,0 +1,462 @@ +// 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. + +#include "driving_environment_analyzer/utils.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace driving_environment_analyzer::utils +{ +namespace +{ +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + return label; +} +} // namespace + +template +std::vector calcElevationAngle(const T & points) +{ + std::vector elevation_vec(points.size(), 0.0); + if (points.size() < 2) { + return elevation_vec; + } + + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i + 1)); + elevation_vec.at(i) = tier4_autoware_utils::calcElevationAngle(p1, p2); + } + elevation_vec.at(elevation_vec.size() - 1) = elevation_vec.at(elevation_vec.size() - 2); + + return elevation_vec; +} + +double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + const auto points = to_ros_msg(lane.centerline()); + + if (points.size() < 2) { + return 0.0; + } + + const size_t idx = motion_utils::findNearestSegmentIndex(points, pose.position); + + const auto p1 = tier4_autoware_utils::getPoint(points.at(idx)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(idx + 1)); + return tier4_autoware_utils::calcElevationAngle(p1, p2); +} + +double getLaneWidth(const lanelet::ConstLanelet & lane) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); + return boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; +} + +double getRouteLength(const lanelet::ConstLanelets & lanes) +{ + return lanelet::utils::getLaneletLength3d(lanes); +} + +double getMaxCurvature(const lanelet::ConstLanelets & lanes) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto values = motion_utils::calcCurvature(to_ros_msg(lane.centerline())); + const auto max_value_itr = std::max_element(values.begin(), values.end()); + if (max_value_itr == values.end()) { + continue; + } + + if (max_value < *max_value_itr) { + max_value = *max_value_itr; + } + } + + return max_value; +} + +std::pair getLaneWidth(const lanelet::ConstLanelets & lanes) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + double min_value = std::numeric_limits::max(); + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto lon_length = motion_utils::calcArcLength(to_ros_msg(lane.centerline())); + const auto width = boost::geometry::area(lane.polygon2d().basicPolygon()) / lon_length; + + if (min_value > width) { + min_value = width; + } + + if (max_value < width) { + max_value = width; + } + } + + return std::make_pair(min_value, max_value); +} + +std::pair getElevation(const lanelet::ConstLanelets & lanes) +{ + const auto to_ros_msg = [](const auto & line) { + std::vector points; + std::for_each(line.begin(), line.end(), [&points](const auto & p) { + points.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return points; + }; + + double min_value = std::numeric_limits::max(); + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto values = calcElevationAngle(to_ros_msg(lane.centerline())); + const auto max_value_itr = std::max_element(values.begin(), values.end()); + const auto min_value_itr = std::min_element(values.begin(), values.end()); + + if (min_value_itr == values.end()) { + continue; + } + + if (min_value > *min_value_itr) { + min_value = *min_value_itr; + } + + if (max_value_itr == values.end()) { + continue; + } + + if (max_value < *max_value_itr) { + max_value = *max_value_itr; + } + } + + return std::make_pair(min_value, max_value); +} + +std::pair getSpeedLimit( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + const auto traffic_rule = route_handler.getTrafficRulesPtr(); + + double min_value = std::numeric_limits::max(); + double max_value = 0.0; + + for (const auto & lane : lanes) { + const auto limit = traffic_rule->speedLimit(lane).speedLimit; + min_value = std::min(limit.value(), min_value); + max_value = std::max(limit.value(), max_value); + } + + return std::make_pair(min_value, max_value); +} + +double getRouteLengthWithSameDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + double value = 0.0; + + for (const auto & lane : lanes) { + if (existSameDirectionLane(lane, route_handler)) { + value += lanelet::utils::getLaneletLength3d(lane); + } + } + + return value; +} + +double getRouteLengthWithOppositeDirectionLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + double value = 0.0; + + for (const auto & lane : lanes) { + if (existOppositeDirectionLane(lane, route_handler)) { + value += lanelet::utils::getLaneletLength3d(lane); + } + } + + return value; +} + +double getRouteLengthWithNoAdjacentLane( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + double value = 0.0; + + for (const auto & lane : lanes) { + if (existSameDirectionLane(lane, route_handler)) { + continue; + } + + if (existOppositeDirectionLane(lane, route_handler)) { + continue; + } + + value += lanelet::utils::getLaneletLength3d(lane); + } + + return value; +} + +bool existSameDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto right_lanelet = route_handler.getRightLanelet(lane, true, false); + const auto left_lanelet = route_handler.getLeftLanelet(lane, true, false); + return right_lanelet.has_value() || left_lanelet.has_value(); +} + +bool existOppositeDirectionLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + { + const auto lanelet = route_handler.getMostRightLanelet(lane, true, false); + if (!route_handler.getRightOppositeLanelets(lanelet).empty()) { + return true; + } + } + + { + const auto lanelet = route_handler.getMostLeftLanelet(lane, true, false); + if (!route_handler.getRightOppositeLanelets(lanelet).empty()) { + return true; + } + } + + return false; +} + +bool existRoadShoulderLane( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + { + const auto lanelet = route_handler.getMostRightLanelet(lane, true, true); + const lanelet::Attribute sub_type = lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + return true; + } + } + + { + const auto lanelet = route_handler.getMostLeftLanelet(lane, true, true); + const lanelet::Attribute sub_type = lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + return true; + } + } + + return false; +} + +int getLeftLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto left_lanelet = route_handler.getLeftLanelet(lane, true, false); + if (left_lanelet.has_value()) { + return getLeftLaneletNum(left_lanelet.value(), route_handler) + 1; + } + + const auto opposite_lanelets = route_handler.getLeftOppositeLanelets(lane); + if (!opposite_lanelets.empty()) { + return getRightLaneletNum(opposite_lanelets.front(), route_handler) + 1; + } + + return 0; +} + +int getRightLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + const auto right_lanelet = route_handler.getRightLanelet(lane, true, false); + if (right_lanelet.has_value()) { + return getRightLaneletNum(right_lanelet.value(), route_handler) + 1; + } + + const auto opposite_lanelets = route_handler.getRightOppositeLanelets(lane); + if (!opposite_lanelets.empty()) { + return getLeftLaneletNum(opposite_lanelets.front(), route_handler) + 1; + } + + return 0; +} + +int getTotalLaneletNum( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + return getRightLaneletNum(lane, route_handler) + getLeftLaneletNum(lane, route_handler) + 1; +} + +bool existTrafficLight(const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + if (!lane.regulatoryElementsAs().empty()) { + return true; + } + } + + return false; +} + +bool existIntersection(const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + std::string turn_direction = lane.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + return true; + } + } + + return false; +} + +bool existCrosswalk( + const lanelet::ConstLanelet & lane, const route_handler::RouteHandler & route_handler) +{ + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto overall_graphs = route_handler.getOverallGraphPtr(); + return !overall_graphs->conflictingInGraph(lane, PEDESTRIAN_GRAPH_ID).empty(); +} + +bool existCrosswalk( + const lanelet::ConstLanelets & lanes, const route_handler::RouteHandler & route_handler) +{ + for (const auto & lane : lanes) { + if (existCrosswalk(lane, route_handler)) { + return true; + } + } + + return false; +} + +bool isStraightLane(const lanelet::ConstLanelet & lane) +{ + constexpr double THRESHOLD = 50.0; // radius [m] + return 1.0 / getMaxCurvature({lane}) > THRESHOLD; +} + +size_t getObjectNumber(const PredictedObjects & objects, const std::uint8_t target_class) +{ + return std::count_if( + objects.objects.begin(), objects.objects.end(), [&target_class](const auto & o) { + return getHighestProbLabel(o.classification) == target_class; + }); +} + +std::string getLaneShape(const lanelet::ConstLanelet & lane) +{ + if (isStraightLane(lane)) { + return "STRAIGHT"; + } + + return "CURVE"; +} + +std::string getModuleStatus(const CooperateStatusArray & status_array, const uint8_t module_type) +{ + const auto itr = std::find_if( + status_array.statuses.begin(), status_array.statuses.end(), + [&module_type](const auto & s) { return s.module.type == module_type; }); + + if (itr == status_array.statuses.end()) { + return "NONE"; + } + + const auto to_string = [](const auto & command_type) { + return command_type == Command::DEACTIVATE ? "deactivate" : "activate"; + }; + + std::ostringstream ss; + ss << std::boolalpha; + ss << "SAFE: " << itr->safe << " COMMAND: " << to_string(itr->command_status.type); + return ss.str(); +} + +std::string getEgoBehavior( + const lanelet::ConstLanelet & lane, + [[maybe_unused]] const route_handler::RouteHandler & route_handler, + [[maybe_unused]] const Pose & pose) +{ + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + + if (turn_direction == "straight") { + return "GO STRAIGHT"; + } + + if (turn_direction == "right") { + return "TURN RIGHT"; + } + + if (turn_direction == "left") { + return "TURN LEFT"; + } + + return "NONE"; +} +} // namespace driving_environment_analyzer::utils diff --git a/evaluation/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluation/tier4_metrics_rviz_plugin/CMakeLists.txt new file mode 100644 index 00000000..8475b596 --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_metrics_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets Charts) +set(QT_WIDGETS_LIB Qt5::Widgets) +set(QT_CHARTS_LIB Qt5::Charts) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/metrics_visualize_panel.cpp + include/metrics_visualize_panel.hpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_WIDGETS_LIB} + ${QT_CHARTS_LIB} +) + +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic) +# 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/evaluation/tier4_metrics_rviz_plugin/README.md b/evaluation/tier4_metrics_rviz_plugin/README.md new file mode 100644 index 00000000..be941412 --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/README.md @@ -0,0 +1,16 @@ +# tier4_metrics_rviz_plugin + +## Purpose + +This plugin panel to visualize `planning_evaluator` output. + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------------------------------- | --------------------------------------- | ------------------------------------- | +| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output | + +## HowToUse + +1. Start rviz and select panels/Add new panel. +2. Select MetricsVisualizePanel and press OK. diff --git a/evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png b/evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png new file mode 100644 index 00000000..6a675737 Binary files /dev/null and b/evaluation/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png differ diff --git a/evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp b/evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp new file mode 100644 index 00000000..3d660999 --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp @@ -0,0 +1,254 @@ +// Copyright 2024 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 METRICS_VISUALIZE_PANEL_HPP_ +#define METRICS_VISUALIZE_PANEL_HPP_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace rviz_plugins +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using QtCharts::QChart; +using QtCharts::QChartView; +using QtCharts::QLineSeries; + +struct Metric +{ +public: + explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget) + { + init(status); + } + + void init(const DiagnosticStatus & status) + { + QStringList header{}; + + { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + label->setText(QString::fromStdString(status.name)); + labels.emplace("metric_name", label); + + header.push_back("metric_name"); + } + + for (const auto & [key, value] : status.values) { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + labels.emplace(key, label); + + auto plot = new QLineSeries; + plot->setName(QString::fromStdString(key)); + plots.emplace(key, plot); + chart->chart()->addSeries(plot); + chart->chart()->createDefaultAxes(); + + header.push_back(QString::fromStdString(key)); + } + + { + chart->chart()->setTitle(QString::fromStdString(status.name)); + chart->chart()->legend()->setVisible(true); + chart->chart()->legend()->detachFromChart(); + chart->setRenderHint(QPainter::Antialiasing); + } + + { + table->setColumnCount(status.values.size() + 1); + table->setHorizontalHeaderLabels(header); + table->verticalHeader()->hide(); + table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table->setRowCount(1); + table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); + } + } + + void updateData(const double time, const DiagnosticStatus & status) + { + for (const auto & [key, value] : status.values) { + const double data = std::stod(value); + labels.at(key)->setText(QString::fromStdString(toString(data))); + plots.at(key)->append(time, data); + updateMinMax(data); + } + + { + const auto area = chart->chart()->plotArea(); + const auto rect = chart->chart()->legend()->rect(); + chart->chart()->legend()->setGeometry( + QRectF(area.x(), area.y(), area.width(), rect.height())); + chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time); + } + + { + table->setCellWidget(0, 0, labels.at("metric_name")); + } + + for (size_t i = 0; i < status.values.size(); ++i) { + table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key)); + } + } + + void updateMinMax(double data) + { + if (data < y_range_min) { + y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data; + chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min); + } + + if (data > y_range_max) { + y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data; + chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max); + } + } + + void updateTable() { table->update(); } + + void updateGraph() { chart->update(); } + + QChartView * getChartView() const { return chart; } + + QTableWidget * getTable() const { return table; } + + std::unordered_map getLabels() const { return labels; } + +private: + static std::optional getValue(const DiagnosticStatus & status, std::string && key) + { + const auto itr = std::find_if( + status.values.begin(), status.values.end(), + [&](const auto & value) { return value.key == key; }); + + if (itr == status.values.end()) { + return std::nullopt; + } + + return itr->value; + } + + static std::string toString(const double & value) + { + std::stringstream ss; + ss << std::scientific << std::setprecision(2) << value; + return ss.str(); + } + + QChartView * chart; + QTableWidget * table; + + std::unordered_map labels; + std::unordered_map plots; + + double y_range_min{std::numeric_limits::max()}; + double y_range_max{std::numeric_limits::lowest()}; +}; + +class MetricsVisualizePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit MetricsVisualizePanel(QWidget * parent = nullptr); + void onInitialize() override; + +private Q_SLOTS: + // Slot functions triggered by UI events + void onTopicChanged(); + void onSpecificMetricChanged(); + void onClearButtonClicked(); + void onTabChanged(); + +private: + // ROS 2 node and subscriptions for handling metrics data + rclcpp::Node::SharedPtr raw_node_; + rclcpp::TimerBase::SharedPtr timer_; + std::unordered_map::SharedPtr> subscriptions_; + + // Topics from which metrics are collected + std::vector topics_ = { + "/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"}; + + // Timer and metrics message callback + void onTimer(); + void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name); + + // Functions to update UI based on selected metrics + void updateViews(); + void updateSelectedMetric(const std::string & metric_name); + + // UI components + QGridLayout * grid_; + QComboBox * topic_selector_; + QTabWidget * tab_widget_; + + // "Specific Metrics" tab components + QComboBox * specific_metric_selector_; + QChartView * specific_metric_chart_view_; + QTableWidget * specific_metric_table_; + + // Selected metrics data + std::optional> selected_metrics_; + + // Cache for received messages by topics + std::unordered_map current_msg_map_; + + // Mapping from topics to metrics widgets (tables and charts) + std::unordered_map< + std::string, std::unordered_map>> + topic_widgets_map_; + + // Synchronization + std::mutex mutex_; + + // Stored metrics data + std::unordered_map metrics_; + + // Utility functions for managing widget visibility based on topics + void updateWidgetVisibility(const std::string & target_topic, const bool show); + void showCurrentTopicWidgets(); + void hideInactiveTopicWidgets(); +}; +} // namespace rviz_plugins + +#endif // METRICS_VISUALIZE_PANEL_HPP_ diff --git a/evaluation/tier4_metrics_rviz_plugin/package.xml b/evaluation/tier4_metrics_rviz_plugin/package.xml new file mode 100644 index 00000000..d06382bc --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + tier4_metrics_rviz_plugin + 0.0.0 + The tier4_metrics_rviz_plugin package + Satoshi OTA + Kyoichi Sugahara + Maxime CLEMENT + Kosuke Takeuchi + Fumiya Watanabe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + libqt5-charts-dev + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml b/evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 00000000..5aca5bd7 --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,5 @@ + + + MetricsVisualizePanel + + diff --git a/evaluation/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluation/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp new file mode 100644 index 00000000..b92a9a7a --- /dev/null +++ b/evaluation/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -0,0 +1,262 @@ +// Copyright 2024 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 "metrics_visualize_panel.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) +: rviz_common::Panel(parent), grid_(new QGridLayout()) +{ + // Initialize the main tab widget + tab_widget_ = new QTabWidget(); + + // Create and configure the "All Metrics" tab + QWidget * all_metrics_widget = new QWidget(); + grid_ = new QGridLayout(all_metrics_widget); // Apply grid layout to the widget directly + all_metrics_widget->setLayout(grid_); + + // Add topic selector combobox + topic_selector_ = new QComboBox(); + for (const auto & topic : topics_) { + topic_selector_->addItem(QString::fromStdString(topic)); + } + grid_->addWidget(topic_selector_, 0, 0, 1, -1); // Add topic selector to the grid layout + connect(topic_selector_, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged())); + + tab_widget_->addTab( + all_metrics_widget, "All Metrics"); // Add "All Metrics" tab to the tab widget + + // Create and configure the "Specific Metrics" tab + QWidget * specific_metrics_widget = new QWidget(); + QVBoxLayout * specific_metrics_layout = new QVBoxLayout(); + specific_metrics_widget->setLayout(specific_metrics_layout); + + // Add specific metric selector combobox + specific_metric_selector_ = new QComboBox(); + specific_metrics_layout->addWidget(specific_metric_selector_); + connect( + specific_metric_selector_, SIGNAL(currentIndexChanged(int)), this, + SLOT(onSpecificMetricChanged())); + + // Add clear button + QPushButton * clear_button = new QPushButton("Clear"); + specific_metrics_layout->addWidget(clear_button); + connect(clear_button, &QPushButton::clicked, this, &MetricsVisualizePanel::onClearButtonClicked); + + // Add chart view for specific metrics + specific_metric_chart_view_ = new QChartView(); + specific_metrics_layout->addWidget(specific_metric_chart_view_); + + tab_widget_->addTab( + specific_metrics_widget, "Specific Metrics"); // Add "Specific Metrics" tab to the tab widget + + // Set the main layout of the panel + QVBoxLayout * main_layout = new QVBoxLayout(); + main_layout->addWidget(tab_widget_); + setLayout(main_layout); +} + +void MetricsVisualizePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + for (const auto & topic_name : topics_) { + const auto callback = [this, topic_name](const DiagnosticArray::ConstSharedPtr msg) { + this->onMetrics(msg, topic_name); + }; + const auto subscription = + raw_node_->create_subscription(topic_name, rclcpp::QoS{1}, callback); + subscriptions_[topic_name] = subscription; + } + + const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); + timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); +} + +void MetricsVisualizePanel::updateWidgetVisibility( + const std::string & target_topic, const bool show) +{ + for (const auto & [topic_name, metric_widgets_pair] : topic_widgets_map_) { + const bool is_target_topic = (topic_name == target_topic); + if ((!is_target_topic && show) || (is_target_topic && !show)) { + continue; + } + for (const auto & [metric, widgets] : metric_widgets_pair) { + widgets.first->setVisible(show); + widgets.second->setVisible(show); + } + } +} + +void MetricsVisualizePanel::showCurrentTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, true); +} + +void MetricsVisualizePanel::hideInactiveTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, false); +} + +void MetricsVisualizePanel::onTopicChanged() +{ + std::lock_guard message_lock(mutex_); + hideInactiveTopicWidgets(); + showCurrentTopicWidgets(); +} + +void MetricsVisualizePanel::updateSelectedMetric(const std::string & metric_name) +{ + std::lock_guard message_lock(mutex_); + + for (const auto & [topic, msg] : current_msg_map_) { + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + for (const auto & status : msg->status) { + if (metric_name == status.name) { + selected_metrics_ = {metric_name, Metric(status)}; + selected_metrics_->second.updateData(time, status); + return; + } + } + } +} + +void MetricsVisualizePanel::updateViews() +{ + if (!selected_metrics_) { + return; + } + + Metric & metric = selected_metrics_->second; + specific_metric_chart_view_->setChart(metric.getChartView()->chart()); + auto * specific_metrics_widget = dynamic_cast(tab_widget_->widget(1)); + auto * specific_metrics_layout = dynamic_cast(specific_metrics_widget->layout()); + specific_metrics_layout->removeWidget(specific_metric_table_); + specific_metric_table_ = metric.getTable(); + QSizePolicy sizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + sizePolicy.setHeightForWidth(specific_metric_table_->sizePolicy().hasHeightForWidth()); + specific_metric_table_->setSizePolicy(sizePolicy); + specific_metrics_layout->insertWidget(1, specific_metric_table_); +} + +void MetricsVisualizePanel::onSpecificMetricChanged() +{ + const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString(); + updateSelectedMetric(selected_metrics_str); + updateViews(); +} + +void MetricsVisualizePanel::onClearButtonClicked() +{ + if (!selected_metrics_) { + return; + } + updateSelectedMetric(selected_metrics_->first); + updateViews(); +} + +void MetricsVisualizePanel::onTimer() +{ + std::lock_guard message_lock(mutex_); + + for (auto & [name, metric] : metrics_) { + metric.updateGraph(); + metric.updateTable(); + } + + if (selected_metrics_) { + selected_metrics_->second.updateGraph(); + selected_metrics_->second.updateTable(); + } +} + +void MetricsVisualizePanel::onMetrics( + const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name) +{ + std::lock_guard message_lock(mutex_); + + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + constexpr size_t GRAPH_COL_SIZE = 5; + + for (const auto & status : msg->status) { + const size_t num_current_metrics = topic_widgets_map_[topic_name].size(); + if (metrics_.count(status.name) == 0) { + const auto metric = Metric(status); + metrics_.emplace(status.name, metric); + + // Calculate grid position + const size_t row = num_current_metrics / GRAPH_COL_SIZE * 2 + + 2; // start from 2 to leave space for the topic selector and tab widget + const size_t col = num_current_metrics % GRAPH_COL_SIZE; + + // Get the widgets from the metric + const auto tableWidget = metric.getTable(); + const auto chartViewWidget = metric.getChartView(); + + // Get the layout for the "All Metrics" tab + auto all_metrics_widget = dynamic_cast(tab_widget_->widget(0)); + QGridLayout * all_metrics_layout = dynamic_cast(all_metrics_widget->layout()); + + // Add the widgets to the "All Metrics" tab layout + all_metrics_layout->addWidget(tableWidget, row, col); + all_metrics_layout->setRowStretch(row, false); + all_metrics_layout->addWidget(chartViewWidget, row + 1, col); + all_metrics_layout->setRowStretch(row + 1, true); + all_metrics_layout->setColumnStretch(col, true); + + // Also add the widgets to the topic_widgets_map_ for easy management + topic_widgets_map_[topic_name][status.name] = std::make_pair(tableWidget, chartViewWidget); + } + metrics_.at(status.name).updateData(time, status); + + // update selected metrics + const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString(); + if (selected_metrics_str == status.name) { + if (selected_metrics_) { + selected_metrics_->second.updateData(time, status); + } + } + } + + // Update the specific metric selector + QSignalBlocker blocker(specific_metric_selector_); + for (const auto & status : msg->status) { + if (specific_metric_selector_->findText(QString::fromStdString(status.name)) == -1) { + specific_metric_selector_->addItem(QString::fromStdString(status.name)); + } + } + + // save the message for metrics selector + current_msg_map_[topic_name] = msg; +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel) diff --git a/planning/planning_debug_tools/README.md b/planning/planning_debug_tools/README.md index a89cec28..e7b86638 100644 --- a/planning/planning_debug_tools/README.md +++ b/planning/planning_debug_tools/README.md @@ -89,8 +89,8 @@ function PlotValue(name, path, timestamp, value) new_series = ScatterXY.new(name) index = 0 while(true) do - series_k = TimeseriesView.find( string.format( "%s/"..value..".%d", path, index) ) - series_s = TimeseriesView.find( string.format( "%s/arclength.%d", path, index) ) + series_k = TimeseriesView.find( string.format( "%s/"..value.."[%d]", path, index) ) + series_s = TimeseriesView.find( string.format( "%s/arclength[%d]", path, index) ) series_size = TimeseriesView.find( string.format( "%s/size", path) ) if series_k == nil or series_s == nil then break end diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index d460cbf0..72ce45cf 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -21,11 +21,11 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float64_multi_array_stamped.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -35,11 +35,11 @@ namespace planning_debug_tools { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; using nav_msgs::msg::Odometry; using planning_debug_tools::msg::TrajectoryDebugInfo; +using tier4_planning_msgs::msg::PathWithLaneId; template class TrajectoryAnalyzer diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index 1f0b9a93..99230de9 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -19,21 +19,21 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include namespace planning_debug_tools { -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getRPY; +using tier4_planning_msgs::msg::PathPointWithLaneId; double getVelocity(const PathPoint & p) { diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index 14db1277..010e9325 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -17,7 +17,7 @@ rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs geometry_msgs motion_utils nav_msgs diff --git a/planning/planning_debug_tools/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py index 4123766a..a8a553a3 100755 --- a/planning/planning_debug_tools/scripts/closest_velocity_checker.py +++ b/planning/planning_debug_tools/scripts/closest_velocity_checker.py @@ -16,12 +16,11 @@ import time -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_planning_msgs.msg import Path -from autoware_auto_planning_msgs.msg import PathWithLaneId -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_vehicle_msgs.msg import Engage -from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_control_msgs.msg import Control as AckermannControlCommand +from autoware_planning_msgs.msg import Path +from autoware_planning_msgs.msg import Trajectory +from autoware_vehicle_msgs.msg import Engage +from autoware_vehicle_msgs.msg import VelocityReport from geometry_msgs.msg import Pose from nav_msgs.msg import Odometry import numpy as np @@ -32,6 +31,7 @@ from tf2_ros.transform_listener import TransformListener from tier4_debug_msgs.msg import Float32MultiArrayStamped from tier4_debug_msgs.msg import Float32Stamped +from tier4_planning_msgs.msg import PathWithLaneId from tier4_planning_msgs.msg import VelocityLimit REF_LINK = "map" diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index 498f7e45..5f9e9575 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -20,6 +20,7 @@ import sys from PyQt5.QtWidgets import QApplication +from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy @@ -46,6 +47,7 @@ def __init__(self, args): for button in self.widget.rate_button: button.clicked.connect(functools.partial(self.onSetRate, button)) self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose) + self.widget.pub_goal_pose_button.clicked.connect(self.publish_goal) # start timer callback self.delta_time = 0.1 @@ -172,6 +174,17 @@ def publish_recorded_ego_pose(self): self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose) print("Published recorded ego pose as /initialpose") + def publish_goal(self): + if not self.rosbag_ego_odom_data: + return + + goal_pose = PoseStamped() + goal_pose.header.stamp = self.get_clock().now().to_msg() + goal_pose.header.frame_id = "map" + goal_pose.pose = self.rosbag_ego_odom_data[-1][1].pose.pose + self.goal_pose_publisher.publish(goal_pose) + print("Published last recorded ego pose as /planning/mission_planning/goal") + if __name__ == "__main__": parser = argparse.ArgumentParser() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 93bb9313..2cf54a4f 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -19,11 +19,11 @@ from subprocess import check_output import time -from autoware_auto_perception_msgs.msg import DetectedObjects -from autoware_auto_perception_msgs.msg import PredictedObjects -from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray -from autoware_perception_msgs.msg import TrafficSignalArray +from autoware_perception_msgs.msg import DetectedObjects +from autoware_perception_msgs.msg import PredictedObjects +from autoware_perception_msgs.msg import TrackedObjects +from autoware_perception_msgs.msg import TrafficLightGroupArray +from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil @@ -44,7 +44,6 @@ def __init__(self, args, name): self.rosbag_objects_data = [] self.rosbag_ego_odom_data = [] self.rosbag_traffic_signals_data = [] - self.is_auto_traffic_signals = False # subscriber self.sub_odom = self.create_subscription( @@ -76,6 +75,10 @@ def __init__(self, args, name): Odometry, "/perception_reproducer/rosbag_ego_odom", 1 ) + self.goal_pose_publisher = self.create_publisher( + PoseStamped, "/planning/mission_planning/goal", 1 + ) + # load rosbag print("Stared loading rosbag") if os.path.isdir(args.bag): @@ -86,15 +89,9 @@ def __init__(self, args, name): self.load_rosbag(args.bag) print("Ended loading rosbag") - # temporary support old auto msgs - if self.is_auto_traffic_signals: - self.auto_traffic_signals_pub = self.create_publisher( - AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) - else: - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) + self.traffic_signals_pub = self.create_publisher( + TrafficLightGroupArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) # wait for ready to publish/subscribe time.sleep(1.0) @@ -131,9 +128,6 @@ def load_rosbag(self, rosbag2_path: str): self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) - self.is_auto_traffic_signals = ( - "autoware_auto_perception_msgs" in type(msg).__module__ - ) def kill_online_perception_node(self): # kill node if required @@ -153,6 +147,9 @@ def kill_online_perception_node(self): pass def binary_search(self, data, timestamp): + if not data: + return None + if data[-1][0] < timestamp: return data[-1][1] elif data[0][0] > timestamp: diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py index a1d87a8b..c9ec36ca 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -91,13 +91,15 @@ def setupUI(self): self.grid_layout.addWidget(self.button, 1, 0, 1, -1) self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose") self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1) + self.pub_goal_pose_button = QPushButton("publish last recorded ego pose as goal pose") + self.grid_layout.addWidget(self.pub_goal_pose_button, 3, 0, 1, -1) # slider self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) self.slider.setMinimum(0) self.slider.setMaximum(self.max_value) self.slider.setValue(0) - self.grid_layout.addWidget(self.slider, 3, 0, 1, -1) + self.grid_layout.addWidget(self.slider, 4, 0, 1, -1) self.setCentralWidget(self.central_widget) diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 10bd41c5..dfa16d49 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -16,13 +16,11 @@ import argparse -from autoware_auto_planning_msgs.msg import Path -from autoware_auto_planning_msgs.msg import PathPoint -from autoware_auto_planning_msgs.msg import PathPointWithLaneId -from autoware_auto_planning_msgs.msg import PathWithLaneId -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_planning_msgs.msg import TrajectoryPoint -from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_planning_msgs.msg import Path +from autoware_planning_msgs.msg import PathPoint +from autoware_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_vehicle_msgs.msg import VelocityReport from geometry_msgs.msg import Pose from matplotlib import animation import matplotlib.pyplot as plt @@ -33,6 +31,8 @@ from rclpy.node import Node from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +from tier4_planning_msgs.msg import PathPointWithLaneId +from tier4_planning_msgs.msg import PathWithLaneId from tier4_planning_msgs.msg import VelocityLimit parser = argparse.ArgumentParser() diff --git a/simulator/simulator_compatibility_test/README.md b/simulator/simulator_compatibility_test/README.md index 5dc9d233..1fcd0ca2 100644 --- a/simulator/simulator_compatibility_test/README.md +++ b/simulator/simulator_compatibility_test/README.md @@ -145,24 +145,24 @@ Detailed process ### Input -| Name | Type | Description | -| ---------------------------------------- | ------------------------------------------------------- | ------------------ | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | for [Test Case #1] | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | for [Test Case #2] | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | for [Test Case #3] | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | for [Test Case #4] | -| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | for [Test Case #5] | -| `/vehicle/status/hazard_lights_status` | `autoware_auto_vehicle_msgs::msg::HazardLightsReport` | for [Test Case #6] | +| Name | Type | Description | +| ---------------------------------------- | -------------------------------------------------- | ------------------ | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | for [Test Case #1] | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | for [Test Case #2] | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | for [Test Case #3] | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | for [Test Case #4] | +| `/vehicle/status/turn_indicators_status` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | for [Test Case #5] | +| `/vehicle/status/hazard_lights_status` | `autoware_vehicle_msgs::msg::HazardLightsReport` | for [Test Case #6] | ### Output -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------- | ---------------------- | -| `/control/command/control_mode_cmd` | `autoware_auto_vehicle_msgs/ControlModeCommand` | for [Test Case #1] | -| `/control/command/gear_cmd` | `autoware_auto_vehicle_msgs/GearCommand` | for [Test Case #2] | -| `/control/command/control_cmd` | `autoware_auto_vehicle_msgs/AckermannControlCommand` | for [Test Case #3, #4] | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs/TurnIndicatorsCommand` | for [Test Case #5] | -| `/control/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs/HazardLightsCommand` | for [Test Case #6] | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------- | ---------------------- | +| `/control/command/control_cmd` | `autoware_control_msgs/Control` | for [Test Case #3, #4] | +| `/control/command/control_mode_cmd` | `autoware_vehicle_msgs/ControlModeCommand` | for [Test Case #1] | +| `/control/command/gear_cmd` | `autoware_vehicle_msgs/GearCommand` | for [Test Case #2] | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs/TurnIndicatorsCommand` | for [Test Case #5] | +| `/control/command/turn_indicators_cmd` | `autoware_vehicle_msgs/HazardLightsCommand` | for [Test Case #6] | ## Parameters diff --git a/simulator/simulator_compatibility_test/package.xml b/simulator/simulator_compatibility_test/package.xml index d0ed3e69..fe7f0cd6 100644 --- a/simulator/simulator_compatibility_test/package.xml +++ b/simulator/simulator_compatibility_test/package.xml @@ -9,14 +9,12 @@ Shumpei Wakabayashi TODO: License declaration - autoware_auto_control_msgs - autoware_auto_geometry_msgs - autoware_auto_mapping_msgs - autoware_auto_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs rclpy ament_copyright diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py index 9e5b0dfb..9c643fbb 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/ackermann_control_command.py @@ -1,6 +1,6 @@ -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_control_msgs.msg import AckermannLateralCommand -from autoware_auto_control_msgs.msg import LongitudinalCommand +from autoware_control_msgs.msg import Control as AckermannControlCommand +from autoware_control_msgs.msg import Lateral as LateralCommand +from autoware_control_msgs.msg import Longitudinal as LongitudinalCommand import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy @@ -9,7 +9,7 @@ from rclpy.qos import QoSReliabilityPolicy -class PublisherAckermannControlCommand(Node): +class ControlCommand(Node): def __init__(self): super().__init__("ackermann_control_command_publisher") @@ -28,7 +28,7 @@ def __init__(self): def publish_msg(self, control_cmd): stamp = self.get_clock().now().to_msg() msg = AckermannControlCommand() - lateral_cmd = AckermannLateralCommand() + lateral_cmd = LateralCommand() longitudinal_cmd = LongitudinalCommand() lateral_cmd.stamp.sec = stamp.sec lateral_cmd.stamp.nanosec = stamp.nanosec @@ -53,7 +53,7 @@ def publish_msg(self, control_cmd): def main(args=None): rclpy.init(args=args) - node = PublisherAckermannControlCommand() + node = ControlCommand() try: rclpy.spin(node) except KeyboardInterrupt: diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py index c39c7848..aaac318b 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/control_mode_command.py @@ -1,6 +1,6 @@ from enum import Enum -from autoware_auto_vehicle_msgs.msg import ControlModeCommand +from autoware_vehicle_msgs.msg import ControlModeCommand import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py index d17329df..dbb6cc17 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/publishers/gear_command.py @@ -1,6 +1,6 @@ from enum import Enum -from autoware_auto_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearCommand import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py index 965d2ac7..347f7326 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/control_mode_report.py @@ -1,6 +1,6 @@ from enum import Enum -from autoware_auto_vehicle_msgs.msg import ControlModeReport +from autoware_vehicle_msgs.msg import ControlModeReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py index f8c9ec30..ebbe470f 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/gear_report.py @@ -1,6 +1,6 @@ from enum import Enum -from autoware_auto_vehicle_msgs.msg import GearReport +from autoware_vehicle_msgs.msg import GearReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py index b9399137..10b30d3a 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/hazard_lights_report.py @@ -1,6 +1,6 @@ from enum import Enum -from autoware_auto_vehicle_msgs.msg import HazardLightsReport +from autoware_vehicle_msgs.msg import HazardLightsReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py index 64967239..493637fd 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/steering_report.py @@ -1,4 +1,4 @@ -from autoware_auto_vehicle_msgs.msg import SteeringReport +from autoware_vehicle_msgs.msg import SteeringReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py index 6d3727ea..599c918d 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/turn_indicators_report.py @@ -1,6 +1,6 @@ from enum import Enum -from autoware_auto_vehicle_msgs.msg import TurnIndicatorsReport +from autoware_vehicle_msgs.msg import TurnIndicatorsReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py index c83f1b09..0e632c87 100644 --- a/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py +++ b/simulator/simulator_compatibility_test/simulator_compatibility_test/subscribers/velocity_report.py @@ -1,4 +1,4 @@ -from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_vehicle_msgs.msg import VelocityReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/test_base/test_01_control_mode_and_report.py b/simulator/simulator_compatibility_test/test_base/test_01_control_mode_and_report.py deleted file mode 100644 index d1aad9c9..00000000 --- a/simulator/simulator_compatibility_test/test_base/test_01_control_mode_and_report.py +++ /dev/null @@ -1,94 +0,0 @@ -from enum import Enum -import time - -from autoware_auto_vehicle_msgs.msg import ControlModeCommand -import pytest -import rclpy -from rclpy.executors import MultiThreadedExecutor -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy -from simulator_compatibility_test.subscribers.control_mode_report import ControlModeReport_Constants -from simulator_compatibility_test.subscribers.control_mode_report import SubscriberControlModeReport - - -class ControlModeCommand_Constants(Enum): - NO_COMMAND = 0 - AUTONOMOUS = 1 - MANUAL = 2 - - -class Test01ControlModeAndReportBase: - msgs_rx = [] - node = None - sub = None - pub = None - sub_control_mode_report = None - executor = None - - @classmethod - def setup_class(cls) -> None: - QOS_RKL10TL = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=10, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - ) - rclpy.init() - cls.msgs_rx = [] - cls.node = rclpy.create_node("test_01_control_mode_and_report_base") - cls.sub = cls.node.create_subscription( - ControlModeCommand, - "/control/command/control_mode_cmd", - lambda msg: cls.msgs_rx.append(msg), - 10, - ) - cls.pub = cls.node.create_publisher( - ControlModeCommand, "/control/command/control_mode_cmd", QOS_RKL10TL - ) - cls.sub_control_mode_report = SubscriberControlModeReport() - cls.executor = MultiThreadedExecutor() - cls.executor.add_node(cls.sub_control_mode_report) - cls.executor.add_node(cls.node) - - @classmethod - def teardown_class(cls) -> None: - cls.node.destroy_node() - cls.executor.shutdown() - rclpy.shutdown() - - @pytest.fixture - def setup_method(self): - self.msgs_rx.clear() - - def set_control_mode(self, control_mode): - while rclpy.ok(): - self.executor.spin_once(timeout_sec=1) - self.pub.publish(self.generate_control_mode_cmd_msg(control_mode)) - if len(self.msgs_rx) > 2: - break - received = self.msgs_rx[-1] - assert received.mode == control_mode.value - self.msgs_rx.clear() - - def generate_control_mode_cmd_msg(self, control_mode): - stamp = self.node.get_clock().now().to_msg() - msg = ControlModeCommand() - msg.stamp.sec = stamp.sec - msg.stamp.nanosec = stamp.nanosec - msg.mode = control_mode.value - return msg - - def get_control_mode_report(self): - time.sleep(1) - self.sub_control_mode_report.received.clear() - received = ControlModeReport_Constants.NO_COMMAND - try: - while rclpy.ok(): - self.executor.spin_once(timeout_sec=1) - if len(self.sub_control_mode_report.received) > 2: - break - received = self.sub_control_mode_report.received.pop() - finally: - return received diff --git a/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py b/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py index d86a569c..75207bb5 100644 --- a/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py +++ b/simulator/simulator_compatibility_test/test_base/test_02_change_gear_and_report.py @@ -1,6 +1,6 @@ import time -from autoware_auto_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearCommand import rclpy from rclpy.executors import MultiThreadedExecutor from rclpy.qos import QoSDurabilityPolicy diff --git a/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py b/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py index d55fa7eb..176ef5a8 100644 --- a/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py +++ b/simulator/simulator_compatibility_test/test_base/test_03_longitudinal_command_and_report.py @@ -1,8 +1,8 @@ import time -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_control_msgs.msg import AckermannLateralCommand -from autoware_auto_control_msgs.msg import LongitudinalCommand +from autoware_control_msgs.msg import Control as ControlCommand +from autoware_control_msgs.msg import Lateral as LateralCommand +from autoware_control_msgs.msg import Longitudinal as LongitudinalCommand import pytest import rclpy from rclpy.executors import MultiThreadedExecutor @@ -39,13 +39,13 @@ def setup_class(cls) -> None: cls.node = rclpy.create_node("test_03_longitudinal_command_and_report_base") cls.sub = cls.node.create_subscription( - AckermannControlCommand, + ControlCommand, "/control/command/control_cmd", lambda msg: cls.msgs_rx.append(msg), 10, ) cls.pub = cls.node.create_publisher( - AckermannControlCommand, "/control/command/control_cmd", QOS_RKL10TL + ControlCommand, "/control/command/control_cmd", QOS_RKL10TL ) cls.sub_velocity_report = SubscriberVelocityReport() cls.executor = MultiThreadedExecutor() @@ -71,8 +71,8 @@ def init_vehicle(self): def generate_control_msg(self, control_cmd): stamp = self.node.get_clock().now().to_msg() - msg = AckermannControlCommand() - lateral_cmd = AckermannLateralCommand() + msg = ControlCommand() + lateral_cmd = LateralCommand() longitudinal_cmd = LongitudinalCommand() lateral_cmd.stamp.sec = stamp.sec lateral_cmd.stamp.nanosec = stamp.nanosec diff --git a/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py b/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py index 0648ffd2..5959bdef 100644 --- a/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py +++ b/simulator/simulator_compatibility_test/test_base/test_04_lateral_command_and_report.py @@ -1,8 +1,8 @@ import time -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_control_msgs.msg import AckermannLateralCommand -from autoware_auto_control_msgs.msg import LongitudinalCommand +from autoware_control_msgs.msg import Control as ControlCommand +from autoware_control_msgs.msg import Lateral as LateralCommand +from autoware_control_msgs.msg import Longitudinal as LongitudinalCommand import pytest import rclpy from rclpy.executors import MultiThreadedExecutor @@ -38,13 +38,13 @@ def setup_class(cls) -> None: } cls.node = rclpy.create_node("test_04_lateral_command_and_report_base") cls.sub = cls.node.create_subscription( - AckermannControlCommand, + ControlCommand, "/control/command/control_cmd", lambda msg: cls.msgs_rx.append(msg), 10, ) cls.pub = cls.node.create_publisher( - AckermannControlCommand, "/control/command/control_cmd", QOS_RKL10TL + ControlCommand, "/control/command/control_cmd", QOS_RKL10TL ) cls.sub_steering_report = SubscriberSteeringReport() cls.executor = MultiThreadedExecutor() @@ -70,8 +70,8 @@ def init_vehicle(self): def generate_control_msg(self, control_cmd): stamp = self.node.get_clock().now().to_msg() - msg = AckermannControlCommand() - lateral_cmd = AckermannLateralCommand() + msg = ControlCommand() + lateral_cmd = LateralCommand() longitudinal_cmd = LongitudinalCommand() lateral_cmd.stamp.sec = stamp.sec lateral_cmd.stamp.nanosec = stamp.nanosec diff --git a/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py b/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py index 974f61e1..c26f7d2f 100644 --- a/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py +++ b/simulator/simulator_compatibility_test/test_base/test_05_turn_indicators_cmd_and_report.py @@ -1,7 +1,7 @@ from enum import Enum import time -from autoware_auto_vehicle_msgs.msg import TurnIndicatorsCommand +from autoware_vehicle_msgs.msg import TurnIndicatorsCommand import pytest import rclpy from rclpy.executors import MultiThreadedExecutor diff --git a/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py b/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py index 7e361d54..950eabcb 100644 --- a/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py +++ b/simulator/simulator_compatibility_test/test_base/test_06_hazard_lights_cmd_and_report.py @@ -1,7 +1,7 @@ from enum import Enum import time -from autoware_auto_vehicle_msgs.msg import HazardLightsCommand +from autoware_vehicle_msgs.msg import HazardLightsCommand import pytest import rclpy from rclpy.executors import MultiThreadedExecutor diff --git a/simulator/simulator_compatibility_test/test_morai_sim/test_01_control_mode_and_report.py b/simulator/simulator_compatibility_test/test_morai_sim/test_01_control_mode_and_report.py deleted file mode 100644 index d664f1f3..00000000 --- a/simulator/simulator_compatibility_test/test_morai_sim/test_01_control_mode_and_report.py +++ /dev/null @@ -1,19 +0,0 @@ -import pytest -from simulator_compatibility_test.publishers.control_mode_command import ( - ControlModeCommand_Constants, -) -from simulator_compatibility_test.subscribers.control_mode_report import ControlModeReport_Constants -from test_base.test_01_control_mode_and_report import Test01ControlModeAndReportBase - - -class Test01ControlModeAndReportMorai(Test01ControlModeAndReportBase): - @pytest.mark.skip(reason="manual mode is not required for now") - def test_1_manual_mode(self, setup_method): - self.set_control_mode(ControlModeCommand_Constants.MANUAL) - result = self.get_control_mode_report() - assert result == ControlModeReport_Constants.MANUAL.value - - def test_2_auto_mode(self, setup_method): - self.set_control_mode(ControlModeCommand_Constants.AUTONOMOUS) - result = self.get_control_mode_report() - assert result == ControlModeReport_Constants.AUTONOMOUS.value diff --git a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_01_control_mode_and_report.py b/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_01_control_mode_and_report.py deleted file mode 100644 index 35e33149..00000000 --- a/simulator/simulator_compatibility_test/test_sim_common_manual_testing/test_01_control_mode_and_report.py +++ /dev/null @@ -1,19 +0,0 @@ -import pytest -from simulator_compatibility_test.publishers.control_mode_command import ( - ControlModeCommand_Constants, -) -from simulator_compatibility_test.subscribers.control_mode_report import ControlModeReport_Constants -from test_base.test_01_control_mode_and_report import Test01ControlModeAndReportBase - - -class Test01ControlModeAndReportSim(Test01ControlModeAndReportBase): - @pytest.mark.skip(reason="manual mode is not required for now") - def test_1_manual_mode(self, setup_method): - self.set_control_mode(ControlModeCommand_Constants.MANUAL) - result = self.get_control_mode_report() - assert result == ControlModeReport_Constants.MANUAL.value - - def test_2_auto_mode(self, setup_method): - self.set_control_mode(ControlModeCommand_Constants.AUTONOMOUS) - result = self.get_control_mode_report() - assert result == ControlModeReport_Constants.AUTONOMOUS.value diff --git a/system/rqt_diagnostic_graph_monitor/CMakeLists.txt b/system/rqt_diagnostic_graph_monitor/CMakeLists.txt new file mode 100644 index 00000000..9e8bb20b --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.14) +project(rqt_diagnostic_graph_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR python) +install(FILES plugin.xml DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS script/rqt_diagnostic_graph_monitor DESTINATION lib/${PROJECT_NAME}) +ament_auto_package(INSTALL_TO_SHARE script) diff --git a/system/rqt_diagnostic_graph_monitor/README.md b/system/rqt_diagnostic_graph_monitor/README.md new file mode 100644 index 00000000..8dccca34 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/README.md @@ -0,0 +1 @@ +# System diagnostic monitor diff --git a/system/rqt_diagnostic_graph_monitor/package.xml b/system/rqt_diagnostic_graph_monitor/package.xml new file mode 100644 index 00000000..60780e27 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/package.xml @@ -0,0 +1,24 @@ + + + + rqt_diagnostic_graph_monitor + 0.1.0 + The rqt_diagnostic_graph_monitor package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + python_qt_binding + rqt_gui + rqt_gui_py + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/system/rqt_diagnostic_graph_monitor/plugin.xml b/system/rqt_diagnostic_graph_monitor/plugin.xml new file mode 100644 index 00000000..6c64185e --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/plugin.xml @@ -0,0 +1,16 @@ + + + + + + + + folder + + + + utilities-system-monitor + + + + diff --git a/system/rqt_diagnostic_graph_monitor/python/__init__.py b/system/rqt_diagnostic_graph_monitor/python/__init__.py new file mode 100644 index 00000000..10f125fa --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/__init__.py @@ -0,0 +1,37 @@ +# Copyright 2023 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. + +from rqt_gui_py.plugin import Plugin + +from .module import MonitorModule +from .widget import MonitorWidget + + +class MonitorPlugin(Plugin): + def __init__(self, context): + super().__init__(context) + self.widget = MonitorWidget() + self.module = MonitorModule(context.node) + self.module.append_struct_callback(self.widget.on_graph) + context.add_widget(self.widget) + + def shutdown_plugin(self): + self.module.shutdown() + self.widget.shutdown() + + def save_settings(self, plugin_settings, instance_settings): + pass + + def restore_settings(self, plugin_settings, instance_settings): + pass diff --git a/system/rqt_diagnostic_graph_monitor/python/graph.py b/system/rqt_diagnostic_graph_monitor/python/graph.py new file mode 100644 index 00000000..cea81c12 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/graph.py @@ -0,0 +1,127 @@ +# Copyright 2023 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. + +from diagnostic_msgs.msg import DiagnosticStatus +from rclpy.time import Time + + +class DummyStatus: + def __init__(self): + self.level = DiagnosticStatus.STALE + + +class BaseUnit: + def __init__(self, status=DummyStatus()): + self._parents = [] + self._children = [] + self._path = None + self._type = None + self._status = status + + @property + def parents(self): + return self._parents + + @property + def children(self): + return self._children + + @property + def path(self): + return self._path + + @property + def kind(self): + return self._type + + +class NodeUnit(BaseUnit): + def __init__(self, struct): + super().__init__() + self._path = struct.path + self._type = struct.type + + def update(self, status): + self._status = status + + @property + def level(self): + return self._status.level + + +class DiagUnit(BaseUnit): + def __init__(self, struct): + super().__init__() + self._path = struct.path + self._name = struct.name + self._type = "diag" + + def update(self, status): + self._status = status + + @property + def level(self): + return self._status.level + + +class UnitLink: + def __init__(self, parent: BaseUnit, child: BaseUnit): + self._parent = parent + self._child = child + parent._children.append(self) + child._parents.append(self) + + def update(self, status): + self.status = status + + @property + def parent(self): + return self._parent + + @property + def child(self): + return self._child + + +class Graph: + def __init__(self, msg): + self._struct_stamp = Time.from_msg(msg.stamp) + self._status_stamp = None + self._id = msg.id + self._nodes = [NodeUnit(struct) for struct in msg.nodes] + self._diags = [DiagUnit(struct) for struct in msg.diags] + self._units = self._nodes + self._diags + self._links = [] + for struct in msg.links: + units = self._diags if struct.is_leaf else self._nodes + nodes = self._nodes + self._links.append(UnitLink(nodes[struct.parent], units[struct.child])) + + def update(self, msg): + if msg.id == self._id: + self._status_stamp = Time.from_msg(msg.stamp) + for node, status in zip(self._nodes, msg.nodes): + node.update(status) + for diag, status in zip(self._diags, msg.diags): + diag.update(status) + for link, status in zip(self._links, msg.links): + link.update(status) + + @property + def units(self): + return self._units + + @property + def links(self): + return self._links diff --git a/system/rqt_diagnostic_graph_monitor/python/items.py b/system/rqt_diagnostic_graph_monitor/python/items.py new file mode 100644 index 00000000..96f60ef0 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/items.py @@ -0,0 +1,54 @@ +# 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. + + +from diagnostic_msgs.msg import DiagnosticStatus +from python_qt_binding import QtGui +from python_qt_binding import QtWidgets + +from .graph import BaseUnit +from .graph import UnitLink + + +class MonitorIcons: + def __init__(self): + self.disable = QtGui.QIcon.fromTheme("dialog-question") + self.unknown = QtGui.QIcon.fromTheme("system-search") + self.ok = QtGui.QIcon.fromTheme("emblem-default") + self.warn = QtGui.QIcon.fromTheme("emblem-important") + self.error = QtGui.QIcon.fromTheme("dialog-error") + self.stale = QtGui.QIcon.fromTheme("appointment-missed") + + self._levels = {} + self._levels[DiagnosticStatus.OK] = self.ok + self._levels[DiagnosticStatus.WARN] = self.warn + self._levels[DiagnosticStatus.ERROR] = self.error + self._levels[DiagnosticStatus.STALE] = self.stale + + def get(self, level): + return self._levels.get(level, self.unknown) + + +class MonitorItem: + icons = MonitorIcons() + + def __init__(self, link: UnitLink, unit: BaseUnit): + item_text = f"{unit.path} ({unit.kind})" if unit.path else f"({unit.kind})" + self.item = QtWidgets.QTreeWidgetItem([item_text]) + self.link = link + self.unit = unit + self.item.setIcon(0, self.icons.stale) + + def update(self): + self.item.setIcon(0, self.icons.get(self.unit.level)) diff --git a/system/rqt_diagnostic_graph_monitor/python/module.py b/system/rqt_diagnostic_graph_monitor/python/module.py new file mode 100644 index 00000000..74294659 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/module.py @@ -0,0 +1,61 @@ +# Copyright 2023 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. + + +from rclpy.node import Node +from tier4_system_msgs.msg import DiagGraphStatus +from tier4_system_msgs.msg import DiagGraphStruct + +from .graph import Graph +from .utils import default_qos +from .utils import durable_qos +from .utils import foreach + + +class MonitorModule: + def __init__(self, node: Node): + self.graph = None + self.struct_callbacks = [] + self.status_callbacks = [] + self.node = node + self.sub_struct = self.subscribe_struct() + self.sub_status = self.subscribe_status() + + def append_struct_callback(self, callback): + self.struct_callbacks.append(callback) + + def append_status_callback(self, callback): + self.status_callbacks.append(callback) + + def on_struct(self, msg): + self.graph = Graph(msg) + foreach(self.struct_callbacks, lambda callback: callback(self.graph)) + + def on_status(self, msg): + self.graph.update(msg) + foreach(self.status_callbacks, lambda callback: callback(self.graph)) + + def subscribe_struct(self): + return self.node.create_subscription( + DiagGraphStruct, "/diagnostics_graph/struct", self.on_struct, durable_qos(1) + ) + + def subscribe_status(self): + return self.node.create_subscription( + DiagGraphStatus, "/diagnostics_graph/status", self.on_status, default_qos(1) + ) + + def shutdown(self): + self.node.destroy_subscription(self.sub_struct) + self.node.destroy_subscription(self.sub_status) diff --git a/system/rqt_diagnostic_graph_monitor/python/utils.py b/system/rqt_diagnostic_graph_monitor/python/utils.py new file mode 100644 index 00000000..6e66102b --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/utils.py @@ -0,0 +1,29 @@ +# 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. + +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile + + +def default_qos(depth): + return QoSProfile(depth=depth) + + +def durable_qos(depth): + return QoSProfile(depth=depth, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + + +def foreach(iterable, function): + for item in iterable: + function(item) diff --git a/system/rqt_diagnostic_graph_monitor/python/widget.py b/system/rqt_diagnostic_graph_monitor/python/widget.py new file mode 100644 index 00000000..e7f022e5 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/python/widget.py @@ -0,0 +1,85 @@ +# Copyright 2023 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. + +from python_qt_binding import QtCore +from python_qt_binding import QtWidgets + +from .graph import Graph +from .items import MonitorItem +from .utils import foreach + + +class MonitorWidget(QtWidgets.QSplitter): + def __init__(self): + super().__init__() + self.graph = None + self.items = [] + self.root_widget = QtWidgets.QTreeWidget() + self.tree_widget = QtWidgets.QTreeWidget() + self.root_widget.setHeaderLabels(["Top-level"]) + self.tree_widget.setHeaderLabels(["Subtrees"]) + self.addWidget(self.root_widget) + self.addWidget(self.tree_widget) + + self._timer = QtCore.QTimer() + self._timer.timeout.connect(self.on_timer) + self._timer.start(500) + + def shutdown(self): + self.clear_graph() + + def on_timer(self): + foreach(self.items, lambda item: item.update()) + + def on_graph(self, graph: Graph): + self.clear_graph() + self.build_graph(graph) + + def clear_graph(self): + self.graph = None + self.items = [] + self.root_widget.clear() + self.tree_widget.clear() + + def build_graph(self, graph: Graph): + self.graph = graph + root_units = filter(self.is_root_unit, self.graph.units) + tree_units = filter(self.is_tree_unit, self.graph.units) + root_items = [MonitorItem(None, unit) for unit in root_units] + tree_items = [MonitorItem(None, unit) for unit in tree_units] + link_items = [MonitorItem(link, link.child) for link in self.graph.links] + self.items = root_items + tree_items + link_items + + # Note: overwrite link items with root/tree items if there is more than one. + parents = {} + for items in [link_items, tree_items, root_items]: + parents.update({item.unit: item.item for item in items}) + + # Connect tree widget items. + root_widget_item = self.root_widget.invisibleRootItem() + tree_widget_item = self.tree_widget.invisibleRootItem() + for item in root_items: + root_widget_item.addChild(item.item) + for item in tree_items: + tree_widget_item.addChild(item.item) + for item in link_items: + parents[item.link.parent].addChild(item.item) + + @staticmethod + def is_root_unit(unit): + return len(unit.parents) == 0 + + @staticmethod + def is_tree_unit(unit): + return len(unit.parents) >= 2 and len(unit.children) != 0 diff --git a/system/rqt_diagnostic_graph_monitor/script/cui_diagnostic_graph_monitor b/system/rqt_diagnostic_graph_monitor/script/cui_diagnostic_graph_monitor new file mode 100755 index 00000000..8b068512 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/script/cui_diagnostic_graph_monitor @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 + +import rclpy +from rqt_diagnostic_graph_monitor.module import MonitorModule + +if __name__ == "__main__": + try: + rclpy.init() + node = rclpy.create_node("test_rqt_diagnostic_graph_monitor") + test = MonitorModule(node) + rclpy.spin(node) + rclpy.shutdown() + except KeyboardInterrupt: + print("KeyboardInterrupt") diff --git a/system/rqt_diagnostic_graph_monitor/script/rqt_diagnostic_graph_monitor b/system/rqt_diagnostic_graph_monitor/script/rqt_diagnostic_graph_monitor new file mode 100755 index 00000000..a5237a87 --- /dev/null +++ b/system/rqt_diagnostic_graph_monitor/script/rqt_diagnostic_graph_monitor @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +import rqt_gui.main + +rqt_main = rqt_gui.main.Main() +sys.exit(rqt_main.main(sys.argv, standalone="rqt_diagnostic_graph_monitor.MonitorPlugin")) diff --git a/vehicle/accel_brake_map_calibrator/CMakeLists.txt b/vehicle/accel_brake_map_calibrator/CMakeLists.txt deleted file mode 100644 index f8aed0ab..00000000 --- a/vehicle/accel_brake_map_calibrator/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(accel_brake_map_calibrator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_executable(accel_brake_map_calibrator - src/accel_brake_map_calibrator_node.cpp - src/main.cpp -) -ament_target_dependencies(accel_brake_map_calibrator) - -install( - PROGRAMS - scripts/__init__.py - scripts/actuation_cmd_publisher.py - scripts/accel_tester.py - scripts/brake_tester.py - scripts/config.py - scripts/delay_estimator.py - scripts/plotter.py - scripts/view_statistics.py - scripts/calc_utils.py - scripts/csv_reader.py - scripts/log_analyzer.py - scripts/view_plot.py - scripts/new_accel_brake_map_server.py - test/plot_csv_client_node.py - test/sim_actuation_status_publisher.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config - rviz) diff --git a/vehicle/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/README.md deleted file mode 100644 index c8813280..00000000 --- a/vehicle/accel_brake_map_calibrator/README.md +++ /dev/null @@ -1,260 +0,0 @@ -# accel_brake_map_calibrator - -The role of this node is to automatically calibrate `accel_map.csv` / `brake_map.csv` used in the `raw_vehicle_cmd_converter` node. - -The base map, which is lexus's one by default, is updated iteratively with the loaded driving data. - -## How to calibrate - -### Launch Calibrator - -After launching Autoware, run the `accel_brake_map_calibrator` by the following command and then perform autonomous driving. Note: You can collect data with manual driving if it is possible to use the same vehicle interface as during autonomous driving (e.g. using a joystick). - -```sh -ros2 launch accel_brake_map_calibrator accel_brake_map_calibrator.launch.xml rviz:=true -``` - -Or if you want to use rosbag files, run the following commands. - -```sh -ros2 launch accel_brake_map_calibrator accel_brake_map_calibrator.launch.xml rviz:=true use_sim_time:=true -ros2 bag play --clock -``` - -During the calibration with setting the parameter `progress_file_output` to true, the log file is output in [directory of *accel_brake_map_calibrator*]/config/ . You can also see accel and brake maps in [directory of *accel_brake_map_calibrator*]/config/accel_map.csv and [directory of *accel_brake_map_calibrator*]/config/brake_map.csv after calibration. - -### Calibration plugin - -The `rviz:=true` option displays the RViz with a calibration plugin as below. - -

- -

- -The current status (velocity and pedal) is shown in the plugin. The color on the current cell varies green/red depending on the current data is valid/invalid. The data that doesn't satisfy the following conditions are considered invalid and will not be used for estimation since aggressive data (e.g. when the pedal is moving fast) causes bad calibration accuracy. - -- The velocity and pedal conditions are within certain ranges from the index values. -- The steer value, pedal speed, pitch value, etc. are less than corresponding thresholds. -- The velocity is higher than a threshold. - -The detailed parameters are described in the parameter section. - -Note: You don't need to worry about whether the current state is red or green during calibration. Just keep getting data until all the cells turn red. - -The value of each cell in the map is gray at first, and it changes from blue to red as the number of valid data in the cell accumulates. It is preferable to continue the calibration until each cell of the map becomes close to red. In particular, the performance near the stop depends strongly on the velocity of 0 ~ 6m/s range and the pedal value of +0.2 ~ -0.4, range so it is desirable to focus on those areas. - -### Diagnostics - -The `accel brake map_calibrator` publishes diagnostics message depending on the calibration status. -Diagnostic type `WARN` indicates that the current accel/brake map is estimated to be inaccurate. In this situation, it is strongly recommended to perform a re-calibration of the accel/brake map. - -| Status | Diagnostics Type | Diagnostics message | Description | -| ----------------------- | ---------------- | ------------------------------------------ | --------------------------------------------------- | -| No calibration required | `OK` | "OK" | | -| Calibration Required | `WARN` | "Accel/brake map Calibration is required." | The accuracy of current accel/brake map may be low. | - -This diagnostics status can be also checked on the following ROS topic. - -```sh -ros2 topic echo /accel_brake_map_calibrator/output/update_suggest -``` - -When the diagnostics type is `WARN`, `True` is published on this topic and the update of the accel/brake map is suggested. - -### Evaluation of the accel / brake map accuracy - -The accuracy of map is evaluated by the **Root Mean Squared Error (RMSE)** between the observed acceleration and predicted acceleration. - -**TERMS:** - -- `Observed acceleration`: the current vehicle acceleration which is calculated as a derivative value of the wheel speed. - -- `Predicted acceleration`: the output of the original accel/brake map, which the Autoware is expecting. The value is calculated using the current pedal and velocity. - -You can check additional error information with the following topics. - -- `/accel_brake_map_calibrator/output/current_map_error` : The error of the original map set in the `csv_path_accel/brake_map` path. The original map is not accurate if this value is large. -- `/accel_brake_map_calibrator/output/updated_map_error` : The error of the map calibrated in this node. The calibration quality is low if this value is large. -- `/accel_brake_map_calibrator/output/map_error_ratio` : The error ratio between the original map and updated map (ratio = updated / current). If this value is less than 1, it is desirable to update the map. - -### How to visualize calibration data - -The process of calibration can be visualized as below. Since these scripts need the log output of the calibration, the `pedal_accel_graph_output` parameter must be set to true while the calibration is running for the visualization. - -#### Visualize plot of relation between acceleration and pedal - -The following command shows the plot of used data in the calibration. In each plot of velocity ranges, you can see the distribution of the relationship between pedal and acceleration, and raw data points with colors according to their pitch angles. - -```sh -ros2 run accel_brake_map_calibrator view_plot.py -``` - -![sample pic](media/log_sample.png) - -#### Visualize statistics about acceleration/velocity/pedal data - -The following command shows the statistics of the calibration: - -- mean value -- standard deviation -- number of data - -of all data in each map cell. - -```sh -ros2 run accel_brake_map_calibrator view_statistics.py -``` - -![sample pic2](media/statistics_sample.png) - -### How to save the calibrated accel / brake map anytime you want - -You can save accel and brake map anytime with the following command. - -```sh -ros2 service call /accel_brake_map_calibrator/update_map_dir tier4_vehicle_msgs/srv/UpdateAccelBrakeMap "path: ''" -``` - -You can also save accel and brake map in the default directory where Autoware reads accel_map.csv/brake_map.csv using the RViz plugin (AccelBrakeMapCalibratorButtonPanel) as following. - -1. Click _Panels_ tab, and select AccelBrakeMapCalibratorButtonPanel. - - ![add_panel](./media/add_panel.png) - -2. Select the panel, and the button will appear at the bottom of RViz. - - ![calibrator_button_panel](./media/calibrator_button_panel.png) - -3. Press the button, and the accel / brake map will be saved. - (The button cannot be pressed in certain situations, such as when the calibrator node is not running.) - - ![push_calibration_button](./media/push_calibration_button.png) - -## Parameters - -## System Parameters - -| Name | Type | Description | Default value | -| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------------------- | -| update_method | string | you can select map calibration method. "update_offset_each_cell" calculates offsets for each grid cells on the map. "update_offset_total" calculates the total offset of the map. | "update_offset_each_cell" | -| get_pitch_method | string | "tf": get pitch from tf, "none": unable to perform pitch validation and pitch compensation | "tf" | -| pedal_accel_graph_output | bool | if true, it will output a log of the pedal accel graph. | true | -| progress_file_output | bool | if true, it will output a log and csv file of the update process. | false | -| default_map_dir | str | directory of default map | [directory of *raw_vehicle_cmd_converter*]/data/default/ | -| calibrated_map_dir | str | directory of calibrated map | [directory of *accel_brake_map_calibrator*]/config/ | -| update_hz | double | hz for update | 10.0 | - -## Algorithm Parameters - -| Name | Type | Description | Default value | -| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| initial_covariance | double | Covariance of initial acceleration map (larger covariance makes the update speed faster) | 0.05 | -| velocity_min_threshold | double | Speeds smaller than this are not used for updating. | 0.1 | -| velocity_diff_threshold | double | When the velocity data is more than this threshold away from the grid reference speed (center value), the associated data is not used for updating. | 0.556 | -| max_steer_threshold | double | If the steer angle is greater than this value, the associated data is not used for updating. | 0.2 | -| max_pitch_threshold | double | If the pitch angle is greater than this value, the associated data is not used for updating. | 0.02 | -| max_jerk_threshold | double | If the ego jerk calculated from ego acceleration is greater than this value, the associated data is not used for updating. | 0.7 | -| pedal_velocity_thresh | double | If the pedal moving speed is greater than this value, the associated data is not used for updating. | 0.15 | -| pedal_diff_threshold | double | If the current pedal value is more then this threshold away from the previous value, the associated data is not used for updating. | 0.03 | -| max_accel | double | Maximum value of acceleration calculated from velocity source. | 5.0 | -| min_accel | double | Minimum value of acceleration calculated from velocity source. | -5.0 | -| pedal_to_accel_delay | double | The delay time between actuation_cmd to acceleration, considered in the update logic. | 0.3 | -| update_suggest_thresh | double | threshold of RMSE ratio that update suggest flag becomes true. ( RMSE ratio: [RMSE of new map] / [RMSE of original map] ) | 0.7 | -| max_data_count | int | For visualization. When the data num of each grid gets this value, the grid color gets red. | 100 | -| accel_brake_value_source | string | Whether to use actuation_status or actuation_command as accel/brake sources. value | status | - -## Test utility scripts - -### Constant accel/brake command test - -These scripts are useful to test for accel brake map calibration. These generate an `ActuationCmd` with a constant accel/brake value given interactively by a user through CLI. - -- accel_tester.py -- brake_tester.py -- actuation_cmd_publisher.py - -The `accel/brake_tester.py` receives a target accel/brake command from CLI. It sends a target value to `actuation_cmd_publisher.py` which generates the `ActuationCmd`. You can run these scripts by the following commands in the different terminals, and it will be as in the screenshot below. - -```bash -ros2 run accel_brake_map_calibrator accel_tester.py -ros2 run accel_brake_map_calibrator brake_tester.py -ros2 run accel_brake_map_calibrator actuation_cmd_publisher.py -``` - -![actuation_cmd_publisher_util](./media/actuation_cmd_publisher_util.png) - -## Calibration Method - -Two algorithms are selectable for the acceleration map update, [update_offset_four_cell_around](#update_offset_four_cell_around-1) and [update_offset_each_cell](#update_offset_each_cell). Please see the link for details. - -### Data Preprocessing - -Before calibration, missing or unusable data (e.g., too large handle angles) must first be eliminated. The following parameters are used to determine which data to remove. - -#### Parameters - -| Name | Description | Default Value | -| ---------------------- | ---------------------------- | ------------- | -| velocity_min_threshold | Exclude minimal velocity | 0.1 | -| max_steer_threshold | Exclude large steering angle | 0.2 | -| max_pitch_threshold | Exclude large pitch angle | 0.02 | -| max_jerk_threshold | Exclude large jerk | 0.7 | -| pedal_velocity_thresh | Exclude large pedaling speed | 0.15 | - -### update_offset_each_cell - -Update by Recursive Least Squares(RLS) method using data close enough to each grid. - -**Advantage** : Only data close enough to each grid is used for calibration, allowing accurate updates at each point. - -**Disadvantage** : Calibration is time-consuming due to a large amount of data to be excluded. - -#### Parameters - -Data selection is determined by the following thresholds. -| Name | Default Value | -| ----------------------- | ------------- | -| velocity_diff_threshold | 0.556 | -| pedal_diff_threshold | 0.03 | - -#### Update formula - -$$ -\begin{align} - \theta[n]=& - \theta[n-1]+\frac{p[n-1]x^{(n)}}{\lambda+p[n-1]{(x^{(n)})}^2}(y^{(n)}-\theta[n-1]x^{(n)})\\ - p[n]=&\frac{p[n-1]}{\lambda+p[n-1]{(x^{(n)})}^2} -\end{align} -$$ - -#### Variables - -| Variable name | Symbol | -| ------------------ | ----------- | -| covariance | $p[n-1]$ | -| map_offset | $\theta[n]$ | -| forgetting*factor* | $\lambda$ | -| phi | $x(=1)$ | -| measured_acc | $y$ | - -### update_offset_four_cell_around [1] - -Update the offsets by RLS in four grids around newly obtained data. By considering linear interpolation, the update takes into account appropriate weights. Therefore, there is no need to remove data by thresholding. - -**Advantage** : No data is wasted because updates are performed on the 4 grids around the data with appropriate weighting. -**Disadvantage** : Accuracy may be degraded due to extreme bias of the data. For example, if data $z(k)$ is biased near $Z_{RR}$ in Fig. 2, updating is performed at the four surrounding points ( $Z_{RR}$, $Z_{RL}$, $Z_{LR}$, and $Z_{LL}$), but accuracy at $Z_{LL}$ is not expected. - - -

- -

- -#### Implementation - -See eq.(7)-(10) in [1] for the updated formula. In addition, eq.(17),(18) from [1] are used for Anti-Windup. - -### References - - - -[1] [Gabrielle Lochrie, Michael Doljevic, Mario Nona, Yongsoon Yoon, Anti-Windup Recursive Least Squares Method for Adaptive Lookup Tables with Application to Automotive Powertrain Control Systems, IFAC-PapersOnLine, Volume 54, Issue 20, 2021, Pages 840-845](https://www.sciencedirect.com/science/article/pii/S240589632102320X) diff --git a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml deleted file mode 100644 index c7ee0a3a..00000000 --- a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - update_hz: 10.0 - initial_covariance: 0.05 - velocity_min_threshold: 0.1 - velocity_diff_threshold: 0.556 - pedal_diff_threshold: 0.03 - max_steer_threshold: 0.2 - max_pitch_threshold: 0.02 - max_jerk_threshold: 0.7 - pedal_velocity_thresh: 0.15 - pedal_to_accel_delay: 0.3 - max_accel: 5.0 - min_accel: -5.0 - max_data_count: 100 - precision: 3 - update_method: "update_offset_four_cell_around" # or "update_offset_each_cell", "update_offset_total", "update_offset_four_cell_around" - update_suggest_thresh: 0.7 # threshold of rmse rate that update suggest flag becomes true. ( rmse_rate: [rmse of update map] / [rmse of original map] ) - progress_file_output: false # flag to output log file - accel_brake_value_source: "status" # "status" or "command" diff --git a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml deleted file mode 100644 index 2f272494..00000000 --- a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml +++ /dev/null @@ -1,818 +0,0 @@ - - - - #ffffff - #000000 - false - false - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/0 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/3 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw accel - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/4 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - accel - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/1 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - accel pedal - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/2 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - brake pedal - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/12 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - success to update - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/13 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - jerk - - - - true - - 30 - main info - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - true - - 30 - Untitled Plot - - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/9 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw pitch - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/10 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - pitch - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/5 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw accel speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/6 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - accel speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/7 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw brake speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/8 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - brake speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/11 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - steer - - - - true - - 30 - sub info - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - true - - 30 - Untitled Plot - - - - false -
- diff --git a/vehicle/accel_brake_map_calibrator/data/accel_map.csv b/vehicle/accel_brake_map_calibrator/data/accel_map.csv deleted file mode 100644 index 32e639ca..00000000 --- a/vehicle/accel_brake_map_calibrator/data/accel_map.csv +++ /dev/null @@ -1,7 +0,0 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 -0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 -0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 -0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 -0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 diff --git a/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv b/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv deleted file mode 100644 index e5492886..00000000 --- a/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv +++ /dev/null @@ -1,12 +0,0 @@ -default,0,0.695,1.39,2.085,2.78,3.475,4.17,4.865,5.56,6.255,6.95,7.645,8.34,9.035,9.73,10.425,11.12,11.815,12.51,13.205,13.9 -0,0.3,0.125,-0.05,-0.175,-0.3,-0.345,-0.39,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5 -0.05,0.45,0.3175,0.185,0.0775,-0.03,-0.0675,-0.105,-0.1225,-0.14,-0.16,-0.18,-0.215,-0.25,-0.275,-0.3,-0.315,-0.33,-0.345,-0.36,-0.375,-0.39 -0.1,0.6,0.51,0.42,0.33,0.24,0.21,0.18,0.15,0.12,0.085,0.05,-0.015,-0.08,-0.12,-0.16,-0.18,-0.2,-0.22,-0.24,-0.26,-0.28 -0.15,0.875,0.7875,0.7,0.605,0.51,0.45,0.39,0.345,0.3,0.2475,0.195,0.1425,0.09,0.055,0.02,-0.015,-0.05,-0.0725,-0.095,-0.125,-0.155 -0.2,1.15,1.065,0.98,0.88,0.78,0.69,0.6,0.54,0.48,0.41,0.34,0.3,0.26,0.23,0.2,0.15,0.1,0.075,0.05,0.01,-0.03 -0.25,1.45,1.37,1.29,1.195,1.1,1.025,0.95,0.88,0.81,0.74,0.67,0.625,0.58,0.54,0.5,0.455,0.41,0.3775,0.345,0.31,0.275 -0.3,1.75,1.675,1.6,1.51,1.42,1.36,1.3,1.22,1.14,1.07,1,0.95,0.9,0.85,0.8,0.76,0.72,0.68,0.64,0.61,0.58 -0.35,2.2,2.12,2.04,1.95,1.86,1.7875,1.715,1.63,1.545,1.46,1.375,1.3075,1.24,1.1825,1.125,1.0725,1.02,0.97,0.92,0.88,0.84 -0.4,2.65,2.565,2.48,2.39,2.3,2.215,2.13,2.04,1.95,1.85,1.75,1.665,1.58,1.515,1.45,1.385,1.32,1.26,1.2,1.15,1.1 -0.45,2.975,2.92,2.865,2.7875,2.71,2.6175,2.525,2.42,2.315,2.1825,2.05,1.9625,1.875,1.795,1.715,1.6575,1.6,1.5325,1.465,1.41,1.355 -0.5,3.3,3.275,3.25,3.185,3.12,3.02,2.92,2.8,2.68,2.515,2.35,2.26,2.17,2.075,1.98,1.93,1.88,1.805,1.73,1.67,1.61 diff --git a/vehicle/accel_brake_map_calibrator/data/brake_map.csv b/vehicle/accel_brake_map_calibrator/data/brake_map.csv deleted file mode 100644 index adf2c809..00000000 --- a/vehicle/accel_brake_map_calibrator/data/brake_map.csv +++ /dev/null @@ -1,10 +0,0 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 -0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 -0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 -0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 -0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 -0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 -0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 -0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv b/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv deleted file mode 100644 index 4e9dfb16..00000000 --- a/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv +++ /dev/null @@ -1,18 +0,0 @@ -default,0,0.695,1.39,2.085,2.78,3.475,4.17,4.865,5.56,6.25,6.94,7.635,8.33,9.025,9.72,10.415,11.11,11.805,12.5,13.195,13.89 -0,0.3,0.125,-0.05,-0.175,-0.3,-0.345,-0.39,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5 -0.05,0.295,0.12,-0.055,-0.18,-0.305,-0.35,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.425,-0.435,-0.445,-0.455,-0.465,-0.475,-0.485,-0.495,-0.505 -0.1,0.29,0.115,-0.06,-0.185,-0.31,-0.355,-0.4,-0.405,-0.41,-0.415,-0.42,-0.425,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5,-0.51 -0.15,-0.045,-0.1375,-0.23,-0.3725,-0.515,-0.5575,-0.6,-0.6075,-0.615,-0.625,-0.635,-0.6425,-0.65,-0.66,-0.67,-0.68,-0.69,-0.7025,-0.715,-0.725,-0.735 -0.2,-0.38,-0.39,-0.4,-0.56,-0.72,-0.76,-0.8,-0.81,-0.82,-0.835,-0.85,-0.86,-0.87,-0.88,-0.89,-0.9,-0.91,-0.925,-0.94,-0.95,-0.96 -0.25,-0.69,-0.705,-0.72,-0.91,-1.1,-1.1375,-1.175,-1.185,-1.195,-1.2075,-1.22,-1.23,-1.24,-1.25,-1.26,-1.26525,-1.2705,-1.27825,-1.286,-1.29125,-1.2965 -0.3,-1,-1.02,-1.04,-1.26,-1.48,-1.515,-1.55,-1.56,-1.57,-1.58,-1.59,-1.6,-1.61,-1.62,-1.63,-1.6305,-1.631,-1.6315,-1.632,-1.6325,-1.633 -0.35,-1.24,-1.255,-1.27,-1.4675,-1.665,-1.7325,-1.8,-1.8175,-1.835,-1.84025,-1.8455,-1.85075,-1.856,-1.86125,-1.8665,-1.867,-1.8675,-1.868,-1.8685,-1.869,-1.8695 -0.4,-1.48,-1.49,-1.5,-1.675,-1.85,-1.95,-2.05,-2.075,-2.1,-2.1005,-2.101,-2.1015,-2.102,-2.1025,-2.103,-2.1035,-2.104,-2.1045,-2.105,-2.1055,-2.106 -0.45,-1.485,-1.495,-1.505,-1.68,-1.855,-1.955,-2.055,-2.08,-2.105,-2.1055,-2.106,-2.1065,-2.107,-2.1075,-2.108,-2.1085,-2.109,-2.1095,-2.11,-2.1105,-2.111 -0.5,-1.49,-1.5,-1.51,-1.685,-1.86,-1.96,-2.06,-2.085,-2.11,-2.1105,-2.111,-2.1115,-2.112,-2.1125,-2.113,-2.1135,-2.114,-2.1145,-2.115,-2.1155,-2.116 -0.55,-1.495,-1.505,-1.515,-1.69,-1.865,-1.965,-2.065,-2.09,-2.115,-2.1155,-2.116,-2.1165,-2.117,-2.1175,-2.118,-2.1185,-2.119,-2.1195,-2.12,-2.1205,-2.121 -0.6,-1.5,-1.51,-1.52,-1.695,-1.87,-1.97,-2.07,-2.095,-2.12,-2.1205,-2.121,-2.1215,-2.122,-2.1225,-2.123,-2.1235,-2.124,-2.1245,-2.125,-2.1255,-2.126 -0.65,-1.505,-1.515,-1.525,-1.7,-1.875,-1.975,-2.075,-2.1,-2.125,-2.1255,-2.126,-2.1265,-2.127,-2.1275,-2.128,-2.1285,-2.129,-2.1295,-2.13,-2.1305,-2.131 -0.7,-1.51,-1.52,-1.53,-1.705,-1.88,-1.98,-2.08,-2.105,-2.13,-2.1305,-2.131,-2.1315,-2.132,-2.1325,-2.133,-2.1335,-2.134,-2.1345,-2.135,-2.1355,-2.136 -0.75,-1.845,-1.855,-1.865,-2.0775,-2.29,-2.365,-2.44,-2.4775,-2.515,-2.52775,-2.5405,-2.541,-2.5415,-2.542,-2.5425,-2.543,-2.5435,-2.544,-2.5445,-2.545,-2.5455 -0.8,-2.18,-2.19,-2.2,-2.45,-2.7,-2.75,-2.8,-2.85,-2.9,-2.925,-2.95,-2.9505,-2.951,-2.9515,-2.952,-2.9525,-2.953,-2.9535,-2.954,-2.9545,-2.955 diff --git a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp deleted file mode 100644 index 3580f054..00000000 --- a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ /dev/null @@ -1,381 +0,0 @@ -// -// 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__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ -#define ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ - -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "raw_vehicle_cmd_converter/accel_map.hpp" -#include "raw_vehicle_cmd_converter/brake_map.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/utils.h" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" - -#include - -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32_multi_array.hpp" -#include "std_msgs/msg/multi_array_dimension.hpp" -#include "std_msgs/msg/string.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_external_api_msgs/msg/calibration_status.hpp" -#include "tier4_external_api_msgs/msg/calibration_status_array.hpp" -#include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp" -#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" -#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" -#include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include -#include -#include -#include -#include -#include - -namespace accel_brake_map_calibrator -{ - -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using geometry_msgs::msg::TwistStamped; -using nav_msgs::msg::OccupancyGrid; -using raw_vehicle_cmd_converter::AccelMap; -using raw_vehicle_cmd_converter::BrakeMap; -using std_msgs::msg::Float32MultiArray; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_external_api_msgs::msg::CalibrationStatus; -using tier4_vehicle_msgs::msg::ActuationCommandStamped; -using tier4_vehicle_msgs::msg::ActuationStatusStamped; -using visualization_msgs::msg::MarkerArray; - -using tier4_vehicle_msgs::srv::UpdateAccelBrakeMap; - -using Map = std::vector>; - -struct DataStamped -{ - DataStamped(const double _data, const rclcpp::Time & _data_time) - : data{_data}, data_time{_data_time} - { - } - double data; - rclcpp::Time data_time; -}; -using DataStampedPtr = std::shared_ptr; - -class AccelBrakeMapCalibrator : public rclcpp::Node -{ -private: - std::shared_ptr transform_listener_; - std::string csv_default_map_dir_; - rclcpp::Publisher::SharedPtr original_map_occ_pub_; - rclcpp::Publisher::SharedPtr update_map_occ_pub_; - rclcpp::Publisher::SharedPtr original_map_raw_pub_; - rclcpp::Publisher::SharedPtr update_map_raw_pub_; - rclcpp::Publisher::SharedPtr offset_covariance_pub_; - rclcpp::Publisher::SharedPtr debug_pub_; - rclcpp::Publisher::SharedPtr data_count_pub_; - rclcpp::Publisher::SharedPtr data_count_with_self_pose_pub_; - rclcpp::Publisher::SharedPtr data_ave_pub_; - rclcpp::Publisher::SharedPtr data_std_pub_; - rclcpp::Publisher::SharedPtr index_pub_; - rclcpp::Publisher::SharedPtr update_suggest_pub_; - rclcpp::Publisher::SharedPtr current_map_error_pub_; - rclcpp::Publisher::SharedPtr updated_map_error_pub_; - rclcpp::Publisher::SharedPtr map_error_ratio_pub_; - rclcpp::Publisher::SharedPtr calibration_status_pub_; - - rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr steer_sub_; - rclcpp::Subscription::SharedPtr actuation_status_sub_; - rclcpp::Subscription::SharedPtr actuation_cmd_sub_; - - // Service - rclcpp::Service::SharedPtr update_map_dir_server_; - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::TimerBase::SharedPtr timer_output_csv_; - void initTimer(double period_s); - void initOutputCSVTimer(double period_s); - - TwistStamped::ConstSharedPtr twist_ptr_; - std::vector> twist_vec_; - std::vector accel_pedal_vec_; // for delayed pedal - std::vector brake_pedal_vec_; // for delayed pedal - SteeringReport::ConstSharedPtr steer_ptr_; - DataStampedPtr accel_pedal_ptr_; - DataStampedPtr brake_pedal_ptr_; - DataStampedPtr delayed_accel_pedal_ptr_; - DataStampedPtr delayed_brake_pedal_ptr_; - - // Diagnostic Updater - std::shared_ptr updater_ptr_; - bool is_default_map_ = true; - - int get_pitch_method_; - int update_method_; - int accel_brake_value_source_; - double acceleration_ = 0.0; - double acceleration_time_; - double pre_acceleration_ = 0.0; - double pre_acceleration_time_; - double jerk_ = 0.0; - double accel_pedal_speed_ = 0.0; - double brake_pedal_speed_ = 0.0; - double pitch_ = 0.0; - double update_hz_; - double velocity_min_threshold_; - double velocity_diff_threshold_; - double pedal_diff_threshold_; - double max_steer_threshold_; - double max_pitch_threshold_; - double max_jerk_threshold_; - double pedal_velocity_thresh_; - double max_accel_; - double min_accel_; - double pedal_to_accel_delay_; - int precision_; - std::string csv_calibrated_map_dir_; - std::string output_accel_file_; - std::string output_brake_file_; - - // for calculating differential of msg - const double dif_twist_time_ = 0.2; // 200ms - const double dif_pedal_time_ = 0.16; // 160ms - const std::size_t twist_vec_max_size_ = 100; - const std::size_t pedal_vec_max_size_ = 100; - const double timeout_sec_ = 0.1; - int max_data_count_; - const int max_data_save_num_ = 10000; - const double map_resolution_ = 0.1; - const double max_jerk_ = 5.0; - bool pedal_accel_graph_output_ = false; - bool progress_file_output_ = false; - - // Algorithm - AccelMap accel_map_; - BrakeMap brake_map_; - - // for evaluation - AccelMap new_accel_map_; - BrakeMap new_brake_map_; - std::vector part_original_accel_mse_que_; - std::vector full_original_accel_mse_que_; - // std::vector full_original_accel_esm_que_; - std::vector full_original_accel_l1_que_; - std::vector full_original_accel_sq_l1_que_; - std::vector new_accel_mse_que_; - std::size_t full_mse_que_size_ = 100000; - std::size_t part_mse_que_size_ = 3000; - double full_original_accel_rmse_ = 0.0; - double full_original_accel_error_l1norm_ = 0.0; - double part_original_accel_rmse_ = 0.0; - double new_accel_rmse_ = 0.0; - double update_suggest_thresh_; - - // Accel / Brake Map update - Map accel_map_value_; - Map brake_map_value_; - Map update_accel_map_value_; - Map update_brake_map_value_; - Map accel_offset_covariance_value_; - Map brake_offset_covariance_value_; - std::vector map_value_data_; - std::vector accel_vel_index_; - std::vector brake_vel_index_; - std::vector accel_pedal_index_; - std::vector brake_pedal_index_; - bool update_success_; - int update_success_count_ = 0; - int update_count_ = 0; - int lack_of_data_count_ = 0; - int failed_to_get_pitch_count_ = 0; - int too_large_pitch_count_ = 0; - int too_low_speed_count_ = 0; - int too_large_steer_count_ = 0; - int too_large_jerk_count_ = 0; - int invalid_acc_brake_count_ = 0; - int too_large_pedal_spd_count_ = 0; - int update_fail_count_ = 0; - - // for map update - double map_offset_ = 0.0; - double map_coef_ = 1.0; - double covariance_; - const double forgetting_factor_ = 0.999; - const double coef_update_skip_thresh_ = 0.1; - - // output log - std::ofstream output_log_; - - bool getCurrentPitchFromTF(double * pitch); - void timerCallback(); - void timerCallbackOutputCSV(); - void executeUpdate( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index); - bool updateFourCellAroundOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc); - bool updateEachValOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc, - const double map_acc); - void updateTotalMapOffset(const double measured_acc, const double map_acc); - void callbackActuation( - const std_msgs::msg::Header header, const double accel, const double brake); - void callbackActuationCommand(const ActuationCommandStamped::ConstSharedPtr msg); - void callbackActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); - void callbackVelocity(const VelocityReport::ConstSharedPtr msg); - void callbackSteer(const SteeringReport::ConstSharedPtr msg); - bool callbackUpdateMapService( - const std::shared_ptr request_header, - UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res); - bool getAccFromMap(const double velocity, const double pedal); - double lowpass(const double original, const double current, const double gain = 0.8); - double getPedalSpeed( - const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, - const double prev_pedal_speed); - double getAccel( - const TwistStamped::ConstSharedPtr & prev_twist, - const TwistStamped::ConstSharedPtr & current_twist); - double getJerk(); - bool indexValueSearch( - const std::vector value_index, const double value, const double value_thresh, - int * searched_index); - int nearestValueSearch(const std::vector value_index, const double value); - int nearestPedalSearch(); - int nearestVelSearch(); - void takeConsistencyOfAccelMap(); - void takeConsistencyOfBrakeMap(); - bool updateAccelBrakeMap(); - void publishFloat32(const std::string publish_type, const double val); - void publishUpdateSuggestFlag(); - double getPitchCompensatedAcceleration(); - void executeEvaluation(); - double calculateEstimatedAcc( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map); - double calculateAccelSquaredError( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map); - double calculateAccelErrorL1Norm( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map); - std::vector getMapColumnFromUnifiedIndex( - const Map & accel_map_value, const Map & brake_map_value, const std::size_t index); - double getPedalValueFromUnifiedIndex(const std::size_t index); - int getUnifiedIndexFromAccelBrakeIndex(const bool accel_map, const std::size_t index); - void pushDataToQue( - const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, - std::queue * que); - template - void pushDataToVec(const T data, const std::size_t max_size, std::vector * vec); - template - T getNearestTimeDataFromVec( - const T base_data, const double back_time, const std::vector & vec); - DataStampedPtr getNearestTimeDataFromVec( - DataStampedPtr base_data, const double back_time, const std::vector & vec); - double getAverage(const std::vector & vec); - double getStandardDeviation(const std::vector & vec); - bool isTimeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec); - bool isTimeout(const DataStampedPtr & data_stamped, const double timeout_sec); - - /* for covariance calculation */ - // mean value on each cell (counting method depends on the update algorithm) - Eigen::MatrixXd accel_data_mean_mat_; - Eigen::MatrixXd brake_data_mean_mat_; - // calculated variance on each cell - Eigen::MatrixXd accel_data_covariance_mat_; - Eigen::MatrixXd brake_data_covariance_mat_; - // number of data on each cell (counting method depends on the update algorithm) - Eigen::MatrixXd accel_data_num_; - Eigen::MatrixXd brake_data_num_; - - OccupancyGrid getOccMsg( - const std::string frame_id, const double height, const double width, const double resolution, - const std::vector & map_value); - - /* Diag*/ - void checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat); - - /* Debug */ - void publishMap( - const Map accel_map_value, const Map brake_map_value, const std::string publish_type); - void publishOffsetCovMap(const Map accel_map_value, const Map brake_map_value); - void publishCountMap(); - void publishIndex(); - bool writeMapToCSV( - std::vector vel_index, std::vector pedal_index, Map value_map, - std::string filename); - void addIndexToCSV(std::ofstream * csv_file); - void addLogToCSV( - std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, - const double pitched_accel, const double accel_pedal, const double brake_pedal, - const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, - const double steer, const double jerk, const double full_original_accel_mse, - const double part_original_accel_mse, const double new_accel_mse); - - mutable Float32MultiArrayStamped debug_values_; - enum DBGVAL { - CURRENT_SPEED = 0, - CURRENT_ACCEL_PEDAL = 1, - CURRENT_BRAKE_PEDAL = 2, - CURRENT_RAW_ACCEL = 3, - CURRENT_ACCEL = 4, - CURRENT_RAW_ACCEL_SPEED = 5, - CURRENT_ACCEL_SPEED = 6, - CURRENT_RAW_BRAKE_SPEED = 7, - CURRENT_BRAKE_SPEED = 8, - CURRENT_RAW_PITCH = 9, - CURRENT_PITCH = 10, - CURRENT_STEER = 11, - SUCCESS_TO_UPDATE = 12, - CURRENT_JERK = 13, - }; - static constexpr unsigned int num_debug_values_ = 14; - - enum GET_PITCH_METHOD { TF = 0, FILE = 1, NONE = 2 }; - - enum UPDATE_METHOD { - UPDATE_OFFSET_EACH_CELL = 0, - UPDATE_OFFSET_TOTAL = 1, - UPDATE_OFFSET_FOUR_CELL_AROUND = 2, - }; - - enum ACCEL_BRAKE_SOURCE { - STATUS = 0, - COMMAND = 1, - }; - - std::unique_ptr logger_configure_; - -public: - explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); -}; - -} // namespace accel_brake_map_calibrator - -#endif // ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ diff --git a/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml deleted file mode 100644 index c664158a..00000000 --- a/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png b/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png deleted file mode 100644 index c29781b2..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/add_panel.png b/vehicle/accel_brake_map_calibrator/media/add_panel.png deleted file mode 100644 index f045042e..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/add_panel.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png b/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png deleted file mode 100644 index f21a2549..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png b/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png deleted file mode 100644 index 28db957c..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png b/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png deleted file mode 100644 index 8929f3d6..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/log_sample.png b/vehicle/accel_brake_map_calibrator/media/log_sample.png deleted file mode 100644 index 13887a42..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/log_sample.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png b/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png deleted file mode 100644 index e57ed269..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/statistics_sample.png b/vehicle/accel_brake_map_calibrator/media/statistics_sample.png deleted file mode 100644 index c36ce9a5..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/statistics_sample.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/package.xml deleted file mode 100644 index 64e7d1fb..00000000 --- a/vehicle/accel_brake_map_calibrator/package.xml +++ /dev/null @@ -1,42 +0,0 @@ - - - - accel_brake_map_calibrator - 0.1.0 - The accel_brake_map_calibrator - Tomoya Kimura - Taiki Tanaka - Takeshi Miura - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_vehicle_msgs - diagnostic_updater - geometry_msgs - motion_utils - raw_vehicle_cmd_converter - rclcpp - std_msgs - std_srvs - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - tier4_external_api_msgs - tier4_vehicle_msgs - visualization_msgs - - python3-matplotlib - python3-pandas - python3-scipy - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz b/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz deleted file mode 100644 index ffcb63f4..00000000 --- a/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz +++ /dev/null @@ -1,141 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Map1 - - /Axes1 - - /MarkerArray1 - Splitter Ratio: 0.5581113696098328 - Tree Height: 1615 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 1 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Map - Topic: /accel_brake_map_calibrator/debug/data_count_self_pose_occ_map - Unreliable: false - Use Timestamp: false - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 0.10000000149011612 - Name: Axes - Radius: 0.009999999776482582 - Reference Frame: - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - occ_pedal_index: true - occ_vel_index: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /accel_brake_map_calibrator/debug/occ_index - Value: true - Enabled: true - Global Options: - Background Color: 238; 238; 236 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - - Class: rviz_default_plugins/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz_default_plugins/SetGoal - Topic: /move_base_simple/goal - - Class: rviz_plugins/PedestrianInitialPoseTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Theta std deviation: 0.0872664600610733 - Velocity: 1 - X std deviation: 0.009999999776482582 - Y std deviation: 0.009999999776482582 - Z position: 0 - Z std deviation: 0.009999999776482582 - - Class: rviz_plugins/CarInitialPoseTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Theta std deviation: 0.0872664600610733 - Velocity: 3 - X std deviation: 0.009999999776482582 - Y std deviation: 0.009999999776482582 - Z position: 0 - Z std deviation: 0.009999999776482582 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Value: true - Views: - Current: - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 1.8697248697280884 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.48724469542503357 - Y: 0.3405919671058655 - Z: 5.960464477539062e-7 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 4.710395336151123 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1862 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000005070000068cfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000068c000000c900fffffffb0000000a00560069006500770073000000030c000003bd000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000005470000012b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d00610067006501000005d6000000f30000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000000a0049006d00610067006500000002ef000003da0000000000000000000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006fb0000005afc0100000002fb0000000800540069006d00650100000000000006fb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006fb0000068c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Views: - collapsed: false - Width: 1787 - X: 1193 - Y: 165 diff --git a/vehicle/accel_brake_map_calibrator/scripts/__init__.py b/vehicle/accel_brake_map_calibrator/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py b/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py deleted file mode 100755 index f73718f7..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py +++ /dev/null @@ -1,57 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# 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. - -import rclpy -from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped - -MAX_ACCEL = 1.0 # [-] -MIN_ACCEL = 0.0 # [-] - - -class AccelTester(Node): - def __init__(self): - super().__init__("vehicle_accel_tester") - self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/accel", 1) - - def run(self): - while rclpy.ok(): - value = float(input(f"target accel [{MIN_ACCEL} ~ {MAX_ACCEL} -] > ")) - if value > MAX_ACCEL: - print("input value is larger than max accel!" + f"input: {value} max: {MAX_ACCEL}") - value = MAX_ACCEL - elif value < MIN_ACCEL: - print("input value is smaller than min accel!" + f"input: {value} min: {MIN_ACCEL}") - value = MIN_ACCEL - - msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value) - - self.pub.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - - accel_tester = AccelTester() - accel_tester.run() - - accel_tester.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py deleted file mode 100755 index 749c9e42..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py +++ /dev/null @@ -1,83 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# 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. - -from autoware_auto_vehicle_msgs.msg import GearCommand -import rclpy -from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped -from tier4_vehicle_msgs.msg import ActuationCommandStamped - - -class ActuationCmdPublisher(Node): - def __init__(self): - super().__init__("actuation_cmd_publisher") - self.target_brake = 0.0 - self.target_accel = 0.0 - - self.sub_acc = self.create_subscription( - Float32Stamped, "/vehicle/tester/accel", self.on_accel, 1 - ) - self.sub_brk = self.create_subscription( - Float32Stamped, "/vehicle/tester/brake", self.on_brake, 1 - ) - self.pub_actuation_cmd = self.create_publisher( - ActuationCommandStamped, "/control/command/actuation_cmd", 1 - ) - self.pub_gear_cmd = self.create_publisher(GearCommand, "/control/command/gear_cmd", 1) - timer_period = 0.02 # seconds - self.timer = self.create_timer(timer_period, self.on_timer) - - def on_brake(self, msg): - self.target_brake = msg.data - print(f"Set target brake : {self.target_brake}") - - def on_accel(self, msg): - self.target_accel = msg.data - print(f"Set target accel : {self.target_accel}") - - def on_timer(self): - msg_actuation_cmd = ActuationCommandStamped() - msg_actuation_cmd.actuation.steer_cmd = 0.0 - msg_actuation_cmd.header.stamp = self.get_clock().now().to_msg() - msg_actuation_cmd.header.frame_id = "base_link" - msg_actuation_cmd.actuation.accel_cmd = self.target_accel - msg_actuation_cmd.actuation.brake_cmd = self.target_brake - self.pub_actuation_cmd.publish(msg_actuation_cmd) - - msg_gear_cmd = GearCommand() - msg_gear_cmd.stamp = self.get_clock().now().to_msg() - msg_gear_cmd.command = GearCommand.DRIVE - self.pub_gear_cmd.publish(msg_gear_cmd) - - print( - f"publish ActuationCommand with accel: {self.target_accel}, brake: {self.target_brake}" - ) - - -def main(args=None): - rclpy.init(args=args) - - actuation_cmd_publisher = ActuationCmdPublisher() - - rclpy.spin(actuation_cmd_publisher) - - actuation_cmd_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py b/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py deleted file mode 100755 index 050f2323..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py +++ /dev/null @@ -1,61 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# 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. - - -import rclpy -from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped - -MAX_BRAKE = 1.0 # [-] -MIN_BRAKE = 0.0 # [-] - - -class BrakeTester(Node): - def __init__(self): - super().__init__("vehicle_brake_tester") - self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/brake", 1) - - def run(self): - while rclpy.ok(): - value = float( - input("target brake [" + str(MIN_BRAKE) + " ~ " + str(MAX_BRAKE) + "] > ") - ) - - if value > MAX_BRAKE: - print("input value is larger than max brake!" + f"input: {value} max: {MAX_BRAKE}") - value = MAX_BRAKE - elif value < MIN_BRAKE: - print("input value is smaller than min brake!" + f"input: {value} min: {MIN_BRAKE}") - value = MIN_BRAKE - - msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value) - - self.pub.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - - brake_tester = BrakeTester() - brake_tester.run() - - brake_tester.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py b/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py deleted file mode 100755 index f4581699..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py +++ /dev/null @@ -1,151 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import sys - -import numpy as np -from scipy import signal - -BIT = 1e-04 - - -def get_map_list(y_num, x_num): - data_list = [] - for yn in range(y_num): - child_data_list = [] - for xn in range(x_num): - child_data_list.append([]) - if xn == x_num - 1: - data_list.append(child_data_list) - return data_list - - -class CalcUtils: - @staticmethod - def average_filter(data, average_num): - if type(average_num) != int: - print( - "Error in average_filter(data, average_num):\ - Type of average_num must be int" - ) - sys.exit(1) - - if average_num % 2 == 0: - print( - "Error in average_filter(data, average_num):\ - average_num must be odd number" - ) - sys.exit(1) - - average_filter = np.ones(average_num) / float(average_num) - average_data = np.convolve(data, average_filter) - cut_num = (average_num - 1) / 2 - return average_data[cut_num : len(average_data) - cut_num] - - @staticmethod - # fp:pass Hz, #fs: block Hz, g_pass: pass dB, g_stop: stop DB - def lowpass_filter_scipy(x, sample_rate, fp, fs, g_pass, g_stop): - fn = sample_rate / 2 - wp = fp / fn - ws = fs / fn - N, Wn = signal.buttord(wp, ws, g_pass, g_stop) - b, a = signal.butter(N, Wn, "low") - y = signal.filtfilt(b, a, x) - return y - - @staticmethod - def create_2d_map( - x, - y, - data, - color_factor, - x_index_list, - x_thresh, - y_index_list, - y_thresh, - calibration_method="four_cell", - ): - if x.shape != y.shape or y.shape != data.shape: - print("Error: the shape of x, y, data must be same") - sys.exit() - data_size = len(x) - - x_num = len(x_index_list) - y_num = len(y_index_list) - data_list = get_map_list(y_num, x_num) - full_data_list = get_map_list(y_num, x_num) - - if calibration_method == "four_cell": - x_thresh = np.abs(x_index_list[1] - x_index_list[0]) / 2 - y_thresh = np.abs(y_index_list[1] - y_index_list[0]) / 2 - - for i in range(0, data_size): - x_index = None - y_index = None - for xi in range(0, x_num): - if np.abs(x_index_list[xi] - x[i]) < x_thresh: - x_index = xi - break - for yi in range(0, y_num): - if np.abs(y_index_list[yi] - y[i]) < y_thresh: - y_index = yi - break - - if x_index is not None and y_index is not None: - # append data - data_list[y_index][x_index].append(data[i]) - full_data_list[y_index][x_index].append([x[i], y[i], data[i], color_factor[i]]) - - return data_list, full_data_list - - @staticmethod - def extract_x_index_map(data_map, x_index): - y_num = len(data_map) - extracted_data = None - # x_num = len(data_map[0]) - for y in range(y_num): - data = np.array(data_map[y][x_index]) - if len(data) == 0: - continue - elif extracted_data is None: - extracted_data = data - else: - extracted_data = np.concatenate([extracted_data, data]) - return extracted_data - - @staticmethod - def create_stat_map(data_map, statistics_type="average"): - y_num = len(data_map) - x_num = len(data_map[0]) - count_map = np.zeros((y_num, x_num)) - average_map = np.zeros((y_num, x_num)) - stddev_map = np.zeros((y_num, x_num)) - for y in range(y_num): - for x in range(x_num): - data = np.array(data_map[y][x]) - count_map[y][x] = data.shape[0] - if count_map[y][x] == 0: - # print('Warn: data_map', y, x, 'is vacant list') - average_map[y][x] = 0.0 - stddev_map[y][x] = 0.0 - else: - if statistics_type == "average": - average_map[y][x] = np.average(data) - elif statistics_type == "median": - average_map[y][x] = np.median(data) - stddev_map[y][x] = np.std(data) - return count_map, average_map, stddev_map diff --git a/vehicle/accel_brake_map_calibrator/scripts/config.py b/vehicle/accel_brake_map_calibrator/scripts/config.py deleted file mode 100755 index c554c406..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/config.py +++ /dev/null @@ -1,44 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import numpy as np - -# config of file index -TS = "timestamp" -VEL = "velocity" -RAW_ACC = "accel" -PITCH_ACC = "pitch_comp_accel" -ACC = "final_accel" -A_PED = "accel_pedal" -B_PED = "brake_pedal" -A_PED_SPD = "accel_pedal_speed" -B_PED_SPD = "brake_pedal_speed" -PITCH = "pitch" -JERK = "jerk" -STEER = "steer" - -# config of accel / brake map -VEL_LIST = np.array([0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50]) # km -PEDAL_LIST = np.array( - [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5] -) -VEL_MIN = VEL_LIST[0] -VEL_MAX = VEL_LIST[-1] -VEL_SPAN = (VEL_MAX - VEL_MIN) / (len(VEL_LIST) - 1) -PEDAL_MIN = PEDAL_LIST[0] -PEDAL_MAX = PEDAL_LIST[-1] -PEDAL_SPAN = (PEDAL_MAX - PEDAL_MIN) / (len(PEDAL_LIST) - 1) diff --git a/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py b/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py deleted file mode 100755 index c4494cdb..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py +++ /dev/null @@ -1,132 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import glob - -import config as CF -import numpy as np -import pandas as pd - -# pre-defined -NUMERIC_LIMITS = 1e-02 - - -class CSVReader(object): - def __init__(self, csv, csv_type="dir"): - if csv_type == "dir": - csv_files = glob.glob(csv + "/*.csv") - csv_list = [] - for cf in csv_files: - csv_list.append(pd.read_csv(cf, engine="python")) - self.csv_data = pd.concat(csv_list, ignore_index=True) - else: - self.csv_data = pd.read_csv(csv, engine="python") - - def removeUnusedData( - self, - min_vel_thr, - max_steer_thr, - max_pitch_thr, - max_pedal_vel_thr, - max_jerk_thr, - remove_by_invalid_pedal=True, - remove_by_vel=True, - remove_by_steer=True, - remove_by_pitch=True, - remove_by_pedal_speed=True, - remove_by_jerk=True, - ): - # remove unused data - raw_size = len(self.csv_data) - - for i in range(0, raw_size)[::-1]: - vel = self.csv_data[CF.VEL][i] - steer = self.csv_data[CF.STEER][i] - accel_pedal = self.csv_data[CF.A_PED][i] - brake_pedal = self.csv_data[CF.B_PED][i] - accel_pedal_speed = self.csv_data[CF.A_PED_SPD][i] - brake_pedal_speed = self.csv_data[CF.B_PED_SPD][i] - jerk = self.csv_data[CF.JERK][i] - pitch = self.csv_data[CF.PITCH][i] - - # invalid pedal - if ( - remove_by_invalid_pedal - and accel_pedal > NUMERIC_LIMITS - and brake_pedal > NUMERIC_LIMITS - ): - self.csv_data = self.csv_data.drop(i) - continue - - # low velocity - if remove_by_vel and vel < min_vel_thr: - self.csv_data = self.csv_data.drop(i) - continue - - # high steer - if remove_by_steer and np.abs(steer) > max_steer_thr: - self.csv_data = self.csv_data.drop(i) - continue - - # high pitch - if remove_by_pitch and np.abs(pitch) > max_pitch_thr: - self.csv_data = self.csv_data.drop(i) - continue - - # high pedal speed - if ( - remove_by_pedal_speed - and np.abs(accel_pedal_speed) > max_pedal_vel_thr - or np.abs(brake_pedal_speed) > max_pedal_vel_thr - ): - self.csv_data = self.csv_data.drop(i) - continue - - # max_jerk_thr - if remove_by_jerk and np.abs(jerk) > max_jerk_thr: - self.csv_data = self.csv_data.drop(i) - continue - - return self.csv_data - - def getVelData(self): - vel_data = np.array(self.csv_data[CF.VEL]) - return vel_data - - def getPedalData(self): - pedal_data = np.array(self.csv_data[CF.A_PED]) - np.array(self.csv_data[CF.B_PED]) - return pedal_data - - def getAccData(self): - acc_data = np.array(self.csv_data[CF.ACC]) - return acc_data - - def getPitchData(self): - pitch_data = np.array(self.csv_data[CF.PITCH]) - return pitch_data - - def extractPedalRangeWithDelay(self, delay_step, pedal_value, pedal_diff_thr): - csv_data = self.csv_data.reset_index(drop=True) - - for i in range(delay_step, len(self.csv_data))[::-1]: - pedal = csv_data[CF.A_PED][i - delay_step] - csv_data[CF.B_PED][i - delay_step] - # extract data of pedal = pedal_value - if pedal > pedal_value + pedal_diff_thr or pedal < pedal_value - pedal_diff_thr: - csv_data = csv_data.drop(i) - continue - - return csv_data diff --git a/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py b/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py deleted file mode 100755 index eb2bce38..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py +++ /dev/null @@ -1,144 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import numpy as np -from plotter import Plotter -import rclpy -from rclpy.node import Node - - -class DelayEstimator(Node): - def __init__(self): - # get parameter - super().__init__("delay_estimator") - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter("log_file", package_path + "/config/log.csv") - log_file = self.get_parameter("log_file").get_parameter_value().string_value - self.declare_parameter("velocity_min_threshold", 0.1) - min_vel_thr = ( - self.get_parameter("velocity_min_threshold").get_parameter_value().double_value - ) - self.declare_parameter("velocity_diff_threshold", 0.556) - vel_diff_thr = ( - self.get_parameter("velocity_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("pedal_diff_threshold", 0.03) - pedal_diff_thr = ( - self.get_parameter("pedal_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("max_steer_threshold", 0.2) - max_steer_thr = self.get_parameter("max_steer_threshold").get_parameter_value().double_value - self.declare_parameter("max_pitch_threshold", 0.02) - max_pitch_thr = self.get_parameter("max_pitch_threshold").get_parameter_value().double_value - self.declare_parameter("max_jerk_threshold", 0.7) - max_jerk_thr = self.get_parameter("max_jerk_threshold").get_parameter_value().double_value - self.declare_parameter("pedal_velocity_thresh", 0.15) - max_pedal_vel_thr = ( - self.get_parameter("pedal_velocity_thresh").get_parameter_value().double_value - ) - self.declare_parameter("update_hz", 10.0) - update_hz = self.get_parameter("update_hz").get_parameter_value().double_value - - self.data_span = 1.0 / update_hz - - # read csv - self.cr = CSVReader(log_file, csv_type="file") - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, - ) - - # plot all data - # value to use for statistics - PEDAL_VALUE = 0.1 - VEL_VALUE_LIST = np.array([10, 15, 20]) / 3.6 - plotter = Plotter(3, 2) - max_delay_step = 5 - for delay_step in range(max_delay_step + 1): - print("data processing... " + str(delay_step) + " / " + str(max_delay_step)) - csv_data = self.cr.extractPedalRangeWithDelay(delay_step, PEDAL_VALUE, pedal_diff_thr) - - # get correlation coefficient - # extract data of velocity is VEL_VALUE - coef_list = [] - for vel_value in VEL_VALUE_LIST: - ex_csv_data = csv_data[csv_data[CF.VEL] < vel_value + vel_diff_thr] - ex_csv_data = csv_data[csv_data[CF.VEL] > vel_value - vel_diff_thr] - pedal_speed = ex_csv_data[CF.A_PED_SPD] - ex_csv_data[CF.B_PED_SPD] - accel = ex_csv_data[CF.ACC] - coef = self.getCorCoef(pedal_speed, accel) - coef_list.append(coef) - - print("delay_step: ", delay_step) - print("coef: ", coef_list) - - ave_coef = np.average(coef_list) - self.plotPedalSpeedAndAccel(csv_data, plotter, delay_step + 1, delay_step, ave_coef) - plotter.show() - - def getCorCoef(self, a, b): - coef = np.corrcoef(np.array(a), np.array(b)) - return coef[0, 1] - - def plotPedalSpeedAndAccel(self, csv_data, plotter, subplot_num, delay_step, coef): - pedal_speed = csv_data[CF.A_PED_SPD] - csv_data[CF.B_PED_SPD] - accel = csv_data[CF.ACC] - velocity = csv_data[CF.VEL] * 3.6 - fig = plotter.subplot(subplot_num) - plotter.scatter_color(pedal_speed, accel, velocity, "hsv", label=None) - delay_time_ms = delay_step * self.data_span * 1000 - plotter.add_label( - "pedal-spd-acc (delay = " + str(delay_time_ms) + " ms), R = " + str(coef), - "pedal-speed", - "accel", - ) - plotter.set_lim(fig, [-0.4, 0.4], [-1.0, 1.0]) - - -def main(args=None): - rclpy.init(args=args) - node = DelayEstimator() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py b/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py deleted file mode 100755 index 71cdb502..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py +++ /dev/null @@ -1,303 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import argparse -import sys - -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import numpy as np -from plotter import Plotter - -# parameter for statistics -SCATTER_GRAPH_PEDAL_LIM = [-0.55, 0.55] -SCATTER_GRAPH_ACCEL_LIM = [-1.8, 1.8] -CONSIDER_PEDAL_VAL_FOR_STAT = [0.0, 0.1, 0.2] - -parser = argparse.ArgumentParser() -parser.add_argument("--csv-file-dir", help="log file directory") -parser.add_argument("--min-vel-thr", type=float, default=0.1, help="thresh of minimum velocity") -parser.add_argument( - "--max-pedal-vel-thr", type=float, default=0.15, help="thresh of maximum pedal velocity" -) -parser.add_argument("--max-steer-thr", type=float, default=0.2, help="thresh of maximum steer") -parser.add_argument("--max-pitch-thr", type=float, default=0.02, help="thresh of maximum pitch") -parser.add_argument("--max-jerk-thr", type=float, default=0.7, help="thresh of maximum jerk") -parser.add_argument( - "--pedal-diff-thr", - type=float, - default=0.03, - help="thresh of max delta-pedal to add statistics map", -) -parser.add_argument( - "--vel-diff-thr", - type=float, - default=0.556, - help="thresh of max delta-velocity to add statistics map", -) -parser.add_argument( - "--view-velocity-1", type=int, default=5, help="velocity(kmh) to visualize by plot (1)" -) -parser.add_argument( - "--view-velocity-2", type=int, default=20, help="velocity(kmh) to visualize by plot (2)" -) -parser.add_argument( - "--view-velocity-3", type=int, default=30, help="velocity(kmh) to visualize by plot (3)" -) -parser.add_argument("--output-stat-csv", default="stat.csv", help="output csv file name") -parser.add_argument("--output-plot-file", default="result.png", help="output plot file name") -parser.add_argument("--no-plot", action="store_true") - -args = parser.parse_args() -# get path -csv_file_dir = args.csv_file_dir -min_vel_thr = args.min_vel_thr -max_pedal_vel_thr = args.max_pedal_vel_thr -max_steer_thr = args.max_steer_thr -max_pitch_thr = args.max_pitch_thr -max_jerk_thr = args.max_jerk_thr -pedal_diff_thr = args.pedal_diff_thr -vel_diff_thr = args.vel_diff_thr -view_velocity_list = [] -view_velocity_list.append(args.view_velocity_1) -view_velocity_list.append(args.view_velocity_2) -view_velocity_list.append(args.view_velocity_3) -output_stat_csv = args.output_stat_csv -output_plot_file = args.output_plot_file -remove_by_invalid_pedal = True -remove_by_vel = True -remove_by_steer = True -remove_by_pitch = True -remove_by_pedal_speed = True -remove_by_jerk = True -is_plot = not args.no_plot - -view_velocity_idx_list = [] -for vv in view_velocity_list: - if not (vv in CF.VEL_LIST): - print("invalid view_velocity. velocity list is: ", CF.VEL_LIST) - sys.exit() - view_velocity_idx_list.append(CF.VEL_LIST.tolist().index(vv)) - -# search index of pedal to use for statistics -stat_pedal_list = [] -for val in CONSIDER_PEDAL_VAL_FOR_STAT: - if not (val in CF.PEDAL_LIST): - print("invalid CONSIDER_PEDAL_VAL_FOR_STAT. pedal list is:", CF.PEDAL_LIST) - stat_pedal_list.append(CF.PEDAL_LIST.tolist().index(val)) - -# read file -cr = CSVReader(csv_file_dir) - -# remove unused_data -csv_data = cr.removeUnusedData( - min_vel_thr, - max_steer_thr, - max_pitch_thr, - max_pedal_vel_thr, - max_jerk_thr, - remove_by_invalid_pedal, - remove_by_vel, - remove_by_steer, - remove_by_pitch, - remove_by_pedal_speed, - remove_by_jerk, -) - - -# get statistics array -vel_data = cr.getVelData() -pedal_data = cr.getPedalData() -acc_data = cr.getAccData() - -# get color factor (pitch) array for plotting -color_data = cr.getPitchData() - -data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, -) - -count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) -velocity_map_list = [] -for v_idx in view_velocity_idx_list: - velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, v_idx)) - -# output statistics -f = open(output_stat_csv, "a") -count_list_over_v = [] -stddev_list_over_v = [] -for vi in view_velocity_idx_list: - count_list = [] - stddev_list = [] - for pi in stat_pedal_list: - count_list.append(count_map[pi][vi]) - if count_map[pi][vi] > 5: - stddev_list.append(stddev_map[pi][vi]) - else: - print("Warning: count is fewer than 5.") - count_sum = int(np.sum(count_list)) - stddev_ave = np.average(stddev_list) - count_list_over_v.append(count_sum) - stddev_list_over_v.append(stddev_ave) - f.write(str(count_sum) + ",") - f.write(str(stddev_ave) + ",") - print("velocity index:", vi) - print("\tcount: ", count_sum) - print("\tstddev: ", stddev_ave) -count_sum_over_v = int(np.sum(count_list_over_v)) -stddev_ave_over_v = np.average(stddev_list_over_v) -f.write(str(count_sum_over_v) + ",") -f.write(str(stddev_ave_over_v) + "\n") -f.close() -print("full data:") -print("\tcount: ", count_sum_over_v) -print("\tstddev: ", stddev_ave_over_v) - -# visualization -plotter = Plotter(2, 3) -plotter.subplot(1) -plotter.imshow( - average_map, CF.VEL_MIN, CF.VEL_MAX, CF.VEL_SPAN, CF.PEDAL_MIN, CF.PEDAL_MAX, CF.PEDAL_SPAN -) -plotter.add_label("average of accel", "velocity(kmh)", "throttle") - -plotter.subplot(2) -plotter.imshow( - stddev_map, CF.VEL_MIN, CF.VEL_MAX, CF.VEL_SPAN, CF.PEDAL_MIN, CF.PEDAL_MAX, CF.PEDAL_SPAN -) -plotter.add_label("std. dev. of accel", "velocity(kmh)", "throttle") - -plotter.subplot(3) -plotter.imshow( - count_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - num_data_type="int", -) -plotter.add_label("number of accel data", "velocity(kmh)", "throttle") - -# view pedal-accel graph - - -def view_pedal_accel_graph(subplot_num, vel_list_idx): - # plot all data - fig = plotter.subplot(subplot_num) - plotter.scatter( - velocity_map_list[vel_list_idx][:, 1], - velocity_map_list[vel_list_idx][:, 2], - "blue", - label="all", - ) - - # plot average data - # remove average score of no data - average_map_v_avoid_0 = ( - average_map[:, view_velocity_idx_list[vel_list_idx]] - + np.where(average_map[:, view_velocity_idx_list[vel_list_idx]] == 0, True, False) * 1e03 - ) - - plotter.scatter(CF.PEDAL_LIST, average_map_v_avoid_0, "red", label="average") - - plotter.set_lim(fig, SCATTER_GRAPH_PEDAL_LIM, SCATTER_GRAPH_ACCEL_LIM) - plotter.add_label( - str(view_velocity_list[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" - ) - - -view_pedal_accel_graph(4, 0) -view_pedal_accel_graph(5, 1) -view_pedal_accel_graph(6, 2) - -if is_plot: - plotter.show() -plotter.save(output_plot_file) - - -# pedal-pitch plot -""" -cr = CSVReader(csv_file_dir) -csv_data = cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, - max_pedal_vel_thr, max_jerk_thr, - remove_by_invalid_pedal, - remove_by_vel=True, - remove_by_steer=False, - remove_by_pitch=False, - remove_by_pedal_speed=False, - remove_by_jerk=False) -pitch = csv_data[CF.PITCH] -pedal = csv_data[CF.A_PED] - csv_data[CF.B_PED] -velocity = csv_data[CF.VEL] * 3.6 # color -plotter.scatter_color(pedal, pitch, velocity, (0.0, 35.0), "hsv", label=None) -plotter.add_label("pedal-pitch relation", "pedal", "pitch") -plotter.show() -""" - -""" -# pedal-speed-accel plot -cr = CSVReader(csv_file_dir) -csv_data = cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, - max_pedal_vel_thr, max_jerk_thr, - remove_by_invalid_pedal, - remove_by_vel=True, - remove_by_steer=True, - remove_by_pitch=True, - remove_by_pedal_speed=False, - remove_by_jerk=False) - -csv_data = csv_data.reset_index(drop=True) -delay = 3 # accel delay (*100ms) -for i in range(delay, len(csv_data))[::-1]: - pedal = csv_data[CF.A_PED][i-delay] - csv_data[CF.B_PED][i-delay] - # extract data of pedal = 0.1 - tgt_ped = 0.1 - if pedal > tgt_ped + pedal_diff_thr or \ - pedal < tgt_ped - pedal_diff_thr: - csv_data = csv_data.drop(i) - continue - - velocity = csv_data[CF.VEL][i] - if velocity > 12.0 / 3.6 or \ - velocity < 8.0 / 3.6: - csv_data = csv_data.drop(i) - continue - -pedal_speed = csv_data[CF.A_PED_SPD] - csv_data[CF.B_PED_SPD] -accel = csv_data[CF.ACC] -velocity = csv_data[CF.VEL] * 3.6 -plotter = Plotter(1, 1) -fig = plotter.subplot(1) -plotter.scatter_color(pedal_speed, accel, velocity, - (0.0, 35.0), "hsv", label=None) -plotter.add_label("pedal-speed-accel relation (only pedal = 0.1)", - "pedal-speed", "accel") -plotter.set_lim(fig, [-0.4, 0.4], [-0.5, 1.0]) -plotter.show() -""" diff --git a/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py deleted file mode 100755 index a2d82a46..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ /dev/null @@ -1,367 +0,0 @@ -#! /usr/bin/env python3 - -# 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. - -import math -from pathlib import Path - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import matplotlib.pyplot as plt -import numpy as np -from plotter import Plotter -import rclpy -from rclpy.node import Node -from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData -import yaml - - -class DrawGraph(Node): - calibrated_map_dir = "" - - def __init__(self): - super().__init__("plot_server") - - self.srv = self.create_service( - CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback - ) - - default_map_path = get_package_share_directory("raw_vehicle_cmd_converter") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_default_map_dir", default_map_path + "/data/default/" - ) - self.default_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") - .get_parameter_value() - .string_value - ) - - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" - ) - self.calibrated_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") - .get_parameter_value() - .string_value - ) - - self.declare_parameter("calibration_method", "each_cell") - self.calibration_method = ( - self.get_parameter("calibration_method").get_parameter_value().string_value - ) - if self.calibration_method is None: - self.calibration_method = "each_cell" - elif not ( - (self.calibration_method == "each_cell") | (self.calibration_method == "four_cell") - ): - print("invalid method.") - self.calibration_method = "each_cell" - - self.log_file = package_path + "/config/log.csv" - - config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" - if Path(config_file).exists(): - self.get_logger().info("config file exists") - with open(config_file) as yml: - data = yaml.safe_load(yml) - self.min_vel_thr = data["/**"]["ros__parameters"]["velocity_min_threshold"] - self.vel_diff_thr = data["/**"]["ros__parameters"]["velocity_diff_threshold"] - self.pedal_diff_thr = data["/**"]["ros__parameters"]["pedal_diff_threshold"] - self.max_steer_thr = data["/**"]["ros__parameters"]["max_steer_threshold"] - self.max_pitch_thr = data["/**"]["ros__parameters"]["max_pitch_threshold"] - self.max_jerk_thr = data["/**"]["ros__parameters"]["max_jerk_threshold"] - else: - self.get_logger().warning("config file is not found in {}".format(config_file)) - self.min_vel_thr = 0.1 - self.vel_diff_thr = 0.556 - self.pedal_diff_thr = 0.03 - self.max_steer_thr = 0.2 - self.max_pitch_thr = 0.02 - self.max_jerk_thr = 0.7 - - self.max_pedal_vel_thr = 0.7 - - # debug - self.get_logger().info("default map dir: {}".format(self.default_map_dir)) - self.get_logger().info("calibrated map dir: {}".format(self.calibrated_map_dir)) - self.get_logger().info("calibrated method: {}".format(self.calibration_method)) - self.get_logger().info("log file :{}".format(self.log_file)) - self.get_logger().info("min_vel_thr : {}".format(self.min_vel_thr)) - self.get_logger().info("vel_diff_thr : {}".format(self.vel_diff_thr)) - self.get_logger().info("pedal_diff_thr : {}".format(self.pedal_diff_thr)) - self.get_logger().info("max_steer_thr : {}".format(self.max_steer_thr)) - self.get_logger().info("max_pitch_thr : {}".format(self.max_pitch_thr)) - self.get_logger().info("max_jerk_thr : {}".format(self.max_jerk_thr)) - self.get_logger().info("max_pedal_vel_thr : {}".format(self.max_pedal_vel_thr)) - - def get_data_callback(self, request, response): - # read csv - # If log file doesn't exist, return empty data - if not Path(self.log_file).exists(): - response.graph_image = [] - self.get_logger().info("svg data is empty") - - response.accel_map = "" - self.get_logger().info("accel map is empty") - - response.brake_map = "" - self.get_logger().info("brake map is empty") - - return response - - self.cr = CSVReader(self.log_file, csv_type="file") - - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - self.min_vel_thr, - self.max_steer_thr, - self.max_pitch_thr, - self.max_pedal_vel_thr, - self.max_jerk_thr, - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - self.vel_diff_thr, - CF.PEDAL_LIST, - self.pedal_diff_thr, - self.calibration_method, - ) - - count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) - velocity_map_list = [] - for i in range(len(CF.VEL_LIST)): - velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) - - default_pedal_list, default_acc_list = self.load_map(self.default_map_dir) - if len(default_pedal_list) == 0 or len(default_acc_list) == 0: - self.get_logger().warning( - "No default map file was found in {}".format(self.default_map_dir) - ) - - response.graph_image = [] - self.get_logger().info("svg data is empty") - - response.accel_map = "" - self.get_logger().info("accel map is empty") - - response.brake_map = "" - self.get_logger().info("brake map is empty") - - return response - - calibrated_pedal_list, calibrated_acc_list = self.load_map(self.calibrated_map_dir) - if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: - self.get_logger().warning( - "No calibrated map file was found in {}".format(self.calibrated_map_dir) - ) - - # visualize point from data - plot_width = 3 - plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) - plotter = Plotter(plot_height, plot_width) - for i in range(len(CF.VEL_LIST)): - self.view_pedal_accel_graph( - plotter, - i, - velocity_map_list, - i, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - ) - plt.savefig("plot.svg") - self.get_logger().info("svg saved") - - # pack response data - text = Path("plot.svg").read_text() - if text == "": - response.graph_image = [] - self.get_logger().info("svg data is empty") - else: - byte = text.encode() - for b in byte: - response.graph_image.append(b) - self.get_logger().info("svg data is packed") - - accel_map_name = Path(self.calibrated_map_dir + "accel_map.csv") - if accel_map_name.exists(): - with open(self.calibrated_map_dir + "accel_map.csv", "r") as calibrated_accel_map: - for accel_data in calibrated_accel_map: - response.accel_map += accel_data - self.get_logger().info("accel map is packed") - else: - response.accel_map = "" - self.get_logger().info("accel map is empty") - - brake_map_name = Path(self.calibrated_map_dir + "brake_map.csv") - if brake_map_name.exists(): - with open(self.calibrated_map_dir + "brake_map.csv", "r") as calibrated_brake_map: - for brake_data in calibrated_brake_map: - response.brake_map += brake_data - self.get_logger().info("brake map is packed") - else: - response.brake_map = "" - self.get_logger().info("brake map is empty") - - return response - - def plotter_function(self): - return self.plotter - - def view_pedal_accel_graph( - self, - plotter, - subplot_num, - velocity_map_list, - vel_list_idx, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - ): - fig = plotter.subplot_more(subplot_num) - - # calibrated map - if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: - plotter.plot( - calibrated_pedal_list[vel_list_idx], - calibrated_acc_list[vel_list_idx], - color="blue", - label="calibrated", - ) - - # default map - if len(default_pedal_list) != 0 and len(default_acc_list) != 0: - plotter.plot( - default_pedal_list[vel_list_idx], - default_acc_list[vel_list_idx], - color="orange", - label="default", - linestyle="dashed", - ) - - # plot all data - pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] - if velocity_map_list[vel_list_idx] is not None: - plotter.scatter_color( - velocity_map_list[vel_list_idx][:, 1], - velocity_map_list[vel_list_idx][:, 2], - color=velocity_map_list[vel_list_idx][:, 3], - label="all", - ) - - for pedal in velocity_map_list[vel_list_idx][:, 1]: - min_pedal = 10 - for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): - if min_pedal > abs(pedal - ref_pedal): - min_pedal = abs(pedal - ref_pedal) - min_pedal_idx = pedal_idx - pedal_list[min_pedal_idx] += 1 - - # plot average data - plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") - - # add label of standard deviation - plotter.scatter([], [], "black", label="std dev") - - # plot average text - for i in range(len(CF.PEDAL_LIST)): - if count_map[i, vel_list_idx] == 0: - continue - x = CF.PEDAL_LIST[i] - y = average_map[i, vel_list_idx] - y2 = stddev_map[i, vel_list_idx] - # plot average - plotter.plot_text(x, y + 1, y, color="red") - - # plot standard deviation - plotter.plot_text(x, y - 1, y2, color="black") - - # plot the number of all data - plotter.plot_text( - x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" - ) - - pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] - accel_lim = [-5.0, 5.0] - - plotter.set_lim(fig, pedal_lim, accel_lim) - plotter.add_label( - str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" - ) - - def load_map(self, csv_dir): - try: - accel_pedal_list = [] - accel_acc_list = [] - with open(csv_dir + "accel_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - accel_pedal_list.append([float(w[0]) for e in w[1:]]) - accel_acc_list.append([float(e) for e in w[1:]]) - - brake_pedal_list = [] - brake_acc_list = [] - with open(csv_dir + "brake_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - brake_pedal_list.append([-float(w[0]) for e in w[1:]]) - brake_acc_list.append([float(e) for e in w[1:]]) - - return np.hstack( - [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] - ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) - except OSError as e: - print(e) - return [], [] - - -def main(args=None): - rclpy.init(args=None) - node = DrawGraph() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/plotter.py b/vehicle/accel_brake_map_calibrator/scripts/plotter.py deleted file mode 100755 index 73e4cf54..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/plotter.py +++ /dev/null @@ -1,123 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import matplotlib.pyplot as plt -import numpy as np - - -class Plotter(object): - def __init__(self, total_height, total_width, plot=True): - self.total_height = total_height - self.total_width = total_width - self.plot_flag = True - self.fig = plt.figure(dpi=100, figsize=(24, 18)) - plt.subplots_adjust(left=0.04, right=0.98, bottom=0.05, top=0.95, wspace=0.1, hspace=0.4) - - def subplot(self, plot_num): - fig = plt.subplot(self.total_height, self.total_width, plot_num) - return fig - - def subplot_more(self, plot_num): - width = plot_num % self.total_width - height = int((plot_num - width) / self.total_width) - fig = plt.subplot2grid((self.total_height, self.total_width), (height, width)) - return fig - - def plot_1d(self, data, color, label): - plt.plot(data, color, label=label) - - def plot(self, x_data, y_data, color, label=None, linestyle="solid"): - plt.plot(x_data, y_data, color, label=label, linestyle=linestyle, zorder=-1) - - def scatter(self, x, y, color, label=None, colorbar=False): - plt.scatter(x, y, c=color, label=label, s=3) - - def scatter_color(self, x, y, color, cmap=None, label=None): - sc = plt.scatter(x, y, c=color, label=label, cmap=cmap, s=3) - plt.colorbar(sc) - - def plot_text(self, x, y, val, num_data_type="float", color="black"): - if num_data_type == "float": - plt.text( - x, - y, - "{0:.2f}".format(float(val)), - horizontalalignment="center", - verticalalignment="center", - color=color, - clip_on=True, - fontsize=8, - ) - elif num_data_type == "int": - plt.text( - x, - y, - "{0:d}".format(int(val)), - horizontalalignment="center", - verticalalignment="center", - color=color, - clip_on=True, - fontsize=8, - ) - elif num_data_type == "str": - plt.text( - x, - y, - val, - horizontalalignment="center", - verticalalignment="center", - color=color, - clip_on=True, - fontsize=8, - ) - - def imshow(self, data, left, right, ws, bottom, top, hs, num_data_type="float"): - wm = (right - left) / data.shape[1] - hm = (top - bottom) / data.shape[0] - hwm = wm / 2.0 - hhm = hm / 2.0 - hws = ws / 2.0 - hhs = hs / 2.0 - width_range = np.linspace(bottom - hhs + hhm, top + hhs - hhm, data.shape[0]) - height_range = np.linspace(left - hws + hwm, right + hws - hwm, data.shape[1]) - plt.imshow( - data, - extent=(left - hws, right + hws, bottom - hhs, top + hhs), - aspect="auto", - origin="lower", - ) - ys, xs = np.meshgrid(width_range, height_range, indexing="ij") - for x, y, val in zip(xs.flatten(), ys.flatten(), data.flatten()): - self.plot_text(x, y, val, num_data_type) - plt.colorbar() - - def set_lim(self, fig, xlim, ylim): - fig.set_xlim(xlim) - fig.set_ylim(ylim) - - def add_label(self, title, xlabel, ylabel): - plt.title(title) - plt.xlabel(xlabel) - plt.ylabel(ylabel) - plt.legend(loc="upper left", fontsize=6, labelspacing=0) - - def show(self): - if self.plot_flag: - plt.show() - - def save(self, file_name): - self.fig.savefig(file_name) diff --git a/vehicle/accel_brake_map_calibrator/scripts/view_plot.py b/vehicle/accel_brake_map_calibrator/scripts/view_plot.py deleted file mode 100755 index 4d713cf4..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/view_plot.py +++ /dev/null @@ -1,316 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import argparse -import math - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import numpy as np -from plotter import Plotter -import rclpy -from rclpy.node import Node - - -class ViewPlot(Node): - def __init__(self, args): - super().__init__("plot_viewer") - default_map_dir = args.default_map_dir - calibrated_map_dir = args.calibrated_map_dir - calibration_method = args.method - scatter_only = args.scatter_only - log_file = args.log_file - min_vel_thr = args.min_vel_thr - vel_diff_thr = args.vel_diff_thr - pedal_diff_thr = args.pedal_diff_thr - max_steer_thr = args.max_steer_thr - max_pitch_thr = args.max_pitch_thr - max_jerk_thr = args.max_jerk_thr - max_pedal_vel_thr = args.max_pedal_vel_thr - - if default_map_dir is None: - package_path = get_package_share_directory("raw_vehicle_cmd_converter") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_default_map_dir", package_path + "/data/default/" - ) - default_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") - .get_parameter_value() - .string_value - ) - - if calibrated_map_dir is None: - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" - ) - calibrated_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") - .get_parameter_value() - .string_value - ) - - if calibration_method is None: - calibration_method = "each_cell" - elif not ((calibration_method == "each_cell") | (calibration_method == "four_cell")): - print("invalid method.") - calibration_method = "each_cell" - - print("default map dir: {}".format(default_map_dir)) - print("calibrated map dir: {}".format(calibrated_map_dir)) - print("calibration method: {}".format(calibration_method)) - - # read csv - self.cr = CSVReader(log_file, csv_type="file") - - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, - calibration_method, - ) - - count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) - velocity_map_list = [] - for i in range(len(CF.VEL_LIST)): - velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) - - default_pedal_list, default_acc_list = self.load_map(default_map_dir) - if len(default_pedal_list) == 0 or len(default_acc_list) == 0: - self.get_logger().warning("No default map file was found in {}".format(default_map_dir)) - - calibrated_pedal_list, calibrated_acc_list = self.load_map(calibrated_map_dir) - if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: - self.get_logger().warning( - "No calibrated map file was found in {}".format(calibrated_map_dir) - ) - - # visualize point from data - plot_width = 3 - plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) - plotter = Plotter(plot_height, plot_width) - for i in range(len(CF.VEL_LIST)): - self.view_pedal_accel_graph( - plotter, - i, - velocity_map_list, - i, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - scatter_only, - ) - plotter.show() - - def plotter_function(self): - return self.plotter - - def view_pedal_accel_graph( - self, - plotter, - subplot_num, - velocity_map_list, - vel_list_idx, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - scatter_only, - ): - fig = plotter.subplot_more(subplot_num) - - if not scatter_only: - # calibrated map - if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: - plotter.plot( - calibrated_pedal_list[vel_list_idx], - calibrated_acc_list[vel_list_idx], - color="blue", - label="calibrated", - ) - - # default map - if len(default_pedal_list) != 0 and len(default_acc_list) != 0: - plotter.plot( - default_pedal_list[vel_list_idx], - default_acc_list[vel_list_idx], - color="orange", - label="default", - linestyle="dashed", - ) - - # plot all data - pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] - if velocity_map_list[vel_list_idx] is not None: - plotter.scatter_color( - velocity_map_list[vel_list_idx][:, 1], - velocity_map_list[vel_list_idx][:, 2], - color=velocity_map_list[vel_list_idx][:, 3], - label="all", - ) - - for pedal in velocity_map_list[vel_list_idx][:, 1]: - min_pedal = 10 - for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): - if min_pedal > abs(pedal - ref_pedal): - min_pedal = abs(pedal - ref_pedal) - min_pedal_idx = pedal_idx - pedal_list[min_pedal_idx] += 1 - - # plot average data - plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") - - # add label of standard deviation - plotter.scatter([], [], "black", label="std dev") - - # plot average text - for i in range(len(CF.PEDAL_LIST)): - if count_map[i, vel_list_idx] == 0: - continue - x = CF.PEDAL_LIST[i] - y = average_map[i, vel_list_idx] - y2 = stddev_map[i, vel_list_idx] - # plot average - plotter.plot_text(x, y + 1, y, color="red") - - # plot standard deviation - plotter.plot_text(x, y - 1, y2, color="black") - - # plot the number of all data - plotter.plot_text( - x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" - ) - - pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] - accel_lim = [-5.0, 5.0] - - plotter.set_lim(fig, pedal_lim, accel_lim) - plotter.add_label( - str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" - ) - - def load_map(self, csv_dir): - try: - accel_pedal_list = [] - accel_acc_list = [] - with open(csv_dir + "accel_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - accel_pedal_list.append([float(w[0]) for e in w[1:]]) - accel_acc_list.append([float(e) for e in w[1:]]) - - brake_pedal_list = [] - brake_acc_list = [] - with open(csv_dir + "brake_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - brake_pedal_list.append([-float(w[0]) for e in w[1:]]) - brake_acc_list.append([float(e) for e in w[1:]]) - - return np.hstack( - [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] - ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) - except OSError as e: - print(e) - return [], [] - - -def main(args=None): - rclpy.init(args=None) - node = ViewPlot(args) - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - package_path = get_package_share_directory("accel_brake_map_calibrator") - parser = argparse.ArgumentParser() - parser.add_argument( - "-d", "--default-map-dir", default=None, type=str, help="directory of default map" - ) - parser.add_argument( - "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" - ) - parser.add_argument( - "-m", - "--method", - default=None, - type=str, - help="calibration method : each_cell or four_cell", - ) - parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") - parser.add_argument( - "-l", - "--log-file", - default=package_path + "/config/log.csv", - type=str, - help="path of log.csv", - ) - parser.add_argument( - "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" - ) - parser.add_argument( - "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" - ) - parser.add_argument( - "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" - ) - parser.add_argument( - "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" - ) - parser.add_argument( - "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" - ) - parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") - parser.add_argument( - "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" - ) - - args = parser.parse_args() - main(args) diff --git a/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py b/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py deleted file mode 100755 index 83cfb869..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py +++ /dev/null @@ -1,135 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -from plotter import Plotter -import rclpy -from rclpy.node import Node - - -class ViewPlot(Node): - def __init__(self): - super().__init__("statistics_viewer") - # get parameter - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter("log_file", package_path + "/config/log.csv") - log_file = self.get_parameter("log_file").get_parameter_value().string_value - self.declare_parameter("velocity_min_threshold", 0.1) - min_vel_thr = ( - self.get_parameter("velocity_min_threshold").get_parameter_value().double_value - ) - self.declare_parameter("velocity_diff_threshold", 0.556) - vel_diff_thr = ( - self.get_parameter("velocity_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("pedal_diff_threshold", 0.03) - pedal_diff_thr = ( - self.get_parameter("pedal_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("max_steer_threshold", 0.2) - max_steer_thr = self.get_parameter("max_steer_threshold").get_parameter_value().double_value - self.declare_parameter("max_pitch_threshold", 0.02) - max_pitch_thr = self.get_parameter("max_pitch_threshold").get_parameter_value().double_value - self.declare_parameter("max_jerk_threshold", 0.7) - max_jerk_thr = self.get_parameter("max_jerk_threshold").get_parameter_value().double_value - self.declare_parameter("pedal_velocity_thresh", 0.15) - max_pedal_vel_thr = ( - self.get_parameter("pedal_velocity_thresh").get_parameter_value().double_value - ) - - # read csv - self.cr = CSVReader(log_file, csv_type="file") - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, - ) - - count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) - - # visualization - plotter = Plotter(1, 3) - plotter.subplot(1) - plotter.imshow( - average_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - ) - plotter.add_label("average of accel", "velocity(kmh)", "throttle") - - plotter.subplot(2) - plotter.imshow( - stddev_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - ) - plotter.add_label("std. dev. of accel", "velocity(kmh)", "throttle") - - plotter.subplot(3) - plotter.imshow( - count_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - num_data_type="int", - ) - plotter.add_label("number of accel data", "velocity(kmh)", "throttle") - plotter.show() - - -def main(args=None): - rclpy.init(args=args) - node = ViewPlot() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp deleted file mode 100644 index ecfc312d..00000000 --- a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ /dev/null @@ -1,1692 +0,0 @@ -// -// 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/accel_brake_map_calibrator_node.hpp" - -#include "rclcpp/logging.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace accel_brake_map_calibrator -{ - -AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) -: Node("accel_brake_map_calibrator", node_options) -{ - transform_listener_ = std::make_shared(this); - // get parameter - update_hz_ = declare_parameter("update_hz", 10.0); - covariance_ = declare_parameter("initial_covariance", 0.05); - velocity_min_threshold_ = declare_parameter("velocity_min_threshold", 0.1); - velocity_diff_threshold_ = declare_parameter("velocity_diff_threshold", 0.556); - pedal_diff_threshold_ = declare_parameter("pedal_diff_threshold", 0.03); - max_steer_threshold_ = declare_parameter("max_steer_threshold", 0.2); - max_pitch_threshold_ = declare_parameter("max_pitch_threshold", 0.02); - max_jerk_threshold_ = declare_parameter("max_jerk_threshold", 0.7); - pedal_velocity_thresh_ = declare_parameter("pedal_velocity_thresh", 0.15); - max_accel_ = declare_parameter("max_accel", 5.0); - min_accel_ = declare_parameter("min_accel", -5.0); - pedal_to_accel_delay_ = declare_parameter("pedal_to_accel_delay", 0.3); - max_data_count_ = declare_parameter("max_data_count", 200); - pedal_accel_graph_output_ = declare_parameter("pedal_accel_graph_output", false); - progress_file_output_ = declare_parameter("progress_file_output", false); - precision_ = declare_parameter("precision", 3); - const auto get_pitch_method_str = declare_parameter("get_pitch_method", std::string("tf")); - if (get_pitch_method_str == std::string("tf")) { - get_pitch_method_ = GET_PITCH_METHOD::TF; - } else if (get_pitch_method_str == std::string("none")) { - get_pitch_method_ = GET_PITCH_METHOD::NONE; - } else { - RCLCPP_ERROR_STREAM(get_logger(), "update_method_ is wrong. (available method: tf, file, none"); - return; - } - const auto accel_brake_value_source_str = - declare_parameter("accel_brake_value_source", std::string("status")); - if (accel_brake_value_source_str == std::string("status")) { - accel_brake_value_source_ = ACCEL_BRAKE_SOURCE::STATUS; - } else if (accel_brake_value_source_str == std::string("command")) { - accel_brake_value_source_ = ACCEL_BRAKE_SOURCE::COMMAND; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), "accel_brake_value_source is wrong. (available source: status, command"); - return; - } - - update_suggest_thresh_ = declare_parameter("update_suggest_thresh", 0.7); - csv_calibrated_map_dir_ = declare_parameter("csv_calibrated_map_dir", std::string("")); - output_accel_file_ = csv_calibrated_map_dir_ + "/accel_map.csv"; - output_brake_file_ = csv_calibrated_map_dir_ + "/brake_map.csv"; - const std::string update_method_str = - declare_parameter("update_method", std::string("update_offset_each_cell")); - if (update_method_str == std::string("update_offset_each_cell")) { - update_method_ = UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL; - } else if (update_method_str == std::string("update_offset_total")) { - update_method_ = UPDATE_METHOD::UPDATE_OFFSET_TOTAL; - } else if (update_method_str == std::string("update_offset_four_cell_around")) { - update_method_ = UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "update_method is wrong. (available method: update_offset_each_cell, update_offset_total"); - return; - } - // initializer - - // QoS setup - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - - // Publisher for checkUpdateSuggest - calibration_status_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/calibration_status", durable_qos); - - /* Diagnostic Updater */ - updater_ptr_ = std::make_shared(this, 1.0 / update_hz_); - updater_ptr_->setHardwareID("accel_brake_map_calibrator"); - updater_ptr_->add( - "accel_brake_map_calibrator", this, &AccelBrakeMapCalibrator::checkUpdateSuggest); - - { - csv_default_map_dir_ = declare_parameter("csv_default_map_dir", std::string("")); - - std::string csv_path_accel_map = csv_default_map_dir_ + "/accel_map.csv"; - std::string csv_path_brake_map = csv_default_map_dir_ + "/brake_map.csv"; - if ( - !accel_map_.readAccelMapFromCSV(csv_path_accel_map) || - !new_accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { - is_default_map_ = false; - RCLCPP_ERROR_STREAM( - get_logger(), - "Cannot read accelmap. csv path = " << csv_path_accel_map.c_str() << ". stop calculation."); - return; - } - if ( - !brake_map_.readBrakeMapFromCSV(csv_path_brake_map) || - !new_brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { - is_default_map_ = false; - RCLCPP_ERROR_STREAM( - get_logger(), - "Cannot read brakemap. csv path = " << csv_path_brake_map.c_str() << ". stop calculation."); - return; - } - } - - std::string output_log_file = declare_parameter("output_log_file", std::string("")); - output_log_.open(output_log_file); - addIndexToCSV(&output_log_); - - debug_values_.data.resize(num_debug_values_); - - // input map info - accel_map_value_ = accel_map_.getAccelMap(); - brake_map_value_ = brake_map_.getBrakeMap(); - accel_vel_index_ = accel_map_.getVelIdx(); - brake_vel_index_ = brake_map_.getVelIdx(); - accel_pedal_index_ = accel_map_.getThrottleIdx(); - brake_pedal_index_ = brake_map_.getBrakeIdx(); - update_accel_map_value_.resize((accel_map_value_.size())); - update_brake_map_value_.resize((brake_map_value_.size())); - for (auto & m : update_accel_map_value_) { - m.resize(accel_map_value_.at(0).size()); - } - for (auto & m : update_brake_map_value_) { - m.resize(brake_map_value_.at(0).size()); - } - accel_offset_covariance_value_.resize((accel_map_value_.size())); - brake_offset_covariance_value_.resize((brake_map_value_.size())); - for (auto & m : accel_offset_covariance_value_) { - m.resize(accel_map_value_.at(0).size(), covariance_); - } - for (auto & m : brake_offset_covariance_value_) { - m.resize(brake_map_value_.at(0).size(), covariance_); - } - map_value_data_.resize(accel_map_value_.size() + brake_map_value_.size() - 1); - for (auto & m : map_value_data_) { - m.resize(accel_map_value_.at(0).size()); - } - - std::copy(accel_map_value_.begin(), accel_map_value_.end(), update_accel_map_value_.begin()); - std::copy(brake_map_value_.begin(), brake_map_value_.end(), update_brake_map_value_.begin()); - - // initialize matrix for covariance calculation - { - const auto genConstMat = [](const Map & map, const auto val) { - return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val); - }; - accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_); - brake_data_mean_mat_ = genConstMat(brake_map_value_, map_offset_); - accel_data_covariance_mat_ = genConstMat(accel_map_value_, covariance_); - brake_data_covariance_mat_ = genConstMat(brake_map_value_, covariance_); - accel_data_num_ = genConstMat(accel_map_value_, 1); - brake_data_num_ = genConstMat(brake_map_value_, 1); - } - - // publisher - update_suggest_pub_ = - create_publisher("~/output/update_suggest", durable_qos); - original_map_occ_pub_ = create_publisher("~/debug/original_occ_map", durable_qos); - update_map_occ_pub_ = create_publisher("~/debug/update_occ_map", durable_qos); - data_ave_pub_ = create_publisher("~/debug/data_average_occ_map", durable_qos); - data_std_pub_ = create_publisher("~/debug/data_std_dev_occ_map", durable_qos); - data_count_pub_ = create_publisher("~/debug/data_count_occ_map", durable_qos); - data_count_with_self_pose_pub_ = - create_publisher("~/debug/data_count_self_pose_occ_map", durable_qos); - index_pub_ = create_publisher("~/debug/occ_index", durable_qos); - original_map_raw_pub_ = - create_publisher("~/debug/original_raw_map", durable_qos); - update_map_raw_pub_ = create_publisher("~/output/update_raw_map", durable_qos); - debug_pub_ = create_publisher("~/output/debug_values", durable_qos); - current_map_error_pub_ = - create_publisher("~/output/current_map_error", durable_qos); - updated_map_error_pub_ = - create_publisher("~/output/updated_map_error", durable_qos); - map_error_ratio_pub_ = create_publisher("~/output/map_error_ratio", durable_qos); - offset_covariance_pub_ = - create_publisher("~/debug/offset_covariance", durable_qos); - - // subscriber - using std::placeholders::_1; - using std::placeholders::_2; - using std::placeholders::_3; - - velocity_sub_ = create_subscription( - "~/input/velocity", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1)); - steer_sub_ = create_subscription( - "~/input/steer", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackSteer, this, _1)); - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { - actuation_status_sub_ = create_subscription( - "~/input/actuation_status", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationStatus, this, _1)); - } - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { - actuation_cmd_sub_ = create_subscription( - "~/input/actuation_cmd", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationCommand, this, _1)); - } - - // Service - update_map_dir_server_ = create_service( - "~/input/update_map_dir", - std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); - - // timer - initTimer(1.0 / update_hz_); - initOutputCSVTimer(30.0); - - logger_configure_ = std::make_unique(this); -} - -void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_output_csv_ = rclcpp::create_timer( - this, get_clock(), period_ns, - std::bind(&AccelBrakeMapCalibrator::timerCallbackOutputCSV, this)); -} - -void AccelBrakeMapCalibrator::initTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AccelBrakeMapCalibrator::timerCallback, this)); -} - -bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) -{ - if (get_pitch_method_ == GET_PITCH_METHOD::NONE) { - // do not get pitch from tf - *pitch = 0.0; - return true; - } - - // get tf - const auto transform = transform_listener_->getTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); - if (!transform) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "cannot get map to base_link transform. "); - return false; - } - double roll, raw_pitch, yaw; - tf2::getEulerYPR(transform->transform.rotation, roll, raw_pitch, yaw); - debug_values_.data.at(CURRENT_RAW_PITCH) = raw_pitch; - *pitch = lowpass(*pitch, raw_pitch, 0.2); - debug_values_.data.at(CURRENT_PITCH) = *pitch; - return true; -} - -void AccelBrakeMapCalibrator::timerCallback() -{ - update_count_++; - - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, - "map updating... count: " << update_success_count_ << " / " << update_count_ << "\n\t" - << "lack_of_data_count: " << lack_of_data_count_ << "\n\t" - << " failed_to_get_pitch_count: " << failed_to_get_pitch_count_ - << "\n\t" - << "too_large_pitch_count: " << too_large_pitch_count_ << "\n\t" - << " too_low_speed_count: " << too_low_speed_count_ << "\n\t" - << "too_large_steer_count: " << too_large_steer_count_ << "\n\t" - << "too_large_jerk_count: " << too_large_jerk_count_ << "\n\t" - << "invalid_acc_brake_count: " << invalid_acc_brake_count_ << "\n\t" - << "too_large_pedal_spd_count: " << too_large_pedal_spd_count_ - << "\n\t" - << "update_fail_count_: " << update_fail_count_ << "\n"); - - /* valid check */ - - // data check - if ( - !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ || - !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) { - // lack of data - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)"); - lack_of_data_count_++; - return; - } - - // data check 2 - if ( - isTimeout(twist_ptr_->header.stamp, timeout_sec_) || - isTimeout(steer_ptr_->stamp, timeout_sec_) || isTimeout(accel_pedal_ptr_, timeout_sec_) || - isTimeout(brake_pedal_ptr_, timeout_sec_)) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "timeout of topics (twist, steer, accel, brake)"); - lack_of_data_count_++; - return; - } - - // data check 3 - if (!getCurrentPitchFromTF(&pitch_)) { - // cannot get pitch - RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot get pitch"); - failed_to_get_pitch_count_++; - return; - } - - /* write data to log */ - if (pedal_accel_graph_output_) { - addLogToCSV( - &output_log_, rclcpp::Time(twist_ptr_->header.stamp).seconds(), twist_ptr_->twist.linear.x, - acceleration_, getPitchCompensatedAcceleration(), delayed_accel_pedal_ptr_->data, - delayed_brake_pedal_ptr_->data, accel_pedal_speed_, brake_pedal_speed_, pitch_, - steer_ptr_->steering_tire_angle, jerk_, full_original_accel_rmse_, part_original_accel_rmse_, - new_accel_rmse_); - } - - /* publish map & debug_values*/ - publishMap(accel_map_value_, brake_map_value_, "original"); - publishMap(update_accel_map_value_, update_brake_map_value_, "update"); - publishOffsetCovMap(accel_offset_covariance_value_, brake_offset_covariance_value_); - publishCountMap(); - publishIndex(); - publishUpdateSuggestFlag(); - debug_pub_->publish(debug_values_); - publishFloat32("current_map_error", part_original_accel_rmse_); - publishFloat32("updated_map_error", new_accel_rmse_); - publishFloat32( - "map_error_ratio", - part_original_accel_rmse_ != 0.0 ? new_accel_rmse_ / part_original_accel_rmse_ : 1.0); - - // -- processing start -- - - /* initialize */ - debug_values_.data.at(SUCCESS_TO_UPDATE) = false; - update_success_ = false; - - // twist check - if (twist_ptr_->twist.linear.x < velocity_min_threshold_) { - // too low speed ( or backward velocity) - too_low_speed_count_++; - return; - } - - // accel / brake map evaluation (do not evaluate when the car stops) - executeEvaluation(); - - // pitch check - if (std::fabs(pitch_) > max_pitch_threshold_) { - // too large pitch - too_large_pitch_count_++; - return; - } - - // steer check - if (std::fabs(steer_ptr_->steering_tire_angle) > max_steer_threshold_) { - // too large steer - too_large_steer_count_++; - return; - } - - // jerk check - if (std::fabs(jerk_) > max_jerk_threshold_) { - // too large jerk - too_large_jerk_count_++; - return; - } - - // pedal check - if ( - delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon() && - delayed_brake_pedal_ptr_->data > std::numeric_limits::epsilon()) { - // both (accel/brake) output - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, - "invalid pedal value (Both of accel output and brake output area not zero. )"); - invalid_acc_brake_count_++; - return; - } - - // pedal speed check - if ( - std::fabs(accel_pedal_speed_) > pedal_velocity_thresh_ || - std::fabs(brake_pedal_speed_) > pedal_velocity_thresh_) { - // too large pedal speed - too_large_pedal_spd_count_++; - return; - } - - /* update map */ - if (updateAccelBrakeMap()) { - update_success_count_++; - debug_values_.data.at(SUCCESS_TO_UPDATE) = true; - update_success_ = true; - } else { - update_fail_count_++; - } -} - -void AccelBrakeMapCalibrator::timerCallbackOutputCSV() -{ - // write accel/ brake map to file - const auto ros_time = std::to_string(this->now().seconds()); - writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, output_accel_file_); - writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, output_brake_file_); - if (progress_file_output_) { - writeMapToCSV( - accel_vel_index_, accel_pedal_index_, update_accel_map_value_, - output_accel_file_ + "_" + ros_time); - writeMapToCSV( - brake_vel_index_, brake_pedal_index_, update_brake_map_value_, - output_brake_file_ + "_" + ros_time); - writeMapToCSV( - accel_vel_index_, accel_pedal_index_, accel_map_value_, output_accel_file_ + "_original"); - writeMapToCSV( - brake_vel_index_, brake_pedal_index_, brake_map_value_, output_brake_file_ + "_original"); - } - - // update newest accel / brake map - // check file existence - std::ifstream af(output_accel_file_); - std::ifstream bf(output_brake_file_); - if (!af.is_open() || !bf.is_open()) { - RCLCPP_WARN(get_logger(), "Accel map or brake map does not exist"); - return; - } - - new_accel_map_ = AccelMap(); - if (!new_accel_map_.readAccelMapFromCSV(output_accel_file_)) { - RCLCPP_WARN(get_logger(), "Cannot read accelmap. csv path = %s. ", output_accel_file_.c_str()); - } - new_brake_map_ = BrakeMap(); - if (!new_brake_map_.readBrakeMapFromCSV(output_brake_file_)) { - RCLCPP_WARN(get_logger(), "Cannot read brakemap. csv path = %s. ", output_brake_file_.c_str()); - } -} - -void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstSharedPtr msg) -{ - // convert velocity-report to twist-stamped - auto twist_msg = std::make_shared(); - twist_msg->header = msg->header; - twist_msg->twist.linear.x = msg->longitudinal_velocity; - twist_msg->twist.linear.y = msg->lateral_velocity; - twist_msg->twist.angular.z = msg->heading_rate; - - if (!twist_vec_.empty()) { - const auto past_msg = getNearestTimeDataFromVec(twist_msg, dif_twist_time_, twist_vec_); - const double raw_acceleration = getAccel(past_msg, twist_msg); - acceleration_ = lowpass(acceleration_, raw_acceleration, 0.25); - acceleration_time_ = rclcpp::Time(msg->header.stamp).seconds(); - debug_values_.data.at(CURRENT_RAW_ACCEL) = raw_acceleration; - debug_values_.data.at(CURRENT_ACCEL) = acceleration_; - - // calculate jerk - if ( - this->now().seconds() - pre_acceleration_time_ > timeout_sec_ || - (acceleration_time_ - pre_acceleration_time_) <= std::numeric_limits::epsilon()) { - RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot calculate jerk"); - // does not update jerk - } else { - const double raw_jerk = getJerk(); - // to avoid to get delayed jerk, set high gain - jerk_ = lowpass(jerk_, raw_jerk, 0.5); - } - debug_values_.data.at(CURRENT_JERK) = jerk_; - pre_acceleration_ = acceleration_; - pre_acceleration_time_ = acceleration_time_; - } - - debug_values_.data.at(CURRENT_SPEED) = twist_msg->twist.linear.x; - twist_ptr_ = twist_msg; - pushDataToVec(twist_msg, twist_vec_max_size_, &twist_vec_); -} - -void AccelBrakeMapCalibrator::callbackSteer(const SteeringReport::ConstSharedPtr msg) -{ - debug_values_.data.at(CURRENT_STEER) = msg->steering_tire_angle; - steer_ptr_ = msg; -} - -void AccelBrakeMapCalibrator::callbackActuation( - const std_msgs::msg::Header header, const double accel, const double brake) -{ - // get accel data - accel_pedal_ptr_ = std::make_shared(accel, rclcpp::Time(header.stamp)); - if (!accel_pedal_vec_.empty()) { - const auto past_accel_ptr = - getNearestTimeDataFromVec(accel_pedal_ptr_, dif_pedal_time_, accel_pedal_vec_); - const double raw_accel_pedal_speed = - getPedalSpeed(past_accel_ptr, accel_pedal_ptr_, accel_pedal_speed_); - accel_pedal_speed_ = lowpass(accel_pedal_speed_, raw_accel_pedal_speed, 0.5); - debug_values_.data.at(CURRENT_RAW_ACCEL_SPEED) = raw_accel_pedal_speed; - debug_values_.data.at(CURRENT_ACCEL_SPEED) = accel_pedal_speed_; - } - debug_values_.data.at(CURRENT_ACCEL_PEDAL) = accel_pedal_ptr_->data; - pushDataToVec(accel_pedal_ptr_, pedal_vec_max_size_, &accel_pedal_vec_); - delayed_accel_pedal_ptr_ = - getNearestTimeDataFromVec(accel_pedal_ptr_, pedal_to_accel_delay_, accel_pedal_vec_); - - // get brake data - brake_pedal_ptr_ = std::make_shared(brake, rclcpp::Time(header.stamp)); - if (!brake_pedal_vec_.empty()) { - const auto past_brake_ptr = - getNearestTimeDataFromVec(brake_pedal_ptr_, dif_pedal_time_, brake_pedal_vec_); - const double raw_brake_pedal_speed = - getPedalSpeed(past_brake_ptr, brake_pedal_ptr_, brake_pedal_speed_); - brake_pedal_speed_ = lowpass(brake_pedal_speed_, raw_brake_pedal_speed, 0.5); - debug_values_.data.at(CURRENT_RAW_BRAKE_SPEED) = raw_brake_pedal_speed; - debug_values_.data.at(CURRENT_BRAKE_SPEED) = brake_pedal_speed_; - } - debug_values_.data.at(CURRENT_BRAKE_PEDAL) = brake_pedal_ptr_->data; - pushDataToVec(brake_pedal_ptr_, pedal_vec_max_size_, &brake_pedal_vec_); - delayed_brake_pedal_ptr_ = - getNearestTimeDataFromVec(brake_pedal_ptr_, pedal_to_accel_delay_, brake_pedal_vec_); -} - -void AccelBrakeMapCalibrator::callbackActuationCommand( - const ActuationCommandStamped::ConstSharedPtr msg) -{ - const auto header = msg->header; - const auto accel = msg->actuation.accel_cmd; - const auto brake = msg->actuation.brake_cmd; - callbackActuation(header, accel, brake); -} - -void AccelBrakeMapCalibrator::callbackActuationStatus( - const ActuationStatusStamped::ConstSharedPtr msg) -{ - const auto header = msg->header; - const auto accel = msg->status.accel_status; - const auto brake = msg->status.brake_status; - callbackActuation(header, accel, brake); -} - -bool AccelBrakeMapCalibrator::callbackUpdateMapService( - [[maybe_unused]] const std::shared_ptr request_header, - UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res) -{ - // if req.path is input, use this as the directory to set updated maps - std::string update_map_dir = req->path.empty() ? csv_default_map_dir_ : req->path; - - RCLCPP_INFO_STREAM(get_logger(), "update accel/brake map. directory: " << update_map_dir); - const auto accel_map_file = update_map_dir + "/accel_map.csv"; - const auto brake_map_file = update_map_dir + "/brake_map.csv"; - if ( - writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, accel_map_file) && - writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, brake_map_file)) { - res->success = true; - res->message = - "Data has been successfully saved on " + update_map_dir + "/(accel/brake)_map.csv"; - } else { - res->success = false; - res->message = "Failed to save data. Maybe invalid path?"; - } - return true; -} - -double AccelBrakeMapCalibrator::lowpass( - const double original, const double current, const double gain) -{ - return current * gain + original * (1.0 - gain); -} - -double AccelBrakeMapCalibrator::getPedalSpeed( - const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, - const double prev_pedal_speed) -{ - const double dt = (current_pedal->data_time - prev_pedal->data_time).seconds(); - if (dt < 1e-03) { - // invalid pedal info. return prev pedal speed - return prev_pedal_speed; - } - - const double d_pedal = current_pedal->data - prev_pedal->data; - return d_pedal / dt; -} - -double AccelBrakeMapCalibrator::getAccel( - const TwistStamped::ConstSharedPtr & prev_twist, - const TwistStamped::ConstSharedPtr & current_twist) -{ - const double dt = - (rclcpp::Time(current_twist->header.stamp) - rclcpp::Time(prev_twist->header.stamp)).seconds(); - if (dt < 1e-03) { - // invalid twist. return prev acceleration - return acceleration_; - } - const double dv = current_twist->twist.linear.x - prev_twist->twist.linear.x; - return std::min(std::max(min_accel_, dv / dt), max_accel_); -} - -double AccelBrakeMapCalibrator::getJerk() -{ - const double jerk = - (acceleration_ - pre_acceleration_) / (acceleration_time_ - pre_acceleration_time_); - return std::min(std::max(-max_jerk_, jerk), max_jerk_); -} - -bool AccelBrakeMapCalibrator::indexValueSearch( - const std::vector value_index, const double value, const double value_thresh, - int * searched_index) -{ - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - /* return lower left index. - +-------+ +:grid - | * | *:given data - | | o:index to return - o-------+ - */ - if (value < 0) { - std::cerr << "error : invalid value." << std::endl; - return false; - } - double old_diff_value = -999; // TODO(Hirano) - for (std::size_t i = 0; i < value_index.size(); i++) { - const double diff_value = value_index.at(i) - value; - if (diff_value <= 0 && old_diff_value < diff_value) { - *searched_index = i; - old_diff_value = diff_value; - } else { - return true; - } - } - } else { - for (std::size_t i = 0; i < value_index.size(); i++) { - const double diff_value = std::fabs(value_index.at(i) - value); - if (diff_value <= value_thresh) { - *searched_index = i; - return true; - } - } - } - return false; -} - -int AccelBrakeMapCalibrator::nearestValueSearch( - const std::vector value_index, const double value) -{ - double max_dist = std::numeric_limits::max(); - int nearest_idx = 0; - - for (std::size_t i = 0; i < value_index.size(); i++) { - const double dist = std::fabs(value - value_index.at(i)); - if (max_dist > dist) { - nearest_idx = i; - max_dist = dist; - } - } - return nearest_idx; -} - -int AccelBrakeMapCalibrator::nearestPedalSearch() -{ - bool accel_mode; - - if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { - accel_mode = true; - } else { - accel_mode = false; - } - - int nearest_idx; - if (accel_mode) { - nearest_idx = nearestValueSearch(accel_pedal_index_, delayed_accel_pedal_ptr_->data); - } else { - nearest_idx = nearestValueSearch(brake_pedal_index_, delayed_brake_pedal_ptr_->data); - } - - return getUnifiedIndexFromAccelBrakeIndex(accel_mode, nearest_idx); -} - -int AccelBrakeMapCalibrator::nearestVelSearch() -{ - const double current_vel = twist_ptr_->twist.linear.x; - return nearestValueSearch(accel_vel_index_, current_vel); -} - -void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() -{ - const double bit = std::pow(1e-01, precision_); - for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { - const double current_acc = update_accel_map_value_.at(ped_idx).at(vel_idx); - const double next_ped_acc = update_accel_map_value_.at(ped_idx + 1).at(vel_idx); - const double next_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx + 1); - - if (current_acc <= next_vel_acc) { - // the higher the velocity, the lower the acceleration - update_accel_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; - } - - if (current_acc >= next_ped_acc) { - // the higher the accel pedal, the higher the acceleration - update_accel_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc + bit; - } - } - } -} - -void AccelBrakeMapCalibrator::takeConsistencyOfBrakeMap() -{ - const double bit = std::pow(1e-01, precision_); - for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_brake_map_value_.at(0).size() - 1; vel_idx++) { - const double current_acc = update_brake_map_value_.at(ped_idx).at(vel_idx); - const double next_ped_acc = update_brake_map_value_.at(ped_idx + 1).at(vel_idx); - const double next_vel_acc = update_brake_map_value_.at(ped_idx).at(vel_idx + 1); - - if (current_acc <= next_vel_acc) { - // the higher the velocity, the lower the acceleration - update_brake_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; - } - - if (current_acc <= next_ped_acc) { - // the higher the brake pedal, the lower the acceleration - update_brake_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc - bit; - } - } - } -} - -bool AccelBrakeMapCalibrator::updateAccelBrakeMap() -{ - // get pedal index - bool accel_mode = false; - int accel_pedal_index = 0; - int brake_pedal_index = 0; - int accel_vel_index = 0; - int brake_vel_index = 0; - - if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { - accel_mode = true; - } else { - accel_mode = false; - } - - if ( - accel_mode && !indexValueSearch( - accel_pedal_index_, delayed_accel_pedal_ptr_->data, pedal_diff_threshold_, - &accel_pedal_index)) { - // not match accel pedal output to pedal value in index - return false; - } - - if ( - !accel_mode && !indexValueSearch( - brake_pedal_index_, delayed_brake_pedal_ptr_->data, pedal_diff_threshold_, - &brake_pedal_index)) { - // not match accel pedal output to pedal value in index - return false; - } - - if ( - accel_mode && - !indexValueSearch( - accel_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &accel_vel_index)) { - // not match current velocity to velocity value in index - return false; - } - - if ( - !accel_mode && - !indexValueSearch( - brake_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &brake_vel_index)) { - // not match current velocity to velocity value in index - return false; - } - - // update map - executeUpdate(accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index); - - // when update 0 pedal index, update another map - if (accel_mode && accel_pedal_index == 0) { - // copy accel map value to brake map value - update_brake_map_value_.at(accel_pedal_index).at(accel_vel_index) = - update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index); - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - update_brake_map_value_.at(accel_pedal_index).at(accel_vel_index + 1) = - update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index + 1); - } - } else if (!accel_mode && brake_pedal_index == 0) { - // copy brake map value to accel map value - update_accel_map_value_.at(brake_pedal_index).at(brake_vel_index) = - update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - update_accel_map_value_.at(brake_pedal_index).at(brake_vel_index + 1) = - update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index + 1); - } - } - - // take consistency of map - takeConsistencyOfAccelMap(); - takeConsistencyOfBrakeMap(); - - return true; -} - -void AccelBrakeMapCalibrator::executeUpdate( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index) -{ - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); - const double map_acc = accel_mode - ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) - : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); - RCLCPP_DEBUG_STREAM(get_logger(), "measured_acc: " << measured_acc << ", map_acc: " << map_acc); - - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL) { - updateEachValOffset( - accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, - measured_acc, map_acc); - } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_TOTAL) { - updateTotalMapOffset(measured_acc, map_acc); - } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - updateFourCellAroundOffset( - accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, - measured_acc); - } - - // add accel data to map - accel_mode ? map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(true, accel_pedal_index)) - .at(accel_vel_index) - .emplace_back(measured_acc) - : map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(false, brake_pedal_index)) - .at(brake_vel_index) - .emplace_back(measured_acc); -} - -bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc) -{ - // pre-defined - static Map accel_map_offset_vec_( - accel_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static Map brake_map_offset_vec_( - brake_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static std::vector> accel_covariance_mat_( - accel_map_value_.size() - 1, - std::vector( - accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); - static std::vector> brake_covariance_mat_( - brake_map_value_.size() - 1, - std::vector( - accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); - - auto & update_map_value = accel_mode ? update_accel_map_value_ : update_brake_map_value_; - auto & offset_covariance_value = - accel_mode ? accel_offset_covariance_value_ : brake_offset_covariance_value_; - const auto & map_value = accel_mode ? accel_map_value_ : brake_map_value_; - const auto & pedal_index = accel_mode ? accel_pedal_index : brake_pedal_index; - const auto & pedal_index_ = accel_mode ? accel_pedal_index_ : brake_pedal_index_; - const auto & vel_index = accel_mode ? accel_vel_index : brake_vel_index; - const auto & vel_index_ = accel_mode ? accel_vel_index_ : brake_vel_index_; - const auto & delayed_pedal = - accel_mode ? delayed_accel_pedal_ptr_->data : delayed_brake_pedal_ptr_->data; - auto & map_offset_vec = accel_mode ? accel_map_offset_vec_ : brake_map_offset_vec_; - auto & covariance_mat = accel_mode ? accel_covariance_mat_ : brake_covariance_mat_; - auto & data_mean_mat = accel_mode ? accel_data_mean_mat_ : brake_data_mean_mat_; - auto & data_covariance_mat = accel_mode ? accel_data_covariance_mat_ : brake_data_covariance_mat_; - auto & data_weighted_num = accel_mode ? accel_data_num_ : brake_data_num_; - - const double zll = update_map_value.at(pedal_index + 0).at(vel_index + 0); - const double zhl = update_map_value.at(pedal_index + 1).at(vel_index + 0); - const double zlh = update_map_value.at(pedal_index + 0).at(vel_index + 1); - const double zhh = update_map_value.at(pedal_index + 1).at(vel_index + 1); - - const double xl = pedal_index_.at(pedal_index + 0); - const double xh = pedal_index_.at(pedal_index + 1); - const double rx = (delayed_pedal - xl) / (xh - xl); - const double yl = vel_index_.at(vel_index + 0); - const double yh = vel_index_.at(vel_index + 1); - const double ry = (twist_ptr_->twist.linear.x - yl) / (yh - yl); - - Eigen::Vector4d phi(4); - phi << (1 - rx) * (1 - ry), rx * (1 - ry), (1 - rx) * ry, rx * ry; - - Eigen::Vector4d theta(4); - theta << zll, zhl, zlh, zhh; - - Eigen::Vector4d weighted_sum(4); - weighted_sum << data_weighted_num(pedal_index + 0, vel_index + 0), - data_weighted_num(pedal_index + 1, vel_index + 0), - data_weighted_num(pedal_index + 0, vel_index + 1), - data_weighted_num(pedal_index + 1, vel_index + 1); - - Eigen::Vector4d sigma(4); - sigma << data_covariance_mat(pedal_index + 0, vel_index + 0), - data_covariance_mat(pedal_index + 1, vel_index + 0), - data_covariance_mat(pedal_index + 0, vel_index + 1), - data_covariance_mat(pedal_index + 1, vel_index + 1); - - Eigen::Vector4d mean(4); - mean << data_mean_mat(pedal_index + 0, vel_index + 0), - data_mean_mat(pedal_index + 1, vel_index + 0), data_mean_mat(pedal_index + 0, vel_index + 1), - data_mean_mat(pedal_index + 1, vel_index + 1); - - const int vel_idx_l = vel_index + 0; - const int vel_idx_h = vel_index + 1; - const int ped_idx_l = pedal_index + 0; - const int ped_idx_h = pedal_index + 1; - - Eigen::VectorXd map_offset(4); - map_offset(0) = map_offset_vec.at(ped_idx_l).at(vel_idx_l); - map_offset(1) = map_offset_vec.at(ped_idx_h).at(vel_idx_l); - map_offset(2) = map_offset_vec.at(ped_idx_l).at(vel_idx_h); - map_offset(3) = map_offset_vec.at(ped_idx_h).at(vel_idx_h); - - Eigen::VectorXd updated_map_offset(4); - - Eigen::MatrixXd covariance = covariance_mat.at(ped_idx_l).at(vel_idx_l); - - /* calculate adaptive map offset */ - Eigen::MatrixXd G(4, 4); - Eigen::RowVectorXd phiT(4); - phiT = phi.transpose(); - double rk = phiT * covariance * phi; - - G = covariance * phi / (forgetting_factor_ + rk); - double beta = rk > 0 ? (forgetting_factor_ - (1 - forgetting_factor_) / rk) : 1; - covariance = covariance - covariance * phi * phiT * covariance / (1 / beta + rk); // anti-windup - double eta_hat = phiT * theta; - - const double error_map_offset = measured_acc - eta_hat; - updated_map_offset = map_offset + G * error_map_offset; - - for (int i = 0; i < 4; i++) { - const double pre_mean = mean(i); - mean(i) = (weighted_sum(i) * pre_mean + error_map_offset) / (weighted_sum(i) + 1); - sigma(i) = - (weighted_sum(i) * (sigma(i) + pre_mean * pre_mean) + error_map_offset * error_map_offset) / - (weighted_sum(i) + 1) - - mean(i) * mean(i); - weighted_sum(i) = weighted_sum(i) + 1; - } - - /* input calculated result and update map */ - map_offset_vec.at(ped_idx_l).at(vel_idx_l) = updated_map_offset(0); - map_offset_vec.at(ped_idx_h).at(vel_idx_l) = updated_map_offset(1); - map_offset_vec.at(ped_idx_l).at(vel_idx_h) = updated_map_offset(2); - map_offset_vec.at(ped_idx_h).at(vel_idx_h) = updated_map_offset(3); - - data_covariance_mat(ped_idx_l, vel_idx_l) = sigma(0); - data_covariance_mat(ped_idx_h, vel_idx_l) = sigma(1); - data_covariance_mat(ped_idx_l, vel_idx_h) = sigma(2); - data_covariance_mat(ped_idx_h, vel_idx_h) = sigma(3); - - data_weighted_num(ped_idx_l, vel_idx_l) = weighted_sum(0); - data_weighted_num(ped_idx_h, vel_idx_l) = weighted_sum(1); - data_weighted_num(ped_idx_l, vel_idx_h) = weighted_sum(2); - data_weighted_num(ped_idx_h, vel_idx_h) = weighted_sum(3); - - data_mean_mat(ped_idx_l, vel_idx_l) = mean(0); - data_mean_mat(ped_idx_h, vel_idx_l) = mean(1); - data_mean_mat(ped_idx_l, vel_idx_h) = mean(2); - data_mean_mat(ped_idx_h, vel_idx_h) = mean(3); - - covariance_mat.at(ped_idx_l).at(vel_idx_l) = covariance; - - update_map_value.at(pedal_index + 0).at(vel_index + 0) = - map_value.at(pedal_index + 0).at(vel_index + 0) + map_offset(0); - update_map_value.at(pedal_index + 1).at(vel_index + 0) = - map_value.at(pedal_index + 1).at(vel_index + 0) + map_offset(1); - update_map_value.at(pedal_index + 0).at(vel_index + 1) = - map_value.at(pedal_index + 0).at(vel_index + 1) + map_offset(2); - update_map_value.at(pedal_index + 1).at(vel_index + 1) = - map_value.at(pedal_index + 1).at(vel_index + 1) + map_offset(3); - - offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(0)); - offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(1)); - offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(2)); - offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(3)); - - return true; -} - -bool AccelBrakeMapCalibrator::updateEachValOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc, - const double map_acc) -{ - // pre-defined - static Map map_offset_vec_( - accel_map_value_.size() + brake_map_value_.size() - 1, - std::vector(accel_map_value_.at(0).size(), map_offset_)); - static Map covariance_vec_( - accel_map_value_.size() + brake_map_value_.size() - 1, - std::vector(accel_map_value_.at(0).size(), covariance_)); - - const int vel_idx = accel_mode ? accel_vel_index : brake_vel_index; - int ped_idx = accel_mode ? accel_pedal_index : brake_pedal_index; - ped_idx = getUnifiedIndexFromAccelBrakeIndex(accel_mode, ped_idx); - double map_offset = map_offset_vec_.at(ped_idx).at(vel_idx); - double covariance = covariance_vec_.at(ped_idx).at(vel_idx); - - /* calculate adaptive map offset */ - const double phi = 1.0; - covariance = (covariance - (covariance * phi * phi * covariance) / - (forgetting_factor_ + phi * covariance * phi)) / - forgetting_factor_; - - const double coef = (covariance * phi) / (forgetting_factor_ + phi * covariance * phi); - - const double error_map_offset = measured_acc - map_acc; - map_offset = map_offset + coef * error_map_offset; - - RCLCPP_DEBUG_STREAM( - get_logger(), "index: " << ped_idx << ", " << vel_idx - << ": map_offset_ = " << map_offset_vec_.at(ped_idx).at(vel_idx) - << " -> " << map_offset << "\t" - << " covariance = " << covariance); - - /* input calculated result and update map */ - map_offset_vec_.at(ped_idx).at(vel_idx) = map_offset; - covariance_vec_.at(ped_idx).at(vel_idx) = covariance; - if (accel_mode) { - update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) = - accel_map_value_.at(accel_pedal_index).at(accel_vel_index) + map_offset; - } else { - update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index) = - brake_map_value_.at(brake_pedal_index).at(brake_vel_index) + map_offset; - } - return true; -} - -void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, const double map_acc) -{ - /* calculate adaptive map offset */ - const double phi = 1.0; - covariance_ = (covariance_ - (covariance_ * phi * phi * covariance_) / - (forgetting_factor_ + phi * covariance_ * phi)) / - forgetting_factor_; - - const double coef = (covariance_ * phi) / (forgetting_factor_ + phi * covariance_ * phi); - const double error_map_offset = measured_acc - map_acc; - map_offset_ = map_offset_ + coef * error_map_offset; - - RCLCPP_DEBUG_STREAM( - get_logger(), "map_offset_ = " << map_offset_ << "\t" - << "covariance = " << covariance_); - - /* update map */ - for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { - update_accel_map_value_.at(ped_idx).at(vel_idx) = - accel_map_value_.at(ped_idx).at(vel_idx) + map_offset_; - } - } - for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_brake_map_value_.at(0).size() - 1; vel_idx++) { - update_brake_map_value_.at(ped_idx).at(vel_idx) = - brake_map_value_.at(ped_idx).at(vel_idx) + map_offset_; - } - } -} - -std::vector AccelBrakeMapCalibrator::getMapColumnFromUnifiedIndex( - const Map & accel_map_value, const Map & brake_map_value, const std::size_t index) -{ - if (index < brake_map_value.size()) { - // input brake map value - return brake_map_value.at(brake_map_value.size() - index - 1); - } else { - // input accel map value - return accel_map_value.at(index - brake_map_value.size() + 1); - } -} - -double AccelBrakeMapCalibrator::getPedalValueFromUnifiedIndex(const std::size_t index) -{ - if (index < brake_pedal_index_.size()) { - // brake index ( minus ) - return -brake_pedal_index_.at(brake_pedal_index_.size() - index - 1); - } else { - // input accel map value - return accel_pedal_index_.at(index - brake_pedal_index_.size() + 1); - } -} - -int AccelBrakeMapCalibrator::getUnifiedIndexFromAccelBrakeIndex( - const bool accel_map, const std::size_t index) -{ - // accel_map=true: accel_map; accel_map=false: brake_map - if (accel_map) { - // accel map - return index + brake_map_value_.size() - 1; - } else { - // brake map - return brake_map_value_.size() - index - 1; - } -} - -double AccelBrakeMapCalibrator::getPitchCompensatedAcceleration() -{ - // It works correctly only when the velocity is positive. - constexpr double gravity = 9.80665; - return gravity * std::sin(pitch_); -} - -void AccelBrakeMapCalibrator::executeEvaluation() -{ - const double full_orig_accel_sq_error = calculateAccelSquaredError( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - accel_map_, brake_map_); - pushDataToVec(full_orig_accel_sq_error, full_mse_que_size_, &full_original_accel_mse_que_); - full_original_accel_rmse_ = getAverage(full_original_accel_mse_que_); - // std::cerr << "rmse : " << sqrt(full_original_accel_rmse_) << std::endl; - - const double full_orig_accel_l1_error = calculateAccelErrorL1Norm( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - accel_map_, brake_map_); - const double full_orig_accel_sq_l1_error = full_orig_accel_l1_error * full_orig_accel_l1_error; - pushDataToVec(full_orig_accel_l1_error, full_mse_que_size_, &full_original_accel_l1_que_); - pushDataToVec(full_orig_accel_sq_l1_error, full_mse_que_size_, &full_original_accel_sq_l1_que_); - full_original_accel_error_l1norm_ = getAverage(full_original_accel_l1_que_); - - /*calculate l1norm_covariance*/ - // const double full_original_accel_error_sql1_ = getAverage(full_original_accel_sq_l1_que_); - // std::cerr << "error_l1norm : " << full_original_accel_error_l1norm_ << std::endl; - // std::cerr << "error_l1_cov : " << - // full_original_accel_error_sql1_-full_original_accel_error_l1norm_*full_original_accel_error_l1norm_ - // << std::endl; - - const double part_orig_accel_sq_error = calculateAccelSquaredError( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - accel_map_, brake_map_); - pushDataToVec(part_orig_accel_sq_error, part_mse_que_size_, &part_original_accel_mse_que_); - part_original_accel_rmse_ = getAverage(part_original_accel_mse_que_); - - const double new_accel_sq_error = calculateAccelSquaredError( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - new_accel_map_, new_brake_map_); - pushDataToVec(new_accel_sq_error, part_mse_que_size_, &new_accel_mse_que_); - new_accel_rmse_ = getAverage(new_accel_mse_que_); -} - -double AccelBrakeMapCalibrator::calculateEstimatedAcc( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map) -{ - const double pedal = throttle - brake; - - double estimated_acc = 0.0; - if (pedal > 0.0) { - accel_map.getAcceleration(pedal, vel, estimated_acc); - } else { - brake_map.getAcceleration(-pedal, vel, estimated_acc); - } - - return estimated_acc; -} - -double AccelBrakeMapCalibrator::calculateAccelSquaredError( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map) -{ - const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); - const double dif_acc = measured_acc - estimated_acc; - return dif_acc * dif_acc; -} - -double AccelBrakeMapCalibrator::calculateAccelErrorL1Norm( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map) -{ - const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); - const double dif_acc = measured_acc - estimated_acc; - return abs(dif_acc); -} - -void AccelBrakeMapCalibrator::pushDataToQue( - const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, - std::queue * que) -{ - que->push(data); - while (que->size() > max_size) { - que->pop(); - } -} - -template -void AccelBrakeMapCalibrator::pushDataToVec( - const T data, const std::size_t max_size, std::vector * vec) -{ - vec->emplace_back(data); - while (vec->size() > max_size) { - vec->erase(vec->begin()); - } -} - -template -T AccelBrakeMapCalibrator::getNearestTimeDataFromVec( - const T base_data, const double back_time, const std::vector & vec) -{ - double nearest_time = std::numeric_limits::max(); - const double target_time = rclcpp::Time(base_data->header.stamp).seconds() - back_time; - T nearest_time_data; - for (const auto & data : vec) { - const double data_time = rclcpp::Time(data->header.stamp).seconds(); - const auto delta_time = std::abs(target_time - data_time); - if (nearest_time > delta_time) { - nearest_time_data = data; - nearest_time = delta_time; - } - } - return nearest_time_data; -} - -DataStampedPtr AccelBrakeMapCalibrator::getNearestTimeDataFromVec( - DataStampedPtr base_data, const double back_time, const std::vector & vec) -{ - double nearest_time = std::numeric_limits::max(); - const double target_time = base_data->data_time.seconds() - back_time; - DataStampedPtr nearest_time_data; - for (const auto & data : vec) { - const double data_time = data->data_time.seconds(); - const auto delta_time = std::abs(target_time - data_time); - if (nearest_time > delta_time) { - nearest_time_data = data; - nearest_time = delta_time; - } - } - return nearest_time_data; -} - -double AccelBrakeMapCalibrator::getAverage(const std::vector & vec) -{ - if (vec.empty()) { - return 0.0; - } - - double sum = 0.0; - for (const auto num : vec) { - sum += num; - } - return sum / vec.size(); -} - -double AccelBrakeMapCalibrator::getStandardDeviation(const std::vector & vec) -{ - if (vec.empty()) { - return 0.0; - } - - const double ave = getAverage(vec); - - double sum = 0.0; - for (const auto num : vec) { - sum += std::pow(num - ave, 2); - } - return std::sqrt(sum / vec.size()); -} - -bool AccelBrakeMapCalibrator::isTimeout( - const builtin_interfaces::msg::Time & stamp, const double timeout_sec) -{ - const double dt = this->now().seconds() - rclcpp::Time(stamp).seconds(); - return dt > timeout_sec; -} - -bool AccelBrakeMapCalibrator::isTimeout( - const DataStampedPtr & data_stamped, const double timeout_sec) -{ - const double dt = (this->now() - data_stamped->data_time).seconds(); - return dt > timeout_sec; -} - -OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( - const std::string frame_id, const double height, const double width, const double resolution, - const std::vector & map_value) -{ - OccupancyGrid occ; - occ.header.frame_id = frame_id; - occ.header.stamp = this->now(); - occ.info.height = height; - occ.info.width = width; - occ.info.map_load_time = this->now(); - occ.info.origin.position.x = 0; - occ.info.origin.position.y = 0; - occ.info.origin.position.z = 0; - occ.info.origin.orientation.x = 0; - occ.info.origin.orientation.y = 0; - occ.info.origin.orientation.z = 0; - occ.info.origin.orientation.w = 1; - occ.info.resolution = resolution; - occ.data = map_value; - return occ; -} - -// function for diagnostics -void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; - using CalibrationStatus = CalibrationStatus; - CalibrationStatus accel_brake_map_status; - int8_t level; - std::string msg; - - accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; - if (is_default_map_ == true) { - accel_brake_map_status.status = CalibrationStatus::NORMAL; - level = DiagStatus::OK; - msg = "OK"; - } else { - accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE; - level = DiagStatus::ERROR; - msg = "Default map is not found in " + csv_default_map_dir_; - } - - if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { - // lack of data - stat.summary(level, msg); - calibration_status_pub_->publish(accel_brake_map_status); - return; - } - - const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; - if (rmse_rate < update_suggest_thresh_) { - // The accuracy of original accel/brake map is low. - // Suggest to update accel brake map - level = DiagStatus::WARN; - msg = "Accel/brake map Calibration is required."; - accel_brake_map_status.status = CalibrationStatus::CALIBRATION_REQUIRED; - } - - stat.summary(level, msg); - calibration_status_pub_->publish(accel_brake_map_status); -} - -// function for debug - -void AccelBrakeMapCalibrator::publishMap( - const Map accel_map_value, const Map brake_map_value, const std::string publish_type) -{ - if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid map. The number of velocity index of accel map and brake map is different."); - return; - } - const double h = accel_map_value.size() + brake_map_value.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value.at(0).size(); // velocity - - // publish occupancy map - const int8_t MAX_OCC_VALUE = 100; - std::vector int_map_value; - int_map_value.resize(h * w); - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - const double value = - getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j); - // convert acc to 0~100 int value - int8_t int_value = - static_cast(MAX_OCC_VALUE * ((value - min_accel_) / (max_accel_ - min_accel_))); - int_map_value.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_value), (int8_t)0); - } - } - - if (publish_type == "original") { - original_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); - } else { - update_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); - } - - // publish raw map - Float32MultiArray float_map; - - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim[0].label = "height"; - float_map.layout.dim[1].label = "width"; - float_map.layout.dim[0].size = h; - float_map.layout.dim[1].size = w; - float_map.layout.dim[0].stride = h * w; - float_map.layout.dim[1].stride = w; - float_map.layout.data_offset = 0; - std::vector vec(h * w, 0); - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - vec[i * w + j] = static_cast( - getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j)); - } - } - float_map.data = vec; - if (publish_type == "original") { - original_map_raw_pub_->publish(float_map); - } else { - update_map_raw_pub_->publish(float_map); - } -} - -void AccelBrakeMapCalibrator::publishOffsetCovMap( - const Map accel_map_value, const Map brake_map_value) -{ - if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid map. The number of velocity index of accel map and brake map is different."); - return; - } - const double h = accel_map_value.size() + brake_map_value.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value.at(0).size(); // velocity - - // publish raw map - Float32MultiArray float_map; - - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim[0].label = "height"; - float_map.layout.dim[1].label = "width"; - float_map.layout.dim[0].size = h; - float_map.layout.dim[1].size = w; - float_map.layout.dim[0].stride = h * w; - float_map.layout.dim[1].stride = w; - float_map.layout.data_offset = 0; - std::vector vec(h * w, 0); - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - vec[i * w + j] = - static_cast(getMapColumnFromUnifiedIndex(accel_map_value, brake_map_value, i).at(j)); - } - } - float_map.data = vec; - offset_covariance_pub_->publish(float_map); -} - -void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val) -{ - Float32Stamped msg; - msg.stamp = this->now(); - msg.data = val; - if (publish_type == "current_map_error") { - current_map_error_pub_->publish(msg); - } else if (publish_type == "updated_map_error") { - updated_map_error_pub_->publish(msg); - } else { - map_error_ratio_pub_->publish(msg); - } -} - -void AccelBrakeMapCalibrator::publishCountMap() -{ - if (accel_map_value_.at(0).size() != brake_map_value_.at(0).size()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid map. The number of velocity index of accel map and brake map is different."); - return; - } - const double h = accel_map_value_.size() + brake_map_value_.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value_.at(0).size(); // velocity - const int8_t MAX_OCC_VALUE = 100; - - std::vector count_map, ave_map, std_map; - count_map.resize(h * w); - ave_map.resize(h * w); - std_map.resize(h * w); - - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - const auto data_vec = map_value_data_.at(i).at(j); - if (data_vec.empty()) { - // input *UNKNOWN* value - count_map.at(i * w + j) = -1; - ave_map.at(i * w + j) = -1; - } else { - const auto count_rate = - MAX_OCC_VALUE * (static_cast(data_vec.size()) / max_data_count_); - count_map.at(i * w + j) = static_cast( - std::max(std::min(static_cast(MAX_OCC_VALUE), static_cast(count_rate)), 0)); - // calculate average - { - const double average = getAverage(data_vec); - int8_t int_average = static_cast( - MAX_OCC_VALUE * ((average - min_accel_) / (max_accel_ - min_accel_))); - ave_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_average), (int8_t)0); - } - // calculate standard deviation - { - const double std_dev = getStandardDeviation(data_vec); - const double max_std_dev = 0.2; - const double min_std_dev = 0.0; - int8_t int_std_dev = static_cast( - MAX_OCC_VALUE * ((std_dev - min_std_dev) / (max_std_dev - min_std_dev))); - std_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_std_dev), (int8_t)0); - } - } - } - } - - // publish average map / standard dev map / count map - data_ave_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, ave_map)); - data_std_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, std_map)); - data_count_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); - - // publish count with self pos map - int nearest_pedal_idx = nearestPedalSearch(); - int nearest_vel_idx = nearestVelSearch(); - - // input self pose (to max value / min value ) and publish this - update_success_ - ? count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::max() - : count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::min(); - data_count_with_self_pose_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); -} - -void AccelBrakeMapCalibrator::publishIndex() -{ - MarkerArray markers; - const double h = accel_map_value_.size() + brake_map_value_.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value_.at(0).size(); // velocity - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "base_link"; - marker.header.stamp = this->now(); - marker.type = marker.TEXT_VIEW_FACING; - marker.action = marker.ADD; - std_msgs::msg::ColorRGBA color; - color.a = 0.999; - color.b = 0.1; - color.g = 0.1; - color.r = 0.1; - marker.color = color; - marker.frame_locked = true; - marker.pose.position.z = 0.0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - - // pedal value - for (int pedal_idx = 0; pedal_idx < h; pedal_idx++) { - const double pedal_value = getPedalValueFromUnifiedIndex(pedal_idx); - marker.ns = "occ_pedal_index"; - marker.id = pedal_idx; - marker.pose.position.x = -map_resolution_ * 0.5; - marker.pose.position.y = map_resolution_ * (0.5 + pedal_idx); - marker.scale.z = 0.03; - std::stringstream stream; - stream << std::fixed << std::setprecision(2) << pedal_value; - marker.text = stream.str(); - markers.markers.push_back(marker); - } - - // pedal index name - marker.ns = "occ_pedal_index"; - marker.id = h; - marker.pose.position.x = -map_resolution_ * 0.5; - marker.pose.position.y = map_resolution_ * (0.5 + h); - marker.scale.z = 0.03; - marker.text = "(accel_pedal/brake_pedal)"; - markers.markers.push_back(marker); - - // velocity value - for (int vel_idx = 0; vel_idx < w; vel_idx++) { - const double vel_value = accel_vel_index_.at(vel_idx); - marker.ns = "occ_vel_index"; - marker.id = vel_idx; - marker.pose.position.x = map_resolution_ * (0.5 + vel_idx); - marker.pose.position.y = -map_resolution_ * 0.2; - marker.scale.z = 0.02; - std::stringstream stream; - stream << std::fixed << std::setprecision(2) << vel_value; - marker.text = stream.str() + "m/s"; - markers.markers.push_back(marker); - } - - // velocity index name - marker.ns = "occ_vel_index"; - marker.id = w; - marker.pose.position.x = map_resolution_ * (0.5 + w); - marker.pose.position.y = -map_resolution_ * 0.2; - marker.scale.z = 0.02; - marker.text = "(velocity)"; - markers.markers.push_back(marker); - - index_pub_->publish(markers); -} - -void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() -{ - std_msgs::msg::Bool update_suggest; - - if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { - // lack of data - update_suggest.data = false; - } else { - const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; - update_suggest.data = (rmse_rate < update_suggest_thresh_); - if (update_suggest.data) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, - "suggest to update accel/brake map. evaluation score = " << rmse_rate); - } - } - - update_suggest_pub_->publish(update_suggest); -} - -bool AccelBrakeMapCalibrator::writeMapToCSV( - std::vector vel_index, std::vector pedal_index, Map value_map, - std::string filename) -{ - if (update_success_count_ == 0) { - return false; - } - - std::ofstream csv_file(filename); - - if (!csv_file.is_open()) { - RCLCPP_WARN(get_logger(), "Failed to open csv file : %s", filename.c_str()); - return false; - } - - csv_file << "default,"; - for (std::size_t v = 0; v < vel_index.size(); v++) { - csv_file << vel_index.at(v); - if (v != vel_index.size() - 1) { - csv_file << ","; - } - } - csv_file << "\n"; - - for (std::size_t p = 0; p < pedal_index.size(); p++) { - csv_file << pedal_index.at(p) << ","; - for (std::size_t v = 0; v < vel_index.size(); v++) { - csv_file << std::fixed << std::setprecision(precision_) << value_map.at(p).at(v); - if (v != vel_index.size() - 1) { - csv_file << ","; - } - } - csv_file << "\n"; - } - csv_file.close(); - RCLCPP_DEBUG_STREAM(get_logger(), "output map to " << filename); - return true; -} - -void AccelBrakeMapCalibrator::addIndexToCSV(std::ofstream * csv_file) -{ - *csv_file << "timestamp,velocity,accel,pitch_comp_accel,final_accel,accel_pedal,brake_pedal," - << "accel_pedal_speed,brake_pedal_speed,pitch,steer,jerk,full_original_accel_rmse, " - "part_original_accel_rmse,new_accel_rmse, rmse_rate" - << std::endl; -} - -void AccelBrakeMapCalibrator::addLogToCSV( - std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, - const double pitched_accel, const double accel_pedal, const double brake_pedal, - const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, - const double steer, const double jerk, const double full_original_accel_mse, - const double part_original_accel_mse, const double new_accel_mse) -{ - const double rmse_rate = - part_original_accel_mse == 0.0 ? 1.0 : (new_accel_mse / part_original_accel_mse); - *csv_file << timestamp << ", " << velocity << ", " << accel << ", " << pitched_accel << ", " - << accel - pitched_accel << ", " << accel_pedal << ", " << brake_pedal << ", " - << accel_pedal_speed << ", " << brake_pedal_speed << ", " << pitch << ", " << steer - << ", " << jerk << ", " << full_original_accel_mse << ", " << part_original_accel_mse - << ", " << new_accel_mse << "," << rmse_rate << std::endl; -} - -} // namespace accel_brake_map_calibrator diff --git a/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py deleted file mode 100755 index 1bbb5820..00000000 --- a/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ /dev/null @@ -1,122 +0,0 @@ -#! /usr/bin/env python3 - -# 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. - -import argparse -import os - -from ament_index_python.packages import get_package_share_directory -import rclpy -from rclpy.node import Node -from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData - - -class CalibrationDataRelay(Node): - def __init__(self, args): - super().__init__("plot_client") - self.cli = self.create_client( - CalibData, "/api/external/get/accel_brake_map_calibrator/data" - ) - - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info("service not available, waiting again...") - - self.request = CalibData.Request() - - def send_request(self): - self.future = self.cli.call_async(self.request) - - -def main(args=None): - rclpy.init(args=None) - client = CalibrationDataRelay(args) - client.send_request() - while rclpy.ok(): - rclpy.spin_once(client) - if client.future.done(): - try: - response = client.future.result() - save_dir = "./test_data_save" - if not os.path.exists(save_dir): - os.makedirs(save_dir) - svg_name = save_dir + "/graph.svg" - - f_svg = open(svg_name, "w") - svg_byte = bytes(response.graph_image) - text = svg_byte.decode() - f_svg.write(text) - - print("svg done") - - acc_map_name = save_dir + "/accel_map.csv" - f_acc = open(acc_map_name, "w") - f_acc.write(response.accel_map) - - print("accel map done") - - brk_map_name = save_dir + "/brake_map.csv" - f_brk = open(brk_map_name, "w") - f_brk.write(response.brake_map) - - print("brake map done") - - except Exception as e: - client.get_logger().info("Service call failed %r" % (e,)) - - break - - client.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - package_path = get_package_share_directory("accel_brake_map_calibrator") - parser = argparse.ArgumentParser() - parser.add_argument( - "-d", "--default-map-dir", default=None, type=str, help="directory of default map" - ) - parser.add_argument( - "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" - ) - parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") - parser.add_argument( - "-l", - "--log-file", - default=package_path + "/config/log.csv", - type=str, - help="path of log.csv", - ) - parser.add_argument( - "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" - ) - parser.add_argument( - "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" - ) - parser.add_argument( - "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" - ) - parser.add_argument( - "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" - ) - parser.add_argument( - "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" - ) - parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") - parser.add_argument( - "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" - ) - - args = parser.parse_args() - main(args) diff --git a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py deleted file mode 100755 index 9e57813b..00000000 --- a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ /dev/null @@ -1,62 +0,0 @@ -#! /usr/bin/env python3 - -# 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. - - -from autoware_auto_vehicle_msgs.msg import VelocityReport -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy -from tier4_vehicle_msgs.msg import ActuationStatusStamped - - -class ActuationStatusPublisher(Node): - def __init__(self): - super().__init__("actuation_status_publisher") - - qos_profile = QoSProfile(depth=1) - qos_profile.reliability = QoSReliabilityPolicy.RELIABLE - qos_profile.history = QoSHistoryPolicy.KEEP_LAST - qos_profile.durability = QoSDurabilityPolicy.VOLATILE - - self.pub = self.create_publisher( - ActuationStatusStamped, "/vehicle/status/actuation_status", qos_profile - ) - self.sub = self.create_subscription( - VelocityReport, "/vehicle/status/velocity_status", self.callback, qos_profile - ) - - def callback(self, msg): - data = ActuationStatusStamped() - data.header = msg.header - data.status.accel_status = msg.longitudinal_velocity * 0.1 - data.status.brake_status = 0.1 - data.status.steer_status = 0.1 - self.pub.publish(data) - - -def main(args=None): - rclpy.init(args=args) - node = ActuationStatusPublisher() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main()