diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 4c01604c51ee4..1ad3476333eee 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -56,7 +56,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -69,6 +69,7 @@ control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tie control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp +control/smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @@ -155,6 +156,7 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -183,7 +185,7 @@ planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kur planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp -planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -198,7 +200,7 @@ planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.j planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp @@ -225,7 +227,7 @@ sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp @@ -249,6 +251,7 @@ system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp +tools/reaction_analyzer/** berkay@leodrive.ai vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp diff --git a/common/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 2b06e6db3e70d..0000000000000 --- a/common/mission_planner_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -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 deleted file mode 100644 index 59d36a0a2f840..0000000000000 --- a/common/mission_planner_rviz_plugin/README.md +++ /dev/null @@ -1,18 +0,0 @@ -# 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 deleted file mode 100644 index e45cf2739f260..0000000000000 --- a/common/mission_planner_rviz_plugin/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - 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 deleted file mode 100644 index c8285fcc604d7..0000000000000 --- a/common/mission_planner_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - MrmGoalTool - - - RouteSelectorPanel - - diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp deleted file mode 100644 index b4b376b1e76ec..0000000000000 --- a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp +++ /dev/null @@ -1,148 +0,0 @@ -// 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 deleted file mode 100644 index 99101730661e0..0000000000000 --- a/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// 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/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 8bb5f13e3fb78..fb151ec56b085 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -27,7 +27,11 @@ namespace resample_utils { constexpr double close_s_threshold = 1e-6; -#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"motion_utils.resample_utils"}; + return rclcpp::get_logger(logger); +} template bool validate_size(const T & points) @@ -62,27 +66,27 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); tier4_autoware_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { - log_error( - "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + RCLCPP_DEBUG( + get_logger(), "invalid argument: The number of resampling intervals is less than 2"); tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - log_error("[resample_utils] invalid argument: input points has some duplicated points"); + RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); tier4_autoware_utils::print_backtrace(); return false; } @@ -95,23 +99,23 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - log_error( - "[resample_utils] invalid argument: resampling interval is less than " + - std::to_string(motion_utils::overlap_threshold)); + RCLCPP_DEBUG( + get_logger(), "invalid argument: resampling interval is less than %f", + motion_utils::overlap_threshold); tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - log_error("[resample_utils] invalid argument: input points has some duplicated points"); + RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 7c9b5a5378ab6..7e2d92c9434fb 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/system/backtrace.hpp" #include +#include #include #include @@ -33,9 +34,14 @@ #include #include #include + namespace motion_utils { -#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"motion_utils.trajectory"}; + return rclcpp::get_logger(logger); +} /** * @brief validate if points container is empty or not @@ -216,7 +222,7 @@ std::optional searchZeroVelocityIndex( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -248,7 +254,7 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -338,7 +344,7 @@ std::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -411,9 +417,10 @@ double calcLongitudinalOffsetToSegment( if (throw_exception) { throw std::out_of_range(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -425,7 +432,7 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return std::nan(""); } } @@ -438,9 +445,10 @@ double calcLongitudinalOffsetToSegment( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -586,9 +594,10 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error( - std::string(e.what()) + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); return std::nan(""); } } @@ -601,9 +610,10 @@ double calcLateralOffset( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -658,9 +668,10 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error( - std::string(e.what()) + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); return std::nan(""); } } @@ -673,9 +684,10 @@ double calcLateralOffset( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -711,7 +723,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -754,7 +766,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -806,7 +818,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -849,7 +861,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -888,7 +900,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -928,7 +940,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -1037,7 +1049,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1071,7 +1083,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1084,9 +1096,10 @@ std::optional calcLongitudinalOffsetPoint( if (throw_exception) { throw std::out_of_range(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return {}; } @@ -1153,7 +1166,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); return {}; } @@ -1201,7 +1214,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); return {}; } @@ -1214,12 +1227,12 @@ std::optional calcLongitudinalOffsetPose( if (throw_exception) { throw std::out_of_range(error_message); } - log_error(error_message); + RCLCPP_DEBUG(get_logger(), "%s", error_message.c_str()); return {}; } if (points.size() == 1) { - log_error("Failed to calculate longitudinal offset: points size is one."); + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1304,7 +1317,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1350,7 +1363,7 @@ std::optional insertTargetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1365,7 +1378,7 @@ std::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1908,20 +1921,21 @@ insertOrientation * radians (default: M_PI_2) */ template -void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) +void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { - for (size_t i = 1; i < points.size();) { - const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPose(points.at(i)); + for (auto itr = points.begin(); std::next(itr) != points.end();) { + const auto p1 = tier4_autoware_utils::getPose(*itr); + const auto p2 = tier4_autoware_utils::getPose(*std::next(itr)); const double yaw1 = tf2::getYaw(p1.orientation); const double yaw2 = tf2::getYaw(p2.orientation); if ( max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || !tier4_autoware_utils::isDrivingForward(p1, p2)) { - points.erase(points.begin() + i); + points.erase(std::next(itr)); + return; } else { - ++i; + itr++; } } } @@ -2231,7 +2245,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error("Failed to calculate stop distance" + std::string(e.what())); + RCLCPP_DEBUG(get_logger(), "Failed to calculate stop distance %s", e.what()); return {}; } @@ -2370,7 +2384,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - log_error("Return original points since cropped_points size is less than 2."); + RCLCPP_DEBUG(get_logger(), "Return original points since cropped_points size is less than 2."); return points; } @@ -2415,7 +2429,7 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } } @@ -2428,9 +2442,10 @@ double calcYawDeviation( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return 0 since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return 0 since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return 0.0; } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index eb6a06041e65d..8e84b111b0688 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1876,40 +1876,34 @@ TEST(trajectory, insertTargetPoint) // Invalid target point(In front of begin point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2304,13 +2298,11 @@ TEST(trajectory, insertTargetPoint_Length) // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4310,40 +4302,34 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) // Invalid target point(In front of begin point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -5351,10 +5337,10 @@ TEST(trajectory, cropPoints) } } -TEST(Trajectory, removeInvalidOrientationPoints) +TEST(Trajectory, removeFirstInvalidOrientationPoints) { using motion_utils::insertOrientation; - using motion_utils::removeInvalidOrientationPoints; + using motion_utils::removeFirstInvalidOrientationPoints; const double max_yaw_diff = M_PI_2; @@ -5365,7 +5351,7 @@ TEST(Trajectory, removeInvalidOrientationPoints) auto modified_traj = traj; insertOrientation(modified_traj.points, true); modifyTrajectory(modified_traj); - removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff); + removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff); EXPECT_EQ(modified_traj.points.size(), expected_size); for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) { EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); @@ -5388,7 +5374,7 @@ TEST(Trajectory, removeInvalidOrientationPoints) [&](Trajectory & t) { auto invalid_point = t.points.back(); invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); t.points.push_back(invalid_point); }, traj.points.size()); @@ -5399,21 +5385,10 @@ TEST(Trajectory, removeInvalidOrientationPoints) [&](Trajectory & t) { auto invalid_point = t.points[4]; invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); t.points.insert(t.points.begin() + 4, invalid_point); }, traj.points.size()); - - // invalid point at the beginning - testRemoveInvalidOrientationPoints( - traj, - [&](Trajectory & t) { - auto invalid_point = t.points.front(); - invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); - t.points.insert(t.points.begin(), invalid_point); - }, - 1); // expected size is 1 since only the first point remains } TEST(trajectory, calcYawDeviation) diff --git a/common/rtc_manager_rviz_plugin/CMakeLists.txt b/common/rtc_manager_rviz_plugin/CMakeLists.txt deleted file mode 100644 index f2fad9e24665f..0000000000000 --- a/common/rtc_manager_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100644 index 7a15d56fe235a..0000000000000 --- a/common/rtc_manager_rviz_plugin/README.md +++ /dev/null @@ -1,36 +0,0 @@ -# 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 deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/icons/classes/RTCManagerPanel.png and /dev/null 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 deleted file mode 100644 index 36c1b4760308b..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/images/rtc_manager_panel.png and /dev/null differ diff --git a/common/rtc_manager_rviz_plugin/images/rtc_selection.png b/common/rtc_manager_rviz_plugin/images/rtc_selection.png deleted file mode 100644 index f9c5d120bdd70..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/images/rtc_selection.png and /dev/null differ diff --git a/common/rtc_manager_rviz_plugin/images/select_panels.png b/common/rtc_manager_rviz_plugin/images/select_panels.png deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/rtc_manager_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml deleted file mode 100644 index 53f00386bdb4d..0000000000000 --- a/common/rtc_manager_rviz_plugin/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - 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 deleted file mode 100644 index 001ae153e6762..0000000000000 --- a/common/rtc_manager_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - 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 deleted file mode 100644 index 058a5d5deb5d6..0000000000000 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ /dev/null @@ -1,474 +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 "rtc_manager_panel.hpp" - -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -inline std::string Bool2String(const bool var) -{ - return var ? "True" : "False"; -} -inline bool uint2bool(uint8_t var) -{ - return var == static_cast(0) ? false : true; -} -using std::placeholders::_1; -using std::placeholders::_2; - -std::string getModuleName(const uint8_t module_type) -{ - switch (module_type) { - case Module::LANE_CHANGE_LEFT: { - return "lane_change_left"; - } - case Module::LANE_CHANGE_RIGHT: { - return "lane_change_right"; - } - case Module::EXT_REQUEST_LANE_CHANGE_LEFT: { - return "external_request_lane_change_left"; - } - case Module::EXT_REQUEST_LANE_CHANGE_RIGHT: { - return "external_request_lane_change_right"; - } - case Module::AVOIDANCE_BY_LC_LEFT: { - return "avoidance_by_lane_change_left"; - } - case Module::AVOIDANCE_BY_LC_RIGHT: { - return "avoidance_by_lane_change_right"; - } - case Module::AVOIDANCE_LEFT: { - return "avoidance_left"; - } - case Module::AVOIDANCE_RIGHT: { - return "avoidance_right"; - } - case Module::GOAL_PLANNER: { - return "goal_planner"; - } - case Module::START_PLANNER: { - return "start_planner"; - } - case Module::TRAFFIC_LIGHT: { - return "traffic_light"; - } - case Module::INTERSECTION: { - return "intersection"; - } - case Module::CROSSWALK: { - return "crosswalk"; - } - case Module::BLIND_SPOT: { - return "blind_spot"; - } - case Module::DETECTION_AREA: { - return "detection_area"; - } - case Module::NO_STOPPING_AREA: { - return "no_stopping_area"; - } - case Module::OCCLUSION_SPOT: { - return "occlusion_spot"; - } - case Module::INTERSECTION_OCCLUSION: { - return "intersection_occlusion"; - } - } - return "NONE"; -} - -bool isPathChangeModule(const uint8_t module_type) -{ - if ( - module_type == Module::LANE_CHANGE_LEFT || module_type == Module::LANE_CHANGE_RIGHT || - module_type == Module::EXT_REQUEST_LANE_CHANGE_LEFT || - module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || - module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT || - module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || - module_type == Module::GOAL_PLANNER || module_type == Module::START_PLANNER) { - return true; - } - return false; -} - -RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) -{ - // TODO(tanaka): replace this magic number to Module::SIZE - const size_t module_size = 19; - auto_modes_.reserve(module_size); - auto * v_layout = new QVBoxLayout; - auto vertical_header = new QHeaderView(Qt::Vertical); - vertical_header->hide(); - auto horizontal_header = new QHeaderView(Qt::Horizontal); - horizontal_header->setSectionResizeMode(QHeaderView::Stretch); - auto_mode_table_ = new QTableWidget(); - auto_mode_table_->setColumnCount(4); - auto_mode_table_->setHorizontalHeaderLabels( - {"Module", "ToAutoMode", "ToManualMode", "ApprovalMode"}); - auto_mode_table_->setVerticalHeader(vertical_header); - auto_mode_table_->setHorizontalHeader(horizontal_header); - const size_t num_modules = module_size; - auto_mode_table_->setRowCount(num_modules); - for (size_t i = 0; i < num_modules; i++) { - auto * rtc_auto_mode = new RTCAutoMode(); - rtc_auto_mode->setParent(this); - // module - { - const uint8_t module_type = static_cast(i); - rtc_auto_mode->module_name = getModuleName(module_type); - std::string module_name = rtc_auto_mode->module_name; - auto label = new QLabel(QString::fromStdString(module_name)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(module_name)); - if (isPathChangeModule(module_type)) - label->setStyleSheet(BG_PURPLE); - else - label->setStyleSheet(BG_ORANGE); - auto_mode_table_->setCellWidget(i, 0, label); - } - // mode button - { - // auto mode button - rtc_auto_mode->auto_module_button_ptr = new QPushButton("auto mode"); - rtc_auto_mode->auto_module_button_ptr->setCheckable(true); - connect( - rtc_auto_mode->auto_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, - &RTCAutoMode::onChangeToAutoMode); - auto_mode_table_->setCellWidget(i, 1, rtc_auto_mode->auto_module_button_ptr); - // manual mode button - rtc_auto_mode->manual_module_button_ptr = new QPushButton("manual mode"); - rtc_auto_mode->manual_module_button_ptr->setCheckable(true); - connect( - rtc_auto_mode->manual_module_button_ptr, &QPushButton::clicked, rtc_auto_mode, - &RTCAutoMode::onChangeToManualMode); - auto_mode_table_->setCellWidget(i, 2, rtc_auto_mode->manual_module_button_ptr); - } - // current mode - { - QString mode = QString::fromStdString("INIT"); - rtc_auto_mode->auto_manual_mode_label = new QLabel(mode); - rtc_auto_mode->auto_manual_mode_label->setAlignment(Qt::AlignCenter); - rtc_auto_mode->auto_manual_mode_label->setText(mode); - auto_mode_table_->setCellWidget(i, 3, rtc_auto_mode->auto_manual_mode_label); - } - auto_modes_.emplace_back(rtc_auto_mode); - } - v_layout->addWidget(auto_mode_table_); - - num_rtc_status_ptr_ = new QLabel("Init"); - v_layout->addWidget(num_rtc_status_ptr_); - - // lateral execution - auto * exe_path_change_layout = new QHBoxLayout; - { - exec_path_change_button_ptr_ = new QPushButton("Execute Path Change"); - exec_path_change_button_ptr_->setCheckable(false); - exec_path_change_button_ptr_->setStyleSheet(BG_PURPLE); - connect( - exec_path_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickExecutePathChange); - exe_path_change_layout->addWidget(exec_path_change_button_ptr_); - wait_path_change_button_ptr_ = new QPushButton("Wait Path Change"); - wait_path_change_button_ptr_->setCheckable(false); - wait_path_change_button_ptr_->setStyleSheet(BG_PURPLE); - connect( - wait_path_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickWaitPathChange); - exe_path_change_layout->addWidget(wait_path_change_button_ptr_); - } - v_layout->addLayout(exe_path_change_layout); - - // longitudinal execution - auto * exe_vel_change_layout = new QHBoxLayout; - { - exec_vel_change_button_ptr_ = new QPushButton("Execute Velocity Change"); - exec_vel_change_button_ptr_->setCheckable(false); - exec_vel_change_button_ptr_->setStyleSheet(BG_ORANGE); - connect( - exec_vel_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickExecuteVelChange); - exe_vel_change_layout->addWidget(exec_vel_change_button_ptr_); - wait_vel_change_button_ptr_ = new QPushButton("Wait Velocity Change"); - wait_vel_change_button_ptr_->setCheckable(false); - wait_vel_change_button_ptr_->setStyleSheet(BG_ORANGE); - connect( - wait_vel_change_button_ptr_, &QPushButton::clicked, this, - &RTCManagerPanel::onClickWaitVelChange); - exe_vel_change_layout->addWidget(wait_vel_change_button_ptr_); - } - v_layout->addLayout(exe_vel_change_layout); - - // execution - auto * rtc_exe_layout = new QHBoxLayout; - { - exec_button_ptr_ = new QPushButton("Execute All"); - exec_button_ptr_->setCheckable(false); - connect(exec_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickExecution); - rtc_exe_layout->addWidget(exec_button_ptr_); - wait_button_ptr_ = new QPushButton("Wait All"); - wait_button_ptr_->setCheckable(false); - connect(wait_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickWait); - rtc_exe_layout->addWidget(wait_button_ptr_); - } - v_layout->addLayout(rtc_exe_layout); - - // statuses - auto * rtc_table_layout = new QHBoxLayout; - { - auto vertical_header = new QHeaderView(Qt::Vertical); - vertical_header->hide(); - auto horizontal_header = new QHeaderView(Qt::Horizontal); - horizontal_header->setSectionResizeMode(QHeaderView::Stretch); - rtc_table_ = new QTableWidget(); - rtc_table_->setColumnCount(column_size_); - rtc_table_->setHorizontalHeaderLabels( - {"ID", "Module", "AW Safe", "Received Cmd", "AutoMode", "StartDistance", "FinishDistance"}); - rtc_table_->setVerticalHeader(vertical_header); - rtc_table_->setHorizontalHeader(horizontal_header); - rtc_table_layout->addWidget(rtc_table_); - v_layout->addLayout(rtc_table_layout); - } - setLayout(v_layout); -} - -void RTCManagerPanel::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - client_rtc_commands_ = - raw_node_->create_client("/api/external/set/rtc_commands"); - - for (size_t i = 0; i < auto_modes_.size(); i++) { - auto & a = auto_modes_.at(i); - // auto mode - a->enable_auto_mode_cli = - raw_node_->create_client(enable_auto_mode_namespace_ + "/" + a->module_name); - } - - sub_rtc_status_ = raw_node_->create_subscription( - "/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1)); -} - -void RTCAutoMode::onChangeToAutoMode() -{ - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_auto_mode_cli->async_send_request(request); - auto_manual_mode_label->setText("AutoMode"); - auto_manual_mode_label->setStyleSheet(BG_BLUE); - auto_module_button_ptr->setChecked(true); - manual_module_button_ptr->setChecked(false); -} - -void RTCAutoMode::onChangeToManualMode() -{ - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = false; - enable_auto_mode_cli->async_send_request(request); - auto_manual_mode_label->setText("ManualMode"); - auto_manual_mode_label->setStyleSheet(BG_YELLOW); - manual_module_button_ptr->setChecked(true); - auto_module_button_ptr->setChecked(false); -} - -CooperateCommand setRTCCommandFromStatus(CooperateStatus & status) -{ - CooperateCommand cooperate_command; - cooperate_command.uuid = status.uuid; - cooperate_command.module = status.module; - cooperate_command.command = status.command_status; - return cooperate_command; -} - -void RTCManagerPanel::onClickChangeRequest(const bool is_path_change, const uint8_t command) -{ - if (!cooperate_statuses_ptr_) return; - if (cooperate_statuses_ptr_->statuses.empty()) return; - auto executable_cooperate_commands_request = std::make_shared(); - executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp; - // send coop request - for (auto status : cooperate_statuses_ptr_->statuses) { - if (is_path_change ^ isPathChangeModule(status.module.type)) continue; - CooperateCommand cooperate_command = setRTCCommandFromStatus(status); - cooperate_command.command.type = command; - executable_cooperate_commands_request->commands.emplace_back(cooperate_command); - // To consider needs to change path step by step - if (is_path_change && !status.auto_mode && status.command_status.type ^ command) { - break; - } - } - client_rtc_commands_->async_send_request(executable_cooperate_commands_request); -} - -void RTCManagerPanel::onClickCommandRequest(const uint8_t command) -{ - if (!cooperate_statuses_ptr_) return; - if (cooperate_statuses_ptr_->statuses.empty()) return; - auto executable_cooperate_commands_request = std::make_shared(); - executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp; - // send coop request - for (auto status : cooperate_statuses_ptr_->statuses) { - CooperateCommand cooperate_command = setRTCCommandFromStatus(status); - cooperate_command.command.type = command; - executable_cooperate_commands_request->commands.emplace_back(cooperate_command); - } - client_rtc_commands_->async_send_request(executable_cooperate_commands_request); -} - -void RTCManagerPanel::onClickExecuteVelChange() -{ - onClickChangeRequest(false, Command::ACTIVATE); -} -void RTCManagerPanel::onClickWaitVelChange() -{ - onClickChangeRequest(false, Command::DEACTIVATE); -} -void RTCManagerPanel::onClickExecutePathChange() -{ - onClickChangeRequest(true, Command::ACTIVATE); -} -void RTCManagerPanel::onClickWaitPathChange() -{ - onClickChangeRequest(true, Command::DEACTIVATE); -} -void RTCManagerPanel::onClickExecution() -{ - onClickCommandRequest(Command::ACTIVATE); -} -void RTCManagerPanel::onClickWait() -{ - onClickCommandRequest(Command::DEACTIVATE); -} - -void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg) -{ - cooperate_statuses_ptr_ = std::make_shared(*msg); - rtc_table_->clearContents(); - num_rtc_status_ptr_->setText( - QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); - if (msg->statuses.empty()) { - rtc_table_->update(); - return; - } - // this is to stable rtc display not to occupy too much - size_t min_display_size{5}; - size_t max_display_size{10}; - // rtc messages are already sorted by distance - rtc_table_->setRowCount( - std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); - int cnt = 0; - - auto sorted_statuses = msg->statuses; - std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { - return !status.auto_mode && !uint2bool(status.command_status.type); - }); - - for (auto status : sorted_statuses) { - if (static_cast(cnt) >= max_display_size) { - rtc_table_->update(); - return; - } - // uuid - { - std::stringstream uuid; - uuid << std::setw(4) << std::setfill('0') << static_cast(status.uuid.uuid.at(0)); - auto label = new QLabel(QString::fromStdString(uuid.str())); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(uuid.str())); - rtc_table_->setCellWidget(cnt, 0, label); - } - - // module name - { - std::string module_name = getModuleName(status.module.type); - auto label = new QLabel(QString::fromStdString(module_name)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(module_name)); - rtc_table_->setCellWidget(cnt, 1, label); - } - - // is aw safe - bool is_aw_safe = status.safe; - { - std::string is_safe = Bool2String(is_aw_safe); - auto label = new QLabel(QString::fromStdString(is_safe)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(is_safe)); - rtc_table_->setCellWidget(cnt, 2, label); - } - - // is operator safe - const bool is_execute = uint2bool(status.command_status.type); - { - std::string text = is_execute ? "EXECUTE" : "WAIT"; - if (status.auto_mode) text = "NONE"; - auto label = new QLabel(QString::fromStdString(text)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(text)); - rtc_table_->setCellWidget(cnt, 3, label); - } - - // is auto mode - const bool is_rtc_auto_mode = status.auto_mode; - { - std::string is_auto_mode = Bool2String(is_rtc_auto_mode); - auto label = new QLabel(QString::fromStdString(is_auto_mode)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(is_auto_mode)); - rtc_table_->setCellWidget(cnt, 4, label); - } - - // start distance - { - std::string start_distance = std::to_string(status.start_distance); - auto label = new QLabel(QString::fromStdString(start_distance)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(start_distance)); - rtc_table_->setCellWidget(cnt, 5, label); - } - - // finish distance - { - std::string finish_distance = std::to_string(status.finish_distance); - auto label = new QLabel(QString::fromStdString(finish_distance)); - label->setAlignment(Qt::AlignCenter); - label->setText(QString::fromStdString(finish_distance)); - rtc_table_->setCellWidget(cnt, 6, label); - } - - // add color for recognition - if (is_rtc_auto_mode || (is_aw_safe && is_execute)) { - rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_GREEN); - } else if (is_aw_safe || is_execute) { - rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_YELLOW); - } else { - rtc_table_->cellWidget(cnt, 1)->setStyleSheet(BG_RED); - } - cnt++; - } - rtc_table_->update(); -} -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::RTCManagerPanel, rviz_common::Panel) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp deleted file mode 100644 index 8bdaef94b6254..0000000000000 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp +++ /dev/null @@ -1,132 +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 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 deleted file mode 100644 index cdc57743a6cb1..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -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 deleted file mode 100644 index 6fd626d5a7642..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/README.md +++ /dev/null @@ -1,98 +0,0 @@ -# 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 deleted file mode 100644 index 4f5b4961f2500..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png and /dev/null 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 deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png and /dev/null 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 deleted file mode 100644 index 58d12f95ebfd6..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/markers.png b/common/tier4_automatic_goal_rviz_plugin/images/markers.png deleted file mode 100644 index 8dd4d9d02bef4..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/markers.png and /dev/null differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/panel.png b/common/tier4_automatic_goal_rviz_plugin/images/panel.png deleted file mode 100644 index 1800202ea9f57..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/panel.png and /dev/null 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 deleted file mode 100644 index 61dd9e1d7a1b3..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png and /dev/null 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 deleted file mode 100644 index a6ddc6d24c575..0000000000000 Binary files a/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png and /dev/null 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 deleted file mode 100644 index a9af89c078348..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml deleted file mode 100644 index fb5331379acb6..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - 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 deleted file mode 100644 index e9d77491941ed..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - 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 deleted file mode 100644 index 4efa6fedbaabd..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// 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 deleted file mode 100644 index 4ea3fa4bd009a..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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 deleted file mode 100644 index 43fc0edcccf84..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// 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 deleted file mode 100644 index 6fc98cee9afa1..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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 deleted file mode 100644 index 6b8d7765a989a..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// -// 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 deleted file mode 100644 index 72a5bd4fb80fe..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp +++ /dev/null @@ -1,151 +0,0 @@ -// -// 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 deleted file mode 100644 index 3ca368a3bd1a4..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ /dev/null @@ -1,227 +0,0 @@ -// 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 deleted file mode 100644 index 88b7b5e7dac20..0000000000000 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp +++ /dev/null @@ -1,184 +0,0 @@ -// 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 deleted file mode 100644 index 6b03fe92da0ee..0000000000000 --- a/common/tier4_calibration_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -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 deleted file mode 100644 index f422847d8cb70..0000000000000 --- a/common/tier4_calibration_rviz_plugin/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - 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 deleted file mode 100644 index 82f35b934208c..0000000000000 --- a/common/tier4_calibration_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - 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 deleted file mode 100644 index d89f13ce74d02..0000000000000 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp +++ /dev/null @@ -1,185 +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_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 deleted file mode 100644 index 70ebe0631fa21..0000000000000 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp +++ /dev/null @@ -1,68 +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_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 deleted file mode 100644 index 95fff455991ba..0000000000000 --- a/common/tier4_control_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -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 deleted file mode 100644 index 8bca77771eee2..0000000000000 --- a/common/tier4_control_rviz_plugin/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# tier4_control_rviz_plugin - -This package is to mimic external control for simulation. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------- | ----------------------- | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Current GATE mode | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Current velocity status | -| `/api/autoware/get/engage` | `tier4_external_api_msgs::srv::Engage` | Getting Engage | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The state of GEAR | - -### Output - -| Name | Type | Description | -| -------------------------------- | ---------------------------------------------------------- | ----------------------- | -| `/control/gate_mode_cmd` | `tier4_control_msgs::msg::GateMode` | GATE mode | -| `/external/selected/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | AckermannControlCommand | -| `/external/selected/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | GEAR | - -## Usage - -1. Start rviz and select Panels. - - ![select_panels](./images/select_panels.png) - -2. Select tier4_control_rviz_plugin/ManualController and press OK. - - ![select_manual_controller](./images/select_manual_controller.png) - -3. Enter velocity in "Set Cruise Velocity" and Press the button to confirm. You can notice that GEAR shows D (DRIVE). - - ![manual_controller_not_ready](./images/manual_controller_not_ready.png) - -4. Press "Enable Manual Control" and you can notice that "GATE" and "Engage" turn "Ready" and the vehicle starts! - - ![manual_controller_ready](./images/manual_controller_ready.png) diff --git a/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png b/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png deleted file mode 100644 index e4a8ddb0b9bb1..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/manual_controller_not_ready.png and /dev/null 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 deleted file mode 100644 index d5da7f0644051..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/manual_controller_ready.png and /dev/null 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 deleted file mode 100644 index 5027ec571339c..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/select_manual_controller.png and /dev/null differ diff --git a/common/tier4_control_rviz_plugin/images/select_panels.png b/common/tier4_control_rviz_plugin/images/select_panels.png deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/tier4_control_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml deleted file mode 100644 index 73562a766ce0b..0000000000000 --- a/common/tier4_control_rviz_plugin/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - tier4_control_rviz_plugin - 0.1.0 - The tier4_vehicle_rviz_plugin package - Taiki Tanaka - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_vehicle_msgs - libqt5-core - libqt5-gui - libqt5-widgets - qtbase5-dev - rviz_common - rviz_default_plugins - rviz_ogre_vendor - tier4_autoware_utils - tier4_control_msgs - tier4_external_api_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/tier4_control_rviz_plugin/plugins/plugin_description.xml b/common/tier4_control_rviz_plugin/plugins/plugin_description.xml deleted file mode 100644 index 068bbcd73f214..0000000000000 --- a/common/tier4_control_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - 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 deleted file mode 100644 index 8bbb096f728ec..0000000000000 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ /dev/null @@ -1,261 +0,0 @@ -// -// Copyright 2022 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - -#include "manual_controller.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -using std::placeholders::_1; - -namespace rviz_plugins -{ - -ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent) -{ - auto * state_layout = new QHBoxLayout; - { - // Enable Button - enable_button_ptr_ = new QPushButton("Enable Manual Control"); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickEnableButton())); - state_layout->addWidget(enable_button_ptr_); - - // Gate Mode - auto * gate_prefix_label_ptr = new QLabel("GATE: "); - gate_prefix_label_ptr->setAlignment(Qt::AlignRight); - gate_mode_label_ptr_ = new QLabel("INIT"); - gate_mode_label_ptr_->setAlignment(Qt::AlignCenter); - state_layout->addWidget(gate_prefix_label_ptr); - state_layout->addWidget(gate_mode_label_ptr_); - - // Engage Status - auto * engage_prefix_label_ptr = new QLabel("Engage: "); - engage_prefix_label_ptr->setAlignment(Qt::AlignRight); - engage_status_label_ptr_ = new QLabel("INIT"); - engage_status_label_ptr_->setAlignment(Qt::AlignCenter); - state_layout->addWidget(engage_prefix_label_ptr); - state_layout->addWidget(engage_status_label_ptr_); - - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - state_layout->addWidget(gear_prefix_label_ptr); - state_layout->addWidget(gear_label_ptr_); - } - - auto * cruise_velocity_layout = new QHBoxLayout(); - // Velocity Limit - { - cruise_velocity_button_ptr_ = new QPushButton("Set Cruise Velocity"); - cruise_velocity_input_ = new QSpinBox(); - cruise_velocity_input_->setRange(-100.0, 100.0); - cruise_velocity_input_->setValue(0.0); - cruise_velocity_input_->setSingleStep(5.0); - connect(cruise_velocity_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickCruiseVelocity())); - cruise_velocity_layout->addWidget(cruise_velocity_button_ptr_); - cruise_velocity_layout->addWidget(cruise_velocity_input_); - cruise_velocity_layout->addWidget(new QLabel(" [km/h]")); - } - - steering_slider_ptr_ = new QDial(); - steering_slider_ptr_->setRange(-90, 90); - steering_slider_ptr_->setValue(0.0); - connect(steering_slider_ptr_, SIGNAL(valueChanged(int)), this, SLOT(onManualSteering())); - steering_angle_ptr_ = new QLabel(); - cruise_velocity_layout->addWidget(new QLabel("steering ")); - cruise_velocity_layout->addWidget(steering_slider_ptr_); - cruise_velocity_layout->addWidget(steering_angle_ptr_); - cruise_velocity_layout->addWidget(new QLabel(" [deg]")); - - // Layout - auto * v_layout = new QVBoxLayout; - v_layout->addLayout(state_layout); - v_layout->addLayout(cruise_velocity_layout); - setLayout(v_layout); - - auto * timer = new QTimer(this); - connect(timer, &QTimer::timeout, this, &ManualController::update); - timer->start(30); -} - -void ManualController::update() -{ - if (!raw_node_) return; - AckermannControlCommand ackermann; - { - ackermann.stamp = raw_node_->get_clock()->now(); - ackermann.lateral.steering_tire_angle = steering_angle_; - ackermann.longitudinal.speed = cruise_velocity_; - /** - * @brief Calculate desired acceleration by simple BackSteppingControl - * V = 0.5*(v-v_des)^2 >= 0 - * D[V] = (D[v] - a_des)*(v-v_des) <=0 - * a_des = k_const *(v - v_des) + a (k < 0 ) - */ - const double k = -0.5; - const double v = current_velocity_; - const double v_des = cruise_velocity_; - const double a = current_acceleration_; - const double a_des = k * (v - v_des) + a; - ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); - } - GearCommand gear_cmd; - { - const double eps = 0.001; - if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) { - gear_cmd.command = GearCommand::DRIVE; - } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) { - gear_cmd.command = GearCommand::REVERSE; - ackermann.longitudinal.acceleration *= -1.0; - } else { - gear_cmd.command = GearCommand::PARK; - } - } - pub_control_command_->publish(ackermann); - pub_gear_cmd_->publish(gear_cmd); -} - -void ManualController::onManualSteering() -{ - const double scale_factor = -0.25; - steering_angle_ = scale_factor * steering_slider_ptr_->sliderPosition() * M_PI / 180.0; - const QString steering_string = - QString::fromStdString(std::to_string(steering_angle_ * 180.0 / M_PI)); - steering_angle_ptr_->setText(steering_string); -} - -void ManualController::onClickCruiseVelocity() -{ - cruise_velocity_ = cruise_velocity_input_->value() / 3.6; -} - -void ManualController::onInitialize() -{ - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - sub_gate_mode_ = raw_node_->create_subscription( - "/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1)); - - sub_velocity_ = raw_node_->create_subscription( - "/vehicle/status/velocity_status", 1, std::bind(&ManualController::onVelocity, this, _1)); - - sub_engage_ = raw_node_->create_subscription( - "/api/autoware/get/engage", 10, std::bind(&ManualController::onEngageStatus, this, _1)); - - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&ManualController::onGear, this, _1)); - - client_engage_ = raw_node_->create_client("/api/autoware/set/engage"); - - pub_gate_mode_ = raw_node_->create_publisher("/control/gate_mode_cmd", rclcpp::QoS(1)); - - pub_control_command_ = raw_node_->create_publisher( - "/external/selected/control_cmd", rclcpp::QoS(1)); - - pub_gear_cmd_ = raw_node_->create_publisher("/external/selected/gear_cmd", 1); -} - -void ManualController::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) -{ - switch (msg->data) { - case tier4_control_msgs::msg::GateMode::AUTO: - gate_mode_label_ptr_->setText("Not Ready"); - gate_mode_label_ptr_->setStyleSheet("background-color: #00FF00;"); - break; - - case tier4_control_msgs::msg::GateMode::EXTERNAL: - gate_mode_label_ptr_->setText("Ready"); - gate_mode_label_ptr_->setStyleSheet("background-color: #FFFF00;"); - break; - - default: - gate_mode_label_ptr_->setText("UNKNOWN"); - gate_mode_label_ptr_->setStyleSheet("background-color: #FF0000;"); - break; - } -} -void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg) -{ - current_engage_ = msg->engage; - if (current_engage_) { - engage_status_label_ptr_->setText(QString::fromStdString("Ready")); - engage_status_label_ptr_->setStyleSheet("background-color: #FFFF00;"); - } else { - engage_status_label_ptr_->setText(QString::fromStdString("Not Ready")); - engage_status_label_ptr_->setStyleSheet("background-color: #00FF00;"); - } -} - -void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg) -{ - current_velocity_ = msg->longitudinal_velocity; -} - -void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - current_acceleration_ = msg->accel.accel.linear.x; -} - -void ManualController::onGear(const GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case GearReport::PARK: - gear_label_ptr_->setText("P"); - break; - case GearReport::REVERSE: - gear_label_ptr_->setText("R"); - break; - case GearReport::DRIVE: - gear_label_ptr_->setText("D"); - break; - case GearReport::LOW: - gear_label_ptr_->setText("L"); - break; - } -} - -void ManualController::onClickEnableButton() -{ - // gate mode - { - pub_gate_mode_->publish(tier4_control_msgs::build().data(GateMode::EXTERNAL)); - } - // engage - { - auto req = std::make_shared(); - req->engage = true; - RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); - if (!client_engage_->service_is_ready()) { - RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); - return; - } - client_engage_->async_send_request( - req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::ManualController, rviz_common::Panel) diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp deleted file mode 100644 index aaa625bff911e..0000000000000 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ /dev/null @@ -1,104 +0,0 @@ -// -// Copyright 2022 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - -#ifndef TOOLS__MANUAL_CONTROLLER_HPP_ -#define TOOLS__MANUAL_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include -#include -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using geometry_msgs::msg::Twist; -using tier4_control_msgs::msg::GateMode; -using EngageSrv = tier4_external_api_msgs::srv::Engage; -using autoware_auto_vehicle_msgs::msg::Engage; -using autoware_auto_vehicle_msgs::msg::GearReport; - -class ManualController : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit ManualController(QWidget * parent = nullptr); - void onInitialize() override; - -public Q_SLOTS: // NOLINT for Qt - void onClickCruiseVelocity(); - void onClickEnableButton(); - void onManualSteering(); - void update(); - -protected: - // Timer - rclcpp::TimerBase::SharedPtr timer_; - void onTimer(); - void onPublishControlCommand(); - void onGateMode(const GateMode::ConstSharedPtr msg); - void onVelocity(const VelocityReport::ConstSharedPtr msg); - void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); - void onEngageStatus(const Engage::ConstSharedPtr msg); - void onGear(const GearReport::ConstSharedPtr msg); - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_velocity_; - rclcpp::Subscription::SharedPtr sub_engage_; - rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_control_command_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; - rclcpp::Client::SharedPtr client_engage_; - rclcpp::Subscription::SharedPtr sub_gear_; - - double cruise_velocity_{0.0}; - double steering_angle_{0.0}; - double current_velocity_{0.0}; - double current_acceleration_{0.0}; - - QLabel * gate_mode_label_ptr_; - QLabel * gear_label_ptr_; - QLabel * engage_status_label_ptr_; - QPushButton * enable_button_ptr_; - QPushButton * cruise_velocity_button_ptr_; - QSpinBox * cruise_velocity_input_; - QDial * steering_slider_ptr_; - QLabel * steering_angle_ptr_; - - bool current_engage_{false}; -}; - -} // namespace rviz_plugins - -#endif // TOOLS__MANUAL_CONTROLLER_HPP_ diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 02e65ad759ed6..0000000000000 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -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 deleted file mode 100644 index 9189816141632..0000000000000 --- a/common/tier4_debug_rviz_plugin/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# 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 deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/tier4_debug_rviz_plugin/icons/classes/Float32MultiArrayStampedPieChart.png and /dev/null differ diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png deleted file mode 100644 index 6a67573717ae1..0000000000000 Binary files a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png and /dev/null 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 deleted file mode 100644 index 7cd7ca753f386..0000000000000 Binary files a/common/tier4_debug_rviz_plugin/images/float32_multi_array_stamped_pie_chart.png and /dev/null 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 deleted file mode 100644 index c8267c7051d9d..0000000000000 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp +++ /dev/null @@ -1,172 +0,0 @@ -// 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 deleted file mode 100644 index 37bf743e35f6a..0000000000000 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp +++ /dev/null @@ -1,143 +0,0 @@ -// 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 deleted file mode 100644 index 0960875d7eee2..0000000000000 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// 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 deleted file mode 100644 index 45b73d5b9815d..0000000000000 --- a/common/tier4_debug_rviz_plugin/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - 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 deleted file mode 100644 index e18900afc8d84..0000000000000 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - 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 deleted file mode 100644 index 0187cc3e22de0..0000000000000 --- a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp +++ /dev/null @@ -1,479 +0,0 @@ -// 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 deleted file mode 100644 index b1933ebb3e157..0000000000000 --- a/common/tier4_debug_rviz_plugin/src/jsk_overlay_utils.cpp +++ /dev/null @@ -1,225 +0,0 @@ -// 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 deleted file mode 100644 index 538dc0cbe7069..0000000000000 --- a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp +++ /dev/null @@ -1,198 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Copyright (c) 2014, JSK Lab -// All rights reserved. -// -// Software License Agreement (BSD License) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "tier4_debug_rviz_plugin/string_stamped.hpp" - -#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" - -#include -#include - -#include - -#include -#include -#include -#include - -namespace rviz_plugins -{ -StringStampedOverlayDisplay::StringStampedOverlayDisplay() -{ - const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); - - constexpr float hight_4k = 2160.0; - const float scale = static_cast(screen_info->height) / hight_4k; - const auto left = static_cast(std::round(1024 * scale)); - const auto top = static_cast(std::round(128 * scale)); - - property_text_color_ = new rviz_common::properties::ColorProperty( - "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); - property_left_ = new rviz_common::properties::IntProperty( - "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); - property_left_->setMin(0); - property_top_ = new rviz_common::properties::IntProperty( - "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); - property_top_->setMin(0); - - property_value_height_offset_ = new rviz_common::properties::IntProperty( - "Value height offset", 0, "Height offset of the plotter window", this, - SLOT(updateVisualization())); - property_font_size_ = new rviz_common::properties::IntProperty( - "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); - property_font_size_->setMin(1); - property_max_letter_num_ = new rviz_common::properties::IntProperty( - "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); - property_max_letter_num_->setMin(10); -} - -StringStampedOverlayDisplay::~StringStampedOverlayDisplay() -{ - if (initialized()) { - overlay_->hide(); - } -} - -void StringStampedOverlayDisplay::onInitialize() -{ - RTDClass::onInitialize(); - - static int count = 0; - rviz_common::UniformStringStream ss; - ss << "StringOverlayDisplayObject" << count++; - auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); - overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); - - overlay_->show(); - - const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); - overlay_->updateTextureSize(texture_size, texture_size); - overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); - overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); -} - -void StringStampedOverlayDisplay::onEnable() -{ - subscribe(); - overlay_->show(); -} - -void StringStampedOverlayDisplay::onDisable() -{ - unsubscribe(); - reset(); - overlay_->hide(); -} - -void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) -{ - (void)wall_dt; - (void)ros_dt; - - std::lock_guard message_lock(mutex_); - if (!last_msg_ptr_) { - return; - } - - // Display - QColor background_color; - background_color.setAlpha(0); - jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); - QImage hud = buffer.getQImage(*overlay_); - hud.fill(background_color); - - QPainter painter(&hud); - painter.setRenderHint(QPainter::Antialiasing, true); - - const int w = overlay_->getTextureWidth() - line_width_; - const int h = overlay_->getTextureHeight() - line_width_; - - // text - QColor text_color(property_text_color_->getColor()); - text_color.setAlpha(255); - painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); - QFont font = painter.font(); - font.setPixelSize(property_font_size_->getInt()); - font.setBold(true); - painter.setFont(font); - - // same as above, but align on right side - painter.drawText( - 0, std::min(property_value_height_offset_->getInt(), h - 1), w, - std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, - last_msg_ptr_->data.c_str()); - painter.end(); - updateVisualization(); -} - -void StringStampedOverlayDisplay::processMessage( - const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) -{ - if (!isEnabled()) { - return; - } - - { - std::lock_guard message_lock(mutex_); - last_msg_ptr_ = msg_ptr; - } - - queueRender(); -} - -void StringStampedOverlayDisplay::updateVisualization() -{ - const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); - overlay_->updateTextureSize(texture_size, texture_size); - overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); - overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt deleted file mode 100644 index cc7da7e322d19..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100644 index ed400e521e6aa..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/README.md +++ /dev/null @@ -1,72 +0,0 @@ -# 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 deleted file mode 100644 index 6b1214b133af4..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ /dev/null @@ -1,132 +0,0 @@ -# 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 - - 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 - - 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 - - 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 - - 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 - - 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 - - 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 deleted file mode 100644 index 37d70b494c74e..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// 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 deleted file mode 100644 index 7849e6049a077..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - 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 deleted file mode 100644 index ce5cbd13fc131..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - 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 deleted file mode 100644 index 72ecf361c96d5..0000000000000 --- a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp +++ /dev/null @@ -1,258 +0,0 @@ -// 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 deleted file mode 100644 index a93aff724bb19..0000000000000 Binary files a/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png and /dev/null differ diff --git a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt b/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt deleted file mode 100644 index 5b6e205bafed5..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -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 deleted file mode 100644 index d16c19c343017..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/README.md +++ /dev/null @@ -1,23 +0,0 @@ -# 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 deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/tier4_screen_capture_rviz_plugin/images/select_panels.png and /dev/null differ diff --git a/common/tier4_screen_capture_rviz_plugin/package.xml b/common/tier4_screen_capture_rviz_plugin/package.xml deleted file mode 100644 index 180bf9eedc9ca..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - 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 deleted file mode 100644 index fdf105d646497..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - 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 deleted file mode 100644 index cad828903e0d3..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ /dev/null @@ -1,220 +0,0 @@ -// 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 deleted file mode 100644 index 5c4d16f57da82..0000000000000 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp +++ /dev/null @@ -1,109 +0,0 @@ -// 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 deleted file mode 100644 index 020766340016e..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100644 index a6218f32b988c..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# 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 deleted file mode 100644 index 9431c2d422363..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/icons/classes/SimulatedClockPanel.png and /dev/null 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 deleted file mode 100644 index 8bf5e3a903751..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/images/select_clock_plugin.png and /dev/null 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 deleted file mode 100644 index a691602c42c3c..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/images/select_panels.png and /dev/null 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 deleted file mode 100644 index 39c3669c2ea31..0000000000000 Binary files a/common/tier4_simulated_clock_rviz_plugin/images/use_clock_plugin.png and /dev/null differ diff --git a/common/tier4_simulated_clock_rviz_plugin/package.xml b/common/tier4_simulated_clock_rviz_plugin/package.xml deleted file mode 100644 index d80b18a5895b0..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - 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 deleted file mode 100644 index 00caf2e5d49e0..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - 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 deleted file mode 100644 index ad698d925fdff..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.cpp +++ /dev/null @@ -1,153 +0,0 @@ -// -// 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 deleted file mode 100644 index b2ac332107202..0000000000000 --- a/common/tier4_simulated_clock_rviz_plugin/src/simulated_clock_panel.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// -// 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_state_rviz_plugin/CMakeLists.txt b/common/tier4_state_rviz_plugin/CMakeLists.txt index afe21f66291b2..6b569ddb6d162 100644 --- a/common/tier4_state_rviz_plugin/CMakeLists.txt +++ b/common/tier4_state_rviz_plugin/CMakeLists.txt @@ -13,8 +13,32 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/autoware_state_panel.cpp src/velocity_steering_factors_panel.cpp + src/custom_toggle_switch.cpp + src/custom_slider.cpp + src/custom_container.cpp + src/custom_button.cpp + src/custom_icon_label.cpp + src/custom_segmented_button.cpp + src/custom_segmented_button_item.cpp + src/custom_label.cpp + src/include/material_colors.hpp + src/include/autoware_state_panel.hpp + src/include/custom_button.hpp + src/include/custom_container.hpp + src/include/custom_icon_label.hpp + src/include/custom_label.hpp + src/include/custom_segmented_button_item.hpp + src/include/custom_segmented_button.hpp + src/include/custom_slider.hpp + src/include/custom_toggle_switch.hpp + src/include/velocity_steering_factors_panel.hpp ) +target_include_directories( + ${PROJECT_NAME} PUBLIC +) + + target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) @@ -22,6 +46,9 @@ target_link_libraries(${PROJECT_NAME} # 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 diff --git a/common/tier4_state_rviz_plugin/icons/assets/active.png b/common/tier4_state_rviz_plugin/icons/assets/active.png new file mode 100644 index 0000000000000..db9c81211abd5 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/active.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/crash.png b/common/tier4_state_rviz_plugin/icons/assets/crash.png new file mode 100644 index 0000000000000..18175a823ae4a Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/crash.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/danger.png b/common/tier4_state_rviz_plugin/icons/assets/danger.png new file mode 100644 index 0000000000000..baed346deea2d Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/danger.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/none.png b/common/tier4_state_rviz_plugin/icons/assets/none.png new file mode 100644 index 0000000000000..c44f9f485dbf1 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/none.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/pending.png b/common/tier4_state_rviz_plugin/icons/assets/pending.png new file mode 100644 index 0000000000000..3925162878fd5 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/pending.png differ diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 06d57bd947af3..1db152e9632f8 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -6,6 +6,7 @@ The autoware state rviz plugin package Hiroki OTA Takagi, Isamu + Khalil Selyan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index ba961b9ac5b00..f9fb7d3dd3958 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -14,15 +14,13 @@ // limitations under the License. // -#include "autoware_state_panel.hpp" +#include "include/autoware_state_panel.hpp" -#include -#include -#include -#include -#include #include +#include +#include + #include #include @@ -35,181 +33,59 @@ namespace rviz_plugins { AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(parent) { - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - auto * gear_layout = new QHBoxLayout; - gear_layout->addWidget(gear_prefix_label_ptr); - gear_layout->addWidget(gear_label_ptr_); - - // Velocity Limit - velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); - pub_velocity_limit_input_ = new QSpinBox(); - pub_velocity_limit_input_->setRange(-100.0, 100.0); - pub_velocity_limit_input_->setValue(0.0); - pub_velocity_limit_input_->setSingleStep(5.0); - connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); - - // Emergency Button - emergency_button_ptr_ = new QPushButton("Set Emergency"); - connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + // Panel Configuration + this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); // Layout - auto * v_layout = new QVBoxLayout; - auto * velocity_limit_layout = new QHBoxLayout(); - v_layout->addWidget(makeOperationModeGroup()); - v_layout->addWidget(makeControlModeGroup()); - { - auto * h_layout = new QHBoxLayout(); - h_layout->addWidget(makeRoutingGroup()); - h_layout->addWidget(makeLocalizationGroup()); - h_layout->addWidget(makeMotionGroup()); - h_layout->addWidget(makeFailSafeGroup()); - v_layout->addLayout(h_layout); - } - - v_layout->addLayout(gear_layout); - velocity_limit_layout->addWidget(velocity_limit_button_ptr_); - velocity_limit_layout->addWidget(pub_velocity_limit_input_); - velocity_limit_layout->addWidget(new QLabel(" [km/h]")); - velocity_limit_layout->addWidget(emergency_button_ptr_); - v_layout->addLayout(velocity_limit_layout); - setLayout(v_layout); -} - -QGroupBox * AutowareStatePanel::makeOperationModeGroup() -{ - auto * group = new QGroupBox("OperationMode"); - auto * grid = new QGridLayout; - - operation_mode_label_ptr_ = new QLabel("INIT"); - operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); - operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); - - auto_button_ptr_ = new QPushButton("AUTO"); - auto_button_ptr_->setCheckable(true); - connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); - grid->addWidget(auto_button_ptr_, 0, 1); - - stop_button_ptr_ = new QPushButton("STOP"); - stop_button_ptr_->setCheckable(true); - connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_button_ptr_, 0, 2); - - local_button_ptr_ = new QPushButton("LOCAL"); - local_button_ptr_->setCheckable(true); - connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); - grid->addWidget(local_button_ptr_, 1, 1); - - remote_button_ptr_ = new QPushButton("REMOTE"); - remote_button_ptr_->setCheckable(true); - connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); - grid->addWidget(remote_button_ptr_, 1, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeControlModeGroup() -{ - auto * group = new QGroupBox("AutowareControl"); - auto * grid = new QGridLayout; - - control_mode_label_ptr_ = new QLabel("INIT"); - control_mode_label_ptr_->setAlignment(Qt::AlignCenter); - control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(control_mode_label_ptr_, 0, 0); - - enable_button_ptr_ = new QPushButton("Enable"); - enable_button_ptr_->setCheckable(true); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); - grid->addWidget(enable_button_ptr_, 0, 1); - - disable_button_ptr_ = new QPushButton("Disable"); - disable_button_ptr_->setCheckable(true); - connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); - grid->addWidget(disable_button_ptr_, 0, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing"); - auto * grid = new QGridLayout; - routing_label_ptr_ = new QLabel("INIT"); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_button_ptr_ = new QPushButton("Clear Route"); - clear_route_button_ptr_->setCheckable(true); - connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - grid->addWidget(clear_route_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeLocalizationGroup() -{ - auto * group = new QGroupBox("Localization"); - auto * grid = new QGridLayout; - - localization_label_ptr_ = new QLabel("INIT"); - localization_label_ptr_->setAlignment(Qt::AlignCenter); - localization_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(localization_label_ptr_, 0, 0); - - init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); - connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); - grid->addWidget(init_by_gnss_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeMotionGroup() -{ - auto * group = new QGroupBox("Motion"); - auto * grid = new QGridLayout; - - motion_label_ptr_ = new QLabel("INIT"); - motion_label_ptr_->setAlignment(Qt::AlignCenter); - motion_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(motion_label_ptr_, 0, 0); - - accept_start_button_ptr_ = new QPushButton("Accept Start"); - accept_start_button_ptr_->setCheckable(true); - connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); - grid->addWidget(accept_start_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeFailSafeGroup() -{ - auto * group = new QGroupBox("FailSafe"); - auto * grid = new QGridLayout; - - mrm_state_label_ptr_ = new QLabel("INIT"); - mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_state_label_ptr_, 0, 0); - - mrm_behavior_label_ptr_ = new QLabel("INIT"); - mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_behavior_label_ptr_, 1, 0); - - group->setLayout(grid); - return group; + // Create a new container widget + QWidget * containerWidget = new QWidget(this); + containerWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + containerWidget->setStyleSheet( + QString("QWidget { background-color: %1; color: %2; }") + .arg(autoware::state_rviz_plugin::colors::default_colors.background.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + + auto * containerLayout = new QVBoxLayout(containerWidget); + // Set the alignment of the layout + containerLayout->setAlignment(Qt::AlignTop); + containerLayout->setSpacing(1); + + auto * operation_mode_group = makeOperationModeGroup(); + auto * diagnostic_v_layout = new QVBoxLayout; + auto * localization_group = makeLocalizationGroup(); + auto * motion_group = makeMotionGroup(); + auto * fail_safe_group = makeFailSafeGroup(); + auto * routing_group = makeRoutingGroup(); + auto * velocity_limit_group = makeVelocityLimitGroup(); + // auto * diagnostic_group = makeDiagnosticGroup(); + + diagnostic_v_layout->addLayout(routing_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(localization_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(motion_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(fail_safe_group); + + // containerLayout->addLayout(diagnostic_group); + + containerLayout->addLayout(operation_mode_group); + // containerLayout->addSpacing(5); + containerLayout->addLayout(diagnostic_v_layout); + // main_v_layout->addSpacing(5); + containerLayout->addLayout(velocity_limit_group); + + // Create a QScrollArea + QScrollArea * scrollArea = new QScrollArea(this); + scrollArea->setWidgetResizable(true); + scrollArea->setWidget(containerWidget); + + // Main layout for AutowareStatePanel + QVBoxLayout * mainLayout = new QVBoxLayout(this); + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); } void AutowareStatePanel::onInitialize() @@ -268,9 +144,9 @@ void AutowareStatePanel::onInitialize() "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMRMState, this, _1)); - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); + // // Diagnostics + // sub_diag_ = raw_node_->create_subscription( + // "/diagnostics", 10, std::bind(&AutowareStatePanel::onDiagnostics, this, _1)); sub_emergency_ = raw_node_->create_subscription( "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); @@ -280,174 +156,498 @@ void AutowareStatePanel::onInitialize() pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} -void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - auto changeButtonState = [this]( - QPushButton * button, const bool is_desired_mode_available, - const uint8_t current_mode = OperationModeState::UNKNOWN, - const uint8_t desired_mode = OperationModeState::STOP) { - if (is_desired_mode_available && current_mode != desired_mode) { - activateButton(button); + QObject::connect(segmented_button, &CustomSegmentedButton::buttonClicked, this, [this](int id) { + const QList buttons = segmented_button->getButtonGroup()->buttons(); + + // Check if the button ID is within valid range + if (id < 0 || id >= buttons.size()) { + return; + } + + // Ensure the button is not null + QAbstractButton * abstractButton = segmented_button->getButtonGroup()->button(id); + if (!abstractButton) { + return; + } + + QPushButton * button = qobject_cast(abstractButton); + if (button) { + // Call the corresponding function for each button + if (button == auto_button_ptr_) { + onClickAutonomous(); + } else if (button == local_button_ptr_) { + onClickLocal(); + } else if (button == remote_button_ptr_) { + onClickRemote(); + } else if (button == stop_button_ptr_) { + onClickStop(); + } } else { - deactivateButton(button); + // qDebug() << "Button not found with ID:" << id; } - }; + }); +} - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; +QVBoxLayout * AutowareStatePanel::makeOperationModeGroup() +{ + control_mode_switch_ptr_ = new CustomToggleSwitch(this); + connect( + control_mode_switch_ptr_, &QCheckBox::stateChanged, this, + &AutowareStatePanel::onSwitchStateChanged); + + control_mode_label_ptr_ = new QLabel("Autoware Control"); + control_mode_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + CustomContainer * group1 = new CustomContainer(this); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(control_mode_switch_ptr_); + horizontal_layout->addWidget(control_mode_label_ptr_); + + // add switch and label to the container + group1->setContentsMargins(0, 0, 0, 10); + group1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + // Create the CustomSegmentedButton + segmented_button = new CustomSegmentedButton(this); + auto_button_ptr_ = segmented_button->addButton("Auto"); + local_button_ptr_ = segmented_button->addButton("Local"); + remote_button_ptr_ = segmented_button->addButton("Remote"); + stop_button_ptr_ = segmented_button->addButton("Stop"); + + QVBoxLayout * groupLayout = new QVBoxLayout; + // set these widgets to show up at the left and not stretch more than needed + groupLayout->setAlignment(Qt::AlignCenter); + groupLayout->setContentsMargins(10, 0, 0, 0); + groupLayout->addWidget(group1); + // groupLayout->addSpacing(5); + groupLayout->addWidget(segmented_button, 0, Qt::AlignCenter); + return groupLayout; +} - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; +QVBoxLayout * AutowareStatePanel::makeRoutingGroup() +{ + auto * group = new QVBoxLayout; - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + auto * custom_container = new CustomContainer(this); - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; + routing_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + clear_route_button_ptr_ = new CustomElevatedButton("Clear Route"); + clear_route_button_ptr_->setCheckable(true); + clear_route_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } + routing_label_ptr_ = new QLabel("Routing | Unknown"); + routing_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); - updateLabel(operation_mode_label_ptr_, text, style_sheet); + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); - // Control Mode - if (msg->is_autoware_control_enabled) { - updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green - } else { - updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow - } + horizontal_layout->addWidget(routing_icon); + horizontal_layout->addWidget(routing_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(clear_route_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} - // Button - changeButtonState( - auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); - changeButtonState( - stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); - changeButtonState( - local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); - changeButtonState( - remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); - - changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); - changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); +QVBoxLayout * AutowareStatePanel::makeLocalizationGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + init_by_gnss_button_ptr_ = new CustomElevatedButton("Initialize with GNSS"); + init_by_gnss_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + + localization_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + localization_label_ptr_ = new QLabel("Localization | Unknown"); + localization_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(localization_icon); + horizontal_layout->addWidget(localization_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(init_by_gnss_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + return group; +} + +QVBoxLayout * AutowareStatePanel::makeMotionGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + accept_start_button_ptr_ = new CustomElevatedButton("Accept Start"); + accept_start_button_ptr_->setCheckable(true); + accept_start_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); + + motion_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + motion_label_ptr_ = new QLabel("Motion | Unknown"); + motion_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + horizontal_layout->setAlignment(Qt::AlignLeft); + + horizontal_layout->addWidget(motion_icon); + horizontal_layout->addWidget(motion_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(accept_start_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} + +QVBoxLayout * AutowareStatePanel::makeFailSafeGroup() +{ + auto * group = new QVBoxLayout; + auto * v_layout = new QVBoxLayout; + auto * custom_container1 = new CustomContainer(this); + auto * custom_container2 = new CustomContainer(this); + + mrm_state_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + mrm_behavior_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + + mrm_state_label_ptr_ = new QLabel("MRM State | Unknown"); + mrm_behavior_label_ptr_ = new QLabel("MRM Behavior | Unknown"); + + // change text color + mrm_state_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + mrm_behavior_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(mrm_state_icon); + horizontal_layout->addWidget(mrm_state_label_ptr_); + + custom_container1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + auto * horizontal_layout2 = new QHBoxLayout; + horizontal_layout2->setSpacing(10); + horizontal_layout2->setContentsMargins(0, 0, 0, 0); + + horizontal_layout2->addWidget(mrm_behavior_icon); + horizontal_layout2->addWidget(mrm_behavior_label_ptr_); + + custom_container2->getLayout()->addLayout(horizontal_layout2, 0, 0, 1, 1, Qt::AlignLeft); + + v_layout->addWidget(custom_container1); + // v_layout->addSpacing(5); + v_layout->addWidget(custom_container2); + + group->setContentsMargins(10, 0, 0, 0); + + group->addLayout(v_layout); + return group; +} + +/* QVBoxLayout * AutowareStatePanel::makeDiagnosticGroup() +{ + auto * group = new QVBoxLayout; + + // Create the scroll area + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setFixedHeight(66); // Adjust the height as needed + scrollArea->setWidgetResizable(true); + scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded); + scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + + // Create a widget to contain the layout + QWidget * scrollAreaWidgetContents = new QWidget; + // use layout to contain the diagnostic label and the diagnostic level + diagnostic_layout_ = new QVBoxLayout(); + diagnostic_layout_->setSpacing(5); // Set space between items + diagnostic_layout_->setContentsMargins(5, 5, 5, 5); // Set margins within the layout + + // Add a QLabel to display the title of what this is + auto * tsm_label_title_ptr_ = new QLabel("Topic State Monitor: "); + // Set the layout on the widget + scrollAreaWidgetContents->setLayout(diagnostic_layout_); + + // Set the widget on the scroll area + scrollArea->setWidget(scrollAreaWidgetContents); + + group->addWidget(tsm_label_title_ptr_); + group->addWidget(scrollArea); + + return group; +} */ + +QVBoxLayout * AutowareStatePanel::makeVelocityLimitGroup() +{ + // Velocity Limit + velocity_limit_setter_ptr_ = new QLabel("Set Velocity Limit"); + // set its width to fit the text + velocity_limit_setter_ptr_->setFixedWidth( + velocity_limit_setter_ptr_->fontMetrics().horizontalAdvance("Set Velocity Limit")); + + velocity_limit_value_label_ = new QLabel("0"); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance("0")); + + CustomSlider * pub_velocity_limit_slider_ = new CustomSlider(Qt::Horizontal); + pub_velocity_limit_slider_->setRange(0, 100); + pub_velocity_limit_slider_->setValue(0); + pub_velocity_limit_slider_->setMaximumWidth(300); + + connect(pub_velocity_limit_slider_, &QSlider::sliderPressed, this, [this]() { + sliderIsDragging = true; // User starts dragging the handle + }); + + connect(pub_velocity_limit_slider_, &QSlider::sliderReleased, this, [this]() { + sliderIsDragging = false; // User finished dragging + onClickVelocityLimit(); // Call when handle is released after dragging + }); + + connect(pub_velocity_limit_slider_, &QSlider::valueChanged, this, [this](int value) { + this->velocity_limit_value_label_->setText(QString::number(value)); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance(QString::number(value))); + if (!sliderIsDragging) { // If the value changed without dragging, it's a click on the track + onClickVelocityLimit(); // Call the function immediately since it's not a drag operation + } + }); + + // Emergency Button + emergency_button_ptr_ = new CustomElevatedButton("Set Emergency"); + + emergency_button_ptr_->setCursor(Qt::PointingHandCursor); + // set fixed width to fit the text + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + auto * utility_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout; + QLabel * velocity_limit_label = new QLabel("km/h"); + + velocity_limit_layout->addWidget(pub_velocity_limit_slider_); + velocity_limit_layout->addSpacing(5); + velocity_limit_layout->addWidget(velocity_limit_value_label_); + velocity_limit_layout->addWidget(velocity_limit_label); + + // Velocity Limit layout + utility_layout->addSpacing(15); + utility_layout->addWidget(velocity_limit_setter_ptr_); + utility_layout->addSpacing(10); + utility_layout->addLayout(velocity_limit_layout); + utility_layout->addSpacing(25); + utility_layout->addWidget(emergency_button_ptr_); + + utility_layout->setContentsMargins(15, 0, 15, 0); + + return utility_layout; +} + +void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + auto updateButtonState = []( + CustomSegmentedButtonItem * button, bool is_available, + uint8_t current_mode, uint8_t desired_mode, bool disable) { + bool is_checked = (current_mode == desired_mode); + button->setHovered(false); + + button->setActivated(is_checked); + button->setChecked(is_checked); + button->setDisabledButton(disable || !is_available); + button->setCheckableButton(!disable && is_available && !is_checked); + }; + + bool disable_buttons = msg->is_in_transition; + + updateButtonState( + auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS, + disable_buttons); + updateButtonState( + stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP, + disable_buttons); + updateButtonState( + local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL, + disable_buttons); + updateButtonState( + remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE, + disable_buttons); + + // toggle switch for control mode + auto changeToggleSwitchState = [](CustomToggleSwitch * toggle_switch, const bool is_enabled) { + // Flick the switch without triggering its function + bool old_state = toggle_switch->blockSignals(true); + toggle_switch->setCheckedState(!is_enabled); + toggle_switch->blockSignals(old_state); + }; + + if (!msg->is_in_transition) { + // would cause an on/off/on flicker if in transition + changeToggleSwitchState(control_mode_switch_ptr_, !msg->is_autoware_control_enabled); + } } void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString route_state = "Routing | Unknown"; + switch (msg->state) { case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + route_state = "Routing | Unset"; break; case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + route_state = "Routing | Set"; break; case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + route_state = "Routing | Arrived"; break; case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + state = Pending; + route_state = "Routing | Changing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(routing_label_ptr_, text, style_sheet); + routing_icon->updateStyle(state, bgColor); + routing_label_ptr_->setText(route_state); if (msg->state == RouteState::SET) { activateButton(clear_route_button_ptr_); } else { + clear_route_button_ptr_->setStyleSheet( + QString("QPushButton {" + "background-color: %1;color: %2;" + "border: 2px solid %3;" + "font-weight: bold;" + "}") + .arg(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()) + .arg( + autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str())); deactivateButton(clear_route_button_ptr_); } } void AutowareStatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString localization_state = "Localization | Unknown"; + switch (msg->state) { case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + localization_state = "Localization | Uninitialized"; break; - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange + case LocalizationInitializationState::INITIALIZED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + localization_state = "Localization | Initialized"; break; - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green + case LocalizationInitializationState::INITIALIZING: + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + localization_state = "Localization | Initializing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(localization_label_ptr_, text, style_sheet); + localization_icon->updateStyle(state, bgColor); + localization_label_ptr_->setText(localization_state); } void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString motion_state = "Motion | Unknown"; + switch (msg->state) { case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + motion_state = "Motion | Starting"; break; - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange + case MotionState::MOVING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + motion_state = "Motion | Moving"; break; - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green + case MotionState::STOPPED: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + motion_state = "Motion | Stopped"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(motion_label_ptr_, text, style_sheet); + motion_icon->updateStyle(state, bgColor); + motion_label_ptr_->setText(motion_state); if (msg->state == MotionState::STARTING) { activateButton(accept_start_button_ptr_); @@ -458,94 +658,85 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) { - // state - { - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; + IconState state; + QColor bgColor; + QString mrm_state = "MRM State | Unknown"; - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; + switch (msg->state) { + case MRMState::NONE: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Inactive"; + break; - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + case MRMState::MRM_OPERATING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Operating"; + break; - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; + case MRMState::MRM_SUCCEEDED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_state = "MRM State | Successful"; + break; - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + case MRMState::MRM_FAILED: + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_state = "MRM State | Failed"; + break; - updateLabel(mrm_state_label_ptr_, text, style_sheet); + default: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Unknown"; + break; } + mrm_state_icon->updateStyle(state, bgColor); + mrm_state_label_ptr_->setText(mrm_state); + // behavior { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString mrm_behavior = "MRM Behavior | Unknown"; + switch (msg->behavior) { case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Inactive"; break; case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_behavior = "MRM Behavior | Pull Over"; break; case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + mrm_behavior = "MRM Behavior | Comfortable Stop"; break; case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_behavior = "MRM Behavior | Emergency Stop"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Unknown"; break; } - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void AutowareStatePanel::onShift( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; + mrm_behavior_icon->updateStyle(state, bgColor); + mrm_behavior_label_ptr_->setText(mrm_behavior); } } @@ -554,18 +745,37 @@ void AutowareStatePanel::onEmergencyStatus( { current_emergency_ = msg->emergency; if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + emergency_button_ptr_->updateStyle( + "Clear Emergency", + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str())); } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + emergency_button_ptr_->updateStyle( + "Set Emergency", QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_tint.c_str())); + } +} + +void AutowareStatePanel::onSwitchStateChanged(int state) +{ + if (state == 0) { + // call the control mode function + onClickDirectControl(); + } else if (state == 2) { + onClickAutowareControl(); } } void AutowareStatePanel::onClickVelocityLimit() { auto velocity_limit = std::make_shared(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + velocity_limit->max_velocity = velocity_limit_value_label_->text().toDouble() / 3.6; pub_velocity_limit_->publish(*velocity_limit); } diff --git a/common/tier4_state_rviz_plugin/src/custom_button.cpp b/common/tier4_state_rviz_plugin/src/custom_button.cpp new file mode 100644 index 0000000000000..0ef2628577135 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_button.cpp @@ -0,0 +1,129 @@ +// 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 "include/custom_button.hpp" + +#include "src/include/material_colors.hpp" + +CustomElevatedButton::CustomElevatedButton( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor, QWidget * parent) +: QPushButton(text, parent), + backgroundColor(bgColor), + textColor(textColor), + hoverColor(hoverColor), + disabledBgColor(disabledBgColor), + disabledTextColor(disabledTextColor) +{ + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + // set font weight to bold and size to 12 + QFont font = this->font(); + font.setWeight(QFont::Bold); + font.setFamily("Roboto"); + setFont(font); + + // Set up drop shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); + shadowEffect->setOffset(3, 3); + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); + setGraphicsEffect(shadowEffect); +} + +QSize CustomElevatedButton::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 45; // Adding padding + int height = textHeight + 25; // Adding padding + return QSize(width, height); +} + +QSize CustomElevatedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomElevatedButton::updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor) +{ + setText(text); + backgroundColor = bgColor; + this->textColor = textColor; + this->hoverColor = hoverColor; + this->disabledBgColor = disabledBgColor; + this->disabledTextColor = disabledTextColor; + update(); // Force repaint +} + +void CustomElevatedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + QStyleOption opt; + opt.initFrom(this); + QRect r = rect(); + + QColor buttonColor; + QColor currentTextColor = textColor; + if (!isEnabled()) { + buttonColor = disabledBgColor; + currentTextColor = disabledTextColor; + } else if (isHovered) { + buttonColor = hoverColor; + } else { + buttonColor = backgroundColor; + } + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw button background + QPainterPath backgroundPath; + backgroundPath.addRoundedRect(r, cornerRadius, cornerRadius); + if (!isEnabled()) { + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + painter.setOpacity(0.12); + } else { + painter.setBrush(buttonColor); + } + painter.setPen(Qt::NoPen); + painter.drawPath(backgroundPath); + + // Draw button text + if (!isEnabled()) { + painter.setOpacity(0.38); + } + painter.setPen(currentTextColor); + painter.drawText(r, Qt::AlignCenter, text()); +} + +void CustomElevatedButton::enterEvent(QEvent * event) +{ + isHovered = true; + update(); + QPushButton::enterEvent(event); +} + +void CustomElevatedButton::leaveEvent(QEvent * event) +{ + isHovered = false; + update(); + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_container.cpp b/common/tier4_state_rviz_plugin/src/custom_container.cpp new file mode 100644 index 0000000000000..0b0aa1ccd6ed5 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_container.cpp @@ -0,0 +1,70 @@ +// 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 "include/custom_container.hpp" + +#include "src/include/material_colors.hpp" + +CustomContainer::CustomContainer(QWidget * parent) : QFrame(parent), cornerRadius(15) +{ + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setContentsMargins(0, 0, 0, 0); + layout = new QGridLayout(this); + layout->setMargin(0); // Margin between the border and child widgets + layout->setSpacing(0); // Spacing between child widgets + layout->setContentsMargins(10, 0, 10, 0); // Margin between the border and the layout + setLayout(layout); +} + +void CustomContainer::setCornerRadius(int radius) +{ + cornerRadius = radius; + update(); +} + +int CustomContainer::getCornerRadius() const +{ + return cornerRadius; +} + +QGridLayout * CustomContainer::getLayout() const +{ + return layout; // Provide access to the layout +} + +QSize CustomContainer::sizeHint() const +{ + QSize size = layout->sizeHint(); + int width = size.width() + 20; // Adding padding + int height = size.height() + 20; // Adding padding + return QSize(width, height); +} + +QSize CustomContainer::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomContainer::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); // Use height for rounded corners + painter.setPen(Qt::NoPen); + painter.setBrush(QColor( + autoware::state_rviz_plugin::colors::default_colors.surface.c_str())); // Background color + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp new file mode 100644 index 0000000000000..6e4d32d40f7fb --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp @@ -0,0 +1,80 @@ +// 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 "include/custom_icon_label.hpp" + +#include + +CustomIconLabel::CustomIconLabel(const QColor & bgColor, QWidget * parent) +: QLabel(parent), backgroundColor(bgColor) +{ + loadIcons(); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + setAlignment(Qt::AlignCenter); + setFixedSize(35, 35); +} + +void CustomIconLabel::loadIcons() +{ + std::string packagePath = ament_index_cpp::get_package_share_directory("tier4_state_rviz_plugin"); + + iconMap[Active] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/active.png")); + iconMap[Pending] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/pending.png")); + iconMap[Danger] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/danger.png")); + iconMap[None] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/none.png")); + iconMap[Crash] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/crash.png")); + + icon = iconMap[None]; // Default icon +} + +void CustomIconLabel::updateStyle(IconState state, const QColor & bgColor) +{ + icon = iconMap[state]; + backgroundColor = bgColor; + update(); // Force repaint +} + +QSize CustomIconLabel::sizeHint() const +{ + int size = qMax(icon.width(), icon.height()) + 20; // Adding padding + return QSize(size, size); +} + +QSize CustomIconLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomIconLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int diameter = qMin(width(), height()); + int radius = diameter / 2; + + // Draw background circle + QPainterPath path; + path.addEllipse(width() / 2 - radius, height() / 2 - radius, diameter, diameter); + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + painter.drawPath(path); + + // Draw icon in the center + if (!icon.isNull()) { + QSize iconSize = icon.size().scaled(diameter * 0.6, diameter * 0.6, Qt::KeepAspectRatio); + QPoint iconPos((width() - iconSize.width()) / 2, (height() - iconSize.height()) / 2); + painter.drawPixmap( + iconPos, icon.scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + } +} diff --git a/common/tier4_state_rviz_plugin/src/custom_label.cpp b/common/tier4_state_rviz_plugin/src/custom_label.cpp new file mode 100644 index 0000000000000..1f96fc0d45095 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_label.cpp @@ -0,0 +1,82 @@ +// 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 "include/custom_label.hpp" + +#include "src/include/material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +CustomLabel::CustomLabel(const QString & text, QWidget * parent) : QLabel(text, parent) +{ + setFont(QFont("Roboto", 10, QFont::Bold)); // Set the font as needed + setAlignment(Qt::AlignCenter); + + // Add shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); // Blur radius for a smoother shadow + shadowEffect->setOffset(3, 3); // Offset for the shadow + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); // Shadow color + setGraphicsEffect(shadowEffect); +} + +QSize CustomLabel::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 40; // Adding padding + int height = textHeight; // Adding padding + return QSize(width, height); +} + +QSize CustomLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw background + QPainterPath path; + path.addRoundedRect(rect().adjusted(1, 1, -1, -1), cornerRadius, cornerRadius); + + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + + painter.drawPath(path); + + // Set text color and draw text + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomLabel::updateStyle( + const QString & text, const QColor & bg_color, const QColor & text_color) +{ + setText(text); + backgroundColor = bg_color; + textColor = text_color; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp new file mode 100644 index 0000000000000..8063bdc0fbc28 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -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. +#include "include/custom_segmented_button.hpp" + +#include + +CustomSegmentedButton::CustomSegmentedButton(QWidget * parent) +: QWidget(parent), buttonGroup(new QButtonGroup(this)), layout(new QHBoxLayout(this)) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + layout->setContentsMargins(0, 0, 0, 0); // Ensure no margins around the layout + layout->setSpacing(0); // Ensure no spacing between buttons + + setLayout(layout); + + buttonGroup->setExclusive(true); + + connect( + buttonGroup, QOverload::of(&QButtonGroup::idClicked), this, + &CustomSegmentedButton::buttonClicked); +} + +CustomSegmentedButtonItem * CustomSegmentedButton::addButton(const QString & text) +{ + CustomSegmentedButtonItem * button = new CustomSegmentedButtonItem(text); + layout->addWidget(button); + buttonGroup->addButton(button, layout->count() - 1); + + return button; +} + +QButtonGroup * CustomSegmentedButton::getButtonGroup() const +{ + return buttonGroup; +} + +QSize CustomSegmentedButton::sizeHint() const +{ + return QSize(400, 40); // Adjust the size hint as needed + + // return QSize( + // layout->count() * (layout->itemAt(0)->widget()->width()), + // layout->itemAt(0)->widget()->height() + 10); +} + +QSize CustomSegmentedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomSegmentedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); + + painter.setPen(Qt::NoPen); + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())); + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp new file mode 100644 index 0000000000000..f2d15260c4e41 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -0,0 +1,186 @@ +// 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 "include/custom_segmented_button_item.hpp" + +CustomSegmentedButtonItem::CustomSegmentedButtonItem(const QString & text, QWidget * parent) +: QPushButton(text, parent), + bgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())), + checkedBgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.secondary_container.c_str())), + inactiveTextColor(QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())), + activeTextColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())), + isHovered(false), + isActivated(false), + isDisabled(false) + +{ + setCheckable(true); + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setCursor(Qt::PointingHandCursor); +} + +void CustomSegmentedButtonItem::setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText) +{ + bgColor = bg; + checkedBgColor = checkedBg; + activeTextColor = activeText; + inactiveTextColor = inactiveText; + update(); +} + +// void CustomSegmentedButtonItem::updateSize() +// { +// QFontMetrics fm(font()); +// int width = fm.horizontalAdvance(text()) + 40; // Add padding +// int height = fm.height() + 20; // Add padding +// setFixedSize(width, height); +// } + +void CustomSegmentedButtonItem::setHovered(bool hovered) +{ + isHovered = hovered; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setCheckableButton(bool checkable) +{ + setCheckable(checkable); + updateCheckableState(); +} + +void CustomSegmentedButtonItem::updateCheckableState() +{ + setCursor( + isDisabled ? Qt::ForbiddenCursor + : (isCheckable() ? Qt::PointingHandCursor : Qt::ForbiddenCursor)); + update(); +} + +void CustomSegmentedButtonItem::setDisabledButton(bool disabled) +{ + isDisabled = disabled; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setActivated(bool activated) +{ + isActivated = activated; + update(); +} + +void CustomSegmentedButtonItem::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Adjust opacity for disabled state + if (isDisabled) { + painter.setOpacity(0.3); + } else { + painter.setOpacity(1.0); + } + + // Determine the button's color based on its state + QColor buttonColor; + if (isDisabled) { + buttonColor = disabledBgColor; + } else if (isHovered && !isChecked() && isCheckable()) { + buttonColor = hoverColor; + } else if (isActivated) { + buttonColor = checkedBgColor; + } else { + buttonColor = isChecked() ? checkedBgColor : bgColor; + } + + // Determine if this is the first or last button + bool isFirstButton = false; + bool isLastButton = false; + + QHBoxLayout * parentLayout = qobject_cast(parentWidget()->layout()); + if (parentLayout) { + int index = parentLayout->indexOf(this); + isFirstButton = (index == 0); + isLastButton = (index == parentLayout->count() - 1); + } + + // Draw button background + + QRect buttonRect = rect().adjusted(0, 1, 0, -1); + + if (isFirstButton) { + buttonRect.setLeft(buttonRect.left() + 1); + } + if (isLastButton) { + buttonRect.setRight(buttonRect.right() - 1); + } + + QPainterPath path; + double radius = (height() - 2) / 2; + + path.setFillRule(Qt::WindingFill); + if (isFirstButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), + buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom Right + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), buttonRect.top(), radius, + radius)); // Top Right + } else if (isLastButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left()), buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom left + path.addRect(QRect((buttonRect.left()), buttonRect.top(), radius, + radius)); // Top Left + } else { + path.addRect(buttonRect); + } + painter.fillPath(path, buttonColor); + + // Draw button border + QPen pen(QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()), 1); + pen.setJoinStyle(Qt::RoundJoin); + pen.setCapStyle(Qt::RoundCap); + painter.setPen(pen); + painter.drawPath(path.simplified()); + + // Draw button text + QColor textColor = (isChecked() ? activeTextColor : inactiveTextColor); + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomSegmentedButtonItem::enterEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = true; + update(); + } + QPushButton::enterEvent(event); +} + +void CustomSegmentedButtonItem::leaveEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = false; + update(); + } + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_slider.cpp b/common/tier4_state_rviz_plugin/src/custom_slider.cpp new file mode 100644 index 0000000000000..cf3f7c3d4638f --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_slider.cpp @@ -0,0 +1,102 @@ +// 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 "include/custom_slider.hpp" + +CustomSlider::CustomSlider(Qt::Orientation orientation, QWidget * parent) +: QSlider(orientation, parent) +{ + setMinimumHeight(40); // Ensure there's enough space for the custom track +} + +void CustomSlider::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(Qt::NoPen); + + // Initialize style option + QStyleOptionSlider opt; + initStyleOption(&opt); + + QRect grooveRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderGroove, this); + int centerY = grooveRect.center().y(); + QRect handleRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderHandle, this); + + int value = this->value(); + int minValue = this->minimum(); + int maxValue = this->maximum(); + + int trackThickness = 14; + int gap = 8; + + QRect beforeRect( + grooveRect.x(), centerY - trackThickness / 2, handleRect.center().x() - grooveRect.x() - gap, + trackThickness); + + QRect afterRect( + handleRect.center().x() + gap, centerY - trackThickness / 2, + grooveRect.right() - handleRect.center().x() - gap, trackThickness); + + QColor inactiveTrackColor( + autoware::state_rviz_plugin::colors::default_colors.primary_container.c_str()); + QColor activeTrackColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor handleColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + // only draw the active track if the value is more than the gap from the minimum + if (value > minValue + gap / 2) { + QPainterPath beforePath; + beforePath.moveTo(beforeRect.left(), centerY + trackThickness / 2); // Start from bottom-left + beforePath.quadTo( + beforeRect.left(), centerY - trackThickness / 2, beforeRect.left() + trackThickness * 0.5, + centerY - trackThickness / 2); + beforePath.lineTo(beforeRect.right() - trackThickness * 0.1, centerY - trackThickness / 2); + beforePath.quadTo( + beforeRect.right(), centerY - trackThickness / 2, beforeRect.right(), centerY); + beforePath.quadTo( + beforeRect.right(), centerY + trackThickness / 2, beforeRect.right() - trackThickness * 0.1, + centerY + trackThickness / 2); + beforePath.lineTo(beforeRect.left() + trackThickness * 0.5, centerY + trackThickness / 2); + beforePath.quadTo(beforeRect.left(), centerY + trackThickness / 2, beforeRect.left(), centerY); + painter.fillPath(beforePath, activeTrackColor); + } + + if (value < maxValue - gap / 2) { + QPainterPath afterPath; + afterPath.moveTo(afterRect.left(), centerY + trackThickness / 2); + afterPath.quadTo( + afterRect.left(), centerY - trackThickness / 2, afterRect.left() + trackThickness * 0.1, + centerY - trackThickness / 2); + afterPath.lineTo(afterRect.right() - trackThickness * 0.5, centerY - trackThickness / 2); + afterPath.quadTo(afterRect.right(), centerY - trackThickness / 2, afterRect.right(), centerY); + afterPath.quadTo( + afterRect.right(), centerY + trackThickness / 2, afterRect.right() - trackThickness * 0.5, + centerY + trackThickness / 2); + afterPath.lineTo(afterRect.left() + trackThickness * 0.1, centerY + trackThickness / 2); + afterPath.quadTo(afterRect.left(), centerY + trackThickness / 2, afterRect.left(), centerY); + painter.fillPath(afterPath, inactiveTrackColor); + } + + painter.setBrush(handleColor); + int handleLineHeight = 30; + int handleLineWidth = 4; + int handleLineRadius = 2; + QRect handleLineRect( + handleRect.center().x() - handleLineWidth / 2, centerY - handleLineHeight / 2, handleLineWidth, + handleLineHeight); + QPainterPath handlePath; + handlePath.addRoundedRect(handleLineRect, handleLineRadius, handleLineRadius); + painter.fillPath(handlePath, handleColor); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp new file mode 100644 index 0000000000000..3b58ded626439 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp @@ -0,0 +1,87 @@ +// 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 "include/custom_toggle_switch.hpp" + +CustomToggleSwitch::CustomToggleSwitch(QWidget * parent) : QCheckBox(parent) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + connect(this, &QCheckBox::stateChanged, this, [this]() { + if (!blockSignalsGuard) { + update(); // Force repaint + } + }); +} + +QSize CustomToggleSwitch::sizeHint() const +{ + return QSize(50, 30); // Preferred size of the toggle switch +} + +void CustomToggleSwitch::paintEvent(QPaintEvent *) +{ + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + int margin = 2; + int circleRadius = height() / 2 - margin * 2; + QRect r = rect().adjusted(margin, margin, -margin, -margin); + bool isChecked = this->isChecked(); + + QColor uncheckedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()); + QColor checkedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()); + QColor indicatorColor = isChecked ? checkedIndicatorColor : uncheckedIndicatorColor; + + QColor uncheckedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor checkedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + QColor bgColor = isChecked ? checkedBgColor : uncheckedBgColor; + + QRect borderR = r.adjusted(-margin, -margin, margin, margin); + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(borderR, circleRadius + 4, circleRadius + 4); + + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(r, circleRadius + 4, circleRadius + 4); + + int minX = r.left() + margin * 2; + int maxX = r.right() - circleRadius * 2 - margin; + int circleX = isChecked ? maxX : minX; + QRect circleRect(circleX, r.top() + margin, circleRadius * 2, circleRadius * 2); + p.setBrush(indicatorColor); + p.drawEllipse(circleRect); +} + +void CustomToggleSwitch::mouseReleaseEvent(QMouseEvent * event) +{ + if (event->button() == Qt::LeftButton) { + setCheckedState(!isChecked()); + } + QCheckBox::mouseReleaseEvent(event); +} + +void CustomToggleSwitch::setCheckedState(bool state) +{ + blockSignalsGuard = true; + setChecked(state); + blockSignalsGuard = false; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp similarity index 72% rename from common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 8f67a90215bd1..9863cbbbf802b 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -17,12 +17,26 @@ #ifndef AUTOWARE_STATE_PANEL_HPP_ #define AUTOWARE_STATE_PANEL_HPP_ -#include +#include "custom_button.hpp" +#include "custom_container.hpp" +#include "custom_icon_label.hpp" +#include "custom_label.hpp" +#include "custom_segmented_button.hpp" +#include "custom_segmented_button_item.hpp" +#include "custom_slider.hpp" +#include "custom_toggle_switch.hpp" +#include "material_colors.hpp" + +#include +#include +#include #include -#include #include +#include #include -#include +#include +#include +#include #include #include @@ -36,11 +50,17 @@ #include #include #include +#include +#include #include #include #include +#include + #include +#include +#include namespace rviz_plugins { @@ -56,6 +76,8 @@ class AutowareStatePanel : public rviz_common::Panel using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; Q_OBJECT @@ -75,17 +97,19 @@ public Q_SLOTS: // NOLINT for Qt void onClickAcceptStart(); void onClickVelocityLimit(); void onClickEmergencyButton(); + void onSwitchStateChanged(int state); protected: // Layout - QGroupBox * makeOperationModeGroup(); - QGroupBox * makeControlModeGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeLocalizationGroup(); - QGroupBox * makeMotionGroup(); - QGroupBox * makeFailSafeGroup(); - - void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + QVBoxLayout * makeVelocityLimitGroup(); + QVBoxLayout * makeOperationModeGroup(); + QVBoxLayout * makeRoutingGroup(); + QVBoxLayout * makeLocalizationGroup(); + QVBoxLayout * makeMotionGroup(); + QVBoxLayout * makeFailSafeGroup(); + // QVBoxLayout * makeDiagnosticGroup(); + + // void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; @@ -97,13 +121,15 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Publisher::SharedPtr pub_velocity_limit_; + QLabel * velocity_limit_value_label_{nullptr}; + bool sliderIsDragging = false; + // Operation Mode - //// Gate Mode QLabel * operation_mode_label_ptr_{nullptr}; - QPushButton * stop_button_ptr_{nullptr}; - QPushButton * auto_button_ptr_{nullptr}; - QPushButton * local_button_ptr_{nullptr}; - QPushButton * remote_button_ptr_{nullptr}; + CustomSegmentedButtonItem * stop_button_ptr_{nullptr}; + CustomSegmentedButtonItem * auto_button_ptr_{nullptr}; + CustomSegmentedButtonItem * local_button_ptr_{nullptr}; + CustomSegmentedButtonItem * remote_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_operation_mode_; rclcpp::Client::SharedPtr client_change_to_autonomous_; @@ -112,6 +138,8 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Client::SharedPtr client_change_to_remote_; //// Control Mode + CustomSegmentedButton * segmented_button; + CustomToggleSwitch * control_mode_switch_ptr_{nullptr}; QLabel * control_mode_label_ptr_{nullptr}; QPushButton * enable_button_ptr_{nullptr}; QPushButton * disable_button_ptr_{nullptr}; @@ -123,8 +151,9 @@ public Q_SLOTS: // NOLINT for Qt void changeOperationMode(const rclcpp::Client::SharedPtr client); // Routing + CustomIconLabel * routing_icon{nullptr}; + CustomElevatedButton * clear_route_button_ptr_{nullptr}; QLabel * routing_label_ptr_{nullptr}; - QPushButton * clear_route_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Client::SharedPtr client_clear_route_; @@ -132,8 +161,9 @@ public Q_SLOTS: // NOLINT for Qt void onRoute(const RouteState::ConstSharedPtr msg); // Localization + CustomIconLabel * localization_icon{nullptr}; + CustomElevatedButton * init_by_gnss_button_ptr_{nullptr}; QLabel * localization_label_ptr_{nullptr}; - QPushButton * init_by_gnss_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_localization_; rclcpp::Client::SharedPtr client_init_by_gnss_; @@ -141,8 +171,9 @@ public Q_SLOTS: // NOLINT for Qt void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); // Motion + CustomIconLabel * motion_icon{nullptr}; + CustomElevatedButton * accept_start_button_ptr_{nullptr}; QLabel * motion_label_ptr_{nullptr}; - QPushButton * accept_start_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_motion_; rclcpp::Client::SharedPtr client_accept_start_; @@ -150,7 +181,9 @@ public Q_SLOTS: // NOLINT for Qt void onMotion(const MotionState::ConstSharedPtr msg); // FailSafe + CustomIconLabel * mrm_state_icon{nullptr}; QLabel * mrm_state_label_ptr_{nullptr}; + CustomIconLabel * mrm_behavior_icon{nullptr}; QLabel * mrm_behavior_label_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_mrm_; @@ -158,11 +191,11 @@ public Q_SLOTS: // NOLINT for Qt void onMRMState(const MRMState::ConstSharedPtr msg); // Others - QPushButton * velocity_limit_button_ptr_; + QLabel * velocity_limit_setter_ptr_; QLabel * gear_label_ptr_; QSpinBox * pub_velocity_limit_input_; - QPushButton * emergency_button_ptr_; + CustomElevatedButton * emergency_button_ptr_; bool current_emergency_{false}; @@ -190,6 +223,17 @@ public Q_SLOTS: // NOLINT for Qt label->setText(text); label->setStyleSheet(style_sheet); } + static void updateCustomLabel( + CustomLabel * label, QString text, QColor bg_color, QColor text_color) + { + label->updateStyle(text, bg_color, text_color); + } + + static void updateButton(QPushButton * button, QString text, QString style_sheet) + { + button->setText(text); + button->setStyleSheet(style_sheet); + } static void activateButton(QAbstractButton * button) { diff --git a/common/tier4_state_rviz_plugin/src/include/custom_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp new file mode 100644 index 0000000000000..b10663ce09933 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp @@ -0,0 +1,69 @@ +// 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 CUSTOM_BUTTON_HPP_ +#define CUSTOM_BUTTON_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class CustomElevatedButton : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomElevatedButton( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + const QColor & hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()), + const QColor & disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + const QColor & disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()), + QWidget * parent = nullptr); + void updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, + const QColor & hoverColor, const QColor & disabledBgColor, const QColor & disabledTextColor); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.disabled_elevated_button_bg.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); + bool isHovered = false; +}; + +#endif // CUSTOM_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_container.hpp b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp new file mode 100644 index 0000000000000..5142b409ebc88 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp @@ -0,0 +1,52 @@ +// 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 CUSTOM_CONTAINER_HPP_ +#define CUSTOM_CONTAINER_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include + +class CustomContainer : public QFrame +{ + Q_OBJECT + +public: + explicit CustomContainer(QWidget * parent = nullptr); + + // Methods to set dimensions and corner radius + void setContainerHeight(int height); + void setContainerWidth(int width); + void setCornerRadius(int radius); + + // Getters + int getContainerHeight() const; + int getContainerWidth() const; + int getCornerRadius() const; + QGridLayout * getLayout() const; // Add a method to access the layout + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QGridLayout * layout; + int cornerRadius; +}; + +#endif // CUSTOM_CONTAINER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp new file mode 100644 index 0000000000000..1b3ab9c8e0c6c --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp @@ -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. +#ifndef CUSTOM_ICON_LABEL_HPP_ +#define CUSTOM_ICON_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +enum IconState { Active, Pending, Danger, None, Crash }; + +class CustomIconLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomIconLabel( + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + QWidget * parent = nullptr); + void updateStyle(IconState state, const QColor & bgColor); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + void loadIcons(); + QPixmap icon; + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()); + QMap iconMap; +}; + +#endif // CUSTOM_ICON_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp new file mode 100644 index 0000000000000..a328c4de56e3d --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp @@ -0,0 +1,46 @@ +// 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 CUSTOM_LABEL_HPP_ +#define CUSTOM_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +class CustomLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomLabel(const QString & text, QWidget * parent = nullptr); + void updateStyle( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); +}; + +#endif // CUSTOM_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp new file mode 100644 index 0000000000000..c6c589d31b8d4 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp @@ -0,0 +1,53 @@ +// 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 CUSTOM_SEGMENTED_BUTTON_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_HPP_ + +#include "custom_segmented_button_item.hpp" +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButton : public QWidget +{ + Q_OBJECT + +public: + explicit CustomSegmentedButton(QWidget * parent = nullptr); + + CustomSegmentedButtonItem * addButton(const QString & text); + QButtonGroup * getButtonGroup() const; + + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +Q_SIGNALS: + void buttonClicked(int id); + +protected: + void paintEvent(QPaintEvent * event) override; + +private: + QButtonGroup * buttonGroup; + QHBoxLayout * layout; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp new file mode 100644 index 0000000000000..33eb9fe16aa31 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp @@ -0,0 +1,64 @@ +// 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 CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButtonItem : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomSegmentedButtonItem(const QString & text, QWidget * parent = nullptr); + + void setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText); + void setActivated(bool activated); + void setCheckableButton(bool checkable); + void setDisabledButton(bool disabled); + void setHovered(bool hovered); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + +private: + void updateCheckableState(); + + QColor bgColor; + QColor checkedBgColor; + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor inactiveTextColor; + QColor activeTextColor; + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_dim.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()); + bool isHovered = false; + bool isActivated = false; + bool isDisabled = false; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp similarity index 60% rename from common/mission_planner_rviz_plugin/src/mrm_goal.hpp rename to common/tier4_state_rviz_plugin/src/include/custom_slider.hpp index e16b0abf0bab3..f0dc9f12aedfe 100644 --- a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp +++ b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp @@ -11,24 +11,25 @@ // 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 CUSTOM_SLIDER_HPP_ +#define CUSTOM_SLIDER_HPP_ +#include "material_colors.hpp" -#ifndef MRM_GOAL_HPP_ -#define MRM_GOAL_HPP_ +#include +#include +#include +#include +#include -#include - -namespace rviz_plugins -{ - -class MrmGoalTool : public rviz_default_plugins::tools::GoalTool +class CustomSlider : public QSlider { Q_OBJECT public: - MrmGoalTool(); - void onInitialize() override; -}; + explicit CustomSlider(Qt::Orientation orientation, QWidget * parent = nullptr); -} // namespace rviz_plugins +protected: + void paintEvent(QPaintEvent * event) override; +}; -#endif // MRM_GOAL_HPP_ +#endif // CUSTOM_SLIDER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp new file mode 100644 index 0000000000000..107d45af8c3f3 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp @@ -0,0 +1,40 @@ +// 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 CUSTOM_TOGGLE_SWITCH_HPP_ +#define CUSTOM_TOGGLE_SWITCH_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +class CustomToggleSwitch : public QCheckBox +{ + Q_OBJECT + +public: + explicit CustomToggleSwitch(QWidget * parent = nullptr); + QSize sizeHint() const override; + void setCheckedState(bool state); + +protected: + void paintEvent(QPaintEvent * event) override; + void mouseReleaseEvent(QMouseEvent * event) override; + +private: + bool blockSignalsGuard = false; // Guard variable to block signals during updates +}; + +#endif // CUSTOM_TOGGLE_SWITCH_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/material_colors.hpp b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp new file mode 100644 index 0000000000000..d146b599ab600 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp @@ -0,0 +1,88 @@ +// 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 MATERIAL_COLORS_HPP_ +#define MATERIAL_COLORS_HPP_ +#include + +namespace autoware +{ +namespace state_rviz_plugin +{ +namespace colors +{ +struct MaterialColors +{ + std::string primary = "#8BD0F0"; + std::string surface_tint = "#8BD0F0"; + std::string on_primary = "#003546"; + std::string primary_container = "#004D64"; + std::string on_primary_container = "#BEE9FF"; + std::string secondary = "#B4CAD6"; + std::string on_secondary = "#1F333C"; + std::string secondary_container = "#354A54"; + std::string on_secondary_container = "#D0E6F2"; + std::string tertiary = "#C6C2EA"; + std::string on_tertiary = "#2F2D4D"; + std::string tertiary_container = "#454364"; + std::string on_tertiary_container = "#E3DFFF"; + std::string error = "#FFB4AB"; + std::string on_error = "#690005"; + std::string error_container = "#93000A"; + std::string on_error_container = "#FFDAD6"; + std::string background = "#0F1417"; + std::string on_background = "#DFE3E7"; + std::string surface = "#0F1417"; + std::string on_surface = "#DFE3E7"; + std::string surface_variant = "#40484C"; + std::string on_surface_variant = "#C0C8CD"; + std::string outline = "#8A9297"; + std::string outline_variant = "#40484C"; + std::string shadow = "#000000"; + std::string scrim = "#000000"; + std::string inverse_surface = "#DFE3E7"; + std::string inverse_on_surface = "#2C3134"; + std::string inverse_primary = "#126682"; + std::string primary_fixed = "#BEE9FF"; + std::string on_primary_fixed = "#001F2A"; + std::string primary_fixed_dim = "#8BD0F0"; + std::string on_primary_fixed_variant = "#004D64"; + std::string secondary_fixed = "#D0E6F2"; + std::string on_secondary_fixed = "#081E27"; + std::string secondary_fixed_dim = "#B4CAD6"; + std::string on_secondary_fixed_variant = "#354A54"; + std::string tertiary_fixed = "#E3DFFF"; + std::string on_tertiary_fixed = "#1A1836"; + std::string tertiary_fixed_dim = "#C6C2EA"; + std::string on_tertiary_fixed_variant = "#454364"; + std::string surface_dim = "#0F1417"; + std::string surface_bright = "#353A3D"; + std::string surface_container_lowest = "#0A0F11"; + std::string surface_container_low = "#171C1F"; + std::string surface_container = "#1B2023"; + std::string surface_container_high = "#262B2E"; + std::string surface_container_highest = "#303538"; + std::string disabled_elevated_button_bg = "#292D30"; + std::string success = "#8DF08B"; + std::string warning = "#EEF08B"; + std::string info = "#8BD0F0"; + std::string danger = "#F08B8B"; +}; + +inline MaterialColors default_colors; +} // namespace colors +} // namespace state_rviz_plugin +} // namespace autoware + +#endif // MATERIAL_COLORS_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp similarity index 100% rename from common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index 6ea142ed66a1b..90ad18fe5af6c 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -14,7 +14,7 @@ // limitations under the License. // -#include "velocity_steering_factors_panel.hpp" +#include "include/velocity_steering_factors_panel.hpp" #include #include diff --git a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt b/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt deleted file mode 100644 index ed458cf924e33..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -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 deleted file mode 100644 index 1296bd3442ab3..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# 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 deleted file mode 100644 index 33aa9a1e5a26e..0000000000000 Binary files a/common/tier4_target_object_type_rviz_plugin/image/window.png and /dev/null differ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml deleted file mode 100644 index 04d2f28d9ba3e..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - 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 deleted file mode 100644 index eb3350e4333bd..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - 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 deleted file mode 100644 index e014307942bab..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.cpp +++ /dev/null @@ -1,316 +0,0 @@ -// 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 deleted file mode 100644 index 1f3934369bfba..0000000000000 --- a/common/tier4_target_object_type_rviz_plugin/src/target_object_type_panel.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 43fb310b17416..27f04603d6a31 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -224,18 +225,28 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +static rclcpp::SensorDataQoS SingleDepthSensorQoS() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + class AEB : public rclcpp::Node { public: explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - rclcpp::Subscription::SharedPtr sub_point_cloud_; - rclcpp::Subscription::SharedPtr sub_velocity_; - rclcpp::Subscription::SharedPtr sub_imu_; - rclcpp::Subscription::SharedPtr sub_predicted_traj_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - + tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", SingleDepthSensorQoS()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ + this, "~/input/velocity"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + this, "~/input/predicted_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ + this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug @@ -245,15 +256,12 @@ class AEB : public rclcpp::Node // callback void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); - void onVelocity(const VelocityReport::ConstSharedPtr input_msg); void onImu(const Imu::ConstSharedPtr input_msg); void onTimer(); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); - void onAutowareState(const AutowareState::ConstSharedPtr input_msg); rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); - bool isDataReady(); + bool fetchLatestData(); // main function void onCheckCollision(DiagnosticStatusWrapper & stat); diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 1ac255c21921b..68c070a86dd97 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Tomoya Kimura Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index d8886672a8ecd..905b66df288b4 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -105,27 +105,6 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), collision_data_keeper_(this->get_clock()) { - // Subscribers - { - sub_point_cloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&AEB::onPointCloud, this, std::placeholders::_1)); - - sub_velocity_ = this->create_subscription( - "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1)); - - sub_imu_ = this->create_subscription( - "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1)); - - sub_predicted_traj_ = this->create_subscription( - "~/input/predicted_trajectory", rclcpp::QoS{1}, - std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1)); - - sub_autoware_state_ = this->create_subscription( - "/autoware/state", rclcpp::QoS{1}, - std::bind(&AEB::onAutowareState, this, std::placeholders::_1)); - } - // Publisher { pub_obstacle_pointcloud_ = @@ -229,11 +208,6 @@ void AEB::onTimer() updater_.force_update(); } -void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) -{ - current_velocity_ptr_ = input_msg; -} - void AEB::onImu(const Imu::ConstSharedPtr input_msg) { // transform imu @@ -253,17 +227,6 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg) tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); } -void AEB::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) -{ - predicted_traj_ptr_ = input_msg; -} - -void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg) -{ - autoware_state_ = input_msg; -} - void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { PointCloud::Ptr pointcloud_ptr(new PointCloud); @@ -316,29 +279,42 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) obstacle_ros_pointcloud_ptr_->header = input_msg->header; } -bool AEB::isDataReady() +bool AEB::fetchLatestData() { const auto missing = [this](const auto & name) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); return false; }; + current_velocity_ptr_ = sub_velocity_.takeData(); if (!current_velocity_ptr_) { return missing("ego velocity"); } + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + onPointCloud(pointcloud_ptr); if (!obstacle_ros_pointcloud_ptr_) { return missing("object pointcloud"); } + const auto imu_ptr = sub_imu_.takeData(); + if (use_imu_path_ && !imu_ptr) { + return missing("imu message"); + } + onImu(imu_ptr); if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); if (use_predicted_trajectory_ && !predicted_traj_ptr_) { return missing("control predicted trajectory"); } + autoware_state_ = sub_autoware_state_.takeData(); if (!autoware_state_) { return missing("autoware_state"); } @@ -375,7 +351,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) using colorTuple = std::tuple; // step1. check data - if (!isDataReady()) { + if (!fetchLatestData()) { return false; } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8e7685180c1f9..8692027e9634a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -464,17 +464,14 @@ bool NDTScanMatcher::callback_sensor_points_main( diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability); diagnostics_scan_points_->addKeyValue( "nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood); - std::string score_name = ""; double score = 0.0; double score_threshold = 0.0; if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { - score_name = "Transform Probability"; score = ndt_result.transform_probability; score_threshold = param_.score_estimation.converged_param_transform_probability; } else if ( param_.score_estimation.converged_param_type == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { - score_name = "Nearest Voxel Transformation Likelihood"; score = ndt_result.nearest_voxel_transformation_likelihood; score_threshold = param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood; diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 848fcfba95f14..aa620256d977b 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -21,6 +21,10 @@ sample-map-rosbag └── pointcloud_map_metadata.yaml ``` +There are three types of transformations from latitude and longitude to XYZ coordinate system as shown in the figure below. Please refer to the following details for the necessary parameters for each projector type. + +![node_diagram](docs/map_projector_type.svg) + ### Using local coordinate ```yaml diff --git a/map/map_projection_loader/docs/map_projector_type.svg b/map/map_projection_loader/docs/map_projector_type.svg new file mode 100644 index 0000000000000..e1c8c2ac68987 --- /dev/null +++ b/map/map_projection_loader/docs/map_projector_type.svg @@ -0,0 +1,3010 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + X(east) + + + O + + + Y(north) + + + + Strict Boundary. + + Outside of the mgrs grid is + projected to undefined xy + + + mgrs_grid=54SUE + + + 54SUE + + + 54S(UTM grid) + + + meridian + + + LocalCartesianUTM + + + TransverseMercator + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + 3+6*floor(map_origin.longitude/6)[deg] + + + 3+6n[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + map_origin.longitude[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp index f1e275a919a8a..aec3f56936828 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp @@ -32,6 +32,10 @@ class EuclideanCluster : public EuclideanClusterInterface bool cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) override; + + bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override; void setTolerance(float tolerance) { tolerance_ = tolerance; } private: diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp index 70c21daabcd66..65b907f747666 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp @@ -16,6 +16,9 @@ #include +#include +#include + #include #include @@ -41,6 +44,10 @@ class EuclideanClusterInterface const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) = 0; + virtual bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & output_clusters) = 0; + protected: bool use_height_ = true; int min_cluster_size_; diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp index 2ac77aebdea0c..50d2306d48f72 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp @@ -31,6 +31,9 @@ void convertPointCloudClusters2Msg( const std_msgs::msg::Header & header, const std::vector> & clusters, tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg); +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, const std::vector & clusters, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg); void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index d6a07503c5ca5..375cc2d19a01f 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -35,6 +35,9 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface bool cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) override; + bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override; void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; } void setTolerance(float tolerance) { tolerance_ = tolerance; } void setMinPointsNumberPerVoxel(int min_points_number_per_voxel) diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index bd8270bd6b881..5ba1b99403280 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -33,6 +33,15 @@ EuclideanCluster::EuclideanCluster( : EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance) { } +// TODO(badai-nguyen): implement input field copy for euclidean_cluster.cpp +bool EuclideanCluster::cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) +{ + (void)pointcloud_msg; + (void)clusters; + return false; +} bool EuclideanCluster::cluster( const pcl::PointCloud::ConstPtr & pointcloud, diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6ff563c157d00..3a906575e2c1e 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -62,6 +62,25 @@ void convertPointCloudClusters2Msg( msg.feature_objects.push_back(feature_object); } } + +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, const std::vector & clusters, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg) +{ + msg.header = header; + for (const auto & ros_pointcloud : clusters) { + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.feature.cluster = ros_pointcloud; + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(ros_pointcloud); + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + msg.feature_objects.push_back(feature_object); + } +} + void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output) diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index b8276296dde5f..d82ec021a5396 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster( min_points_number_per_voxel_(min_points_number_per_voxel) { } - +// TODO(badai-nguyen): remove this function when field copying also implemented for +// euclidean_cluster.cpp bool VoxelGridBasedEuclideanCluster::cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) +{ + (void)pointcloud; + (void)clusters; + return false; +} + +bool VoxelGridBasedEuclideanCluster::cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects) { // TODO(Saito) implement use_height is false version // create voxel + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); + int point_step = pointcloud_msg->point_step; + pcl::fromROSMsg(*pointcloud_msg, *pointcloud); pcl::PointCloud::Ptr voxel_map_ptr(new pcl::PointCloud); voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0); voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_); @@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster( // create map to search cluster index from voxel grid index std::unordered_map map; + std::vector temporary_clusters; // no check about cluster size + std::vector clusters_data_size; + temporary_clusters.resize(cluster_indices.size()); for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) { const auto & cluster = cluster_indices.at(cluster_idx); + auto & temporary_cluster = temporary_clusters.at(cluster_idx); for (const auto & point_idx : cluster.indices) { map[point_idx] = cluster_idx; } + temporary_cluster.height = pointcloud_msg->height; + temporary_cluster.fields = pointcloud_msg->fields; + temporary_cluster.point_step = point_step; + temporary_cluster.data.resize(max_cluster_size_ * point_step); + clusters_data_size.push_back(0); } // create vector of point cloud cluster. vector index is voxel grid index. - std::vector> temporary_clusters; // no check about cluster size - temporary_clusters.resize(cluster_indices.size()); - for (const auto & point : pointcloud->points) { + for (size_t i = 0; i < pointcloud->points.size(); ++i) { + const auto & point = pointcloud->points.at(i); const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); if (map.find(index) != map.end()) { - temporary_clusters.at(map[index]).points.push_back(point); + auto & cluster_data_size = clusters_data_size.at(map[index]); + if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) { + continue; + } + std::memcpy( + &temporary_clusters.at(map[index]).data[cluster_data_size], + &pointcloud_msg->data[i * point_step], point_step); + cluster_data_size += point_step; } } // build output and check cluster size { - for (const auto & cluster : temporary_clusters) { - if (!(min_cluster_size_ <= static_cast(cluster.points.size()) && - static_cast(cluster.points.size()) <= max_cluster_size_)) { + for (size_t i = 0; i < temporary_clusters.size(); ++i) { + auto & i_cluster_data_size = clusters_data_size.at(i); + if (!(min_cluster_size_ <= static_cast(i_cluster_data_size / point_step) && + static_cast(i_cluster_data_size / point_step) <= max_cluster_size_)) { continue; } - clusters.push_back(cluster); - clusters.back().width = cluster.points.size(); - clusters.back().height = 1; - clusters.back().is_dense = false; + const auto & cluster = temporary_clusters.at(i); + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.feature.cluster = cluster; + feature_object.feature.cluster.data.resize(i_cluster_data_size); + feature_object.feature.cluster.header = pointcloud_msg->header; + feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian; + feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense; + feature_object.feature.cluster.point_step = point_step; + feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height; + feature_object.feature.cluster.width = + i_cluster_data_size / point_step / pointcloud_msg->height; + + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(feature_object.feature.cluster); + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + + objects.feature_objects.push_back(feature_object); } + objects.header = pointcloud_msg->header; } return true; diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index f58d9ac6dcc48..7e6a456561900 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -55,24 +55,15 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( stop_watch_ptr_->toc("processing_time", true); // convert ros to pcl - pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); if (input_msg->data.empty()) { // NOTE: prevent pcl log spam RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - } else { - pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); } - - // clustering - std::vector> clusters; - if (!raw_pointcloud_ptr->empty()) { - cluster_->cluster(raw_pointcloud_ptr, clusters); - } - - // build output msg + // cluster and build output msg tier4_perception_msgs::msg::DetectedObjectsWithFeature output; - convertPointCloudClusters2Msg(input_msg->header, clusters, output); + + cluster_->cluster(input_msg, output); cluster_pub_->publish(output); // build debug msg diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index 587db9c9e0dac..d7fa777dc58c9 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -102,9 +102,7 @@ RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptio unit_vec_ = Eigen::Vector3d::UnitX(); } else if (unit_axis_ == "y") { unit_vec_ = Eigen::Vector3d::UnitY(); - } else if (unit_axis_ == "z") { - unit_vec_ = Eigen::Vector3d::UnitZ(); - } else { + } else { // including (unit_axis_ == "z") unit_vec_ = Eigen::Vector3d::UnitZ(); } @@ -384,9 +382,7 @@ rcl_interfaces::msg::SetParametersResult RANSACGroundFilterComponent::paramCallb unit_vec_ = Eigen::Vector3d::UnitX(); } else if (unit_axis_ == "y") { unit_vec_ = Eigen::Vector3d::UnitY(); - } else if (unit_axis_ == "z") { - unit_vec_ = Eigen::Vector3d::UnitZ(); - } else { + } else { // including (unit_axis_ == "z") unit_vec_ = Eigen::Vector3d::UnitZ(); } RCLCPP_DEBUG(get_logger(), "Setting unit_axis to: %s.", unit_axis_.c_str()); diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 6399726740041..a63d218df62de 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -349,13 +349,12 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // check the first point in ray auto * p = &in_radial_ordered_clouds[i][0]; - auto * prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point bool initialized_first_gnd_grid = false; bool prev_list_init = false; pcl::PointXYZ p_orig_point, prev_p_orig_point; for (auto & point : in_radial_ordered_clouds[i]) { - prev_p = p; + auto * prev_p = p; // for checking the distance to prev point prev_p_orig_point = p_orig_point; p = &point; get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); @@ -469,7 +468,6 @@ void ScanGroundFilterComponent::classifyPointCloud( float prev_gnd_slope = 0.0f; float points_distance = 0.0f; PointsCentroid ground_cluster, non_ground_cluster; - float local_slope = 0.0f; PointLabel prev_point_label = PointLabel::INIT; pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; // loop through each point in the radial div @@ -524,7 +522,7 @@ void ScanGroundFilterComponent::classifyPointCloud( } if (calculate_slope) { // far from the previous point - local_slope = std::atan2(height_from_gnd, radius_distance_from_gnd); + auto local_slope = std::atan2(height_from_gnd, radius_distance_from_gnd); if (local_slope - prev_gnd_slope > local_slope_max_angle) { // the point is outside of the local slope threshold p->point_state = PointLabel::NON_GROUND; diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index b13f68b07181e..7baed86088811 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -160,6 +160,13 @@ else() message("Skipping build of some nodes due to missing dependencies") endif() +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_utils + test/test_utils.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index a5b41a30be166..57fe994aedf5a 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -36,6 +36,7 @@ tier4_autoware_utils tier4_perception_msgs + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/perception/image_projection_based_fusion/test/test_utils.cpp b/perception/image_projection_based_fusion/test/test_utils.cpp new file mode 100644 index 0000000000000..e578ce1987cc3 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_utils.cpp @@ -0,0 +1,170 @@ +// 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 +#include + +#include + +using autoware_point_types::PointXYZI; + +void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + pointcloud.fields.resize(4); + pointcloud.fields[0].name = "x"; + pointcloud.fields[1].name = "y"; + pointcloud.fields[2].name = "z"; + pointcloud.fields[3].name = "intensity"; + pointcloud.fields[0].offset = 0; + pointcloud.fields[1].offset = 4; + pointcloud.fields[2].offset = 8; + pointcloud.fields[3].offset = 12; + pointcloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[0].count = 1; + pointcloud.fields[1].count = 1; + pointcloud.fields[2].count = 1; + pointcloud.fields[3].count = 1; + pointcloud.height = 1; + pointcloud.point_step = 16; + pointcloud.is_bigendian = false; + pointcloud.is_dense = true; + pointcloud.header.frame_id = "dummy_frame_id"; + pointcloud.header.stamp.sec = 0; + pointcloud.header.stamp.nanosec = 0; +} + +// Test case 1: Test case when the input pointcloud is empty +TEST(UtilsTest, closest_cluster_empty_cluster_test_case1) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + pointcloud.data.resize(0); + pointcloud.width = 0; + pointcloud.row_step = 0; + + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), std::size_t(0)); +} + +// Test case 2: Test case when the input pointcloud as one cluster +TEST(UtilsTest, closest_cluster_test_case2) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + std::vector dummy_x = {0.0, 0.1, 0.2}; + pointcloud.data.resize(dummy_x.size() * pointcloud.point_step); + size_t global_offset = 0; + for (auto x : dummy_x) { + PointXYZI point; + point.x = x; + point.y = 0.0; + point.z = 0.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[global_offset], &point, pointcloud.point_step); + global_offset += pointcloud.point_step; + } + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), dummy_x.size() * pointcloud.point_step); +} + +// Test case 3: Test case when the input pointcloud as two clusters +TEST(UtilsTest, closest_cluster_test_case3) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + std::vector dummy_x = {0.0, 0.1, 0.2, 1.0, 1.1, 1.2, 1.21, 1.22}; + pointcloud.data.resize(dummy_x.size() * pointcloud.point_step); + size_t global_offset = 0; + for (auto x : dummy_x) { + PointXYZI point; + point.x = x; + point.y = 0.0; + point.z = 0.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[global_offset], &point, pointcloud.point_step); + global_offset += pointcloud.point_step; + } + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), 3 * pointcloud.point_step); +} + +// Test case 4: Test case when the input pointcloud as two clusters +TEST(UtilsTest, closest_cluster_test_case4) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + std::vector dummy_x = {0.0, 0.1, 1.0, 1.01, 1.1, 1.2, 1.21, 1.22}; + pointcloud.data.resize(dummy_x.size() * pointcloud.point_step); + size_t global_offset = 0; + for (auto x : dummy_x) { + PointXYZI point; + point.x = x; + point.y = 0.0; + point.z = 0.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[global_offset], &point, pointcloud.point_step); + global_offset += pointcloud.point_step; + } + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), 6 * pointcloud.point_step); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 8fab65ef6b4d7..353f797b0bee1 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -147,6 +147,20 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) TARGETS centerpoint_cuda_lib DESTINATION lib ) + + if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_detection_class_remapper + test/test_detection_class_remapper.cpp + ) + ament_auto_add_gtest(test_ros_utils + test/test_ros_utils.cpp + ) + ament_auto_add_gtest(test_nms + test/test_nms.cpp + ) + endif() + else() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp b/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp new file mode 100644 index 0000000000000..6559736ede198 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp @@ -0,0 +1,92 @@ +// 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 + +#include + +TEST(DetectionClassRemapperTest, MapClasses) +{ + centerpoint::DetectionClassRemapper remapper; + + // Set up the parameters for the remapper + // Labels: CAR, TRUCK, TRAILER + std::vector allow_remapping_by_area_matrix = { + 0, 0, 0, // CAR cannot be remapped + 0, 0, 1, // TRUCK can be remapped to TRAILER + 0, 1, 0 // TRAILER can be remapped to TRUCK + }; + std::vector min_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0}; + std::vector max_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 999.0, 0.0, 10.0, 0.0}; + + remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + // Create a DetectedObjects message with some objects + autoware_auto_perception_msgs::msg::DetectedObjects msg; + + // CAR with area 4.0, which is out of the range for remapping + autoware_auto_perception_msgs::msg::DetectedObject obj1; + autoware_auto_perception_msgs::msg::ObjectClassification obj1_classification; + obj1.shape.dimensions.x = 2.0; + obj1.shape.dimensions.y = 2.0; + obj1_classification.label = 0; + obj1_classification.probability = 1.0; + obj1.classification = {obj1_classification}; + msg.objects.push_back(obj1); + + // TRUCK with area 16.0, which is in the range for remapping to TRAILER + autoware_auto_perception_msgs::msg::DetectedObject obj2; + autoware_auto_perception_msgs::msg::ObjectClassification obj2_classification; + obj2.shape.dimensions.x = 8.0; + obj2.shape.dimensions.y = 2.0; + obj2_classification.label = 1; + obj2_classification.probability = 1.0; + obj2.classification = {obj2_classification}; + msg.objects.push_back(obj2); + + // TRAILER with area 9.0, which is in the range for remapping to TRUCK + autoware_auto_perception_msgs::msg::DetectedObject obj3; + autoware_auto_perception_msgs::msg::ObjectClassification obj3_classification; + obj3.shape.dimensions.x = 3.0; + obj3.shape.dimensions.y = 3.0; + obj3_classification.label = 2; + obj3_classification.probability = 1.0; + obj3.classification = {obj3_classification}; + msg.objects.push_back(obj3); + + // TRAILER with area 12.0, which is out of the range for remapping + autoware_auto_perception_msgs::msg::DetectedObject obj4; + autoware_auto_perception_msgs::msg::ObjectClassification obj4_classification; + obj4.shape.dimensions.x = 4.0; + obj4.shape.dimensions.y = 3.0; + obj4_classification.label = 2; + obj4_classification.probability = 1.0; + obj4.classification = {obj4_classification}; + msg.objects.push_back(obj4); + + // Call the mapClasses function + remapper.mapClasses(msg); + + // Check the remapped labels + EXPECT_EQ(msg.objects[0].classification[0].label, 0); + EXPECT_EQ(msg.objects[1].classification[0].label, 2); + EXPECT_EQ(msg.objects[2].classification[0].label, 1); + EXPECT_EQ(msg.objects[3].classification[0].label, 2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_nms.cpp b/perception/lidar_centerpoint/test/test_nms.cpp new file mode 100644 index 0000000000000..bf443d8d3ad0f --- /dev/null +++ b/perception/lidar_centerpoint/test/test_nms.cpp @@ -0,0 +1,121 @@ +// 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 "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" + +#include + +TEST(NonMaximumSuppressionTest, Apply) +{ + centerpoint::NonMaximumSuppression nms; + centerpoint::NMSParams params; + params.search_distance_2d_ = 1.0; + params.iou_threshold_ = 0.2; + params.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; + params.target_class_names_ = {"CAR"}; + nms.setParameters(params); + + std::vector input_objects(4); + + // Object 1 + autoware_auto_perception_msgs::msg::ObjectClassification obj1_classification; + obj1_classification.label = 0; // Assuming "car" has label 0 + obj1_classification.probability = 1.0; + input_objects[0].classification = {obj1_classification}; // Assuming "car" has label 0 + input_objects[0].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[0].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[0].shape.dimensions.x = 4.0; + input_objects[0].shape.dimensions.y = 2.0; + + // Object 2 (overlaps with Object 1) + autoware_auto_perception_msgs::msg::ObjectClassification obj2_classification; + obj2_classification.label = 0; // Assuming "car" has label 0 + obj2_classification.probability = 1.0; + input_objects[1].classification = {obj2_classification}; // Assuming "car" has label 0 + input_objects[1].kinematics.pose_with_covariance.pose.position.x = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.position.y = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[1].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[1].shape.dimensions.x = 4.0; + input_objects[1].shape.dimensions.y = 2.0; + + // Object 3 + autoware_auto_perception_msgs::msg::ObjectClassification obj3_classification; + obj3_classification.label = 0; // Assuming "car" has label 0 + obj3_classification.probability = 1.0; + input_objects[2].classification = {obj3_classification}; // Assuming "car" has label 0 + input_objects[2].kinematics.pose_with_covariance.pose.position.x = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.position.y = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[2].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[2].shape.dimensions.x = 4.0; + input_objects[2].shape.dimensions.y = 2.0; + + // Object 4 (different class) + autoware_auto_perception_msgs::msg::ObjectClassification obj4_classification; + obj4_classification.label = 1; // Assuming "pedestrian" has label 1 + obj4_classification.probability = 1.0; + input_objects[3].classification = {obj4_classification}; // Assuming "pedestrian" has label 1 + input_objects[3].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[3].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[3].shape.dimensions.x = 0.5; + input_objects[3].shape.dimensions.y = 0.5; + + std::vector output_objects = nms.apply(input_objects); + + // Assert the expected number of output objects + EXPECT_EQ(output_objects.size(), 3); + + // Assert that input_objects[2] is included in the output_objects + bool is_input_object_2_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[2]) { + is_input_object_2_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_2_included); + + // Assert that input_objects[3] is included in the output_objects + bool is_input_object_3_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[3]) { + is_input_object_3_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_3_included); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_ros_utils.cpp b/perception/lidar_centerpoint/test/test_ros_utils.cpp new file mode 100644 index 0000000000000..8d943bac61123 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_ros_utils.cpp @@ -0,0 +1,141 @@ +// 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 "lidar_centerpoint/ros_utils.hpp" + +#include + +TEST(TestSuite, box3DToDetectedObject) +{ + std::vector class_names = {"CAR", "TRUCK", "BUS", "TRAILER", + "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; + + // Test case 1: Test with valid label, has_twist=true, has_variance=true + { + centerpoint::Box3D box3d; + box3d.score = 0.8f; + box3d.label = 0; // CAR + box3d.x = 1.0; + box3d.y = 2.0; + box3d.z = 3.0; + box3d.yaw = 0.5; + box3d.length = 4.0; + box3d.width = 2.0; + box3d.height = 1.5; + box3d.vel_x = 1.0; + box3d.vel_y = 0.5; + box3d.x_variance = 0.1; + box3d.y_variance = 0.2; + box3d.z_variance = 0.3; + box3d.yaw_variance = 0.4; + box3d.vel_x_variance = 0.5; + box3d.vel_y_variance = 0.6; + + autoware_auto_perception_msgs::msg::DetectedObject obj; + centerpoint::box3DToDetectedObject(box3d, class_names, true, true, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); + EXPECT_EQ( + obj.classification[0].label, autoware_auto_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.x, 1.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.y, 2.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.z, 3.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.x, 4.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.y, 2.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.z, 1.5); + EXPECT_TRUE(obj.kinematics.has_position_covariance); + EXPECT_TRUE(obj.kinematics.has_twist); + EXPECT_TRUE(obj.kinematics.has_twist_covariance); + } + + // Test case 2: Test with invalid label, has_twist=false, has_variance=false + { + centerpoint::Box3D box3d; + box3d.score = 0.5f; + box3d.label = 10; // Invalid + + autoware_auto_perception_msgs::msg::DetectedObject obj; + centerpoint::box3DToDetectedObject(box3d, class_names, false, false, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); + EXPECT_EQ( + obj.classification[0].label, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN); + EXPECT_FALSE(obj.kinematics.has_position_covariance); + EXPECT_FALSE(obj.kinematics.has_twist); + EXPECT_FALSE(obj.kinematics.has_twist_covariance); + } +} + +TEST(TestSuite, getSemanticType) +{ + EXPECT_EQ( + centerpoint::getSemanticType("CAR"), + autoware_auto_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_EQ( + centerpoint::getSemanticType("TRUCK"), + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK); + EXPECT_EQ( + centerpoint::getSemanticType("BUS"), + autoware_auto_perception_msgs::msg::ObjectClassification::BUS); + EXPECT_EQ( + centerpoint::getSemanticType("TRAILER"), + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER); + EXPECT_EQ( + centerpoint::getSemanticType("BICYCLE"), + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE); + EXPECT_EQ( + centerpoint::getSemanticType("MOTORBIKE"), + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE); + EXPECT_EQ( + centerpoint::getSemanticType("PEDESTRIAN"), + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN); + EXPECT_EQ( + centerpoint::getSemanticType("UNKNOWN"), + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN); +} + +TEST(TestSuite, convertPoseCovarianceMatrix) +{ + centerpoint::Box3D box3d; + box3d.x_variance = 0.1; + box3d.y_variance = 0.2; + box3d.z_variance = 0.3; + box3d.yaw_variance = 0.4; + + std::array pose_covariance = centerpoint::convertPoseCovarianceMatrix(box3d); + + EXPECT_FLOAT_EQ(pose_covariance[0], 0.1); + EXPECT_FLOAT_EQ(pose_covariance[7], 0.2); + EXPECT_FLOAT_EQ(pose_covariance[14], 0.3); + EXPECT_FLOAT_EQ(pose_covariance[35], 0.4); +} + +TEST(TestSuite, convertTwistCovarianceMatrix) +{ + centerpoint::Box3D box3d; + box3d.vel_x_variance = 0.1; + box3d.vel_y_variance = 0.2; + + std::array twist_covariance = centerpoint::convertTwistCovarianceMatrix(box3d); + + EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); + EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 9378e09f099cc..786446457143f 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -26,6 +26,22 @@ rclcpp_components_register_node(map_based_prediction_node EXECUTABLE map_based_prediction ) +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files}) + + target_link_libraries(test_map_based_prediction + map_based_prediction_node + ) +endif() + ament_auto_package( INSTALL_TO_SHARE config diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 6dfc4a8db9e20..5023051da5e71 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -84,14 +84,14 @@ class PathGenerator const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, const double min_crosswalk_user_velocity); - PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); + PredictedPath generatePathForNonVehicleObject(const TrackedObject & object) const; PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; - PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); + PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object) const; PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0) const; PredictedPath generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; @@ -122,24 +122,25 @@ class PathGenerator PredictedPath generateStraightPath(const TrackedObject & object) const; PredictedPath generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); + const FrenetPoint & current_point, const FrenetPoint & target_point, + const double max_length) const; Eigen::Vector3d calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; Eigen::Vector2d calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; PosePath interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path); + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const; PredictedPath convertToPredictedPath( const TrackedObject & object, const FrenetPath & frenet_predicted_path, - const PosePath & ref_path); + const PosePath & ref_path) const; FrenetPoint getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 83fbc16feb7fa..4238859535c8e 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -32,7 +32,7 @@ PathGenerator::PathGenerator( { } -PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) +PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) const { return generateStraightPath(object); } @@ -143,13 +143,13 @@ PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) +PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) const { return generateStraightPath(object); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) const { if (ref_paths.size() < 2) { return generateStraightPath(object); @@ -178,7 +178,7 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); @@ -210,7 +210,8 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length) + const FrenetPoint & current_point, const FrenetPoint & target_point, + const double max_length) const { FrenetPath path; const double duration = time_horizon_; @@ -252,7 +253,7 @@ FrenetPath PathGenerator::generateFrenetPath( } Eigen::Vector3d PathGenerator::calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Lateral Path Calculation // Quintic polynomial for d @@ -278,7 +279,7 @@ Eigen::Vector3d PathGenerator::calcLatCoefficients( } Eigen::Vector2d PathGenerator::calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Longitudinal Path Calculation // Quadric polynomial @@ -296,7 +297,7 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients( } PosePath PathGenerator::interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path) + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const { PosePath interpolated_path; const size_t interpolate_num = frenet_predicted_path.size(); @@ -356,7 +357,8 @@ PosePath PathGenerator::interpolateReferencePath( } PredictedPath PathGenerator::convertToPredictedPath( - const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) + const TrackedObject & object, const FrenetPath & frenet_predicted_path, + const PosePath & ref_path) const { PredictedPath predicted_path; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -385,7 +387,7 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp new file mode 100644 index 0000000000000..555dc217fb5ed --- /dev/null +++ b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -0,0 +1,218 @@ +// 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 "map_based_prediction/path_generator.hpp" + +#include +#include + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +TrackedObject generate_static_object(const int label) +{ + ObjectClassification classification; + classification.probability = 1.0; + classification.label = label; + + TrackedObjectKinematics kinematics; + kinematics.pose_with_covariance = geometry_msgs::msg::PoseWithCovariance(); // At origin + kinematics.twist_with_covariance = geometry_msgs::msg::TwistWithCovariance(); // Not moving + kinematics.acceleration_with_covariance = + geometry_msgs::msg::AccelWithCovariance(); // Not accelerating + kinematics.orientation_availability = TrackedObjectKinematics::UNAVAILABLE; + + TrackedObject tracked_object; + tracked_object.object_id = unique_identifier_msgs::msg::UUID(); + tracked_object.existence_probability = 1.0; + tracked_object.classification.push_back(classification); + tracked_object.kinematics = kinematics; + + return tracked_object; +} + +TEST(PathGenerator, test_generatePathForNonVehicleObject) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate pedestrian object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForNonVehicleObject(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForLowSpeedVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForLowSpeedVehicle(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOffLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOnLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate reference path + map_based_prediction::PosePath ref_paths; + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + ref_paths.push_back(pose); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForCrosswalkUser) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate dummy crosswalk + map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; + reachable_crosswalk.front_center_point << 0.0, 0.0; + reachable_crosswalk.front_right_point << 1.0, 0.0; + reachable_crosswalk.front_left_point << -1.0, 0.0; + reachable_crosswalk.back_center_point << 0.0, 1.0; + reachable_crosswalk.back_right_point << 1.0, 1.0; + reachable_crosswalk.back_left_point << -1.0, 1.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathToTargetPoint) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate target point + Eigen::Vector2d target_point; + target_point << 0.0, 0.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathToTargetPoint(tracked_object, target_point); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp b/perception/map_based_prediction/test/test_map_based_prediction.cpp similarity index 59% rename from common/mission_planner_rviz_plugin/src/mrm_goal.cpp rename to perception/map_based_prediction/test/test_map_based_prediction.cpp index ef5529abfb4a7..4c8ad7dd25916 100644 --- a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp +++ b/perception/map_based_prediction/test/test_map_based_prediction.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Contributors +// 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. @@ -12,23 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mrm_goal.hpp" +#include -namespace rviz_plugins -{ - -MrmGoalTool::MrmGoalTool() -{ - shortcut_key_ = 'm'; -} +#include -void MrmGoalTool::onInitialize() +int main(int argc, char * argv[]) { - GoalTool::onInitialize(); - setName("MRM Goal Pose"); + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 7c1b98c10d14b..ade45847f8a9f 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -13,18 +13,23 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(nlohmann_json REQUIRED) # for debug find_package(glog REQUIRED) +find_package(ament_cmake_gtest REQUIRED) include_directories( SYSTEM ${EIGEN3_INCLUDE_DIR} ) +ament_auto_add_library(radar_object_tracker_utils SHARED + src/utils/utils.cpp + src/utils/radar_object_tracker_utils.cpp +) + ament_auto_add_library(radar_object_tracker_node SHARED src/radar_object_tracker_node/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp src/tracker/model/linear_motion_tracker.cpp src/tracker/model/constant_turn_rate_motion_tracker.cpp - src/utils/utils.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) @@ -34,6 +39,7 @@ target_link_libraries(radar_object_tracker_node yaml-cpp nlohmann_json::nlohmann_json # for debug glog::glog + radar_object_tracker_utils ) rclcpp_components_register_node(radar_object_tracker_node @@ -42,7 +48,21 @@ rclcpp_components_register_node(radar_object_tracker_node ) # testing -# todo +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_radar_object_tracker_utils + test/test_radar_object_tracker_utils.cpp + src/utils/radar_object_tracker_utils.cpp + ) + target_include_directories(test_radar_object_tracker_utils PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + target_link_libraries(test_radar_object_tracker_utils + radar_object_tracker_utils + ) +endif() # Package ament_auto_package( diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp new file mode 100644 index 0000000000000..48f40f62e3963 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp @@ -0,0 +1,77 @@ +// 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 RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ +#define RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +namespace radar_object_tracker_utils +{ + +boost::optional getTransformAnonymous( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time); + +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets); + +bool checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane); + +lanelet::ConstLanelets getClosestValidLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const double max_distance_from_lane, + const double max_angle_diff_from_lane); + +bool hasValidVelocityDirectionToLanelet( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity); + +} // namespace radar_object_tracker_utils + +#endif // RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 3697903e77e41..5be2ccef1e6cc 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -33,6 +33,7 @@ ament_lint_auto autoware_lint_common + gtest ament_cmake diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 460578699e780..dc87a180451d8 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -14,6 +14,7 @@ #include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp" +#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" #include "radar_object_tracker/utils/utils.hpp" #include @@ -36,158 +37,6 @@ #include #define EIGEN_MPL2_ONLY -namespace -{ -boost::optional getTransformAnonymous( - const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, - const std::string & target_frame_id, const rclcpp::Time & time) -{ - try { - // check if the frames are ready - std::string errstr; // This argument prevents error msg from being displayed in the terminal. - if (!tf_buffer.canTransform( - target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { - return boost::none; - } - - geometry_msgs::msg::TransformStamped self_transform_stamped; - self_transform_stamped = tf_buffer.lookupTransform( - /*target*/ target_frame_id, /*src*/ source_frame_id, time, - rclcpp::Duration::from_seconds(0.5)); - return self_transform_stamped.transform; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what()); - return boost::none; - } -} - -// check if lanelet is close enough to the target lanelet -bool isDuplicated( - const std::pair & target_lanelet, - const lanelet::ConstLanelets & lanelets) -{ - const double CLOSE_LANELET_THRESHOLD = 0.1; - for (const auto & lanelet : lanelets) { - const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); - const auto lanelet_end_p = lanelet.centerline2d().back(); - const double dist = std::hypot( - target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); - if (dist < CLOSE_LANELET_THRESHOLD) { - return true; - } - } - - return false; -} - -// check if the lanelet is valid for object tracking -bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const double max_distance_from_lane, const double max_angle_diff_from_lane) -{ - // Step1. If we only have one point in the centerline, we will ignore the lanelet - if (lanelet.second.centerline().size() <= 1) { - return false; - } - - // Step2. Check if the obstacle is inside or close enough to the lanelet - lanelet::BasicPoint2d search_point( - object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y); - if (!lanelet::geometry::inside(lanelet.second, search_point)) { - const auto distance = lanelet.first; - if (distance > max_distance_from_lane) { - return false; - } - } - - // Step3. Calculate the angle difference between the lane angle and obstacle angle - const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet.second, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - const double abs_norm_delta = std::fabs(normalized_delta_yaw); - - // Step4. Check if the closest lanelet is valid, and add all - // of the lanelets that are below max_dist and max_delta_yaw - const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; - const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0; - if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) { - return true; - } - - return false; -} - -lanelet::ConstLanelets getClosestValidLanelets( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const double max_distance_from_lane, const double max_angle_diff_from_lane) -{ - // obstacle point - lanelet::BasicPoint2d search_point( - object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y); - - // nearest lanelet - std::vector> surrounding_lanelets = - lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); - - // No Closest Lanelets - if (surrounding_lanelets.empty()) { - return {}; - } - - lanelet::ConstLanelets closest_lanelets; - for (const auto & lanelet : surrounding_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets and - // Check if similar lanelet is inside the closest lanelet - if ( - !checkCloseLaneletCondition( - lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || - isDuplicated(lanelet, closest_lanelets)) { - continue; - } - - closest_lanelets.push_back(lanelet.second); - } - - return closest_lanelets; -} - -bool hasValidVelocityDirectionToLanelet( - const TrackedObject & object, const lanelet::ConstLanelets & lanelets, - const double max_lateral_velocity) -{ - // get object velocity direction - const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; - const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; - const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); // local frame - const double object_vel_yaw_global = - tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); - const double object_vel = std::hypot(object_vel_x, object_vel_y); - - for (const auto & lanelet : lanelets) { - // get lanelet angle - const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_vel_yaw_global - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - - // get lateral velocity - const double lane_vel = object_vel * std::sin(normalized_delta_yaw); - // std::cout << "lane_vel: " << lane_vel << " , delta yaw:" << normalized_delta_yaw << " , - // object_vel " << object_vel <header.stamp); if (!self_transform) { return; @@ -387,8 +236,8 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( void RadarObjectTrackerNode::onTimer() { rclcpp::Time current_time = this->now(); - const auto self_transform = - getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); + const auto self_transform = radar_object_tracker_utils::getTransformAnonymous( + tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { return; } @@ -434,14 +283,15 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter( for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { autoware_auto_perception_msgs::msg::TrackedObject object; (*itr)->getTrackedObject(time, object); - const auto closest_lanelets = getClosestValidLanelets( + const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets( object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_); // 1. If the object is not close to any lanelet, delete the tracker const bool no_closest_lanelet = closest_lanelets.empty(); // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker const bool is_velocity_direction_close_to_lanelet = - hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_); + radar_object_tracker_utils::hasValidVelocityDirectionToLanelet( + object, closest_lanelets, max_lateral_velocity_); if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) { // std::cout << "object removed due to map based noise filter" << " no close lanelet: " << // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp new file mode 100644 index 0000000000000..870e77cc20d41 --- /dev/null +++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -0,0 +1,154 @@ +// 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 "radar_object_tracker/utils/radar_object_tracker_utils.hpp" + +namespace radar_object_tracker_utils +{ + +boost::optional getTransformAnonymous( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + std::string errstr; + if (!tf_buffer.canTransform( + target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return boost::none; + } + + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what()); + return boost::none; + } +} + +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet : lanelets) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +bool checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + const auto distance = lanelet.first; + if (distance > max_distance_from_lane) { + return false; + } + } + + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + double object_motion_yaw = object_yaw; + bool velocity_is_reverted = object.kinematics.twist_with_covariance.twist.linear.x < 0.0; + if (velocity_is_reverted) { + object_motion_yaw = tier4_autoware_utils::normalizeRadian(object_yaw + M_PI); + } + const double delta_yaw = object_motion_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw > max_angle_diff_from_lane) { + return false; + } + + return true; +} + +lanelet::ConstLanelets getClosestValidLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const double max_distance_from_lane, + const double max_angle_diff_from_lane) +{ + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); + + if (surrounding_lanelets.empty()) { + return {}; + } + + lanelet::ConstLanelets closest_lanelets; + for (const auto & lanelet : surrounding_lanelets) { + if ( + !checkCloseLaneletCondition( + lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || + isDuplicated(lanelet, closest_lanelets)) { + continue; + } + + closest_lanelets.push_back(lanelet.second); + } + + return closest_lanelets; +} + +bool hasValidVelocityDirectionToLanelet( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); + const double object_vel_yaw_global = + tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + const double object_vel = std::hypot(object_vel_x, object_vel_y); + + for (const auto & lanelet : lanelets) { + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_vel_yaw_global - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + + const double lane_vel = object_vel * std::sin(normalized_delta_yaw); + if (std::fabs(lane_vel) < max_lateral_velocity) { + return true; + } + } + return false; +} + +} // namespace radar_object_tracker_utils diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp new file mode 100644 index 0000000000000..50b1de7132f6f --- /dev/null +++ b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp @@ -0,0 +1,121 @@ +// 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 "radar_object_tracker/utils/radar_object_tracker_utils.hpp" + +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using radar_object_tracker_utils::checkCloseLaneletCondition; + +// helper function to create a dummy straight lanelet +lanelet::Lanelet createDummyStraightLanelet(double length, double width) +{ + lanelet::LineString3d left_line( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 0, 0, 0), + lanelet::Point3d(lanelet::utils::getId(), length, 0, 0)}); + lanelet::LineString3d right_line( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 0, width, 0), + lanelet::Point3d(lanelet::utils::getId(), length, width, 0)}); + lanelet::LineString3d center_line( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 0, width / 2, 0), + lanelet::Point3d(lanelet::utils::getId(), length, width / 2, 0)}); + + // Correct constructor call + return lanelet::Lanelet( + lanelet::utils::getId(), left_line, right_line, lanelet::AttributeMap(), + lanelet::RegulatoryElementPtrs()); +} + +// helper function to create a dummy tracked object +TrackedObject createDummyTrackedObject(double x, double y, double yaw, double velocity) +{ + TrackedObject obj; + obj.kinematics.pose_with_covariance.pose.position.x = x; + obj.kinematics.pose_with_covariance.pose.position.y = y; + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + obj.kinematics.pose_with_covariance.pose.orientation = tf2::toMsg(q); + obj.kinematics.twist_with_covariance.twist.linear.x = velocity; + return obj; +} + +// 1. Test checkCloseLaneletCondition +// 1. Inside lanelet +TEST(CheckCloseLaneletConditionTest, InsideLanelet) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + TrackedObject obj = createDummyTrackedObject(50.0, 1.5, 0.0, 10.0); + + bool result = checkCloseLaneletCondition({0.0, lanelet}, obj, 5.0, M_PI / 4); + EXPECT_TRUE(result); +} + +// 2. Outside lanelet but close +TEST(CheckCloseLaneletConditionTest, OutsideLaneletButClose) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + TrackedObject obj = createDummyTrackedObject(5.0, 50.0, 0.0, 10.0); + + bool result = checkCloseLaneletCondition({3.0, lanelet}, obj, 5.0, M_PI / 4); + EXPECT_TRUE(result); +} + +// 3. Outside lanelet and far +TEST(CheckCloseLaneletConditionTest, OutsideLaneletAndFar) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + TrackedObject obj = createDummyTrackedObject(10.0, 50.0, 0.0, 10.0); + + bool result = checkCloseLaneletCondition({10.0, lanelet}, obj, 5.0, M_PI / 4); + EXPECT_FALSE(result); +} + +// 4. Inside but has multiple angle difference and velocity conditions +TEST(CheckCloseLaneletConditionTest, AngleDifferenceTooLarge) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + constexpr double eps = 1e-6; + // 1. forward velocity and angle difference is 0 + const TrackedObject obj1_1 = createDummyTrackedObject(50.0, 1.5, 0.0, 10.0); + const TrackedObject obj1_2 = createDummyTrackedObject(50.0, 1.5, eps, 10.0); + const TrackedObject obj1_3 = createDummyTrackedObject(50.0, 1.5, -eps, 10.0); + // 2. forward velocity and angle difference is Inverse + const TrackedObject obj2_1 = createDummyTrackedObject(50.0, 1.5, M_PI, 10.0); + const TrackedObject obj2_2 = createDummyTrackedObject(50.0, 1.5, M_PI + eps, 10.0); + const TrackedObject obj2_3 = createDummyTrackedObject(50.0, 1.5, M_PI - eps, 10.0); + // 3. backward velocity and angle difference is 0 + const TrackedObject obj3_1 = createDummyTrackedObject(50.0, 1.5, M_PI, -10.0); + const TrackedObject obj3_2 = createDummyTrackedObject(50.0, 1.5, M_PI + eps, -10.0); + const TrackedObject obj3_3 = createDummyTrackedObject(50.0, 1.5, M_PI - eps, -10.0); + // 4. backward velocity and angle difference is Inverse + const TrackedObject obj4_1 = createDummyTrackedObject(50.0, 1.5, 0.0, -10.0); + const TrackedObject obj4_2 = createDummyTrackedObject(50.0, 1.5, eps, -10.0); + const TrackedObject obj4_3 = createDummyTrackedObject(50.0, 1.5, -eps, -10.0); + + // 1 and 3 should be true, 2 and 4 should be false + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj1_1, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj1_2, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj1_3, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj3_1, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj3_2, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj3_3, 5.0, M_PI / 4)); + + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj2_1, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj2_2, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj2_3, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj4_1, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj4_2, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj4_3, 5.0, M_PI / 4)); +} diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 8eb7a15b5a885..527c565b91e05 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -66,3 +66,19 @@ ament_auto_package(INSTALL_TO_SHARE launch config ) + +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) + + target_link_libraries(test_shape_estimation + shape_estimation_node + ) +endif() diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index 334b6cc2af123..137d925de6d54 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -12,7 +12,6 @@ autoware_cmake autoware_auto_perception_msgs - builtin_interfaces eigen libopencv-dev libpcl-all-dev @@ -25,6 +24,7 @@ tier4_autoware_utils tier4_perception_msgs + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 9d7e8b4d4729d..04a96391ca165 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -61,6 +61,22 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option published_time_publisher_ = std::make_unique(this); } +static autoware_auto_perception_msgs::msg::ObjectClassification::_label_type get_label( + const autoware_auto_perception_msgs::msg::DetectedObject::_classification_type & classification) +{ + if (classification.empty()) { + return Label::UNKNOWN; + } + return classification.front().label; +} + +static bool label_is_vehicle( + const autoware_auto_perception_msgs::msg::ObjectClassification::_label_type & label) +{ + return Label::CAR == label || Label::TRUCK == label || Label::BUS == label || + Label::TRAILER == label; +} + void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg) { stop_watch_ptr_->toc("processing_time", true); @@ -76,11 +92,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Estimate shape for each object and pack msg for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; - const auto & label = object.classification.front().label; + const auto label = get_label(object.classification); + const auto is_vehicle = label_is_vehicle(label); const auto & feature = feature_object.feature; - const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || - Label::TRAILER == label; - // convert ros to pcl pcl::PointCloud::Ptr cluster(new pcl::PointCloud); pcl::fromROSMsg(feature.cluster, *cluster); diff --git a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp new file mode 100644 index 0000000000000..d59cfd11b69df --- /dev/null +++ b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp @@ -0,0 +1,236 @@ + +// 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 "shape_estimation/corrector/corrector.hpp" +#include "shape_estimation/filter/filter.hpp" +#include "shape_estimation/model/model.hpp" +#include "shape_estimation/shape_estimator.hpp" + +#include +#include + +namespace +{ +double yawFromQuaternion(const geometry_msgs::msg::Quaternion & q) +{ + return atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); +} + +double deg2rad(const double deg) +{ + return deg / 180.0 * M_PI; +} + +pcl::PointCloud createLShapeCluster( + const double length, const double width, const double height, const double yaw, + const double offset_x, const double offset_y) +{ + pcl::PointCloud cluster; + for (double x = -length / 2; x < length / 2; x += 0.4) { + cluster.push_back(pcl::PointXYZ(x, width / 2, 0.0)); + } + for (double y = -width / 2; y < width / 2; y += 0.2) { + cluster.push_back(pcl::PointXYZ(-length / 2, y, 0.0)); + } + cluster.push_back(pcl::PointXYZ(length / 2, -width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(length / 2, width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(-length / 2, -width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(-length / 2, width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(0.0, 0.0, height)); + + for (auto & point : cluster) { + const double x = point.x; + const double y = point.y; + // rotate + point.x = x * cos(yaw) - y * sin(yaw); + point.y = x * sin(yaw) + y * cos(yaw); + // offset + point.x += offset_x; + point.y += offset_y; + } + + return cluster; +} +} // namespace + +// test BoundingBoxShapeModel +// 1. base case +TEST(BoundingBoxShapeModel, test_estimateShape) +{ + // Generate BoundingBoxShapeModel + BoundingBoxShapeModel bbox_shape_model = BoundingBoxShapeModel(); + + // Generate cluster + const double length = 4.0; + const double width = 2.0; + const double height = 1.0; + + pcl::PointCloud cluster = + createLShapeCluster(length, width, height, 0.0, 0.0, 0.0); + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = bbox_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); + + // Check shape_output + EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX); + EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1); + EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1); + EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1); + + // Check pose_output + EXPECT_NEAR(pose_output.position.x, 0.0, 1e-2); + EXPECT_NEAR(pose_output.position.y, 0.0, 1e-2); + EXPECT_NEAR(pose_output.position.z, height / 2, 1e-2); + + // transform quaternion to yaw + const double pose_output_yaw = yawFromQuaternion(pose_output.orientation); + EXPECT_NEAR(pose_output_yaw, 0, 1e-3); +} + +// 2. rotated case +TEST(BoundingBoxShapeModel, test_estimateShape_rotated) +{ + // Generate cluster + const double length = 4.0; + const double width = 2.0; + const double height = 1.0; + const double yaw = deg2rad(30.0); + const double offset_x = 10.0; + const double offset_y = 1.0; + pcl::PointCloud cluster = + createLShapeCluster(length, width, height, yaw, offset_x, offset_y); + + const auto ref_yaw_info = + ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const bool use_boost_bbox_optimizer = true; + // Generate BoundingBoxShapeModel + BoundingBoxShapeModel bbox_shape_model = + BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer); + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = bbox_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); + + // Check shape_output + EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX); + EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1); + EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1); + EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1); + + // Check pose_output + EXPECT_NEAR(pose_output.position.x, offset_x, 1.0); + EXPECT_NEAR(pose_output.position.y, offset_y, 1.0); + EXPECT_NEAR(pose_output.position.z, height / 2, 1.0); + + // transform quaternion to yaw + const double pose_output_yaw = yawFromQuaternion(pose_output.orientation); + EXPECT_NEAR(pose_output_yaw, yaw, deg2rad(15.0)); +} + +// test CylinderShapeModel +TEST(CylinderShapeModel, test_estimateShape) +{ + // Generate CylinderShapeModel + CylinderShapeModel cylinder_shape_model = CylinderShapeModel(); + + // Generate cluster + pcl::PointCloud cluster = createLShapeCluster(4.0, 2.0, 1.0, 0.0, 0.0, 0.0); + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = cylinder_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); +} + +// test ConvexHullShapeModel +TEST(ConvexHullShapeModel, test_estimateShape) +{ + // Generate ConvexHullShapeModel + ConvexHullShapeModel convex_hull_shape_model = ConvexHullShapeModel(); + + // Generate cluster + pcl::PointCloud cluster = createLShapeCluster(2.0, 1.0, 1.0, 0.0, 0.0, 0.0); + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = convex_hull_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); +} + +// test ShapeEstimator +TEST(ShapeEstimator, test_estimateShapeAndPose) +{ + // Generate cluster + double length = 4.0; + double width = 2.0; + double height = 1.0; + const double yaw = deg2rad(60.0); + const double offset_x = 6.0; + const double offset_y = -2.0; + pcl::PointCloud cluster = + createLShapeCluster(length, width, height, yaw, offset_x, offset_y); + + // Generate ShapeEstimator + const bool use_corrector = true; + const bool use_filter = true; + const bool use_boost_bbox_optimizer = true; + ShapeEstimator shape_estimator = + ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); + + // Generate ref_yaw_info + boost::optional ref_yaw_info = boost::none; + boost::optional ref_shape_size_info = boost::none; + + ref_yaw_info = ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const auto label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShapeAndPose + const bool result = shape_estimator.estimateShapeAndPose( + label, cluster, ref_yaw_info, ref_shape_size_info, shape_output, pose_output); + EXPECT_TRUE(result); + + // Check shape_output + EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX); + EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1); + EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1); + EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1); + + // Check pose_output + EXPECT_NEAR(pose_output.position.x, offset_x, 1.0); + EXPECT_NEAR(pose_output.position.y, offset_y, 1.0); + EXPECT_NEAR(pose_output.position.z, height / 2, 1.0); + + // transform quaternion to yaw + const double pose_output_yaw = yawFromQuaternion(pose_output.orientation); + EXPECT_NEAR(pose_output_yaw, yaw, deg2rad(15.0)); +} diff --git a/perception/shape_estimation/test/test_shape_estimation.cpp b/perception/shape_estimation/test/test_shape_estimation.cpp new file mode 100644 index 0000000000000..4c8ad7dd25916 --- /dev/null +++ b/perception/shape_estimation/test/test_shape_estimation.cpp @@ -0,0 +1,26 @@ +// 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp index e7ab6077250f8..7cd7d532d07f5 100644 --- a/perception/tracking_object_merger/src/data_association/data_association.cpp +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -180,13 +180,6 @@ double DataAssociation::calcScoreBetweenObjects( const std::uint8_t object0_label = object_recognition_utils::getHighestProbLabel(object0.classification); - std::vector tracker_pose = { - object1.kinematics.pose_with_covariance.pose.position.x, - object1.kinematics.pose_with_covariance.pose.position.y}; - std::vector measurement_pose = { - object0.kinematics.pose_with_covariance.pose.position.x, - object0.kinematics.pose_with_covariance.pose.position.y}; - double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 2a2fbe4340fa8..8e016e2b63391 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 57d292c348f2c..74002916bb23c 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -21,6 +21,7 @@ #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "map_projection_loader/load_info_from_lanelet2_map.hpp" +#include "map_projection_loader/map_projection_loader.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -36,7 +37,6 @@ #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" -#include #include #include @@ -204,8 +204,10 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( const auto selected_centerline = std::vector( c.centerline.begin() + traj_range_indices_.first, c.centerline.begin() + traj_range_indices_.second + 1); + const auto selected_route_lane_ids = get_route_lane_ids_from_points(selected_centerline); save_map( - lanelet2_output_file_path, CenterlineWithRoute{selected_centerline, c.route_lane_ids}); + lanelet2_output_file_path, + CenterlineWithRoute{selected_centerline, selected_route_lane_ids}); } }); sub_traj_resample_interval_ = create_subscription( @@ -280,6 +282,8 @@ void StaticCenterlineGeneratorNode::run() load_map(lanelet2_input_file_path); const auto centerline_with_route = generate_centerline_with_route(); traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1); + + evaluate(centerline_with_route.route_lane_ids, centerline_with_route.centerline); save_map(lanelet2_output_file_path, centerline_with_route); centerline_with_route_ = centerline_with_route; @@ -304,19 +308,7 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout return CenterlineWithRoute{optimized_centerline, route_lane_ids}; } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { const auto bag_centerline = generate_centerline_with_bag(*this); - const auto start_lanelets = - route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.front().pose); - const auto end_lanelets = - route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.back().pose); - if (start_lanelets.empty() || end_lanelets.empty()) { - RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); - return CenterlineWithRoute{}; - } - - const lanelet::Id start_lanelet_id = start_lanelets.front().id(); - const lanelet::Id end_lanelet_id = end_lanelets.front().id(); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - + const auto route_lane_ids = get_route_lane_ids_from_points(bag_centerline); return CenterlineWithRoute{bag_centerline, route_lane_ids}; } throw std::logic_error( @@ -337,6 +329,24 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout return centerline_with_route; } +std::vector StaticCenterlineGeneratorNode::get_route_lane_ids_from_points( + const std::vector & points) +{ + const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.front().pose); + const auto end_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.back().pose); + if (start_lanelets.empty() || end_lanelets.empty()) { + RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's points are not found."); + return std::vector{}; + } + + const lanelet::Id start_lanelet_id = start_lanelets.front().id(); + const lanelet::Id end_lanelet_id = end_lanelets.front().id(); + if (start_lanelet_id == end_lanelet_id) { + return std::vector{start_lanelet_id}; + } + return plan_route(start_lanelet_id, end_lanelet_id); +} + void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // copy the input LL2 map to the temporary file for debugging @@ -349,21 +359,18 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map - const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); + map_projector_info_ = + std::make_unique(load_info_from_lanelet2_map(lanelet2_input_file_path)); const auto map_ptr = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, *map_projector_info_); if (!map_ptr) { return nullptr; } - // NOTE: generate map projector for lanelet::write(). - // Without this, lat/lon of the generated LL2 map will be wrong. - map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); - - // NOTE: The original map is stored here since the various ids in the lanelet map will change - // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. + // NOTE: The original map is stored here since the centerline will be added to all the + // lanelet when lanelet::utils::overwriteLaneletCenterline is called. original_map_ptr_ = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, *map_projector_info_); // overwrite more dense centerline lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); @@ -639,11 +646,13 @@ void StaticCenterlineGeneratorNode::save_map( const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); // update centerline in map - utils::update_centerline(*route_handler_ptr_, route_lanelets, c.centerline); + utils::update_centerline(original_map_ptr_, route_lanelets, c.centerline); RCLCPP_INFO(get_logger(), "Updated centerline in map."); // save map with modified center line - lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_); + std::filesystem::create_directory("/tmp/static_centerline_generator"); // TODO(murooka) + const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_); + lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); RCLCPP_INFO(get_logger(), "Saved map."); // copy the output LL2 map to the temporary file for debugging diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index 9baa1c3fbb892..dfe5af68c270b 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -23,11 +23,10 @@ #include "type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include - #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" +#include "tier4_map_msgs/msg/map_projector_info.hpp" #include #include @@ -39,6 +38,7 @@ namespace autoware::static_centerline_generator using autoware_static_centerline_generator::srv::LoadMap; using autoware_static_centerline_generator::srv::PlanPath; using autoware_static_centerline_generator::srv::PlanRoute; +using tier4_map_msgs::msg::MapProjectorInfo; struct CenterlineWithRoute { @@ -66,6 +66,8 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // plan centerline CenterlineWithRoute generate_centerline_with_route(); + std::vector get_route_lane_ids_from_points( + const std::vector & points); void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); @@ -80,7 +82,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node lanelet::LaneletMapPtr original_map_ptr_{nullptr}; HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; - std::unique_ptr map_projector_{nullptr}; + std::unique_ptr map_projector_info_{nullptr}; std::pair traj_range_indices_{0, 0}; std::optional centerline_with_route_{std::nullopt}; diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 4ea56e10935b2..2dbd82772f7ef 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -35,12 +35,19 @@ nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs: return odometry_ptr; } -lanelet::Point3d createPoint3d(const double x, const double y, const double z = 19.0) +lanelet::Point3d createPoint3d(const double x, const double y, const double z) { lanelet::Point3d point(lanelet::utils::getId()); + + // position + point.x() = x; + point.y() = y; + point.z() = z; + + // attributes point.setAttribute("local_x", x); point.setAttribute("local_y", y); - point.setAttribute("ele", z); + // NOTE: It seems that the attribute "ele" is assigned automatically. return point; } @@ -76,11 +83,13 @@ geometry_msgs::msg::Pose get_center_pose( geometry_msgs::msg::Point middle_pos; middle_pos.x = center_line[middle_point_idx].x(); middle_pos.y = center_line[middle_point_idx].y(); + middle_pos.z = center_line[middle_point_idx].z(); // get next middle position of the lanelet geometry_msgs::msg::Point next_middle_pos; next_middle_pos.x = center_line[middle_point_idx + 1].x(); next_middle_pos.y = center_line[middle_point_idx + 1].y(); + next_middle_pos.z = center_line[middle_point_idx + 1].z(); // calculate middle pose geometry_msgs::msg::Pose middle_pose; @@ -119,13 +128,13 @@ PathWithLaneId get_path_with_lane_id( } void update_centerline( - RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, + lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, const std::vector & new_centerline) { // get lanelet as reference to update centerline lanelet::Lanelets lanelets_ref; for (const auto & lanelet : lanelets) { - for (auto & lanelet_ref : route_handler.getLaneletMapPtr()->laneletLayer) { + for (auto & lanelet_ref : lanelet_map_ptr->laneletLayer) { if (lanelet_ref.id() == lanelet.id()) { lanelets_ref.push_back(lanelet_ref); } @@ -148,13 +157,13 @@ void update_centerline( // set center point centerline.push_back(center_point); - route_handler.getLaneletMapPtr()->add(center_point); + lanelet_map_ptr->add(center_point); break; } if (!centerline.empty()) { // set centerline - route_handler.getLaneletMapPtr()->add(centerline); + lanelet_map_ptr->add(centerline); lanelet_ref.setCenterline(centerline); // prepare new centerline @@ -166,7 +175,7 @@ void update_centerline( auto & lanelet_ref = lanelets_ref.at(lanelet_idx); // set centerline - route_handler.getLaneletMapPtr()->add(centerline); + lanelet_map_ptr->add(centerline); lanelet_ref.setCenterline(centerline); } } diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp index 6c6257bee59a3..1d7eb1f18cc44 100644 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -42,7 +42,7 @@ PathWithLaneId get_path_with_lane_id( const double nearest_ego_yaw_threshold); void update_centerline( - RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, + lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, const std::vector & new_centerline); MarkerArray create_footprint_marker( diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 6b11d490e8c23..194e5ecdaf86c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -138,9 +139,15 @@ class AvoidanceModule : public SceneModuleInterface void removeCandidateRTCStatus() { if (rtc_interface_ptr_map_.at("left")->isRegistered(candidate_uuid_)) { - rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_); - } else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { - rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_); + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + candidate_uuid_, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + + if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + candidate_uuid_, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); } } @@ -362,10 +369,21 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); + for (const auto & left_shift : left_shift_array_) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + + for (const auto & right_shift : right_shift_array_) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + if (!path_shifter_.getShiftLines().empty()) { left_shift_array_.clear(); right_shift_array_.clear(); - removeRTCStatus(); } generator_.reset(); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 6858b3809a73b..51fe19e4b3237 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1283,7 +1283,6 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); - removeRTCStatus(); } void AvoidanceModule::processOnExit() diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 1e83769dde599..3559981fed38d 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -2221,7 +2221,7 @@ DrivableLanes generateExpandedDrivableLanes( break; } if (i == max_recursive_search_num - 1) { - RCLCPP_ERROR( + RCLCPP_DEBUG( rclcpp::get_logger(logger_namespace), "Drivable area expansion reaches max iteration."); } } diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 03f8c8d2a5696..beaa1ae6c5263 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -32,8 +32,8 @@ Behavior Path Planner has following scene modules | Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | | Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | | External Lane Change | WIP | LINK | -| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | -| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | | Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | !!! Note diff --git a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 38eafb2f38d7d..1ea555f4675ef 100644 --- a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -53,7 +53,7 @@ namespace behavior_path_planner using motion_utils::findNearestIndex; using motion_utils::insertOrientation; -using motion_utils::removeInvalidOrientationPoints; +using motion_utils::removeFirstInvalidOrientationPoints; using motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) @@ -155,14 +155,14 @@ bool PathShifter::generate( shifted_path->path.points = removeOverlapPoints(shifted_path->path.points); // Use orientation before shift to remove points in reverse order // before setting wrong azimuth orientation - removeInvalidOrientationPoints(shifted_path->path.points); + removeFirstInvalidOrientationPoints(shifted_path->path.points); size_t previous_size{shifted_path->path.points.size()}; do { previous_size = shifted_path->path.points.size(); // Set the azimuth orientation to the next point at each point insertOrientation(shifted_path->path.points, true); // Use azimuth orientation to remove points in reverse order - removeInvalidOrientationPoints(shifted_path->path.points); + removeFirstInvalidOrientationPoints(shifted_path->path.points); } while (previous_size != shifted_path->path.points.size()); // DEBUG diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 92afd25026a70..7b886fea09b34 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -87,12 +87,8 @@ boost::optional NoStoppingAreaModule::getStopLineGeometry2d( const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; std::vector collision_points; bg::intersection(area_poly, line, collision_points); - if (collision_points.empty()) { - continue; - } - const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); if (!collision_points.empty()) { - geometry_msgs::msg::Point left_point; + const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); const double w = planner_data_->vehicle_info_.vehicle_width_m; const double l = stop_line_margin; stop_line.emplace_back( @@ -306,7 +302,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( const int num_ignore_nearest = 1; // Do not consider nearest lane polygon size_t ego_area_start_idx = closest_idx + num_ignore_nearest; - size_t ego_area_end_idx = ego_area_start_idx; // return if area size is not intentional if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) { return ego_area; @@ -330,7 +325,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } double dist_from_area_sum = 0.0; // decide end idx with extract distance - ego_area_end_idx = ego_area_start_idx; + size_t ego_area_end_idx = ego_area_start_idx; for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 519e68764b450..a68705e3f73a2 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -56,3 +56,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the | `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | | `system_delay` | double | (to be a global parameter) delay time until output control command | | `delay_response_time` | double | (to be a global parameter) delay time of the vehicle's response to control commands | + +## Traffic Light Handling in sim/real + +The handling of traffic light information varies depending on the usage. In the below table, the traffic signal topic element for the corresponding lane is denoted as `info`, and if `info` is not available, it is denoted as `null`. + +| module \\ case | `info` is `null` | `info` is not `null` | +| :--------------------------------------------------------------------------------------------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| intersection_occlusion(`is_simulation = *`)
  • `info` is the latest non-`null` information
| GO(occlusion is ignored) | intersection_occlusion uses the latest non UNKNOWN observation in the queue up to present.
  • If `info` is `GREEN or UNKNOWN`, occlusion is cared
  • If `info` is `RED or YELLOW`, occlusion is ignored(GO)
  • NOTE: Currently timeout is not considered
| +| traffic_light(sim, `is_simulation = true`)
  • `info` is current information
| GO | traffic_light uses the perceived traffic light information at present directly.
  • If `info` is timeout, STOP whatever the color is
  • If `info` is not timeout, then act according to the color. If `info` is `UNKNOWN`, STOP
{: rowspan=2} | +| traffic_light(real, `is_simulation = false`)
  • `info` is current information
| STOP | ⁠ {: style="padding:0"} | +| crosswalk with Traffic Light(`is_simulation = *`)
  • `info` is current information
| default |
  • If `disable_yield_for_new_stopped_object` is true, each sub scene_module ignore newly detected pedestrians after module instantiation.
  • If `ignore_with_traffic_light` is true, occlusion detection is skipped.
| +| map_based_prediction(`is_simulation = *`)
  • `info` is current information
| default | If a pedestrian traffic light is
  • RED, surrounding pedestrians are not predicted.
  • GREEN, stopped pedestrians are not predicted.
| diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index aa9613a35dd7c..1801db4658a72 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -311,6 +311,7 @@ void BehaviorVelocityPlannerNode::onParam() // lock(mutex_); planner_data_.velocity_smoother_ = std::make_unique(*this); + planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } void BehaviorVelocityPlannerNode::onLaneletMap( @@ -352,6 +353,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else { + // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; } } diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 33bb471e5920c..544e14f130a7e 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -8,6 +8,7 @@ Satoshi Ota Tomoya Kimura Shumpei Wakabayashi + Mamoru Sobue Apache License 2.0 diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 6e6871e20a354..edddef5cef4d8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -62,7 +62,7 @@ void TrafficLightModuleManager::modifyPathVelocity( stop_reason_array.header.stamp = path->header.stamp; first_stop_path_point_index_ = static_cast(path->points.size() - 1); - first_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); + nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( @@ -85,8 +85,8 @@ void TrafficLightModuleManager::modifyPathVelocity( } if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < - first_ref_stop_path_point_index_) { - first_ref_stop_path_point_index_ = + nearest_ref_stop_path_point_index_) { + nearest_ref_stop_path_point_index_ = traffic_light_scene_module->getFirstRefStopPathPointIndex(); if ( traffic_light_scene_module->getTrafficLightModuleState() != @@ -126,15 +126,14 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); - const auto module_id = lane_id; - if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) { + if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( - module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, - planner_param_, logger_.get_child("traffic_light_module"), clock_)); - generateUUID(module_id); + lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, + logger_.get_child("traffic_light_module"), clock_)); + generateUUID(lane_id); updateRTCStatus( - getUUID(module_id), true, State::WAITING_FOR_EXECUTION, - std::numeric_limits::lowest(), path.header.stamp); + getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + path.header.stamp); } } } @@ -146,9 +145,10 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [this, lanelet_id_set](const std::shared_ptr & scene_module) { + return [this, lanelet_id_set]( + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { - if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) { + if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; } } @@ -156,23 +156,6 @@ TrafficLightModuleManager::getModuleExpiredFunction( }; } -bool TrafficLightModuleManager::isModuleRegisteredFromRegElement( - const lanelet::Id & id, const size_t module_id) const -{ - const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); - - const auto registered_lane = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id); - for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { - for (const auto & element : lane.regulatoryElementsAs()) { - if (hasSameTrafficLight(element, registered_element)) { - return true; - } - } - } - return false; -} - bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule( const lanelet::Id & id) const { diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 87213d8a5ed3f..959ef2f91d36c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -58,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; - std::optional first_ref_stop_path_point_index_; + std::optional nearest_ref_stop_path_point_index_; }; class TrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 413ce78beacee..0158251bb42b4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -162,11 +162,10 @@ bool calcStopPointAndInsertIndex( } // namespace TrafficLightModule::TrafficLightModule( - const int64_t module_id, const int64_t lane_id, - const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterface(lane_id, logger, clock), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), @@ -365,7 +364,7 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_tra { // get traffic signal associated with the regulatory element id const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( - traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + traffic_light_reg_elem_.id(), false /* traffic light module does not keep last observation */); if (!traffic_signal_stamped_opt) { RCLCPP_WARN_STREAM_ONCE( logger_, "the traffic signal data associated with regulatory element id " diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 102ddbe2e9fa8..d6a409ca7034a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -66,9 +66,8 @@ class TrafficLightModule : public SceneModuleInterface public: TrafficLightModule( - const int64_t module_id, const int64_t lane_id, - const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 4bc0f21dd947b..d4f12316052d9 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -98,7 +98,7 @@ bool is_in_parking_lot( } double project_goal_to_map( - const lanelet::Lanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) { const lanelet::ConstLineString3d center_line = lanelet::utils::generateFineCenterline(lanelet_component); @@ -182,12 +182,6 @@ bool DefaultPlanner::ready() const void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) { route_handler_.setMap(*msg); - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); is_graph_ready_ = true; } @@ -199,7 +193,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) for (const auto & route_section : route.segments) { for (const auto & lane_id : route_section.primitives) { - auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id.id); + auto lanelet = route_handler_.getLaneletsFromId(lane_id.id); route_lanelets.push_back(lanelet); if (route_section.preferred_primitive.id == lane_id.id) { goal_lanelets.push_back(lanelet); @@ -274,15 +268,15 @@ bool DefaultPlanner::check_goal_footprint_inside_lanes( if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) { return true; } - + const auto following = route_handler_.getNextLanelets(current_lanelet); // check if goal footprint is in between many lanelets in depth-first search manner - for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) { + for (const auto & next_lane : following) { next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); lanelet::ConstLanelet combined_lanelets = - combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); + combine_lanelets_with_shoulder(lanelets, route_handler_); // if next lanelet length is longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -317,23 +311,38 @@ bool DefaultPlanner::is_goal_valid( // check if goal is in shoulder lanelet lanelet::Lanelet closest_shoulder_lanelet; + const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(goal); if (lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } + shoulder_lanelets, goal, &closest_shoulder_lanelet)) { + const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; } } - - lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { - return false; + lanelet::ConstLanelet closest_lanelet; + const auto road_lanelets_at_goal = route_handler_.getRoadLaneletsAtPose(goal); + if (!lanelet::utils::query::getClosestLanelet(road_lanelets_at_goal, goal, &closest_lanelet)) { + // if no road lanelets directly at the goal, find the closest one + const lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y}; + auto closest_dist = std::numeric_limits::max(); + const auto closest_road_lanelet_found = + route_handler_.getLaneletMapPtr()->laneletLayer.nearestUntil( + goal_point, [&](const auto & bbox, const auto & ll) { + // this search is done by increasing distance between the bounding box and the goal + // we stop the search when the bounding box is further than the closest dist found + if (lanelet::geometry::distance2d(bbox, goal_point) > closest_dist) + return true; // stop the search + const auto dist = lanelet::geometry::distance2d(goal_point, ll.polygon2d()); + if (route_handler_.isRoadLanelet(ll) && dist < closest_dist) { + closest_dist = dist; + closest_lanelet = ll; + } + return false; // continue the search + }); + if (!closest_road_lanelet_found) return false; } const auto local_vehicle_footprint = vehicle_info_.createFootprint(); @@ -345,7 +354,7 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets const lanelet::ConstLanelet combined_prev_lanelet = - combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); + combine_lanelets_with_shoulder(path_lanelets, route_handler_); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( @@ -353,7 +362,7 @@ bool DefaultPlanner::is_goal_valid( !check_goal_footprint_inside_lanes( closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && !is_in_parking_lot( - lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_), + lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()), lanelet::utils::conversion::toLaneletPoint(goal.position))) { RCLCPP_WARN(logger, "Goal's footprint exceeds lane!"); return false; @@ -371,13 +380,15 @@ bool DefaultPlanner::is_goal_valid( } // check if goal is in parking space - const auto parking_spaces = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); + const auto parking_spaces = + lanelet::utils::query::getAllParkingSpaces(route_handler_.getLaneletMapPtr()); if (is_in_parking_space(parking_spaces, goal_lanelet_pt)) { return true; } // check if goal is in parking lot - const auto parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_); + const auto parking_lots = + lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()); if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) { return true; } @@ -424,7 +435,8 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) auto goal_pose = points.back(); if (param_.enable_correct_goal_pose) { goal_pose = get_closest_centerline_pose( - lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_); + lanelet::utils::query::laneletLayer(route_handler_.getLaneletMapPtr()), goal_pose, + vehicle_info_); } if (!is_goal_valid(goal_pose, all_route_lanelets)) { @@ -451,7 +463,7 @@ geometry_msgs::msg::Pose DefaultPlanner::refine_goal_height( const Pose & goal, const RouteSections & route_sections) { const auto goal_lane_id = route_sections.back().preferred_primitive.id; - lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id); + const auto goal_lanelet = route_handler_.getLaneletsFromId(goal_lane_id); const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); const auto goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt); diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 9f1a9d206e376..8c0fbf3b33955 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -48,6 +48,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) override; bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; + void updateRoute(const PlannerPlugin::LaneletRoute & route) override; + void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; vehicle_info_util::VehicleInfo vehicle_info_; @@ -56,11 +58,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin using RouteSections = std::vector; using Pose = geometry_msgs::msg::Pose; bool is_graph_ready_; - lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::routing::RoutingGraphPtr routing_graph_ptr_; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; - lanelet::ConstLanelets road_lanelets_; - lanelet::ConstLanelets shoulder_lanelets_; route_handler::RouteHandler route_handler_; DefaultPlannerParameters param_; @@ -102,9 +99,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin * route_sections) and return the z-aligned goal position */ Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections); - - void updateRoute(const PlannerPlugin::LaneletRoute & route); - void clearRoute(); }; } // namespace mission_planner::lanelet2 diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 4ef4bd6f20ed0..e7797946aa619 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,7 +42,7 @@ void insert_marker_array( } lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) + const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -57,30 +57,18 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder( } } + // lambda to add bound to target_bound + const auto add_bound = [](const auto & bound, auto & target_bound) { + std::transform( + bound.begin(), bound.end(), std::back_inserter(target_bound), + [](const auto & pt) { return lanelet::Point3d(pt); }); + }; for (const auto & llt : lanelets) { - // lambda to check if shoulder lane which share left bound with lanelets exist - const auto find_bound_shared_shoulder = - [&shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) { - return std::find_if( - shoulder_lanelets.begin(), shoulder_lanelets.end(), - [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) { - return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id(); - }); - }; - - // lambda to add bound to target_bound - const auto add_bound = [](const auto & bound, auto & target_bound) { - std::transform( - bound.begin(), bound.end(), std::back_inserter(target_bound), - [](const auto & pt) { return lanelet::Point3d(pt); }); - }; - // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist - const auto left_shared_shoulder_it = find_bound_shared_shoulder( - llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); }); - if (left_shared_shoulder_it != shoulder_lanelets.end()) { + const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt); + if (left_shared_shoulder) { // if exist, add left bound of SHOULDER lanelet to lefts - add_bound(left_shared_shoulder_it->leftBound(), lefts); + add_bound(left_shared_shoulder->leftBound(), lefts); } else if ( // if not exist, add left bound of lanelet to lefts // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`, @@ -90,11 +78,10 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder( } // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist - const auto right_shared_shoulder_it = find_bound_shared_shoulder( - llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); }); - if (right_shared_shoulder_it != shoulder_lanelets.end()) { + const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt); + if (right_shared_shoulder) { // if exist, add right bound of SHOULDER lanelet to rights - add_bound(right_shared_shoulder_it->rightBound(), rights); + add_bound(right_shared_shoulder->rightBound(), rights); } else if ( // if not exist, add right bound of lanelet to rights // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`, diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3eb38638e17b7..428cd979a2526 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,6 +16,7 @@ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #include +#include #include #include @@ -50,10 +51,10 @@ void insert_marker_array( * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively * @param lanelets topologically sorted sequence of lanelets - * @param shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets + * @param route_handler route handler to query the lanelet map */ lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); + const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index a819f000412b1..3f18c6e1728c6 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -60,7 +60,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & traj_points) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( - const Odometry & odometry, const PredictedObjects & objecrts, + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 1d490abca291a..71d0f84637abc 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -47,8 +47,7 @@ std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, const rclcpp::Time & current_time, const bool is_driving_forward, - std::vector & collision_index, - const double max_lat_dist = std::numeric_limits::max(), + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); } // namespace polygon_utils diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 94e9707d55c4f..f637911dae903 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1079,7 +1079,11 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise)); return collision_points; } @@ -1119,7 +1123,10 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), is_driving_forward_, collision_index, - vehicle_info_.vehicle_width_m + p.max_lat_margin_for_cruise, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), p.max_prediction_time_for_collision_check); if (collision_points.empty()) { // Ignore vehicle obstacles outside the trajectory without collision @@ -1196,7 +1203,11 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop)); if (collision_points.empty()) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index fa5a96b934f7a..1bd2de0bd985c 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -44,18 +44,18 @@ PointWithStamp calcNearestCollisionPoint( return collision_points.at(min_idx); } -// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon +// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon // calculation. std::optional>> getCollisionIndex( const std::vector & traj_points, const std::vector & traj_polygons, const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, - const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) + const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose); - if (approximated_dist > max_lat_dist) { + if (approximated_dist > max_dist) { continue; } diff --git a/planning/planning_test_utils/CMakeLists.txt b/planning/planning_test_utils/CMakeLists.txt index 29ee79c2cab1a..56e985c7fa2b7 100644 --- a/planning/planning_test_utils/CMakeLists.txt +++ b/planning/planning_test_utils/CMakeLists.txt @@ -4,7 +4,23 @@ project(planning_test_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(mock_data_parser SHARED + src/mock_data_parser.cpp) + +target_link_libraries(mock_data_parser + yaml-cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_mock_data_parser + test/test_mock_data_parser.cpp) + + target_link_libraries(test_mock_data_parser + mock_data_parser) +endif() + ament_auto_package(INSTALL_TO_SHARE config test_map + test_data ) diff --git a/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp b/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp new file mode 100644 index 0000000000000..09d4474dda15d --- /dev/null +++ b/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 TIER IV +// +// 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 PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_ +#define PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + +namespace test_utils +{ +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using geometry_msgs::msg::Pose; + +Pose parse_pose(const YAML::Node & node); + +LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node); + +std::vector parse_lanelet_primitives(const YAML::Node & node); + +std::vector parse_segments(const YAML::Node & node); + +LaneletRoute parse_lanelet_route_file(const std::string & filename); +} // namespace test_utils + +#endif // PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_ diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index c47404b45bb5d..6af1ce165d7ab 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -193,14 +193,10 @@ OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double res return costmap_msg; } -HADMapBin makeMapBinMsg() +HADMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution = 5.0) { - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; - double center_line_resolution = 5.0; - // load map from file - const auto map = loadMap(lanelet2_path); + const auto map = loadMap(absolute_path); if (!map) { return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; } @@ -210,10 +206,20 @@ HADMapBin makeMapBinMsg() // create map bin msg const auto map_bin_msg = - convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now()); + convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); return map_bin_msg; } +HADMapBin makeMapBinMsg() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; + double center_line_resolution = 5.0; + + return make_map_bin_msg(lanelet2_path, center_line_resolution); +} + Odometry makeOdometry(const double shift = 0.0) { Odometry odometry; diff --git a/planning/planning_test_utils/src/mock_data_parser.cpp b/planning/planning_test_utils/src/mock_data_parser.cpp new file mode 100644 index 0000000000000..f9f9a7b0b1594 --- /dev/null +++ b/planning/planning_test_utils/src/mock_data_parser.cpp @@ -0,0 +1,92 @@ +// Copyright 2024 TIER IV +// +// 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 "planning_test_utils/mock_data_parser.hpp" + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace test_utils +{ +Pose parse_pose(const YAML::Node & node) +{ + Pose pose; + pose.position.x = node["position"]["x"].as(); + pose.position.y = node["position"]["y"].as(); + pose.position.z = node["position"]["z"].as(); + pose.orientation.x = node["orientation"]["x"].as(); + pose.orientation.y = node["orientation"]["y"].as(); + pose.orientation.z = node["orientation"]["z"].as(); + pose.orientation.w = node["orientation"]["w"].as(); + return pose; +} + +LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node) +{ + LaneletPrimitive primitive; + primitive.id = node["id"].as(); + primitive.primitive_type = node["primitive_type"].as(); + + return primitive; +} + +std::vector parse_lanelet_primitives(const YAML::Node & node) +{ + std::vector primitives; + primitives.reserve(node.size()); + std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) { + return parse_lanelet_primitive(p); + }); + + return primitives; +} + +std::vector parse_segments(const YAML::Node & node) +{ + std::vector segments; + std::transform(node.begin(), node.end(), std::back_inserter(segments), [&](const auto & input) { + LaneletSegment segment; + segment.preferred_primitive = parse_lanelet_primitive(input["preferred_primitive"]); + segment.primitives = parse_lanelet_primitives(input["primitives"]); + return segment; + }); + + return segments; +} + +LaneletRoute parse_lanelet_route_file(const std::string & filename) +{ + LaneletRoute lanelet_route; + try { + YAML::Node config = YAML::LoadFile(filename); + + lanelet_route.start_pose = (config["start_pose"]) ? parse_pose(config["start_pose"]) : Pose(); + lanelet_route.goal_pose = (config["goal_pose"]) ? parse_pose(config["goal_pose"]) : Pose(); + lanelet_route.segments = parse_segments(config["segments"]); + } catch (const std::exception & e) { + RCLCPP_DEBUG(rclcpp::get_logger("planning_test_utils"), "Exception caught: %s", e.what()); + } + return lanelet_route; +} +} // namespace test_utils diff --git a/planning/planning_test_utils/test/test_mock_data_parser.cpp b/planning/planning_test_utils/test/test_mock_data_parser.cpp new file mode 100644 index 0000000000000..cda09522d8e6e --- /dev/null +++ b/planning/planning_test_utils/test/test_mock_data_parser.cpp @@ -0,0 +1,135 @@ +// Copyright 2024 TIER IV +// +// 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 + +// Assuming the parseRouteFile function is in 'RouteHandler.h' +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "planning_test_utils/mock_data_parser.hpp" + +namespace test_utils +{ +// Example YAML structure as a string for testing +const char g_complete_yaml[] = R"( +start_pose: + position: + x: 1.0 + y: 2.0 + z: 3.0 + orientation: + x: 0.1 + y: 0.2 + z: 0.3 + w: 0.4 +goal_pose: + position: + x: 4.0 + y: 5.0 + z: 6.0 + orientation: + x: 0.5 + y: 0.6 + z: 0.7 + w: 0.8 +segments: +- preferred_primitive: + id: 11 + primitive_type: '' + primitives: + - id: 22 + primitive_type: lane + - id: 33 + primitive_type: lane +)"; + +TEST(ParseFunctions, CompleteYAMLTest) +{ + YAML::Node config = YAML::Load(g_complete_yaml); + + // Test parsing of start_pose and goal_pose + Pose start_pose = parse_pose(config["start_pose"]); + Pose goal_pose = parse_pose(config["goal_pose"]); + + EXPECT_DOUBLE_EQ(start_pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(start_pose.position.y, 2.0); + EXPECT_DOUBLE_EQ(start_pose.position.z, 3.0); + + EXPECT_DOUBLE_EQ(start_pose.orientation.x, 0.1); + EXPECT_DOUBLE_EQ(start_pose.orientation.y, 0.2); + EXPECT_DOUBLE_EQ(start_pose.orientation.z, 0.3); + EXPECT_DOUBLE_EQ(start_pose.orientation.w, 0.4); + + EXPECT_DOUBLE_EQ(goal_pose.position.x, 4.0); + EXPECT_DOUBLE_EQ(goal_pose.position.y, 5.0); + EXPECT_DOUBLE_EQ(goal_pose.position.z, 6.0); + EXPECT_DOUBLE_EQ(goal_pose.orientation.x, 0.5); + EXPECT_DOUBLE_EQ(goal_pose.orientation.y, 0.6); + EXPECT_DOUBLE_EQ(goal_pose.orientation.z, 0.7); + EXPECT_DOUBLE_EQ(goal_pose.orientation.w, 0.8); + + // Test parsing of segments + std::vector segments = parse_segments(config["segments"]); + ASSERT_EQ( + segments.size(), uint64_t(1)); // Assuming only one segment in the provided YAML for this test + + const auto & segment0 = segments[0]; + EXPECT_EQ(segment0.preferred_primitive.id, 11); + EXPECT_EQ(segment0.primitives.size(), uint64_t(2)); + EXPECT_EQ(segment0.primitives[0].id, 22); + EXPECT_EQ(segment0.primitives[0].primitive_type, "lane"); + EXPECT_EQ(segment0.primitives[1].id, 33); + EXPECT_EQ(segment0.primitives[1].primitive_type, "lane"); +} + +TEST(ParseFunction, CompleteFromFilename) +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto parser_test_route = + planning_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml"; + + const auto lanelet_route = parse_lanelet_route_file(parser_test_route); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0); + + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3); + EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4); + + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7); + EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8); + + ASSERT_EQ( + lanelet_route.segments.size(), + uint64_t(2)); // Assuming only one segment in the provided YAML for this test + const auto & segment1 = lanelet_route.segments[1]; + EXPECT_EQ(segment1.preferred_primitive.id, 44); + EXPECT_EQ(segment1.primitives.size(), uint64_t(4)); + EXPECT_EQ(segment1.primitives[0].id, 55); + EXPECT_EQ(segment1.primitives[0].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[1].id, 66); + EXPECT_EQ(segment1.primitives[1].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[2].id, 77); + EXPECT_EQ(segment1.primitives[2].primitive_type, "lane"); + EXPECT_EQ(segment1.primitives[3].id, 88); + EXPECT_EQ(segment1.primitives[3].primitive_type, "lane"); +} +} // namespace test_utils diff --git a/planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml b/planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml new file mode 100644 index 0000000000000..06ed15d20d3c9 --- /dev/null +++ b/planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml @@ -0,0 +1,41 @@ +start_pose: + position: + x: 1.0 + y: 2.0 + z: 3.0 + orientation: + x: 0.1 + y: 0.2 + z: 0.3 + w: 0.4 +goal_pose: + position: + x: 4.0 + y: 5.0 + z: 6.0 + orientation: + x: 0.5 + y: 0.6 + z: 0.7 + w: 0.8 +segments: + - preferred_primitive: + id: 11 + primitive_type: "" + primitives: + - id: 22 + primitive_type: lane + - id: 33 + primitive_type: lane + - preferred_primitive: + id: 44 + primitive_type: "" + primitives: + - id: 55 + primitive_type: lane + - id: 66 + primitive_type: lane + - id: 77 + primitive_type: lane + - id: 88 + primitive_type: lane diff --git a/planning/planning_test_utils/test_map/2km_test.osm b/planning/planning_test_utils/test_map/2km_test.osm new file mode 100644 index 0000000000000..43c22f8de6bb1 --- /dev/null +++ b/planning/planning_test_utils/test_map/2km_test.osm @@ -0,0 +1,33390 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 54b34f11f5363..414489e899fff 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/planning/route_handler/CMakeLists.txt @@ -8,4 +8,16 @@ ament_auto_add_library(route_handler SHARED src/route_handler.cpp ) -ament_auto_package() +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_route_handler + test/test_route_handler.cpp) + + target_link_libraries(test_route_handler + route_handler + ) + +endif() + +ament_auto_package(INSTALL_TO_SHARE + test_route +) diff --git a/planning/route_handler/README.md b/planning/route_handler/README.md index 4d369349bf364..e8df4fd087930 100644 --- a/planning/route_handler/README.md +++ b/planning/route_handler/README.md @@ -1,3 +1,18 @@ # route handler `route_handler` is a library for calculating driving route on the lanelet map. + +## Unit Testing + +The unit testing depends on `planning_test_utils` package. +`planning_test_utils` is a library that provides several common functions to simplify unit test creating. + +![route_handler_test](./images/route_handler_test.svg) + +By default, route file is necessary to create tests. The following illustrates the route that are used in the unit test + +### Lane change test route + +![lane_change_test_route](./images/lane_change_test_route.svg) + +- The route is based on map that can be obtained from `planning_test_utils\test_map` diff --git a/planning/route_handler/images/lane_change_test_route.svg b/planning/route_handler/images/lane_change_test_route.svg new file mode 100644 index 0000000000000..e71c8e13b1760 --- /dev/null +++ b/planning/route_handler/images/lane_change_test_route.svg @@ -0,0 +1,618 @@ + + + + + + + + + + + + + +
+
+
+ 4765 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ 4770 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4775 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4424 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4780 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5088 +
+
+
+ + + + + + + + + + + + +
+
+
+
goal pose
+ (70.0, -1.75) +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ 9590 +
+
+
+ + + + + + + + + + + + +
+
+
+ 9594 +
+
+
+ + + + + + + + + + + + +
+
+
+ 9598 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5072 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5084 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4765 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + +
+
+
+ preferred primitive +
+
+
+ + + + + + + + + +
+
+
+ primitive +
+
+
+ + + + + + + + + + + + +
+
+
+ pose1 +
+ (0.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ check point +
+ (25.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+ start pose +
+ (-50.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + diff --git a/planning/route_handler/images/route_handler_test.svg b/planning/route_handler/images/route_handler_test.svg new file mode 100644 index 0000000000000..38b2a97da0432 --- /dev/null +++ b/planning/route_handler/images/route_handler_test.svg @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + +
+
+
+ + route msg in yaml format +
+ + (to simulate /planning/mission_planner/route topic) +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ mock_data_parser +
+
+
+ + + + + + + + + + + + +
+
+
+ LaneletRoute +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ lanelet file +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ HADMapBin +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ test route handler +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ planning_test_utils +
+
+
+ + + + + + + diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 22db01d833023..097bc62f8bf39 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -18,6 +18,7 @@ ament_cmake_auto autoware_cmake + ament_cmake_ros ament_lint_auto autoware_lint_common @@ -26,10 +27,12 @@ autoware_planning_msgs geometry_msgs lanelet2_extension + planning_test_utils rclcpp rclcpp_components tf2_ros tier4_autoware_utils + yaml-cpp ament_cmake diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index e9fb3cc7d2fd7..4dd559b9bbc98 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -573,44 +573,26 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelet current_lanelet = lanelet; double length = 0; + lanelet::ConstLanelets previous_lanelets; while (rclcpp::ok() && length < min_length) { - lanelet::ConstLanelets candidate_lanelets; - if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - if (only_route_lanes) { - break; - } - const auto prev_lanes = getPreviousLanelets(current_lanelet); - if (prev_lanes.empty()) { - break; - } - candidate_lanelets = prev_lanes; + previous_lanelets.clear(); + if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) { + if (only_route_lanes) break; + const auto previous_lanelets = getPreviousLanelets(current_lanelet); + if (previous_lanelets.empty()) break; } // loop check - if (std::any_of( - candidate_lanelets.begin(), candidate_lanelets.end(), - [lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); })) { - break; - } - - // If lanelet_sequence_backward with input lanelet contains all candidate lanelets, - // break the loop. - if (std::all_of( - candidate_lanelets.begin(), candidate_lanelets.end(), - [lanelet_sequence_backward, lanelet](auto & prev_llt) { - return std::any_of( - lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), - [prev_llt, lanelet](auto & llt) { - return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); - }); - })) { + if (std::any_of(previous_lanelets.begin(), previous_lanelets.end(), [lanelet](auto & prev_llt) { + return lanelet.id() == prev_llt.id(); + })) { break; } - for (const auto & prev_lanelet : candidate_lanelets) { + for (const auto & prev_lanelet : previous_lanelets) { if (std::any_of( lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), - [prev_lanelet, lanelet](auto & llt) { - return (llt.id() == prev_lanelet.id() || lanelet.id() == prev_lanelet.id()); + [prev_lanelet, lanelet](auto & backward) { + return (backward.id() == prev_lanelet.id()); })) { continue; } @@ -680,14 +662,13 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance, const double forward_distance, const bool only_route_lanes) const { - lanelet::ConstLanelets lanelet_sequence; if (only_route_lanes && !exists(route_lanelets_, lanelet)) { - return lanelet_sequence; + return {}; } lanelet::ConstLanelets lanelet_sequence_forward = getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); - const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { + lanelet::ConstLanelets lanelet_sequence = std::invoke([&]() { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); @@ -696,17 +677,15 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( }); // loop check - if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) { - if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) { + if (!lanelet_sequence_forward.empty() && !lanelet_sequence.empty()) { + if (lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) { return lanelet_sequence_forward; } } - lanelet_sequence.insert( - lanelet_sequence.end(), lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); lanelet_sequence.push_back(lanelet); - lanelet_sequence.insert( - lanelet_sequence.end(), lanelet_sequence_forward.begin(), lanelet_sequence_forward.end()); - + std::move( + lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(), + std::back_inserter(lanelet_sequence)); return lanelet_sequence; } @@ -1681,8 +1660,9 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo( } lanelet::ConstLanelet current_lanelet = lanelet; + lanelet::ConstLanelets candidate_lanelets; while (rclcpp::ok()) { - lanelet::ConstLanelets candidate_lanelets; + candidate_lanelets.clear(); if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { break; } diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp new file mode 100644 index 0000000000000..212fc0ce84d89 --- /dev/null +++ b/planning/route_handler/test/test_route_handler.cpp @@ -0,0 +1,110 @@ +// Copyright 2024 TIER IV +// +// 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 "test_route_handler.hpp" + +#include + +#include + +namespace route_handler::test +{ +TEST_F(TestRouteHandler, isRouteHandlerReadyTest) +{ + ASSERT_TRUE(route_handler_->isHandlerReady()); +} + +TEST_F(TestRouteHandler, checkIfIDReturned) +{ + const auto lanelet1 = route_handler_->getLaneletsFromId(4785); + const auto is_lanelet1_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet1); + ASSERT_TRUE(is_lanelet1_in_goal_route_section); + + const auto lanelet2 = route_handler_->getLaneletsFromId(4780); + const auto is_lanelet2_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet2); + ASSERT_FALSE(is_lanelet2_in_goal_route_section); +} + +TEST_F(TestRouteHandler, getGoalLaneId) +{ + lanelet::ConstLanelet goal_lane; + + const auto goal_lane_obtained = route_handler_->getGoalLanelet(&goal_lane); + ASSERT_TRUE(goal_lane_obtained); + ASSERT_EQ(goal_lane.id(), 5088); +} + +// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) +// { +// lanelet::ConstLanelet closest_lane; + +// Pose search_pose; + +// search_pose.position = tier4_autoware_utils::createPoint(-1.0, 1.75, 0); +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// const auto closest_lane_obtained7 = +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained7); +// ASSERT_EQ(closest_lane.id(), 4775); + +// search_pose.position = tier4_autoware_utils::createPoint(-0.5, 1.75, 0); +// const auto closest_lane_obtained = +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained); +// ASSERT_EQ(closest_lane.id(), 4775); + +// search_pose.position = tier4_autoware_utils::createPoint(-.01, 1.75, 0); +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// const auto closest_lane_obtained3 = +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained3); +// ASSERT_EQ(closest_lane.id(), 4775); + +// search_pose.position = tier4_autoware_utils::createPoint(0.0, 1.75, 0); +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// const auto closest_lane_obtained1 = +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained1); +// ASSERT_EQ(closest_lane.id(), 4775); + +// search_pose.position = tier4_autoware_utils::createPoint(0.01, 1.75, 0); +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// const auto closest_lane_obtained2 = +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained2); +// ASSERT_EQ(closest_lane.id(), 4424); + +// search_pose.position = tier4_autoware_utils::createPoint(0.5, 1.75, 0); +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// const auto closest_lane_obtained4 = +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained4); +// ASSERT_EQ(closest_lane.id(), 4424); + +// search_pose.position = tier4_autoware_utils::createPoint(1.0, 1.75, 0); +// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); +// const auto closest_lane_obtained5 = +// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + +// ASSERT_TRUE(closest_lane_obtained5); +// ASSERT_EQ(closest_lane.id(), 4424); +// } +} // namespace route_handler::test diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp new file mode 100644 index 0000000000000..3c6893da3f9d1 --- /dev/null +++ b/planning/route_handler/test/test_route_handler.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV +// +// 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 TEST_ROUTE_HANDLER_HPP_ +#define TEST_ROUTE_HANDLER_HPP_ + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "gtest/gtest.h" +#include "planning_test_utils/mock_data_parser.hpp" +#include "planning_test_utils/planning_test_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +namespace route_handler::test +{ + +using autoware_auto_mapping_msgs::msg::HADMapBin; + +class TestRouteHandler : public ::testing::Test +{ +public: + TestRouteHandler() + { + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/2km_test.osm"; + constexpr double center_line_resolution = 5.0; + const auto map_bin_msg = test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); + route_handler_ = std::make_shared(map_bin_msg); + set_lane_change_test_route(); + } + + TestRouteHandler(const TestRouteHandler &) = delete; + TestRouteHandler(TestRouteHandler &&) = delete; + TestRouteHandler & operator=(const TestRouteHandler &) = delete; + TestRouteHandler & operator=(TestRouteHandler &&) = delete; + ~TestRouteHandler() override = default; + + void set_lane_change_test_route() + { + const auto route_handler_dir = ament_index_cpp::get_package_share_directory("route_handler"); + const auto rh_test_route = route_handler_dir + "/test_route/lane_change_test_route.yaml"; + route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route)); + } + + std::shared_ptr route_handler_; +}; +} // namespace route_handler::test + +#endif // TEST_ROUTE_HANDLER_HPP_ diff --git a/planning/route_handler/test_route/lane_change_test_route.yaml b/planning/route_handler/test_route/lane_change_test_route.yaml new file mode 100644 index 0000000000000..8b4c423c622a0 --- /dev/null +++ b/planning/route_handler/test_route/lane_change_test_route.yaml @@ -0,0 +1,93 @@ +header: + stamp: + sec: 1715936953 + nanosec: 67206425 + frame_id: map +start_pose: + position: + x: -50.0 + y: 1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +goal_pose: + position: + x: 70.0 + y: -1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +segments: + - preferred_primitive: + id: 4765 + primitive_type: "" + primitives: + - id: 4765 + primitive_type: lane + - id: 9590 + primitive_type: lane + - preferred_primitive: + id: 4770 + primitive_type: "" + primitives: + - id: 4770 + primitive_type: lane + - id: 5072 + primitive_type: lane + - preferred_primitive: + id: 4775 + primitive_type: "" + primitives: + - id: 4775 + primitive_type: lane + - id: 9594 + primitive_type: lane + - preferred_primitive: + id: 9598 + primitive_type: "" + primitives: + - id: 4424 + primitive_type: lane + - id: 9598 + primitive_type: lane + - preferred_primitive: + id: 5084 + primitive_type: "" + primitives: + - id: 4780 + primitive_type: lane + - id: 5084 + primitive_type: lane + - preferred_primitive: + id: 5088 + primitive_type: "" + primitives: + - id: 4785 + primitive_type: lane + - id: 5088 + primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 7180812c7500b..7ab796321e041 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -84,6 +84,7 @@ class RTCInterface rclcpp::Service::SharedPtr srv_auto_mode_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; Module module_; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 623f899f55c70..5e4b202071e49 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -81,7 +81,8 @@ Module getModuleType(const std::string & module_name) namespace rtc_interface { RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc) -: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, +: clock_{node->get_clock()}, + logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, is_auto_mode_enabled_{!enable_rtc}, is_locked_{false} { @@ -270,9 +271,7 @@ void RTCInterface::removeExpiredCooperateStatus() std::lock_guard lock(mutex_); const auto itr = std::remove_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), - [](const auto & status) { - return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0; - }); + [this](const auto & status) { return (clock_->now() - status.stamp).seconds() > 10.0; }); registered_status_.statuses.erase(itr, registered_status_.statuses.end()); } diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 61f65bc40f32d..e87023ef00149 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -10,6 +10,40 @@ A method of operating scan in chronological order and removing noise based on th ![ring_outlier_filter](./image/outlier_filter-ring.drawio.svg) +Another feature of this node is that it calculates visibility score based on outlier pointcloud and publish score as a topic. + +### visibility score calculation algorithm + +The pointcloud is divided into vertical bins (rings) and horizontal bins (azimuth divisions). +The algorithm starts by splitting the input point cloud into separate rings based on the ring value of each point. Then, for each ring, it iterates through the points and calculates the frequency of points within each horizontal bin. The frequency is determined by incrementing a counter for the corresponding bin based on the point's azimuth value. +The frequency values are stored in a frequency image matrix, where each cell represents a specific ring and azimuth bin. After calculating the frequency image, the algorithm applies a noise threshold to create a binary image. Points with frequency values above the noise threshold are considered valid, while points below the threshold are considered noise. +Finally, the algorithm calculates the visibility score by counting the number of non-zero pixels in the frequency image and dividing it by the total number of pixels (vertical bins multiplied by horizontal bins). + +```plantuml +@startuml +start + +:Convert input point cloud to PCL format; + +:Initialize vertical and horizontal bins; + +:Split point cloud into rings; + +while (For each ring) is (not empty) + :Calculate frequency of points in each azimuth bin; + :Update frequency image matrix; +endwhile + +:Apply noise threshold to create binary image; + +:Count non-zero pixels in frequency image; + +:Calculate visibility score as complement of filled pixel ratio; + +stop +@enduml +``` + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -22,14 +56,20 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------------- | ------- | ------------- | -------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud. Due to performance concerns, please set to false during experiments. | +| Name | Type | Default Value | Description | +| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | +| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | +| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | +| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | +| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | +| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | +| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index c871f1acbe5b9..55ba79aac4593 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -19,9 +19,17 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" +#include #include +#if __has_include() +#include +#else +#include +#endif + #include +#include #include #include @@ -42,6 +50,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info); + rclcpp::Publisher::SharedPtr visibility_pub_; + private: /** \brief publisher of excluded pointcloud for debug reason. **/ rclcpp::Publisher::SharedPtr outlier_pointcloud_publisher_; @@ -53,6 +63,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter size_t max_points_num_per_ring_; bool publish_outlier_pointcloud_; + // for visibility score + int noise_threshold_; + int vertical_bins_; + int horizontal_bins_; + + float min_azimuth_deg_; + float max_azimuth_deg_; + float max_distance_; + /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -77,6 +96,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter void setUpPointCloudFormat( const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, size_t num_fields); + float calculateVisibilityScore(const PointCloud2 & input); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 9a1eae786c379..47fa39e7fcda2 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -22,7 +22,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_geometry autoware_point_types cgal cv_bridge diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 762c62dc03b39..ada7d2616ed38 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,16 +14,16 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" -#include "autoware_auto_geometry/common_3d.hpp" +#include "autoware_point_types/types.hpp" #include -#include - #include #include namespace pointcloud_preprocessor { +using autoware_point_types::PointXYZIRADRT; + RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) : Filter("RingOutlierFilter", options) { @@ -35,6 +35,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1); + visibility_pub_ = create_publisher( + "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -50,6 +52,13 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("max_points_num_per_ring", 4000)); publish_outlier_pointcloud_ = static_cast(declare_parameter("publish_outlier_pointcloud", false)); + + min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 0.0)); + max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 360.0)); + max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); + vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); + horizontal_bins_ = static_cast(declare_parameter("horizontal_bins", 36)); + noise_threshold_ = static_cast(declare_parameter("noise_threshold", 2)); } using std::placeholders::_1; @@ -73,13 +82,7 @@ void RingOutlierFilterComponent::faster_filter( output.data.resize(output.point_step * input->width); size_t output_size = 0; - // Set up the noise points cloud, if noise points are to be published. - PointCloud2 outlier_points; - size_t outlier_points_size = 0; - if (publish_outlier_pointcloud_) { - outlier_points.point_step = sizeof(PointXYZI); - outlier_points.data.resize(outlier_points.point_step * input->width); - } + pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); const auto ring_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; @@ -89,6 +92,10 @@ void RingOutlierFilterComponent::faster_filter( input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; const auto intensity_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + const auto return_type_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; + const auto time_stamp_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -163,24 +170,32 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto outlier_ptr = - reinterpret_cast(&outlier_points.data[outlier_points_size]); + PointXYZIRADRT outlier_point; auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; - outlier_ptr->x = p[0]; - outlier_ptr->y = p[1]; - outlier_ptr->z = p[2]; + outlier_point.x = p[0]; + outlier_point.y = p[1]; + outlier_point.z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_point = *input_ptr; } - const float & intensity = *reinterpret_cast( - &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_ptr->intensity = intensity; - outlier_points_size += outlier_points.point_step; + outlier_point.intensity = *reinterpret_cast( + &input->data[indices[walk_first_idx] + intensity_offset]); + outlier_point.ring = *reinterpret_cast( + &input->data[indices[walk_first_idx] + ring_offset]); + outlier_point.azimuth = *reinterpret_cast( + &input->data[indices[walk_first_idx] + azimuth_offset]); + outlier_point.distance = *reinterpret_cast( + &input->data[indices[walk_first_idx] + distance_offset]); + outlier_point.return_type = *reinterpret_cast( + &input->data[indices[walk_first_idx] + return_type_offset]); + outlier_point.time_stamp = *reinterpret_cast( + &input->data[indices[walk_first_idx] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } @@ -213,21 +228,31 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + PointXYZIRADRT outlier_point; + + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; - outlier_ptr->x = p[0]; - outlier_ptr->y = p[1]; - outlier_ptr->z = p[2]; + outlier_point.x = p[0]; + outlier_point.y = p[1]; + outlier_point.z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_point = *input_ptr; } - const float & intensity = + outlier_point.intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_ptr->intensity = intensity; - outlier_points_size += outlier_points.point_step; + outlier_point.ring = + *reinterpret_cast(&input->data[indices[i] + ring_offset]); + outlier_point.azimuth = + *reinterpret_cast(&input->data[indices[i] + azimuth_offset]); + outlier_point.distance = + *reinterpret_cast(&input->data[indices[i] + distance_offset]); + outlier_point.return_type = + *reinterpret_cast(&input->data[indices[i] + return_type_offset]); + outlier_point.time_stamp = + *reinterpret_cast(&input->data[indices[i] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } } @@ -235,8 +260,15 @@ void RingOutlierFilterComponent::faster_filter( setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); if (publish_outlier_pointcloud_) { - setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4); - outlier_pointcloud_publisher_->publish(outlier_points); + PointCloud2 outlier; + pcl::toROSMsg(*outlier_pcl, outlier); + outlier.header = input->header; + outlier_pointcloud_publisher_->publish(outlier); + + tier4_debug_msgs::msg::Float32Stamped visibility_msg; + visibility_msg.data = calculateVisibilityScore(outlier); + visibility_msg.stamp = input->header.stamp; + visibility_pub_->publish(visibility_msg); } // add processing time for debug @@ -288,6 +320,24 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba RCLCPP_DEBUG( get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_); } + if (get_param(p, "vertical_bins", vertical_bins_)) { + RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_); + } + if (get_param(p, "horizontal_bins", horizontal_bins_)) { + RCLCPP_DEBUG(get_logger(), "Setting new horizontal_bins to: %d.", horizontal_bins_); + } + if (get_param(p, "noise_threshold", noise_threshold_)) { + RCLCPP_DEBUG(get_logger(), "Setting new noise_threshold to: %d.", noise_threshold_); + } + if (get_param(p, "max_azimuth_deg", max_azimuth_deg_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_deg to: %f.", max_azimuth_deg_); + } + if (get_param(p, "min_azimuth_deg", min_azimuth_deg_)) { + RCLCPP_DEBUG(get_logger(), "Setting new min_azimuth_deg to: %f.", min_azimuth_deg_); + } + if (get_param(p, "max_distance", max_distance_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_distance to: %f.", max_distance_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -319,6 +369,61 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); } +float RingOutlierFilterComponent::calculateVisibilityScore( + const sensor_msgs::msg::PointCloud2 & input) +{ + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::fromROSMsg(input, *input_cloud); + + const uint32_t vertical_bins = vertical_bins_; + const uint32_t horizontal_bins = horizontal_bins_; + const float max_azimuth = max_azimuth_deg_ * 100.0; + const float min_azimuth = min_azimuth_deg_ * 100.0; + + const uint32_t horizontal_resolution = + static_cast((max_azimuth - min_azimuth) / horizontal_bins); + + std::vector> ring_point_clouds(vertical_bins); + cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + + // Split points into rings + for (const auto & point : input_cloud->points) { + ring_point_clouds.at(point.ring).push_back(point); + } + + // Calculate frequency for each bin in each ring + for (const auto & ring_points : ring_point_clouds) { + if (ring_points.empty()) continue; + + const uint ring_id = ring_points.front().ring; + std::vector frequency_in_ring(horizontal_bins, 0); + + for (const auto & point : ring_points.points) { + if (point.azimuth < min_azimuth || point.azimuth >= max_azimuth) continue; + if (point.distance >= max_distance_) continue; + + const uint bin_index = + static_cast((point.azimuth - min_azimuth) / horizontal_resolution); + + frequency_in_ring[bin_index]++; + frequency_in_ring[bin_index] = + std::min(frequency_in_ring[bin_index], 255); // Ensure value is within uchar range + + frequency_image.at(ring_id, bin_index) = + static_cast(frequency_in_ring[bin_index]); + } + } + + cv::Mat binary_image; + cv::inRange(frequency_image, noise_threshold_, 255, binary_image); + + const int num_pixels = cv::countNonZero(frequency_image); + const float num_filled_pixels = + static_cast(num_pixels) / static_cast(vertical_bins * horizontal_bins); + + return 1.0f - num_filled_pixels; +} + } // namespace pointcloud_preprocessor #include diff --git a/tools/reaction_analyzer/CMakeLists.txt b/tools/reaction_analyzer/CMakeLists.txt new file mode 100644 index 0000000000000..d94a5bd7a794e --- /dev/null +++ b/tools/reaction_analyzer/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.14) +project(reaction_analyzer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(reaction_analyzer SHARED + include/reaction_analyzer_node.hpp + src/reaction_analyzer_node.cpp + include/subscriber.hpp + src/subscriber.cpp + include/topic_publisher.hpp + src/topic_publisher.cpp + include/utils.hpp + src/utils.cpp +) + +target_include_directories(reaction_analyzer + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +target_link_libraries(reaction_analyzer + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(reaction_analyzer + PLUGIN "reaction_analyzer::ReactionAnalyzerNode" + EXECUTABLE reaction_analyzer_exe +) + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md new file mode 100644 index 0000000000000..f5a22aaf20176 --- /dev/null +++ b/tools/reaction_analyzer/README.md @@ -0,0 +1,229 @@ +# Reaction Analyzer + +## Description + +The main purpose of the reaction analyzer package is to measure the reaction times of various nodes within a ROS-based +autonomous driving simulation environment by subscribing to pre-determined topics. This tool is particularly useful for +evaluating the performance of perception, planning, and control pipelines in response to dynamic changes in the +environment, such as sudden obstacles. To be able to measure both control outputs and perception outputs, it was +necessary to divide the node into two running_mode: `planning_control` and `perception_planning`. + +![ReactionAnalyzerDesign.png](media%2FReactionAnalyzerDesign.png) + +### Planning Control Mode + +In this mode, the reaction analyzer creates a dummy publisher for the PredictedObjects and PointCloud2 topics. In the +beginning of the test, it publishes the initial position of the ego vehicle and the goal position to set the test +environment. Then, it spawns a sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to +search reacted messages of the planning and control nodes in the pre-determined topics. When all the topics are reacted, +it calculates the reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes +with `spawn_cmd_time`, and it creates a csv file to store the results. + +### Perception Planning Mode + +In this mode, the reaction analyzer reads the rosbag files which are recorded from AWSIM, and it creates a topic +publisher for each topic inside the rosbag to replay the rosbag. It reads two rosbag files: `path_bag_without_object` +and `path_bag_with_object`. Firstly, it replays the `path_bag_without_object` to set the initial position of the ego +vehicle and the goal position. After `spawn_time_after_init` seconds , it replays the `path_bag_with_object` to spawn a +sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to search the reacted messages of +the perception and planning nodes in the pre-determined topics. When all the topics are reacted, it calculates the +reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes with `spawn_cmd_time`, and +it creates a csv file to store the results. + +#### Point Cloud Publisher Type + +To get better analyze for Perception & Sensing pipeline, the reaction analyzer can publish the point cloud messages in 3 +different ways: `async_header_sync_publish`, `sync_header_sync_publish` or `async_publish`. (`T` is the period of the +lidar's output) + +![PointcloudPublisherType.png](media%2FPointcloudPublisherType.png) + +- `async_header_sync_publish`: It publishes the point cloud messages synchronously with asynchronous header times. It + means that each of the lidar's output will be published at the same time, but the headers of the point cloud messages + includes different timestamps because of the phase difference. +- `sync_header_sync_publish`: It publishes the point cloud messages synchronously with synchronous header times. It + means that each of the lidar's output will be published at the same time, and the headers of the point cloud messages + includes the same timestamps. +- `async_publish`: It publishes the point cloud messages asynchronously. It means that each of the lidar's output will + be published at different times. + +## Usage + +The common parameters you need to define for both running modes are `output_file_path`, `test_iteration`, +and `reaction_chain` list. `output_file_path` is the output file path is the path where the results and statistics +will be stored. `test_iteration` defines how many tests will be performed. The `reaction_chain` list is the list of the +pre-defined topics you want to measure their reaction times. + +**IMPORTANT:** Ensure the `reaction_chain` list is correctly defined: + +- For `perception_planning` mode, **do not** define `Control` nodes. +- For `planning_control` mode, **do not** define `Perception` nodes. + +### Prepared Test Environment + +- Download the demonstration test map from the + link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After + downloading, + extract the zip file and use its path as `[MAP_PATH]` in the following commands. + +#### Planning Control Mode + +- You need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters, + you can start to test with the following command: + +```bash +ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=planning_control vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit map_path:=[MAP_PATH] +``` + +After the command, the `simple_planning_simulator` and the `reaction_analyzer` will be launched. It will automatically +start to test. After the test is completed, the results will be stored in the `output_file_path` you defined. + +#### Perception Planning Mode + +- Download the rosbag files from the Google Drive + link [here](https://drive.google.com/file/d/1-Qcv7gYfR-usKOjUH8I997w8I4NMhXlX/view?usp=sharing). +- Extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object` + and `path_bag_with_object`. +- You can start to test with the following command: + +```bash +ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=perception_planning vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH] +``` + +After the command, the `e2e_simulator` and the `reaction_analyzer` will be launched. It will automatically start +to test. After the test is completed, the results will be stored in the `output_file_path` you defined. + +#### Prepared Test Environment + +**Scene without object:** +![sc1-awsim.png](media%2Fsc1-awsim.png) +![sc1-rviz.png](media%2Fsc1-rviz.png) + +**Scene object:** +![sc2-awsim.png](media%2Fsc2-awsim.png) +![sc2-rviz.png](media%2Fsc2-rviz.png) + +### Custom Test Environment + +**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the +parameters. +The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher` ( +for `perception_planning` mode) parameters.** + +- To set `initialization_pose`, `entity_params`, `goal_pose`: +- Run the AWSIM environment. Tutorial for AWSIM can be found + [here](https://autowarefoundation.github.io/AWSIM/main/GettingStarted/QuickStartDemo/). +- Run the e2e_simulator with the following command: + +```bash +ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH] +``` + +- After EGO is initialized, you can move the ego vehicle to the desired position by using the `SetGoal` button in the + RViz. +- After the EGO stopped in desired position, please localize the dummy obstacle by using the traffic controller. You can + control the traffic by pressing `ESC` button. + +**After localize EGO and dummy vehicle, we should write the positions of these entities in the map frame +in `reaction_analyzer.param.yaml`. To achieve this:** + +- Get initialization pose from `/awsim/ground_truth/vehicle/pose` topic. +- Get entity params from `/perception/object_recognition/objects` topic. +- Get goal pose from `/planning/mission_planning/goal` topic. + +**PS: `initialization_pose` is only valid for `planning_control` mode.** + +- After the parameters were noted, we should record the rosbags for the test. To record the rosbags, you can use the + following command: + +```bash +ros2 bag record --all +``` + +- You should record two rosbags: one without the object and one with the object. You can use the traffic controller to + spawn the object in front of the EGO vehicle or remove it. + +**NOTE: You should record the rosbags in the same environment with the same position of the EGO vehicle. You don't need +to run Autoware while recording.** + +- After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the + paths of the recorded rosbags. + +## Results + +The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each +pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, `Pipeline Latency`, +and `Total Latency` +for each of the nodes. + +- `Node Latency`: The time difference between previous and current node's reaction timestamps. If it is the first node + in the pipeline, it is same as `Pipeline Latency`. +- `Pipeline Latency`: The time difference between published time of the message and pipeline header time. +- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent + timestamp. + +## Parameters + +| Name | Type | Description | +| ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- | +| `timer_period` | double | [s] Period for the main processing timer. | +| `test_iteration` | int | Number of iterations for the test. | +| `output_file_path` | string | Directory path where test results and statistics will be stored. | +| `spawn_time_after_init` | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode. | +| `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. | +| `poses.initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. | +| `poses.entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. | +| `poses.goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. | +| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. | +| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. | +| `topic_publisher.spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. | +| `topic_publisher.dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. | +| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. | +| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. | +| `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object` | bool | Default false. Publish only the point cloud messages with the object. | +| `reaction_params.first_brake_params.debug_control_commands` | bool | Debug publish flag. | +| `reaction_params.first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. | +| `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. | +| `reaction_params.first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. | +| `reaction_params.search_zero_vel_params.max_looking_distance` | double | [m] Maximum looking distance for zero velocity on trajectory | +| `reaction_params.search_entity_params.search_radius` | double | [m] Searching radius for spawned entity. Distance between ego pose and entity pose. | +| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. | + +## Limitations + +- Reaction analyzer has some limitation like `PublisherMessageType`, `SubscriberMessageType` and `ReactionType`. It is + currently supporting following list: + +- **Publisher Message Types:** + + - `sensor_msgs/msg/PointCloud2` + - `sensor_msgs/msg/CameraInfo` + - `sensor_msgs/msg/Image` + - `geometry_msgs/msg/PoseWithCovarianceStamped` + - `sensor_msgs/msg/Imu` + - `autoware_auto_vehicle_msgs/msg/ControlModeReport` + - `autoware_auto_vehicle_msgs/msg/GearReport` + - `autoware_auto_vehicle_msgs/msg/HazardLightsReport` + - `autoware_auto_vehicle_msgs/msg/SteeringReport` + - `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport` + - `autoware_auto_vehicle_msgs/msg/VelocityReport` + +- **Subscriber Message Types:** + + - `sensor_msgs/msg/PointCloud2` + - `autoware_auto_perception_msgs/msg/DetectedObjects` + - `autoware_auto_perception_msgs/msg/TrackedObjects` + - `autoware_auto_msgs/msg/PredictedObject` + - `autoware_auto_planning_msgs/msg/Trajectory` + - `autoware_auto_control_msgs/msg/AckermannControlCommand` + +- **Reaction Types:** + - `FIRST_BRAKE` + - `SEARCH_ZERO_VEL` + - `SEARCH_ENTITY` + +## Future improvements + +- The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`, + `SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of + the message types. diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp new file mode 100644 index 0000000000000..6dc3dd896dc92 --- /dev/null +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -0,0 +1,158 @@ +// 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 REACTION_ANALYZER_NODE_HPP_ +#define REACTION_ANALYZER_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer +{ +using autoware_adapi_v1_msgs::msg::LocalizationInitializationState; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_adapi_v1_msgs::msg::RouteState; +using autoware_adapi_v1_msgs::srv::ChangeOperationMode; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_internal_msgs::msg::PublishedTime; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +struct NodeParams +{ + std::string running_mode; + double timer_period; + std::string output_file_path; + size_t test_iteration; + double spawn_time_after_init; + double spawn_distance_threshold; + PoseParams initial_pose; + PoseParams goal_pose; + EntityParams entity_params; +}; + +class ReactionAnalyzerNode : public rclcpp::Node +{ +public: + explicit ReactionAnalyzerNode(rclcpp::NodeOptions node_options); + + Odometry::ConstSharedPtr odometry_ptr_; + +private: + std::mutex mutex_; + RunningMode node_running_mode_; + + // Parameters + NodeParams node_params_; + + // Initialization Variables + geometry_msgs::msg::Pose entity_pose_; + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_; + geometry_msgs::msg::PoseStamped goal_pose_; + + // Subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_route_state_; + rclcpp::Subscription::SharedPtr sub_localization_init_state_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Subscription::SharedPtr + sub_ground_truth_pose_; // Only for perception_planning mode + + // Publishers + rclcpp::Publisher::SharedPtr pub_initial_pose_; + rclcpp::Publisher::SharedPtr pub_goal_pose_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // Variables + std::vector pipeline_map_vector_; + std::optional last_test_environment_init_request_time_; + std::optional test_environment_init_time_; + std::optional spawn_cmd_time_; + std::atomic spawn_object_cmd_{false}; + std::atomic ego_initialized_{false}; + bool is_initialization_requested{false}; + bool is_route_set_{false}; + size_t test_iteration_count_{0}; + visualization_msgs::msg::Marker entity_debug_marker_; + + // Functions + void init_analyzer_variables(); + void init_test_env( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const RouteState::ConstSharedPtr & route_state_ptr, + const OperationModeState::ConstSharedPtr & operation_mode_ptr, + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr); + bool init_ego( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time); + bool set_route( + const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time); + bool check_ego_init_correctly( + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr); + void call_operation_mode_service_without_response(); + void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose); + void calculate_results( + const std::map & message_buffers, + const rclcpp::Time & spawn_cmd_time); + void on_timer(); + void reset(); + + // Callbacks + void on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr); + void on_localization_initialization_state( + LocalizationInitializationState::ConstSharedPtr msg_ptr); + void on_route_state(RouteState::ConstSharedPtr msg_ptr); + void on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr); + void on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Client + rclcpp::Client::SharedPtr client_change_to_autonomous_; + + // Pointers + std::unique_ptr topic_publisher_ptr_; + std::unique_ptr subscriber_ptr_; + LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_; + RouteState::ConstSharedPtr current_route_state_ptr_; + OperationModeState::ConstSharedPtr operation_mode_ptr_; + PoseStamped::ConstSharedPtr ground_truth_pose_ptr_; +}; +} // namespace reaction_analyzer + +#endif // REACTION_ANALYZER_NODE_HPP_ diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp new file mode 100644 index 0000000000000..d14d41da545f7 --- /dev/null +++ b/tools/reaction_analyzer/include/subscriber.hpp @@ -0,0 +1,191 @@ +// 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 SUBSCRIBER_HPP_ +#define SUBSCRIBER_HPP_ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer::subscriber +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_internal_msgs::msg::PublishedTime; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +// Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime +using MessageBuffer = std::optional; +// We need to store the past AckermannControlCommands to analyze the first brake +using ControlCommandBuffer = std::pair, MessageBuffer>; +// Variant to store different types of buffers +using MessageBufferVariant = std::variant; + +template +struct SubscriberVariables +{ + using ExactTimePolicy = message_filters::sync_policies::ExactTime; + + std::unique_ptr> sub1_; + std::unique_ptr> sub2_; + std::unique_ptr> synchronizer_; + // tmp: only for the messages who don't have header e.g. AckermannControlCommand + std::unique_ptr> cache_; +}; + +// Variant to create subscribers for different message types +using SubscriberVariablesVariant = std::variant< + SubscriberVariables, SubscriberVariables, + SubscriberVariables, SubscriberVariables, + SubscriberVariables, SubscriberVariables>; + +// The configuration of the topic to be subscribed which are defined in reaction_chain +struct TopicConfig +{ + std::string node_name; + std::string topic_address; + std::string time_debug_topic_address; + SubscriberMessageType message_type; +}; + +// Place for the reaction functions' parameter configuration +struct FirstBrakeParams +{ + bool debug_control_commands; + double control_cmd_buffer_time_interval; + size_t min_number_descending_order_control_cmd; + double min_jerk_for_brake_cmd; +}; + +struct SearchZeroVelParams +{ + double max_looking_distance; +}; + +struct SearchEntityParams +{ + double search_radius_offset; +}; + +// Place for the store the reaction parameter configuration (currently only for first brake) +struct ReactionParams +{ + FirstBrakeParams first_brake_params; + SearchZeroVelParams search_zero_vel_params; + SearchEntityParams search_entity_params; +}; + +using ChainModules = std::vector; + +class SubscriberBase +{ +public: + explicit SubscriberBase( + rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic & spawn_object_cmd, + const EntityParams & entity_params); + + std::optional> get_message_buffers_map(); + void reset(); + +private: + std::mutex mutex_; + + // Init + rclcpp::Node * node_; + Odometry::ConstSharedPtr odometry_; + std::atomic & spawn_object_cmd_; + EntityParams entity_params_; + + // Variables to be initialized in constructor + ChainModules chain_modules_{}; + ReactionParams reaction_params_{}; + geometry_msgs::msg::Pose entity_pose_{}; + double entity_search_radius_{0.0}; + + // Variants + std::map subscriber_variables_map_; + std::map message_buffers_; + + // tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Functions + void init_reaction_chains_and_params(); + bool init_subscribers(); + std::optional get_subscriber_variable( + const TopicConfig & topic_config); + std::optional find_first_brake_idx( + const std::vector & cmd_array); + void set_control_command_to_buffer( + std::vector & buffer, const AckermannControlCommand & cmd) const; + + // Callbacks for modules are subscribed + void on_control_command( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr); + void on_trajectory(const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr); + void on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_pointcloud(const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr); + void on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr); + void on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr); + void on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr); + void on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); +}; + +} // namespace reaction_analyzer::subscriber + +#endif // SUBSCRIBER_HPP_ diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp new file mode 100644 index 0000000000000..c6d3a90650884 --- /dev/null +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -0,0 +1,249 @@ +// 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 TOPIC_PUBLISHER_HPP_ +#define TOPIC_PUBLISHER_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 + +namespace reaction_analyzer::topic_publisher +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +struct TopicPublisherParams +{ + double dummy_perception_publisher_period; // Only for planning_control mode + double spawned_pointcloud_sampling_distance; + std::string path_bag_without_object; // Path to the bag file without object + std::string path_bag_with_object; // Path to the bag file with object + std::string pointcloud_publisher_type; // Type of the pointcloud publisher + double pointcloud_publisher_period; // Period of the pointcloud publisher + bool publish_only_pointcloud_with_object; // Publish only pointcloud with object for only + // perception pipeline debug purpose make it true. +}; + +enum class PointcloudPublisherType { + ASYNC_PUBLISHER = 0, // Asynchronous publisher + SYNC_HEADER_SYNC_PUBLISHER = 1, // Synchronous publisher with header synchronization + ASYNC_HEADER_SYNC_PUBLISHER = 2, // Asynchronous publisher with header synchronization +}; + +/** + * @brief Message type template struct for the variables of the Publisher. + */ +template +struct PublisherVariables +{ + std::chrono::milliseconds period_ms{0}; + typename MessageType::SharedPtr empty_area_message; + typename MessageType::SharedPtr object_spawned_message; + typename rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; +}; + +/** + * @brief Struct for accessing the variables of the Publisher. + */ +struct PublisherVarAccessor +{ + // Template struct to check if a type has a header member. + template > + struct HasHeader : std::false_type + { + }; + + template + struct HasHeader> : std::true_type + { + }; + + // Template struct to check if a type has a stamp member. + template > + struct HasStamp : std::false_type + { + }; + + template + struct HasStamp> : std::true_type + { + }; + + template + void publish_with_current_time( + const PublisherVariables & publisher_var, const rclcpp::Time & current_time, + const bool is_object_spawned) const + { + std::unique_ptr msg_to_be_published = std::make_unique(); + + if (is_object_spawned) { + *msg_to_be_published = *publisher_var.object_spawned_message; + } else { + *msg_to_be_published = *publisher_var.empty_area_message; + } + if constexpr (HasHeader::value) { + msg_to_be_published->header.stamp = current_time; + } else if constexpr (HasStamp::value) { + msg_to_be_published->stamp = current_time; + } + publisher_var.publisher->publish(std::move(msg_to_be_published)); + } + + template + void set_period(T & publisher_var, std::chrono::milliseconds new_period) + { + publisher_var.period_ms = new_period; + } + + template + std::chrono::milliseconds get_period(const T & publisher_var) const + { + return publisher_var.period_ms; + } + + template + std::shared_ptr get_empty_area_message(const T & publisher_var) const + { + return std::static_pointer_cast(publisher_var.empty_area_message); + } + + template + std::shared_ptr get_object_spawned_message(const T & publisher_var) const + { + return std::static_pointer_cast(publisher_var.object_spawned_message); + } +}; + +using PublisherVariablesVariant = std::variant< + PublisherVariables, PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables>; + +using LidarOutputPair = std::pair< + std::shared_ptr>, + std::shared_ptr>>; + +class TopicPublisher +{ +public: + explicit TopicPublisher( + rclcpp::Node * node, std::atomic & spawn_object_cmd, std::atomic & ego_initialized, + std::optional & spawn_cmd_time, const RunningMode & node_running_mode, + const EntityParams & entity_params); + + void reset(); + +private: + std::mutex mutex_; + + // Initialized variables + rclcpp::Node * node_; + RunningMode node_running_mode_; + std::atomic & spawn_object_cmd_; + std::atomic & ego_initialized_; // used for planning_control mode because if pointcloud is + // published before ego is initialized, it causes crash + EntityParams entity_params_; + std::optional & spawn_cmd_time_; // Set by a publisher function when the + // spawn_object_cmd_ is true + + // tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Parameters + TopicPublisherParams topic_publisher_params_; + + // Variables planning_control mode + rclcpp::Publisher::SharedPtr pub_pointcloud_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + PointCloud2::SharedPtr entity_pointcloud_ptr_; + PredictedObjects::SharedPtr predicted_objects_ptr_; + + // Variables perception_planning mode + PointcloudPublisherType pointcloud_publisher_type_; + std::map topic_publisher_map_; + std::map + lidar_pub_variable_pair_map_; // used to publish pointcloud_raw and pointcloud_raw_ex + bool is_object_spawned_message_published_{false}; + std::shared_ptr one_shot_timer_shared_ptr_; + + // Functions + template + void set_message( + const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message, + const bool is_empty_area_message); + void set_period(const std::map> & time_map); + void set_publishers_and_timers_to_variable(); + void set_timers_for_pointcloud_msgs( + const std::map> & pointcloud_variables_map); + bool check_publishers_initialized_correctly(); + void init_rosbag_publishers(); + void init_rosbag_publisher_buffer(const std::string & bag_path, const bool is_empty_area_message); + void pointcloud_messages_sync_publisher(const PointcloudPublisherType type); + void pointcloud_messages_async_publisher( + const std::pair< + std::shared_ptr>, + std::shared_ptr>> & lidar_output_pair_); + void generic_message_publisher(const std::string & topic_name); + void dummy_perception_publisher(); // Only for planning_control mode + + // Timers + std::map pointcloud_publish_timers_map_; + rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_; + rclcpp::TimerBase::SharedPtr dummy_perception_timer_; +}; +} // namespace reaction_analyzer::topic_publisher + +#endif // TOPIC_PUBLISHER_HPP_ diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp new file mode 100644 index 0000000000000..da1defc03f34c --- /dev/null +++ b/tools/reaction_analyzer/include/utils.hpp @@ -0,0 +1,363 @@ +// 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 UTILS_HPP_ +#define UTILS_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 + +// The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool. +namespace reaction_analyzer +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_internal_msgs::msg::PublishedTime; +using sensor_msgs::msg::PointCloud2; + +/** + * @brief A pair containing the ReactionPair. + * The first element is name of the node that published the PublishedTime msg. + * The second element is the PublishedTime msg itself + */ +using ReactionPair = std::pair; + +/** + * @brief A map containing the pipeline and the reaction pairs. + * The key is the time at which the pipeline was executed. + * The value is a vector of ReactionPair. + */ +using PipelineMap = std::map>; + +/** + * @brief A vector containing the pipeline and the reaction pairs. + * The first element is the time at which the pipeline was executed. + * The second element is a vector of ReactionPair. + */ +using TimestampedReactionPairsVector = + std::vector>>; + +/** + * @brief Enum for the different types of messages that can be published. + */ +enum class PublisherMessageType { + UNKNOWN = 0, + CAMERA_INFO = 1, + IMAGE = 2, + POINTCLOUD2 = 3, + POSE_WITH_COVARIANCE_STAMPED = 4, + POSE_STAMPED = 5, + ODOMETRY = 6, + IMU = 7, + CONTROL_MODE_REPORT = 8, + GEAR_REPORT = 9, + HAZARD_LIGHTS_REPORT = 10, + STEERING_REPORT = 11, + TURN_INDICATORS_REPORT = 12, + VELOCITY_REPORT = 13, +}; + +/** + * @brief Enum for the different types of messages that can be subscribed to. + */ +enum class SubscriberMessageType { + UNKNOWN = 0, + ACKERMANN_CONTROL_COMMAND = 1, + TRAJECTORY = 2, + POINTCLOUD2 = 3, + DETECTED_OBJECTS = 4, + PREDICTED_OBJECTS = 5, + TRACKED_OBJECTS = 6, +}; + +/** + * @brief Enum for the different types of reactions that can be analyzed. + */ +enum class ReactionType { + UNKNOWN = 0, + FIRST_BRAKE = 1, + SEARCH_ZERO_VEL = 2, + SEARCH_ENTITY = 3, +}; + +/** + * @brief Enum for the different running modes of the Reaction Analyzer. + */ +enum class RunningMode { + PerceptionPlanning = 0, + PlanningControl = 1, +}; + +/** + * @brief Structs containing the parameters of a pose. + */ +struct PoseParams +{ + double x; + double y; + double z; + double roll; + double pitch; + double yaw; +}; + +struct EntityParams +{ + double x; + double y; + double z; + double roll; + double pitch; + double yaw; + double x_l; + double y_l; + double z_l; +}; + +/** + * @brief Struct containing statistics of the latencies. + */ +struct LatencyStats +{ + double min; + double max; + double mean; + double median; + double std_dev; +}; + +/** + * @brief Convert string to SubscriberMessageType. + */ +SubscriberMessageType get_subscriber_message_type(const std::string & message_type); + +/** + * @brief Convert string to PublisherMessageType. + */ +PublisherMessageType get_publisher_message_type(const std::string & message_type); + +/** + * @brief Get the PublisherMessageType for a given topic. + */ +PublisherMessageType get_publisher_message_type_for_topic( + const std::vector & topics, const std::string & topic_name); + +/** + * @brief Convert string to ReactionType. + */ +ReactionType get_reaction_type(const std::string & reaction_type); + +/** + * @brief Calculates the statistics of a vector of latencies. + * @param latency_vec The vector of latencies. + * @return The statistics of the latencies. + */ +LatencyStats calculate_statistics(const std::vector & latency_vec); + +/** + * @brief Generates a UUID message from a string. + * @param input The string to generate the UUID from. + * @return The generated UUID message. + */ +unique_identifier_msgs::msg::UUID generate_uuid_msg(const std::string & input); + +/** + * @brief Generate pose from PoseParams. + * @param pose_params + * @return Pose + */ +geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params); + +/** + * @brief Generate entity pose from entity params. + * @param entity_params + * @return Pose + */ +geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params); + +/** + * @brief Generate entity pose from entity params. + * @param entity_params + * @return Pose + */ +double calculate_entity_search_radius(const EntityParams & entity_params); + +/** + * @brief Create a pointcloud from entity params. + * @param entity_params + * @param pointcloud_sampling_distance + * @return PointCloud2::SharedPtr + */ +PointCloud2::SharedPtr create_entity_pointcloud_ptr( + const EntityParams & entity_params, const double pointcloud_sampling_distance); + +/** + * @brief Create a predicted objects message from entity params. + * @param entity_params + * @return PredictedObjects::SharedPtr + */ +PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityParams & entity_params); + +/** + * @brief Creates a subscription option with a new callback group. + * + * @param node A pointer to the node for which the subscription options are being created. + * @return The created SubscriptionOptions. + */ +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node); + +/** + * @brief Creates a visualization marker for a polyhedron based on the provided entity parameters. + * + * @param params The parameters of the entity for which the marker is to be created. It includes the + * position, orientation, and dimensions of the entity. + * @return The created visualization marker for the polyhedron. + */ +visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params); + +/** + * @brief Splits a string by a given delimiter. + * + * @param str The string to be split. + * @param delimiter The delimiter to split the string by. + * @return A vector of strings, each of which is a segment of the original string split by the + * delimiter. + */ +std::vector split(const std::string & str, char delimiter); + +/** + * @brief Checks if a folder exists. + * + * @param path The path to the folder. + * @return True if the folder exists, false otherwise. + */ +bool does_folder_exist(const std::string & path); + +/** + * @brief Get the index of the trajectory point that is a certain distance away from the current + * point. + * + * @param traj The trajectory to search. + * @param curr_id The index of the current point in the trajectory. + * @param distance The distance to search for a point. + * @return The index of the point that is at least the specified distance away from the current + * point. + */ +size_t get_index_after_distance( + const Trajectory & traj, const size_t curr_id, const double distance); + +/** + * @brief Search for a pointcloud near the pose. + * @param pcl_pointcloud, pose, search_radius + * @return bool + */ +bool search_pointcloud_near_pose( + const pcl::PointCloud & pcl_pointcloud, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * + * @brief Search for a predicted object near the pose. + * @param predicted_objects, pose, search_radius + * @return bool + */ +bool search_predicted_objects_near_pose( + const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * @brief Search for a detected object near the pose. + * @param detected_objects, pose, search_radius + * @return bool + */ +bool search_detected_objects_near_pose( + const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * @brief Search for a tracked object near the pose. + * @param tracked_objects, pose, search_radius + * @return bool + */ +bool search_tracked_objects_near_pose( + const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); + +/** + * Calculates the time difference in milliseconds between two rclcpp::Time instances. + * + * @param start The start time. + * @param end The end time. + * @return The time difference in milliseconds as a double. + */ +double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & end); + +/** + * @brief Converts a PipelineMap to a PipelineMapVector. + * + * @param pipelineMap The PipelineMap to be converted. + * @return The PipelineMapVector that is equivalent to the PipelineMap. + */ +TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector( + const PipelineMap & pipelineMap); + +/** + * @brief Writes the results to a file. + * + * @param node A pointer to the node for which the results are being written. + * @param output_file_path The path to the file where the results will be written. + * @param node_running_mode The running mode of the node. + * @param pipeline_map_vector The vector of PipelineMap containing the results to be written. + */ +void write_results( + rclcpp::Node * node, const std::string & output_file_path, const RunningMode & node_running_mode, + const std::vector & pipeline_map_vector); +} // namespace reaction_analyzer + +#endif // UTILS_HPP_ diff --git a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml new file mode 100644 index 0000000000000..b0b1d4fb9bf2e --- /dev/null +++ b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/reaction_analyzer/media/PointcloudPublisherType.png b/tools/reaction_analyzer/media/PointcloudPublisherType.png new file mode 100644 index 0000000000000..597b73318bcac Binary files /dev/null and b/tools/reaction_analyzer/media/PointcloudPublisherType.png differ diff --git a/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png b/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png new file mode 100644 index 0000000000000..cb0f2dd8577fc Binary files /dev/null and b/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png differ diff --git a/tools/reaction_analyzer/media/sc1-awsim.png b/tools/reaction_analyzer/media/sc1-awsim.png new file mode 100644 index 0000000000000..6bdc104a93b87 Binary files /dev/null and b/tools/reaction_analyzer/media/sc1-awsim.png differ diff --git a/tools/reaction_analyzer/media/sc1-rviz.png b/tools/reaction_analyzer/media/sc1-rviz.png new file mode 100644 index 0000000000000..ae7c6e882bf98 Binary files /dev/null and b/tools/reaction_analyzer/media/sc1-rviz.png differ diff --git a/tools/reaction_analyzer/media/sc2-awsim.png b/tools/reaction_analyzer/media/sc2-awsim.png new file mode 100644 index 0000000000000..8224b516897be Binary files /dev/null and b/tools/reaction_analyzer/media/sc2-awsim.png differ diff --git a/tools/reaction_analyzer/media/sc2-rviz.png b/tools/reaction_analyzer/media/sc2-rviz.png new file mode 100644 index 0000000000000..8f4f5feae8ba3 Binary files /dev/null and b/tools/reaction_analyzer/media/sc2-rviz.png differ diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml new file mode 100644 index 0000000000000..05081eac01751 --- /dev/null +++ b/tools/reaction_analyzer/package.xml @@ -0,0 +1,49 @@ + + + + reaction_analyzer + 1.0.0 + Analyzer that measures reaction times of the nodes + + Berkay Karaman + + Apache License 2.0 + + Berkay Karaman + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_internal_msgs + eigen + libpcl-all-dev + message_filters + motion_utils + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + rosbag2_cpp + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + + + ament_cmake + + diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml new file mode 100644 index 0000000000000..62c1191bd345a --- /dev/null +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -0,0 +1,117 @@ +/**: + ros__parameters: + timer_period: 0.033 # s + test_iteration: 10 + output_file_path: + spawn_time_after_init: 10.0 # s for perception_planning mode + spawn_distance_threshold: 15.0 # m # for planning_control mode + poses: + initialization_pose: + x: 81546.984375 + y: 50011.96875 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 11.1130405 + goal_pose: + x: 81643.0703125 + y: 50029.8828125 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 13.12 + entity_params: + x: 81633.86068376624 + y: 50028.383586673124 + z: 42.44818343779461 + roll: 0.0 + pitch: 0.0 + yaw: 11.8235848 + x_dimension: 3.95 + y_dimension: 1.77 + z_dimension: 1.43 + topic_publisher: + path_bag_without_object: /rosbag2_awsim_labs/rosbag2_awsim_labs.db3 + path_bag_with_object: /rosbag2_awsim_labs_obstacle/rosbag2_awsim_labs_obstacle.db3 + pointcloud_publisher: + pointcloud_publisher_type: "sync_header_sync_publish" # "async_header_sync_publish", "sync_header_sync_publish" or "async_publish" + pointcloud_publisher_period: 0.1 # s + publish_only_pointcloud_with_object: false # use it only for debug purposes. If true, only pointclouds with object will be published + spawned_pointcloud_sampling_distance: 0.1 # m for planning_control mode + dummy_perception_publisher_period: 0.1 # s for planning_control mode + subscriber: + reaction_params: + first_brake_params: + debug_control_commands: false + control_cmd_buffer_time_interval: 1.0 # s + min_number_descending_order_control_cmd: 3 + min_jerk_for_brake_cmd: 0.3 # m/s^3 + search_zero_vel_params: + max_looking_distance: 15.0 # m + search_entity_params: + search_radius_offset: 0.0 # m + reaction_chain: + obstacle_cruise_planner: + topic_name: /planning/scenario_planning/lane_driving/trajectory + time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + scenario_selector: + topic_name: /planning/scenario_planning/scenario_selector/trajectory + time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + motion_velocity_smoother: + topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory + time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + planning_validator: + topic_name: /planning/scenario_planning/trajectory + time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + trajectory_follower: + topic_name: /control/trajectory_follower/control_cmd + time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time + message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + vehicle_cmd_gate: + topic_name: /control/command/control_cmd + time_debug_topic_name: /control/command/control_cmd/debug/published_time + message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + common_ground_filter: + topic_name: /perception/obstacle_segmentation/single_frame/pointcloud + time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud/debug/published_time + message_type: sensor_msgs/msg/PointCloud2 + occupancy_grid_map_outlier: + topic_name: /perception/obstacle_segmentation/pointcloud + time_debug_topic_name: /perception/obstacle_segmentation/pointcloud/debug/published_time + message_type: sensor_msgs/msg/PointCloud2 + multi_object_tracker: + topic_name: /perception/object_recognition/tracking/near_objects + time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/TrackedObjects + lidar_centerpoint: + topic_name: /perception/object_recognition/detection/centerpoint/objects + time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + obstacle_pointcloud_based_validator: + topic_name: /perception/object_recognition/detection/centerpoint/validation/objects + time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + decorative_tracker_merger: + topic_name: /perception/object_recognition/tracking/objects + time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/TrackedObjects + detected_object_feature_remover: + topic_name: /perception/object_recognition/detection/clustering/objects + time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + detection_by_tracker: + topic_name: /perception/object_recognition/detection/detection_by_tracker/objects + time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + object_lanelet_filter: + topic_name: /perception/object_recognition/detection/objects + time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + map_based_prediction: + topic_name: /perception/object_recognition/objects + time_debug_topic_name: /perception/object_recognition/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/PredictedObjects diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp new file mode 100644 index 0000000000000..53313f28eb4d9 --- /dev/null +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -0,0 +1,442 @@ +// 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 "reaction_analyzer_node.hpp" + +#include +#include + +namespace reaction_analyzer +{ + +void ReactionAnalyzerNode::on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + operation_mode_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_route_state(RouteState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + current_route_state_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + odometry_ptr_ = msg_ptr; +} + +void ReactionAnalyzerNode::on_localization_initialization_state( + LocalizationInitializationState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + initialization_state_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + ground_truth_pose_ptr_ = std::move(msg_ptr); +} + +ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options) +: Node("reaction_analyzer_node", node_options.automatically_declare_parameters_from_overrides(true)) +{ + using std::placeholders::_1; + + node_params_.running_mode = get_parameter("running_mode").as_string(); + + // set running mode + if (node_params_.running_mode == "planning_control") { + node_running_mode_ = RunningMode::PlanningControl; + } else if (node_params_.running_mode == "perception_planning") { + node_running_mode_ = RunningMode::PerceptionPlanning; + } else { + RCLCPP_ERROR(get_logger(), "Invalid running mode. Node couldn't be initialized. Failed."); + return; + } + + node_params_.output_file_path = get_parameter("output_file_path").as_string(); + // Check if the output file path is valid + if (!does_folder_exist(node_params_.output_file_path)) { + RCLCPP_ERROR(get_logger(), "Output file path is not valid. Node couldn't be initialized."); + return; + } + + node_params_.timer_period = get_parameter("timer_period").as_double(); + node_params_.test_iteration = get_parameter("test_iteration").as_int(); + node_params_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double(); + node_params_.spawn_distance_threshold = get_parameter("spawn_distance_threshold").as_double(); + + // Position parameters + node_params_.initial_pose.x = get_parameter("poses.initialization_pose.x").as_double(); + node_params_.initial_pose.y = get_parameter("poses.initialization_pose.y").as_double(); + node_params_.initial_pose.z = get_parameter("poses.initialization_pose.z").as_double(); + node_params_.initial_pose.roll = get_parameter("poses.initialization_pose.roll").as_double(); + node_params_.initial_pose.pitch = get_parameter("poses.initialization_pose.pitch").as_double(); + node_params_.initial_pose.yaw = get_parameter("poses.initialization_pose.yaw").as_double(); + + node_params_.goal_pose.x = get_parameter("poses.goal_pose.x").as_double(); + node_params_.goal_pose.y = get_parameter("poses.goal_pose.y").as_double(); + node_params_.goal_pose.z = get_parameter("poses.goal_pose.z").as_double(); + node_params_.goal_pose.roll = get_parameter("poses.goal_pose.roll").as_double(); + node_params_.goal_pose.pitch = get_parameter("poses.goal_pose.pitch").as_double(); + node_params_.goal_pose.yaw = get_parameter("poses.goal_pose.yaw").as_double(); + + node_params_.entity_params.x = get_parameter("poses.entity_params.x").as_double(); + node_params_.entity_params.y = get_parameter("poses.entity_params.y").as_double(); + node_params_.entity_params.z = get_parameter("poses.entity_params.z").as_double(); + node_params_.entity_params.roll = get_parameter("poses.entity_params.roll").as_double(); + node_params_.entity_params.pitch = get_parameter("poses.entity_params.pitch").as_double(); + node_params_.entity_params.yaw = get_parameter("poses.entity_params.yaw").as_double(); + node_params_.entity_params.x_l = get_parameter("poses.entity_params.x_dimension").as_double(); + node_params_.entity_params.y_l = get_parameter("poses.entity_params.y_dimension").as_double(); + node_params_.entity_params.z_l = get_parameter("poses.entity_params.z_dimension").as_double(); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, std::bind(&ReactionAnalyzerNode::on_vehicle_pose, this, _1), + create_subscription_options(this)); + sub_localization_init_state_ = create_subscription( + "input/localization_initialization_state", rclcpp::QoS(1).transient_local(), + std::bind(&ReactionAnalyzerNode::on_localization_initialization_state, this, _1), + create_subscription_options(this)); + sub_route_state_ = create_subscription( + "input/routing_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::on_route_state, this, _1), create_subscription_options(this)); + sub_operation_mode_ = create_subscription( + "input/operation_mode_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::on_operation_mode_state, this, _1), + create_subscription_options(this)); + + pub_goal_pose_ = create_publisher("output/goal", rclcpp::QoS(1)); + pub_marker_ = create_publisher("~/debug", 10); + + init_analyzer_variables(); + + if (node_running_mode_ == RunningMode::PlanningControl) { + pub_initial_pose_ = create_publisher( + "output/initialpose", rclcpp::QoS(1)); + + client_change_to_autonomous_ = + create_client("service/change_to_autonomous"); + + } else if (node_running_mode_ == RunningMode::PerceptionPlanning) { + sub_ground_truth_pose_ = create_subscription( + "input/ground_truth_pose", rclcpp::QoS{1}, + std::bind(&ReactionAnalyzerNode::on_ground_truth_pose, this, _1), + create_subscription_options(this)); + } + + topic_publisher_ptr_ = std::make_unique( + this, spawn_object_cmd_, ego_initialized_, spawn_cmd_time_, node_running_mode_, + node_params_.entity_params); + + // initialize the odometry before init the subscriber + odometry_ptr_ = std::make_shared(); + // Load the subscriber to listen the topics for reactions + subscriber_ptr_ = std::make_unique( + this, odometry_ptr_, spawn_object_cmd_, node_params_.entity_params); + + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(node_params_.timer_period)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ReactionAnalyzerNode::on_timer, this)); +} + +void ReactionAnalyzerNode::on_timer() +{ + mutex_.lock(); + const auto current_odometry_ptr = odometry_ptr_; + const auto initialization_state_ptr = initialization_state_ptr_; + const auto route_state_ptr = current_route_state_ptr_; + const auto operation_mode_ptr = operation_mode_ptr_; + const auto ground_truth_pose_ptr = ground_truth_pose_ptr_; + const auto spawn_cmd_time = spawn_cmd_time_; + mutex_.unlock(); + + // Init the test environment + if (!test_environment_init_time_) { + init_test_env( + initialization_state_ptr, route_state_ptr, operation_mode_ptr, ground_truth_pose_ptr, + current_odometry_ptr); + return; + } + + pub_marker_->publish(entity_debug_marker_); + + // Spawn the obstacle if the conditions are met + spawn_obstacle(current_odometry_ptr->pose.pose.position); + + // If the spawn_cmd_time is not set by pointcloud publishers, don't do anything + if (!spawn_cmd_time) return; + + // Get the reacted messages, if all modules reacted + const auto message_buffers = subscriber_ptr_->get_message_buffers_map(); + + if (message_buffers) { + // if reacted, calculate the results + calculate_results(message_buffers.value(), spawn_cmd_time.value()); + // reset the variables if the iteration is not finished, otherwise write the results + reset(); + } +} + +void ReactionAnalyzerNode::spawn_obstacle(const geometry_msgs::msg::Point & ego_pose) +{ + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + rclcpp::Duration time_diff = this->now() - test_environment_init_time_.value(); + if (time_diff > rclcpp::Duration::from_seconds(node_params_.spawn_time_after_init)) { + if (!spawn_object_cmd_) { + spawn_object_cmd_ = true; + RCLCPP_INFO(this->get_logger(), "Spawn command is sent."); + } + } + } else { + if ( + tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) < + node_params_.spawn_distance_threshold) { + if (!spawn_object_cmd_) { + spawn_object_cmd_ = true; + RCLCPP_INFO(this->get_logger(), "Spawn command is sent."); + } + } + } +} + +void ReactionAnalyzerNode::calculate_results( + const std::map & message_buffers, + const rclcpp::Time & spawn_cmd_time) +{ + // Map the reaction times w.r.t its header time to categorize it + PipelineMap pipeline_map; + + { + // set the spawn_cmd_time as the first reaction pair + ReactionPair reaction_pair; + reaction_pair.first = "spawn_cmd_time"; + reaction_pair.second.header.stamp = spawn_cmd_time; + reaction_pair.second.published_stamp = spawn_cmd_time; + pipeline_map[reaction_pair.second.header.stamp].emplace_back(reaction_pair); + } + + for (const auto & [key, variant] : message_buffers) { + ReactionPair reaction_pair; + if (auto * control_message = std::get_if(&variant)) { + if (control_message->second) { + reaction_pair.first = key; + reaction_pair.second = control_message->second.value(); + } + } else if (auto * general_message = std::get_if(&variant)) { + if (general_message->has_value()) { + reaction_pair.first = key; + reaction_pair.second = general_message->value(); + } + } + pipeline_map[reaction_pair.second.header.stamp].emplace_back(reaction_pair); + } + pipeline_map_vector_.emplace_back(pipeline_map); + test_iteration_count_++; +} + +void ReactionAnalyzerNode::init_analyzer_variables() +{ + entity_pose_ = create_entity_pose(node_params_.entity_params); + entity_debug_marker_ = create_polyhedron_marker(node_params_.entity_params); + goal_pose_.pose = pose_params_to_pose(node_params_.goal_pose); + + if (node_running_mode_ == RunningMode::PlanningControl) { + init_pose_.pose.pose = pose_params_to_pose(node_params_.initial_pose); + } +} + +void ReactionAnalyzerNode::init_test_env( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const RouteState::ConstSharedPtr & route_state_ptr, + const OperationModeState::ConstSharedPtr & operation_mode_ptr, + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr) +{ + const auto current_time = this->now(); + + // Initialize the test environment + constexpr double initialize_call_period = 1.0; // sec + if ( + !last_test_environment_init_request_time_ || + (current_time - last_test_environment_init_request_time_.value()).seconds() >= + initialize_call_period) { + last_test_environment_init_request_time_ = current_time; + + // Pose initialization of the ego + is_initialization_requested = !is_initialization_requested + ? init_ego(initialization_state_ptr, odometry_ptr, current_time) + : is_initialization_requested; + + if ( + is_initialization_requested && + initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { + is_initialization_requested = false; + return; + } + + // Check is position initialized accurately, if node is running in perception_planning mode + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + if (!check_ego_init_correctly(ground_truth_pose_ptr, odometry_ptr)) return; + } + + // Set route + is_route_set_ = !is_route_set_ ? set_route(route_state_ptr, current_time) : is_route_set_; + + if (!is_route_set_) { + return; + } + + // if node is running PlanningControl mode, change ego to Autonomous mode. + if (node_running_mode_ == RunningMode::PlanningControl) { + // change to autonomous + if (operation_mode_ptr && operation_mode_ptr->mode != OperationModeState::AUTONOMOUS) { + call_operation_mode_service_without_response(); + } + } + ego_initialized_ = true; + + const bool is_ready = + (is_initialization_requested && is_route_set_ && + (operation_mode_ptr->mode == OperationModeState::AUTONOMOUS || + node_running_mode_ == RunningMode::PerceptionPlanning)); + if (is_ready) { + test_environment_init_time_ = this->now(); + } + } +} + +void ReactionAnalyzerNode::call_operation_mode_service_without_response() +{ + auto req = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), "client request"); + + if (!client_change_to_autonomous_->service_is_ready()) { + RCLCPP_INFO(this->get_logger(), "client is unavailable"); + return; + } + + client_change_to_autonomous_->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + this->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); +} + +bool ReactionAnalyzerNode::init_ego( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time) +{ + // Pose initialization of the ego + if (initialization_state_ptr) { + if (node_running_mode_ == RunningMode::PlanningControl) { + // publish initial pose + init_pose_.header.stamp = current_time; + init_pose_.header.frame_id = "map"; + pub_initial_pose_->publish(init_pose_); + RCLCPP_WARN_ONCE(get_logger(), "Initialization position is published. Waiting for init..."); + } + // Wait until odometry_ptr is initialized + if (!odometry_ptr) { + RCLCPP_WARN_ONCE(get_logger(), "Odometry is not received. Waiting for odometry..."); + return false; + } + return true; + } + return false; +} + +bool ReactionAnalyzerNode::set_route( + const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time) +{ + if (route_state_ptr) { + if (route_state_ptr->state != RouteState::SET) { + // publish goal pose + goal_pose_.header.stamp = current_time; + goal_pose_.header.frame_id = "map"; + pub_goal_pose_->publish(goal_pose_); + return false; + } + return true; + } + return false; +} + +bool ReactionAnalyzerNode::check_ego_init_correctly( + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr) +{ + if (!ground_truth_pose_ptr) { + RCLCPP_WARN( + get_logger(), "Ground truth pose is not received. Waiting for Ground truth pose..."); + return false; + } + if (!odometry_ptr) { + RCLCPP_WARN(get_logger(), "Odometry is not received. Waiting for odometry..."); + return false; + } + + constexpr double deviation_threshold = 0.1; + const auto deviation = + tier4_autoware_utils::calcPoseDeviation(ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); + const bool is_position_initialized_correctly = deviation.longitudinal < deviation_threshold && + deviation.lateral < deviation_threshold && + deviation.yaw < deviation_threshold; + if (!is_position_initialized_correctly) { + RCLCPP_ERROR( + get_logger(), + "Deviation between ground truth position and ego position is high. Node is shutting " + "down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f", + deviation.longitudinal, deviation.lateral, deviation.yaw); + rclcpp::shutdown(); + } + return true; +} + +void ReactionAnalyzerNode::reset() +{ + if (test_iteration_count_ >= node_params_.test_iteration) { + write_results(this, node_params_.output_file_path, node_running_mode_, pipeline_map_vector_); + RCLCPP_INFO(get_logger(), "%zu Tests are finished. Node shutting down.", test_iteration_count_); + rclcpp::shutdown(); + return; + } + // reset the variables + is_initialization_requested = false; + is_route_set_ = false; + test_environment_init_time_ = std::nullopt; + last_test_environment_init_request_time_ = std::nullopt; + spawn_object_cmd_ = false; + if (topic_publisher_ptr_) { + topic_publisher_ptr_->reset(); + } + std::lock_guard lock(mutex_); + spawn_cmd_time_ = std::nullopt; + subscriber_ptr_->reset(); + ego_initialized_ = false; + RCLCPP_INFO(this->get_logger(), "Test - %zu is done, resetting..", test_iteration_count_); +} +} // namespace reaction_analyzer + +#include + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(reaction_analyzer::ReactionAnalyzerNode) diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp new file mode 100644 index 0000000000000..8c735c42b9cd5 --- /dev/null +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -0,0 +1,996 @@ +// 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 "subscriber.hpp" + +#include +#include +#include + +namespace reaction_analyzer::subscriber +{ + +SubscriberBase::SubscriberBase( + rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic & spawn_object_cmd, + const EntityParams & entity_params) +: node_(node), + odometry_(odometry), + spawn_object_cmd_(spawn_object_cmd), + entity_params_(entity_params) +{ + // init tf + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // init reaction parameters and chain configuration + init_reaction_chains_and_params(); + init_subscribers(); +} + +void SubscriberBase::init_reaction_chains_and_params() +{ + // Init Chains: get the topic addresses and message types of the modules in chain + { + const auto param_key = std::string("subscriber.reaction_chain"); + const auto module_names = node_->list_parameters({param_key}, 3).prefixes; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + TopicConfig tmp; + tmp.node_name = splitted_name.back(); + tmp.topic_address = node_->get_parameter(module_name + ".topic_name").as_string(); + tmp.time_debug_topic_address = + node_->get_parameter_or(module_name + ".time_debug_topic_name", std::string("")); + tmp.message_type = get_subscriber_message_type( + node_->get_parameter(module_name + ".message_type").as_string()); + if (tmp.message_type != SubscriberMessageType::UNKNOWN) { + chain_modules_.emplace_back(tmp); + } else { + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for module name: %s, skipping..", + tmp.node_name.c_str()); + } + } + } + + // Init Params: get the parameters for the reaction functions + { + const auto param_key = std::string("subscriber.reaction_params"); + const auto module_names = node_->list_parameters({param_key}, 3).prefixes; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + const auto type = get_reaction_type(splitted_name.back()); + switch (type) { + case ReactionType::FIRST_BRAKE: { + reaction_params_.first_brake_params.debug_control_commands = + node_->get_parameter(module_name + ".debug_control_commands").as_bool(); + reaction_params_.first_brake_params.control_cmd_buffer_time_interval = + node_->get_parameter(module_name + ".control_cmd_buffer_time_interval").as_double(); + reaction_params_.first_brake_params.min_number_descending_order_control_cmd = + node_->get_parameter(module_name + ".min_number_descending_order_control_cmd").as_int(); + reaction_params_.first_brake_params.min_jerk_for_brake_cmd = + node_->get_parameter(module_name + ".min_jerk_for_brake_cmd").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), + "First brake parameters are set: debug_control_commands %s, " + "control_cmd_buffer_time_interval %f, min_number_descending_order_control_cmd %zu, " + "min_jerk_for_brake_cmd %f", + reaction_params_.first_brake_params.debug_control_commands ? "true" : "false", + reaction_params_.first_brake_params.control_cmd_buffer_time_interval, + reaction_params_.first_brake_params.min_number_descending_order_control_cmd, + reaction_params_.first_brake_params.min_jerk_for_brake_cmd); + break; + } + case ReactionType::SEARCH_ZERO_VEL: { + reaction_params_.search_zero_vel_params.max_looking_distance = + node_->get_parameter(module_name + ".max_looking_distance").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), "Search Zero Vel parameters are set: max_looking_distance %f", + reaction_params_.search_zero_vel_params.max_looking_distance); + break; + } + case ReactionType::SEARCH_ENTITY: { + reaction_params_.search_entity_params.search_radius_offset = + node_->get_parameter(module_name + ".search_radius_offset").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), "Search Entity parameters are set: search_radius_offset %f", + reaction_params_.search_entity_params.search_radius_offset); + break; + } + default: + RCLCPP_WARN( + node_->get_logger(), "Unknown reaction type for module name: %s, skipping..", + splitted_name.back().c_str()); + break; + } + } + } + + // Init variables + { + entity_pose_ = create_entity_pose(entity_params_); + entity_search_radius_ = calculate_entity_search_radius(entity_params_) + + reaction_params_.search_entity_params.search_radius_offset; + } +} + +bool SubscriberBase::init_subscribers() +{ + if (chain_modules_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No module to initialize subscribers, failed."); + return false; + } + + for (const auto & module : chain_modules_) { + if (module.message_type == SubscriberMessageType::UNKNOWN) { + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for topic_config name: %s, skipping..", + module.node_name.c_str()); + } + auto subscriber_var = get_subscriber_variable(module); + if (!subscriber_var) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to initialize subscriber for module name: %s", + module.node_name.c_str()); + return false; + } + subscriber_variables_map_[module.node_name] = std::move(subscriber_var.value()); + } + return true; +} + +std::optional> SubscriberBase::get_message_buffers_map() +{ + std::lock_guard lock(mutex_); + if (message_buffers_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No message buffers are initialized."); + return std::nullopt; + } + + // Check all reacted or not + bool all_reacted = true; + for (const auto & [key, variant] : message_buffers_) { + if (auto * control_message = std::get_if(&variant)) { + if (!control_message->second) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Waiting for %s to react", key.c_str()); + all_reacted = false; + } + } else if (auto * general_message = std::get_if(&variant)) { + if (!general_message->has_value()) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Waiting for %s to react", key.c_str()); + all_reacted = false; + } + } + } + if (!all_reacted) { + return std::nullopt; + } + return message_buffers_; +} + +void SubscriberBase::reset() +{ + std::lock_guard lock(mutex_); + message_buffers_.clear(); +} + +// Callbacks + +void SubscriberBase::on_control_command( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr) +{ + std::lock_guard lock(mutex_); + auto & variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); + variant = buffer; + } + auto & cmd_buffer = std::get(variant); + if (cmd_buffer.second) { + // reacted + return; + } + set_control_command_to_buffer(cmd_buffer.first, *msg_ptr); + if (!spawn_object_cmd_) return; + + const auto brake_idx = find_first_brake_idx(cmd_buffer.first); + if (brake_idx) { + const auto brake_cmd = cmd_buffer.first.at(brake_idx.value()); + + // TODO(brkay54): update here if message_filters package add support for the messages which + // does not have header + const auto & subscriber_variant = + std::get>(subscriber_variables_map_[node_name]); + + // check if the cache was initialized or not, if there is, use it to set the published time + if (subscriber_variant.cache_) { + // use cache to get the published time + const auto published_time_vec = + subscriber_variant.cache_->getSurroundingInterval(brake_cmd.stamp, node_->now()); + if (!published_time_vec.empty()) { + for (const auto & published_time : published_time_vec) { + if (published_time->header.stamp == brake_cmd.stamp) { + cmd_buffer.second = *published_time; + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + return; + } + } + RCLCPP_ERROR( + node_->get_logger(), "Published time couldn't found for the node: %s", node_name.c_str()); + + } else { + RCLCPP_ERROR( + node_->get_logger(), "Published time vector is empty for the node: %s", + node_name.c_str()); + } + } else { + cmd_buffer.second->header.stamp = brake_cmd.stamp; + cmd_buffer.second->published_stamp = brake_cmd.stamp; + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + } + } +} + +void SubscriberBase::on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!current_odometry_ptr || !spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + msg_ptr->points, current_odometry_ptr->pose.pose.position); + + const auto nearest_objects_seg_idx = + motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); + + const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( + msg_ptr->points, nearest_seg_idx, nearest_objects_seg_idx); + + if (zero_vel_idx) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!current_odometry_ptr || !spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + msg_ptr->points, current_odometry_ptr->pose.pose.position); + + // find the target index which we will search for zero velocity + auto tmp_target_idx = get_index_after_distance( + *msg_ptr, nearest_seg_idx, reaction_params_.search_zero_vel_params.max_looking_distance); + if (tmp_target_idx == msg_ptr->points.size() - 1) { + tmp_target_idx = msg_ptr->points.size() - 2; // Last trajectory points might be zero velocity + } + const auto target_idx = tmp_target_idx; + const auto zero_vel_idx = + motion_utils::searchZeroVelocityIndex(msg_ptr->points, nearest_seg_idx, target_idx); + + if (zero_vel_idx) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points); + + pcl::PointCloud pcl_pointcloud; + pcl::fromROSMsg(transformed_points, pcl_pointcloud); + + if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} +void SubscriberBase::on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points); + + pcl::PointCloud pcl_pointcloud; + pcl::fromROSMsg(transformed_points, pcl_pointcloud); + + if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} +void SubscriberBase::on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + // transform objects + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + // transform objects + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_tracked_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_tracked_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +std::optional SubscriberBase::get_subscriber_variable( + const TopicConfig & topic_config) +{ + switch (topic_config.message_type) { + case SubscriberMessageType::ACKERMANN_CONTROL_COMMAND: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + // If not empty, user should define a time debug topic + // NOTE: Because message_filters package does not support the messages without headers, we + // can not use the synchronizer. After we reacted, we are going to use the cache + // of the both PublishedTime and AckermannControlCommand subscribers to find the messages + // which have same header time. + + std::function callback = + [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + this->on_control_command(topic_config.node_name, ptr); + }; + subscriber_variable.sub1_ = + std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + constexpr int cache_size = 5; + subscriber_variable.cache_ = std::make_unique>( + *subscriber_variable.sub2_, cache_size); + + } else { + std::function callback = + [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + this->on_control_command(topic_config.node_name, ptr); + }; + + subscriber_variable.sub1_ = + std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::TRAJECTORY: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const Trajectory::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_trajectory(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const Trajectory::ConstSharedPtr & msg) { + this->on_trajectory(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::POINTCLOUD2: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const PointCloud2::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_pointcloud(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::SensorDataQoS().get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const PointCloud2::ConstSharedPtr & msg) { + this->on_pointcloud(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::PREDICTED_OBJECTS: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const PredictedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_predicted_objects(topic_config.node_name, ptr, published_time_ptr); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const PredictedObjects::ConstSharedPtr & msg) { + this->on_predicted_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::DETECTED_OBJECTS: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const DetectedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_detected_objects(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const DetectedObjects::ConstSharedPtr & msg) { + this->on_detected_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::TRACKED_OBJECTS: { + SubscriberVariables subscriber_variable; + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const TrackedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_tracked_objects(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const TrackedObjects::ConstSharedPtr & msg) { + this->on_tracked_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + default: + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for topic_config name: %s, skipping..", + topic_config.node_name.c_str()); + return std::nullopt; + } +} + +std::optional SubscriberBase::find_first_brake_idx( + const std::vector & cmd_array) +{ + if ( + cmd_array.size() < + reaction_params_.first_brake_params.min_number_descending_order_control_cmd) { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Control command buffer size is less than the minimum required size for first brake check"); + return {}; + } + + for (size_t i = 0; + i < cmd_array.size() - + reaction_params_.first_brake_params.min_number_descending_order_control_cmd + 1; + ++i) { + size_t decreased_cmd_counter = 1; // because # of the decreased cmd = iteration + 1 + for (size_t j = i; j < cmd_array.size() - 1; ++j) { + const auto & cmd_first = cmd_array.at(j).longitudinal; + const auto & cmd_second = cmd_array.at(j + 1).longitudinal; + constexpr double jerk_time_epsilon = 0.001; + const auto jerk = + abs(cmd_second.acceleration - cmd_first.acceleration) / + std::max( + (rclcpp::Time(cmd_second.stamp) - rclcpp::Time(cmd_first.stamp)).seconds(), + jerk_time_epsilon); + + if ( + (cmd_second.acceleration < cmd_first.acceleration) && + (jerk > reaction_params_.first_brake_params.min_jerk_for_brake_cmd)) { + decreased_cmd_counter++; + } else { + break; + } + } + if ( + decreased_cmd_counter < + static_cast( + reaction_params_.first_brake_params.min_number_descending_order_control_cmd)) + continue; + if (reaction_params_.first_brake_params.debug_control_commands) { + std::stringstream ss; + + // debug print to show the first brake command in the all control commands + for (size_t k = 0; k < cmd_array.size(); ++k) { + if (k == i + 1) { + ss << "First Brake(" << cmd_array.at(k).longitudinal.acceleration << ") "; + } else { + ss << cmd_array.at(k).longitudinal.acceleration << " "; + } + } + + RCLCPP_INFO(node_->get_logger(), "%s", ss.str().c_str()); + } + return i + 1; + } + return {}; +} + +void SubscriberBase::set_control_command_to_buffer( + std::vector & buffer, const AckermannControlCommand & cmd) const +{ + const auto last_cmd_time = cmd.stamp; + if (!buffer.empty()) { + for (auto itr = buffer.begin(); itr != buffer.end();) { + const auto expired = (rclcpp::Time(last_cmd_time) - rclcpp::Time(itr->stamp)).seconds() > + reaction_params_.first_brake_params.control_cmd_buffer_time_interval; + + if (expired) { + itr = buffer.erase(itr); + continue; + } + + itr++; + } + } + buffer.emplace_back(cmd); +} +} // namespace reaction_analyzer::subscriber diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp new file mode 100644 index 0000000000000..8a66bf9e33911 --- /dev/null +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -0,0 +1,536 @@ +// 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 "topic_publisher.hpp" + +#include +#include + +namespace reaction_analyzer::topic_publisher +{ + +TopicPublisher::TopicPublisher( + rclcpp::Node * node, std::atomic & spawn_object_cmd, std::atomic & ego_initialized, + std::optional & spawn_cmd_time, const RunningMode & node_running_mode, + const EntityParams & entity_params) +: node_(node), + node_running_mode_(node_running_mode), + spawn_object_cmd_(spawn_object_cmd), + ego_initialized_(ego_initialized), + entity_params_(entity_params), + spawn_cmd_time_(spawn_cmd_time) +{ + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + // get perception_planning mode parameters + topic_publisher_params_.path_bag_with_object = + node_->get_parameter("topic_publisher.path_bag_with_object").as_string(); + topic_publisher_params_.path_bag_without_object = + node_->get_parameter("topic_publisher.path_bag_without_object").as_string(); + topic_publisher_params_.pointcloud_publisher_type = + node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_type") + .as_string(); + topic_publisher_params_.pointcloud_publisher_period = + node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_period") + .as_double(); + topic_publisher_params_.publish_only_pointcloud_with_object = + node_ + ->get_parameter("topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object") + .as_bool(); + + // set pointcloud publisher type + if (topic_publisher_params_.pointcloud_publisher_type == "sync_header_sync_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER; + } else if (topic_publisher_params_.pointcloud_publisher_type == "async_header_sync_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER; + } else if (topic_publisher_params_.pointcloud_publisher_type == "async_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_PUBLISHER; + } else { + RCLCPP_ERROR(node_->get_logger(), "Invalid pointcloud_publisher_type"); + rclcpp::shutdown(); + return; + } + + // Init the publishers which will read the messages from the rosbag + init_rosbag_publishers(); + } else if (node_running_mode_ == RunningMode::PlanningControl) { + // init tf + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // get parameters + topic_publisher_params_.dummy_perception_publisher_period = + node_->get_parameter("topic_publisher.dummy_perception_publisher_period").as_double(); + topic_publisher_params_.spawned_pointcloud_sampling_distance = + node_->get_parameter("topic_publisher.spawned_pointcloud_sampling_distance").as_double(); + + // init the messages that will be published to spawn the object + entity_pointcloud_ptr_ = create_entity_pointcloud_ptr( + entity_params_, topic_publisher_params_.spawned_pointcloud_sampling_distance); + predicted_objects_ptr_ = create_entity_predicted_objects_ptr(entity_params_); + + // init the publishers + pub_pointcloud_ = + node_->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); + pub_predicted_objects_ = + node_->create_publisher("output/objects", rclcpp::QoS(1)); + + // init dummy perception publisher + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.dummy_perception_publisher_period)); + + dummy_perception_timer_ = rclcpp::create_timer( + node_, node_->get_clock(), period_ns, + std::bind(&TopicPublisher::dummy_perception_publisher, this)); + } else { + RCLCPP_ERROR(node_->get_logger(), "Invalid running mode"); + rclcpp::shutdown(); + return; + } +} + +void TopicPublisher::pointcloud_messages_sync_publisher(const PointcloudPublisherType type) +{ + const auto current_time = node_->now(); + const bool is_object_spawned = spawn_object_cmd_; + + switch (type) { + case PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER: { + PublisherVarAccessor accessor; + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + accessor.publish_with_current_time( + *publisher_var_pair.second.first, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *publisher_var_pair.second.second, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + } + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } + break; + } + case PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER: { + PublisherVarAccessor accessor; + const auto period_pointcloud_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); + const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size(); + + size_t counter = 0; + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + const auto header_time = + current_time - std::chrono::nanoseconds(counter * phase_dif.count()); + accessor.publish_with_current_time( + *publisher_var_pair.second.first, header_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *publisher_var_pair.second.second, header_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + counter++; + } + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } + break; + } + default: + break; + } +} + +void TopicPublisher::pointcloud_messages_async_publisher( + const std::pair< + std::shared_ptr>, + std::shared_ptr>> & lidar_output_pair_) +{ + PublisherVarAccessor accessor; + const auto current_time = node_->now(); + const bool is_object_spawned = spawn_object_cmd_; + accessor.publish_with_current_time( + *lidar_output_pair_.first, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *lidar_output_pair_.second, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } +} + +void TopicPublisher::generic_message_publisher(const std::string & topic_name) +{ + PublisherVarAccessor accessor; + const bool is_object_spawned = spawn_object_cmd_; + const auto current_time = node_->now(); + const auto & publisher_variant = topic_publisher_map_[topic_name]; + + std::visit( + [&](const auto & var) { + accessor.publish_with_current_time(var, current_time, is_object_spawned); + }, + publisher_variant); +} + +void TopicPublisher::dummy_perception_publisher() +{ + if (!ego_initialized_) { + return; // do not publish anything if ego is not initialized + } + if (!spawn_object_cmd_) { + // do not spawn it, send empty pointcloud + pcl::PointCloud pcl_empty; + PointCloud2 empty_pointcloud; + PredictedObjects empty_predicted_objects; + pcl::toROSMsg(pcl_empty, empty_pointcloud); + + const auto current_time = node_->now(); + empty_pointcloud.header.frame_id = "base_link"; + empty_pointcloud.header.stamp = current_time; + + empty_predicted_objects.header.frame_id = "map"; + empty_predicted_objects.header.stamp = current_time; + + pub_pointcloud_->publish(empty_pointcloud); + pub_predicted_objects_->publish(empty_predicted_objects); + } else { + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "base_link", "map", node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to look up transform from map to base_link"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *entity_pointcloud_ptr_, transformed_points); + const auto current_time = node_->now(); + + transformed_points.header.frame_id = "base_link"; + transformed_points.header.stamp = current_time; + + predicted_objects_ptr_->header.frame_id = "map"; + predicted_objects_ptr_->header.stamp = current_time; + + pub_pointcloud_->publish(transformed_points); + pub_predicted_objects_->publish(*predicted_objects_ptr_); + if (!is_object_spawned_message_published_) { + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + is_object_spawned_message_published_ = true; + } + } +} + +void TopicPublisher::reset() +{ + is_object_spawned_message_published_ = false; +} + +void TopicPublisher::init_rosbag_publishers() +{ + // read messages without object + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_without_object, true); + + // read messages with object + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_with_object, false); + + // before create publishers and timers, check all the messages are correctly initialized with + // their conjugate messages. + if (check_publishers_initialized_correctly()) { + RCLCPP_INFO(node_->get_logger(), "Messages are initialized correctly"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Messages are not initialized correctly"); + rclcpp::shutdown(); + } + set_publishers_and_timers_to_variable(); +} + +template +void TopicPublisher::set_message( + const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message, + const bool is_empty_area_message) +{ + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message.serialized_data); + + // Deserialize the message + auto deserialized_message = std::make_shared(); + serialization.deserialize_message(&extracted_serialized_msg, &*deserialized_message); + auto & publisher_variant = topic_publisher_map_[topic_name]; + + if (!std::holds_alternative>(publisher_variant)) { + publisher_variant = PublisherVariables{}; + } + + auto & publisher_variable = std::get>(publisher_variant); + + if (is_empty_area_message) { + if (!publisher_variable.empty_area_message) { + publisher_variable.empty_area_message = deserialized_message; + } + } else { + if (!publisher_variable.object_spawned_message) { + publisher_variable.object_spawned_message = deserialized_message; + } + } +} + +void TopicPublisher::set_period(const std::map> & time_map) +{ + for (auto & topic_pair : time_map) { + auto timestamps_tmp = topic_pair.second; + + // Sort the timestamps + std::sort(timestamps_tmp.begin(), timestamps_tmp.end()); + + // Then proceed with the frequency calculation + std::string topic_name = topic_pair.first; + if (timestamps_tmp.size() > 1) { + int64_t total_time_diff_ns = 0; + + // Accumulate the differences in nanoseconds + for (size_t i = 1; i < timestamps_tmp.size(); ++i) { + total_time_diff_ns += (timestamps_tmp[i] - timestamps_tmp[i - 1]).nanoseconds(); + } + + // Conversion to std::chrono::milliseconds + auto total_duration_ns = std::chrono::nanoseconds(total_time_diff_ns); + auto period_ms = std::chrono::duration_cast(total_duration_ns) / + (timestamps_tmp.size() - 1); + + PublisherVariablesVariant & publisher_variant = topic_publisher_map_[topic_name]; + PublisherVarAccessor accessor; + + std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisher_variant); + } + } +} + +void TopicPublisher::set_publishers_and_timers_to_variable() +{ + std::map> + pointcloud_variables_map; // temp map for pointcloud publishers + + // initialize timers and message publishers except pointcloud messages + for (auto & [topic_name, variant] : topic_publisher_map_) { + PublisherVarAccessor accessor; + const auto & topic_ref = topic_name; + const auto period_ns = std::chrono::duration( + std::visit([&](const auto & var) { return accessor.get_period(var); }, variant)); + + // Dynamically create the correct publisher type based on the topic + std::visit( + [&](auto & var) { + using MessageType = typename decltype(var.empty_area_message)::element_type; + + if constexpr ( + std::is_same_v || + std::is_same_v || + std::is_same_v) { + var.publisher = node_->create_publisher(topic_ref, rclcpp::SensorDataQoS()); + } else { + // For other message types, use the QoS setting depth of 1 + var.publisher = node_->create_publisher(topic_ref, rclcpp::QoS(1)); + } + }, + variant); + + // Conditionally create the timer based on the message type, if message type is not + // PointCloud2 + std::visit( + [&](auto & var) { + using MessageType = typename decltype(var.empty_area_message)::element_type; + + if constexpr (!std::is_same_v) { + var.timer = node_->create_wall_timer( + period_ns, [this, topic_ref]() { this->generic_message_publisher(topic_ref); }); + } else { + // For PointCloud2, Store the variables in a temporary map + pointcloud_variables_map[topic_ref] = var; + } + }, + variant); + } + + // To be able to publish pointcloud messages with async, I need to create a timer for each lidar + // output. So different operations are needed for pointcloud messages. + set_timers_for_pointcloud_msgs(pointcloud_variables_map); +} + +void TopicPublisher::set_timers_for_pointcloud_msgs( + const std::map> & pointcloud_variables_map) +{ + // Set the point cloud publisher timers + if (pointcloud_variables_map.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No pointcloud publishers found!"); + rclcpp::shutdown(); + } + + // Arrange the PointCloud2 variables w.r.t. the lidars' name + for (auto & [topic_name, pointcloud_variant] : pointcloud_variables_map) { + const auto lidar_name = split(topic_name, '/').at(3); + + if (lidar_pub_variable_pair_map_.find(lidar_name) == lidar_pub_variable_pair_map_.end()) { + lidar_pub_variable_pair_map_[lidar_name] = std::make_pair( + std::make_shared>(pointcloud_variant), nullptr); + } else { + if (lidar_pub_variable_pair_map_[lidar_name].second) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Lidar name: " << lidar_name << " is already used by another pointcloud publisher"); + rclcpp::shutdown(); + } + lidar_pub_variable_pair_map_[lidar_name].second = + std::make_shared>(pointcloud_variant); + } + } + + // Create the timer(s) to publish PointCloud2 Messages + const auto period_pointcloud_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); + + if (pointcloud_publisher_type_ != PointcloudPublisherType::ASYNC_PUBLISHER) { + // Create 1 timer to publish all PointCloud2 messages + pointcloud_sync_publish_timer_ = node_->create_wall_timer(period_pointcloud_ns, [this]() { + this->pointcloud_messages_sync_publisher(this->pointcloud_publisher_type_); + }); + } else { + // Create multiple timers which will run with a phase difference + const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size(); + + // Create a timer to create phase difference bw timers which will be created for each lidar + // topics + auto one_shot_timer = node_->create_wall_timer(phase_dif, [this, period_pointcloud_ns]() { + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + const auto & lidar_name = publisher_var_pair.first; + const auto & publisher_var = publisher_var_pair.second; + if ( + pointcloud_publish_timers_map_.find(lidar_name) == pointcloud_publish_timers_map_.end()) { + auto periodic_timer = node_->create_wall_timer( + period_pointcloud_ns, + [this, publisher_var]() { this->pointcloud_messages_async_publisher(publisher_var); }); + pointcloud_publish_timers_map_[lidar_name] = periodic_timer; + return; + } + } + one_shot_timer_shared_ptr_->cancel(); + }); + one_shot_timer_shared_ptr_ = one_shot_timer; + } +} + +bool TopicPublisher::check_publishers_initialized_correctly() +{ + // check messages are correctly initialized or not from rosbags + for (const auto & [topic_name, variant] : topic_publisher_map_) { + PublisherVarAccessor accessor; + auto empty_area_message = + std::visit([&](const auto & var) { return accessor.get_empty_area_message(var); }, variant); + auto object_spawned_message = std::visit( + [&](const auto & var) { return accessor.get_object_spawned_message(var); }, variant); + + if (!empty_area_message) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Empty area message couldn't found in the topic named: " << topic_name); + return false; + } + if (!object_spawned_message) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Object spawned message couldn't found in the topic named: " << topic_name); + return false; + } + } + return true; +} + +void TopicPublisher::init_rosbag_publisher_buffer( + const std::string & bag_path, const bool is_empty_area_message) +{ + rosbag2_cpp::Reader reader; + + try { + reader.open(bag_path); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error opening bag file: " << e.what()); + rclcpp::shutdown(); + return; + } + + const auto & topics = reader.get_metadata().topics_with_message_count; + + while (reader.has_next()) { + auto bag_message = reader.read_next(); + const auto current_topic = bag_message->topic_name; + + const auto message_type = get_publisher_message_type_for_topic(topics, current_topic); + + if (message_type == PublisherMessageType::UNKNOWN) { + continue; + } + if (message_type == PublisherMessageType::CAMERA_INFO) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::IMAGE) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POINTCLOUD2) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POSE_STAMPED) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::ODOMETRY) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::IMU) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::CONTROL_MODE_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::GEAR_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::HAZARD_LIGHTS_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::STEERING_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::TURN_INDICATORS_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::VELOCITY_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } + } + reader.close(); +} +} // namespace reaction_analyzer::topic_publisher diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp new file mode 100644 index 0000000000000..4a6110322440e --- /dev/null +++ b/tools/reaction_analyzer/src/utils.cpp @@ -0,0 +1,589 @@ +// 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 "utils.hpp" + +namespace reaction_analyzer +{ +SubscriberMessageType get_subscriber_message_type(const std::string & message_type) +{ + if (message_type == "autoware_auto_control_msgs/msg/AckermannControlCommand") { + return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND; + } + if (message_type == "autoware_auto_planning_msgs/msg/Trajectory") { + return SubscriberMessageType::TRAJECTORY; + } + if (message_type == "sensor_msgs/msg/PointCloud2") { + return SubscriberMessageType::POINTCLOUD2; + } + if (message_type == "autoware_auto_perception_msgs/msg/PredictedObjects") { + return SubscriberMessageType::PREDICTED_OBJECTS; + } + if (message_type == "autoware_auto_perception_msgs/msg/DetectedObjects") { + return SubscriberMessageType::DETECTED_OBJECTS; + } + if (message_type == "autoware_auto_perception_msgs/msg/TrackedObjects") { + return SubscriberMessageType::TRACKED_OBJECTS; + } + return SubscriberMessageType::UNKNOWN; +} + +PublisherMessageType get_publisher_message_type(const std::string & message_type) +{ + if (message_type == "sensor_msgs/msg/PointCloud2") { + return PublisherMessageType::POINTCLOUD2; + } + if (message_type == "sensor_msgs/msg/CameraInfo") { + return PublisherMessageType::CAMERA_INFO; + } + if (message_type == "sensor_msgs/msg/Image") { + return PublisherMessageType::IMAGE; + } + if (message_type == "geometry_msgs/msg/PoseWithCovarianceStamped") { + return PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED; + } + if (message_type == "geometry_msgs/msg/PoseStamped") { + return PublisherMessageType::POSE_STAMPED; + } + if (message_type == "nav_msgs/msg/Odometry") { + return PublisherMessageType::ODOMETRY; + } + if (message_type == "sensor_msgs/msg/Imu") { + return PublisherMessageType::IMU; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/ControlModeReport") { + return PublisherMessageType::CONTROL_MODE_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/GearReport") { + return PublisherMessageType::GEAR_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/HazardLightsReport") { + return PublisherMessageType::HAZARD_LIGHTS_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/SteeringReport") { + return PublisherMessageType::STEERING_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport") { + return PublisherMessageType::TURN_INDICATORS_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/VelocityReport") { + return PublisherMessageType::VELOCITY_REPORT; + } + return PublisherMessageType::UNKNOWN; +} + +PublisherMessageType get_publisher_message_type_for_topic( + const std::vector & topics, const std::string & topic_name) +{ + auto it = std::find_if(topics.begin(), topics.end(), [&topic_name](const auto & topic) { + return topic.topic_metadata.name == topic_name; + }); + if (it != topics.end()) { + return get_publisher_message_type(it->topic_metadata.type); // Return the message type if found + } + return PublisherMessageType::UNKNOWN; +} + +ReactionType get_reaction_type(const std::string & reaction_type) +{ + if (reaction_type == "first_brake_params") { + return ReactionType::FIRST_BRAKE; + } + if (reaction_type == "search_zero_vel_params") { + return ReactionType::SEARCH_ZERO_VEL; + } + if (reaction_type == "search_entity_params") { + return ReactionType::SEARCH_ENTITY; + } + return ReactionType::UNKNOWN; +} + +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} + +visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "entity"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = params.x; + marker.pose.position.y = params.y; + marker.pose.position.z = params.z; + + tf2::Quaternion quaternion; + quaternion.setRPY( + tier4_autoware_utils::deg2rad(params.roll), tier4_autoware_utils::deg2rad(params.pitch), + tier4_autoware_utils::deg2rad(params.yaw)); + marker.pose.orientation = tf2::toMsg(quaternion); + + marker.scale.x = 0.1; // Line width + + marker.color.a = 1.0; // Alpha + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + // Define the 8 corners of the polyhedron + geometry_msgs::msg::Point p1, p2, p3, p4, p5, p6, p7, p8; + + p1.x = params.x_l / 2.0; + p1.y = params.y_l / 2.0; + p1.z = params.z_l / 2.0; + p2.x = -params.x_l / 2.0; + p2.y = params.y_l / 2.0; + p2.z = params.z_l / 2.0; + p3.x = -params.x_l / 2.0; + p3.y = -params.y_l / 2.0; + p3.z = params.z_l / 2.0; + p4.x = params.x_l / 2.0; + p4.y = -params.y_l / 2.0; + p4.z = params.z_l / 2.0; + p5.x = params.x_l / 2.0; + p5.y = params.y_l / 2.0; + p5.z = -params.z_l / 2.0; + p6.x = -params.x_l / 2.0; + p6.y = params.y_l / 2.0; + p6.z = -params.z_l / 2.0; + p7.x = -params.x_l / 2.0; + p7.y = -params.y_l / 2.0; + p7.z = -params.z_l / 2.0; + p8.x = params.x_l / 2.0; + p8.y = -params.y_l / 2.0; + p8.z = -params.z_l / 2.0; + + // Add points to the marker + marker.points.push_back(p1); + marker.points.push_back(p2); + marker.points.push_back(p3); + marker.points.push_back(p4); + marker.points.push_back(p1); + + marker.points.push_back(p5); + marker.points.push_back(p6); + marker.points.push_back(p7); + marker.points.push_back(p8); + marker.points.push_back(p5); + + marker.points.push_back(p1); + marker.points.push_back(p5); + marker.points.push_back(p6); + marker.points.push_back(p2); + marker.points.push_back(p3); + marker.points.push_back(p7); + marker.points.push_back(p4); + marker.points.push_back(p8); + + return marker; +} + +std::vector split(const std::string & str, char delimiter) +{ + std::vector elements; + std::stringstream ss(str); + std::string item; + while (std::getline(ss, item, delimiter)) { + elements.push_back(item); + } + return elements; +} + +bool does_folder_exist(const std::string & path) +{ + return std::filesystem::exists(path) && std::filesystem::is_directory(path); +} + +size_t get_index_after_distance( + const Trajectory & traj, const size_t curr_id, const double distance) +{ + const TrajectoryPoint & curr_p = traj.points.at(curr_id); + + size_t target_id = curr_id; + double current_distance = 0.0; + for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { + current_distance = tier4_autoware_utils::calcDistance3d(traj.points.at(traj_id), curr_p); + if (current_distance >= distance) { + break; + } + target_id = traj_id; + } + return target_id; +} + +double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & end) +{ + const auto duration = end - start; + + const auto duration_ns = duration.to_chrono(); + return static_cast(duration_ns.count()) / 1e6; +} + +TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector( + const PipelineMap & pipelineMap) +{ + std::vector>> sorted_vector; + + for (const auto & entry : pipelineMap) { + auto sorted_reactions = entry.second; + // Sort the vector of ReactionPair based on the published stamp + std::sort( + sorted_reactions.begin(), sorted_reactions.end(), + [](const ReactionPair & a, const ReactionPair & b) { + return rclcpp::Time(a.second.published_stamp) < rclcpp::Time(b.second.published_stamp); + }); + + // Add to the vector as a tuple + sorted_vector.emplace_back(std::make_tuple(entry.first, sorted_reactions)); + } + + // Sort the vector of tuples by rclcpp::Time + std::sort(sorted_vector.begin(), sorted_vector.end(), [](const auto & a, const auto & b) { + return std::get<0>(a) < std::get<0>(b); + }); + + return sorted_vector; +} + +unique_identifier_msgs::msg::UUID generate_uuid_msg(const std::string & input) +{ + static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); + const auto uuid = generate_uuid(input); + + unique_identifier_msgs::msg::UUID uuid_msg; + std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); + return uuid_msg; +} + +geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params) +{ + geometry_msgs::msg::Pose entity_pose; + entity_pose.position.x = entity_params.x; + entity_pose.position.y = entity_params.y; + entity_pose.position.z = entity_params.z; + + tf2::Quaternion entity_q_orientation; + entity_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(entity_params.roll), + tier4_autoware_utils::deg2rad(entity_params.pitch), + tier4_autoware_utils::deg2rad(entity_params.yaw)); + entity_pose.orientation = tf2::toMsg(entity_q_orientation); + return entity_pose; +} + +geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = pose_params.x; + pose.position.y = pose_params.y; + pose.position.z = pose_params.z; + + tf2::Quaternion pose_q_orientation; + pose_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(pose_params.roll), + tier4_autoware_utils::deg2rad(pose_params.pitch), + tier4_autoware_utils::deg2rad(pose_params.yaw)); + pose.orientation = tf2::toMsg(pose_q_orientation); + return pose; +} + +PointCloud2::SharedPtr create_entity_pointcloud_ptr( + const EntityParams & entity_params, const double pointcloud_sampling_distance) +{ + pcl::PointCloud point_cloud; + tf2::Quaternion entity_q_orientation; + + entity_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(entity_params.roll), + tier4_autoware_utils::deg2rad(entity_params.pitch), + tier4_autoware_utils::deg2rad(entity_params.yaw)); + tf2::Transform tf(entity_q_orientation); + const auto origin = tf2::Vector3(entity_params.x, entity_params.y, entity_params.z); + tf.setOrigin(origin); + + const double it_x = entity_params.x_l / pointcloud_sampling_distance; + const double it_y = entity_params.y_l / pointcloud_sampling_distance; + const double it_z = entity_params.z_l / pointcloud_sampling_distance; + + // Sample the box and rotate + for (int i = 0; i <= it_z; ++i) { + for (int j = 0; j <= it_y; ++j) { + for (int k = 0; k <= it_x; ++k) { + const double p_x = -entity_params.x_l / 2 + k * pointcloud_sampling_distance; + const double p_y = -entity_params.y_l / 2 + j * pointcloud_sampling_distance; + const double p_z = -entity_params.z_l / 2 + i * pointcloud_sampling_distance; + const auto tmp = tf2::Vector3(p_x, p_y, p_z); + tf2::Vector3 data_out = tf * tmp; + point_cloud.emplace_back(pcl::PointXYZ( + static_cast(data_out.x()), static_cast(data_out.y()), + static_cast(data_out.z()))); + } + } + } + PointCloud2::SharedPtr entity_pointcloud_ptr; + entity_pointcloud_ptr = std::make_shared(); + pcl::toROSMsg(point_cloud, *entity_pointcloud_ptr); + return entity_pointcloud_ptr; +} + +PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityParams & entity_params) +{ + unique_identifier_msgs::msg::UUID uuid_msg; + + PredictedObject obj; + const auto entity_pose = create_entity_pose(entity_params); + geometry_msgs::msg::Vector3 dimension; + dimension.set__x(entity_params.x_l); + dimension.set__y(entity_params.y_l); + dimension.set__z(entity_params.z_l); + obj.shape.set__dimensions(dimension); + + obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.existence_probability = 1.0; + obj.kinematics.initial_pose_with_covariance.pose = entity_pose; + + autoware_auto_perception_msgs::msg::PredictedPath path; + path.confidence = 1.0; + path.path.emplace_back(entity_pose); + obj.kinematics.predicted_paths.emplace_back(path); + + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + classification.probability = 1.0; + obj.classification.emplace_back(classification); + obj.set__object_id(generate_uuid_msg("test_obstacle")); + + PredictedObjects pred_objects; + pred_objects.objects.emplace_back(obj); + return std::make_shared(pred_objects); +} + +double calculate_entity_search_radius(const EntityParams & entity_params) +{ + return std::sqrt( + std::pow(entity_params.x_l, 2) + std::pow(entity_params.y_l, 2) + + std::pow(entity_params.z_l, 2)) / + 2.0; +} + +bool search_pointcloud_near_pose( + const pcl::PointCloud & pcl_pointcloud, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), + [pose, search_radius](const auto & point) { + return tier4_autoware_utils::calcDistance3d(pose.position, point) <= search_radius; + }); +} + +bool search_predicted_objects_near_pose( + const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [pose, search_radius](const PredictedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.initial_pose_with_covariance.pose.position) <= + search_radius; + }); + ; +} + +bool search_detected_objects_near_pose( + const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + detected_objects.objects.begin(), detected_objects.objects.end(), + [pose, search_radius](const DetectedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.pose_with_covariance.pose.position) <= + search_radius; + }); +} + +bool search_tracked_objects_near_pose( + const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + tracked_objects.objects.begin(), tracked_objects.objects.end(), + [pose, search_radius](const TrackedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.pose_with_covariance.pose.position) <= + search_radius; + }); +} + +LatencyStats calculate_statistics(const std::vector & latency_vec) +{ + LatencyStats stats{0.0, 0.0, 0.0, 0.0, 0.0}; + stats.max = *max_element(latency_vec.begin(), latency_vec.end()); + stats.min = *min_element(latency_vec.begin(), latency_vec.end()); + + const double sum = std::accumulate(latency_vec.begin(), latency_vec.end(), 0.0); + stats.mean = sum / static_cast(latency_vec.size()); + + std::vector sorted_latencies = latency_vec; + std::sort(sorted_latencies.begin(), sorted_latencies.end()); + stats.median = sorted_latencies.size() % 2 == 0 + ? (sorted_latencies[sorted_latencies.size() / 2 - 1] + + sorted_latencies[sorted_latencies.size() / 2]) / + 2 + : sorted_latencies[sorted_latencies.size() / 2]; + + const double sq_sum = + std::inner_product(latency_vec.begin(), latency_vec.end(), latency_vec.begin(), 0.0); + stats.std_dev = + std::sqrt(sq_sum / static_cast(latency_vec.size()) - stats.mean * stats.mean); + return stats; +} + +void write_results( + rclcpp::Node * node, const std::string & output_file_path, const RunningMode & node_running_mode, + const std::vector & pipeline_map_vector) +{ + // create csv file + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + + std::stringstream ss; + ss << output_file_path; + if (!output_file_path.empty() && output_file_path.back() != '/') { + ss << "/"; // Ensure the path ends with a slash + } + if (node_running_mode == RunningMode::PlanningControl) { + ss << "planning_control-"; + } else { + ss << "perception_planning-"; + } + + ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d-%H-%M-%S"); + ss << "-reaction-results.csv"; + + // open file + std::ofstream file(ss.str()); + + // Check if the file was opened successfully + if (!file.is_open()) { + RCLCPP_ERROR_ONCE(node->get_logger(), "Failed to open file: %s", ss.str().c_str()); + return; + } + + // tmp map to store latency results for statistics + std::map>> tmp_latency_map; + + size_t test_count = 0; + for (const auto & pipeline_map : pipeline_map_vector) { + test_count++; + // convert pipeline_map to vector of tuples + file << "Test " << test_count << "\n"; + const auto sorted_results_vector = convert_pipeline_map_to_sorted_vector(pipeline_map); + const auto spawn_cmd_time = std::get<0>(*sorted_results_vector.begin()); + + for (size_t i = 0; i < sorted_results_vector.size(); ++i) { + const auto & [pipeline_header_time, pipeline_reactions] = sorted_results_vector[i]; + + // total time pipeline lasts + file << "Pipeline - " << i << ","; + + // pipeline nodes + for (const auto & [node_name, reaction] : pipeline_reactions) { + file << node_name << ","; + } + + file << "\nNode - Pipeline - Total Latency [ms],"; + + for (size_t j = 0; j < pipeline_reactions.size(); ++j) { + const auto & reaction = pipeline_reactions[j].second; + const auto & node_name = pipeline_reactions[j].first; + if (j == 0) { + const auto node_latency = + calculate_time_diff_ms(reaction.header.stamp, reaction.published_stamp); + const auto pipeline_latency = + calculate_time_diff_ms(pipeline_header_time, reaction.published_stamp); + const auto total_latency = + calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp); + file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; + tmp_latency_map[node_name].emplace_back( + std::make_tuple(node_latency, pipeline_latency, total_latency)); + } else { + const auto & prev_reaction = pipeline_reactions[j - 1].second; + const auto node_latency = + calculate_time_diff_ms(prev_reaction.published_stamp, reaction.published_stamp); + const auto pipeline_latency = + calculate_time_diff_ms(pipeline_header_time, reaction.published_stamp); + const auto total_latency = + calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp); + file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; + tmp_latency_map[node_name].emplace_back( + std::make_tuple(node_latency, pipeline_latency, total_latency)); + } + } + file << "\n"; + } + } + + // write statistics + + file << "\nStatistics\n"; + file << "Node " + "Name,Min-NL,Max-NL,Mean-NL,Median-NL,Std-Dev-NL,Min-PL,Max-PL,Mean-PL,Median-PL,Std-Dev-" + "PL,Min-TL,Max-TL,Mean-TL,Median-TL,Std-Dev-TL\n"; + for (const auto & [node_name, latency_vec] : tmp_latency_map) { + file << node_name << ","; + + std::vector node_latencies; + std::vector pipeline_latencies; + std::vector total_latencies; + + // Extract latencies + for (const auto & latencies : latency_vec) { + double node_latency, pipeline_latency, total_latency; + std::tie(node_latency, pipeline_latency, total_latency) = latencies; + node_latencies.push_back(node_latency); + pipeline_latencies.push_back(pipeline_latency); + total_latencies.push_back(total_latency); + } + + const auto stats_node_latency = calculate_statistics(node_latencies); + const auto stats_pipeline_latency = calculate_statistics(pipeline_latencies); + const auto stats_total_latency = calculate_statistics(total_latencies); + + file << stats_node_latency.min << "," << stats_node_latency.max << "," + << stats_node_latency.mean << "," << stats_node_latency.median << "," + << stats_node_latency.std_dev << "," << stats_pipeline_latency.min << "," + << stats_pipeline_latency.max << "," << stats_pipeline_latency.mean << "," + << stats_pipeline_latency.median << "," << stats_pipeline_latency.std_dev << "," + << stats_total_latency.min << "," << stats_total_latency.max << "," + << stats_total_latency.mean << "," << stats_total_latency.median << "," + << stats_total_latency.std_dev << "\n"; + } + file.close(); + RCLCPP_INFO(node->get_logger(), "Results written to: %s", ss.str().c_str()); +} +} // namespace reaction_analyzer