diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f7603af847d1f..256b617116432 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -77,7 +77,7 @@ evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4 evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp +evaluator/tier4_metrics_rviz_plugin/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -121,7 +121,7 @@ perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp -perception/euclidean_cluster/** yukihiro.saito@tier4.jp +perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp @@ -212,7 +212,7 @@ sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index be2411cd3268b..c658cf4497973 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -121,7 +121,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 811e1652fcb4a..c48383a17ab4b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -321,33 +321,39 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { + tier4_autoware_utils::Polygon2d p; + auto & outer = p.outer(); + + for (const auto & p : poly) { + tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + outer.push_back(p2d); + } + boost::geometry::correct(p); + return p; + }; + // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - if (lanelets_distance_pair.size() == 1) - return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + tier4_autoware_utils::MultiPolygon2d lanelet_unions; + tier4_autoware_utils::MultiPolygon2d result; - lanelet::BasicPolygon2d merged_polygon = - lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); - for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; - const auto & poly = route_lanelet.polygon2d().basicPolygon(); - - std::vector lanelet_union_temp; - boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); - - // Update merged_polygon by accumulating all merged results - merged_polygon.clear(); - for (const auto & temp_poly : lanelet_union_temp) { - merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); - } + const auto & p = route_lanelet.polygon2d().basicPolygon(); + tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + boost::geometry::union_(lanelet_unions, poly, result); + lanelet_unions = result; + result.clear(); } - if (merged_polygon.empty()) return std::nullopt; - return merged_polygon; + + if (lanelet_unions.empty()) return std::nullopt; + return lanelet_unions.front(); }(); return fused_lanelets; diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8fa95f4b3b87b..373de2e0bd911 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -355,12 +355,15 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd Wd(DIM_X, 1); MatrixXd Cd(DIM_Y, DIM_X); + const double sign_vx = m_is_forward_shift ? 1 : -1; + MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try { - k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time); + // NOTE: When driving backward, the curvature's sign should be reversed. + k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); } catch (const std::exception & e) { RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); @@ -446,6 +449,7 @@ MPCMatrix MPC::generateMPCMatrix( const double ref_vx = reference_trajectory.vx.at(i); const double ref_vx_squared = ref_vx * ref_vx; + // NOTE: When driving backward, the curvature's sign should be reversed. const double ref_k = reference_trajectory.k.at(i) * sign_vx; const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx; diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 54281358219d1..18dde41c99566 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -21,6 +21,7 @@ diagnostic_msgs eigen geometry_msgs + lanelet2_extension libgoogle-glog-dev motion_utils nav_msgs diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 1bfd27828c98e..16c46d0eecd92 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -79,7 +79,9 @@ class EvalTest : public ::testing::Test marker_sub_ = rclcpp::create_subscription( eval_node, "perception_online_evaluator/markers", 10, - [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; }); + [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) { + has_received_marker_ = true; + }); uuid_ = generateUUID(); } diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt index 4c1e8cf58c262..8475b596e6d6b 100644 --- a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt +++ b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -8,11 +8,11 @@ find_package(Qt5 REQUIRED Core Widgets Charts) set(QT_WIDGETS_LIB Qt5::Widgets) set(QT_CHARTS_LIB Qt5::Charts) set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/metrics_visualize_panel.cpp + include/metrics_visualize_panel.hpp ) target_link_libraries(${PROJECT_NAME} diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp similarity index 72% rename from evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp rename to evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp index 6708ecd7071e9..3d66099988d8b 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp +++ b/evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp @@ -19,11 +19,13 @@ #ifndef Q_MOC_RUN #include #include +#include #include #include #include #include #include +#include #include #include #endif @@ -33,9 +35,12 @@ #include +#include #include #include #include +#include +#include namespace rviz_plugins { @@ -62,10 +67,10 @@ struct Metric { auto label = new QLabel; label->setAlignment(Qt::AlignCenter); - label->setText("metric_name"); + label->setText(QString::fromStdString(status.name)); labels.emplace("metric_name", label); - header.push_back(QString::fromStdString(status.name)); + header.push_back("metric_name"); } for (const auto & [key, value] : status.values) { @@ -146,6 +151,8 @@ struct Metric QTableWidget * getTable() const { return table; } + std::unordered_map getLabels() const { return labels; } + private: static std::optional getValue(const DiagnosticStatus & status, std::string && key) { @@ -186,19 +193,61 @@ class MetricsVisualizePanel : public rviz_common::Panel void onInitialize() override; private Q_SLOTS: + // Slot functions triggered by UI events + void onTopicChanged(); + void onSpecificMetricChanged(); + void onClearButtonClicked(); + void onTabChanged(); private: + // ROS 2 node and subscriptions for handling metrics data rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_; + std::unordered_map::SharedPtr> subscriptions_; + + // Topics from which metrics are collected + std::vector topics_ = { + "/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"}; + // Timer and metrics message callback void onTimer(); - void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name); + + // Functions to update UI based on selected metrics + void updateViews(); + void updateSelectedMetric(const std::string & metric_name); + // UI components QGridLayout * grid_; + QComboBox * topic_selector_; + QTabWidget * tab_widget_; + + // "Specific Metrics" tab components + QComboBox * specific_metric_selector_; + QChartView * specific_metric_chart_view_; + QTableWidget * specific_metric_table_; + + // Selected metrics data + std::optional> selected_metrics_; + // Cache for received messages by topics + std::unordered_map current_msg_map_; + + // Mapping from topics to metrics widgets (tables and charts) + std::unordered_map< + std::string, std::unordered_map>> + topic_widgets_map_; + + // Synchronization std::mutex mutex_; + + // Stored metrics data std::unordered_map metrics_; + + // Utility functions for managing widget visibility based on topics + void updateWidgetVisibility(const std::string & target_topic, const bool show); + void showCurrentTopicWidgets(); + void hideInactiveTopicWidgets(); }; } // namespace rviz_plugins diff --git a/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml index fe84a0ab81760..d06382bc8c539 100644 --- a/evaluator/tier4_metrics_rviz_plugin/package.xml +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -7,6 +7,8 @@ Satoshi OTA Kyoichi Sugahara Maxime CLEMENT + Kosuke Takeuchi + Fumiya Watanabe Apache License 2.0 ament_cmake_auto diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp index 79da02b9b65c6..b92a9a7ace53d 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -29,7 +29,53 @@ namespace rviz_plugins MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) : rviz_common::Panel(parent), grid_(new QGridLayout()) { - setLayout(grid_); + // Initialize the main tab widget + tab_widget_ = new QTabWidget(); + + // Create and configure the "All Metrics" tab + QWidget * all_metrics_widget = new QWidget(); + grid_ = new QGridLayout(all_metrics_widget); // Apply grid layout to the widget directly + all_metrics_widget->setLayout(grid_); + + // Add topic selector combobox + topic_selector_ = new QComboBox(); + for (const auto & topic : topics_) { + topic_selector_->addItem(QString::fromStdString(topic)); + } + grid_->addWidget(topic_selector_, 0, 0, 1, -1); // Add topic selector to the grid layout + connect(topic_selector_, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged())); + + tab_widget_->addTab( + all_metrics_widget, "All Metrics"); // Add "All Metrics" tab to the tab widget + + // Create and configure the "Specific Metrics" tab + QWidget * specific_metrics_widget = new QWidget(); + QVBoxLayout * specific_metrics_layout = new QVBoxLayout(); + specific_metrics_widget->setLayout(specific_metrics_layout); + + // Add specific metric selector combobox + specific_metric_selector_ = new QComboBox(); + specific_metrics_layout->addWidget(specific_metric_selector_); + connect( + specific_metric_selector_, SIGNAL(currentIndexChanged(int)), this, + SLOT(onSpecificMetricChanged())); + + // Add clear button + QPushButton * clear_button = new QPushButton("Clear"); + specific_metrics_layout->addWidget(clear_button); + connect(clear_button, &QPushButton::clicked, this, &MetricsVisualizePanel::onClearButtonClicked); + + // Add chart view for specific metrics + specific_metric_chart_view_ = new QChartView(); + specific_metrics_layout->addWidget(specific_metric_chart_view_); + + tab_widget_->addTab( + specific_metrics_widget, "Specific Metrics"); // Add "Specific Metrics" tab to the tab widget + + // Set the main layout of the panel + QVBoxLayout * main_layout = new QVBoxLayout(); + main_layout->addWidget(tab_widget_); + setLayout(main_layout); } void MetricsVisualizePanel::onInitialize() @@ -38,14 +84,103 @@ void MetricsVisualizePanel::onInitialize() raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - sub_ = raw_node_->create_subscription( - "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, - std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + for (const auto & topic_name : topics_) { + const auto callback = [this, topic_name](const DiagnosticArray::ConstSharedPtr msg) { + this->onMetrics(msg, topic_name); + }; + const auto subscription = + raw_node_->create_subscription(topic_name, rclcpp::QoS{1}, callback); + subscriptions_[topic_name] = subscription; + } const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); } +void MetricsVisualizePanel::updateWidgetVisibility( + const std::string & target_topic, const bool show) +{ + for (const auto & [topic_name, metric_widgets_pair] : topic_widgets_map_) { + const bool is_target_topic = (topic_name == target_topic); + if ((!is_target_topic && show) || (is_target_topic && !show)) { + continue; + } + for (const auto & [metric, widgets] : metric_widgets_pair) { + widgets.first->setVisible(show); + widgets.second->setVisible(show); + } + } +} + +void MetricsVisualizePanel::showCurrentTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, true); +} + +void MetricsVisualizePanel::hideInactiveTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, false); +} + +void MetricsVisualizePanel::onTopicChanged() +{ + std::lock_guard message_lock(mutex_); + hideInactiveTopicWidgets(); + showCurrentTopicWidgets(); +} + +void MetricsVisualizePanel::updateSelectedMetric(const std::string & metric_name) +{ + std::lock_guard message_lock(mutex_); + + for (const auto & [topic, msg] : current_msg_map_) { + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + for (const auto & status : msg->status) { + if (metric_name == status.name) { + selected_metrics_ = {metric_name, Metric(status)}; + selected_metrics_->second.updateData(time, status); + return; + } + } + } +} + +void MetricsVisualizePanel::updateViews() +{ + if (!selected_metrics_) { + return; + } + + Metric & metric = selected_metrics_->second; + specific_metric_chart_view_->setChart(metric.getChartView()->chart()); + auto * specific_metrics_widget = dynamic_cast(tab_widget_->widget(1)); + auto * specific_metrics_layout = dynamic_cast(specific_metrics_widget->layout()); + specific_metrics_layout->removeWidget(specific_metric_table_); + specific_metric_table_ = metric.getTable(); + QSizePolicy sizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + sizePolicy.setHeightForWidth(specific_metric_table_->sizePolicy().hasHeightForWidth()); + specific_metric_table_->setSizePolicy(sizePolicy); + specific_metrics_layout->insertWidget(1, specific_metric_table_); +} + +void MetricsVisualizePanel::onSpecificMetricChanged() +{ + const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString(); + updateSelectedMetric(selected_metrics_str); + updateViews(); +} + +void MetricsVisualizePanel::onClearButtonClicked() +{ + if (!selected_metrics_) { + return; + } + updateSelectedMetric(selected_metrics_->first); + updateViews(); +} + void MetricsVisualizePanel::onTimer() { std::lock_guard message_lock(mutex_); @@ -54,30 +189,71 @@ void MetricsVisualizePanel::onTimer() metric.updateGraph(); metric.updateTable(); } + + if (selected_metrics_) { + selected_metrics_->second.updateGraph(); + selected_metrics_->second.updateTable(); + } } -void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +void MetricsVisualizePanel::onMetrics( + const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name) { std::lock_guard message_lock(mutex_); const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; - constexpr size_t GRAPH_COL_SIZE = 5; - for (size_t i = 0; i < msg->status.size(); ++i) { - const auto & status = msg->status.at(i); + for (const auto & status : msg->status) { + const size_t num_current_metrics = topic_widgets_map_[topic_name].size(); if (metrics_.count(status.name) == 0) { - auto metric = Metric(status); + const auto metric = Metric(status); metrics_.emplace(status.name, metric); - grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE); - grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false); - grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE); - grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true); - grid_->setColumnStretch(i % GRAPH_COL_SIZE, true); - } + // Calculate grid position + const size_t row = num_current_metrics / GRAPH_COL_SIZE * 2 + + 2; // start from 2 to leave space for the topic selector and tab widget + const size_t col = num_current_metrics % GRAPH_COL_SIZE; + + // Get the widgets from the metric + const auto tableWidget = metric.getTable(); + const auto chartViewWidget = metric.getChartView(); + + // Get the layout for the "All Metrics" tab + auto all_metrics_widget = dynamic_cast(tab_widget_->widget(0)); + QGridLayout * all_metrics_layout = dynamic_cast(all_metrics_widget->layout()); + + // Add the widgets to the "All Metrics" tab layout + all_metrics_layout->addWidget(tableWidget, row, col); + all_metrics_layout->setRowStretch(row, false); + all_metrics_layout->addWidget(chartViewWidget, row + 1, col); + all_metrics_layout->setRowStretch(row + 1, true); + all_metrics_layout->setColumnStretch(col, true); + + // Also add the widgets to the topic_widgets_map_ for easy management + topic_widgets_map_[topic_name][status.name] = std::make_pair(tableWidget, chartViewWidget); + } metrics_.at(status.name).updateData(time, status); + + // update selected metrics + const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString(); + if (selected_metrics_str == status.name) { + if (selected_metrics_) { + selected_metrics_->second.updateData(time, status); + } + } } + + // Update the specific metric selector + QSignalBlocker blocker(specific_metric_selector_); + for (const auto & status : msg->status) { + if (specific_metric_selector_->findText(QString::fromStdString(status.name)) == -1) { + specific_metric_selector_->addItem(QString::fromStdString(status.name)); + } + } + + // save the message for metrics selector + current_msg_map_[topic_name] = msg; } } // namespace rviz_plugins diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp index bc62bfa690946..201c743471efd 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -40,6 +40,7 @@ std::optional SmartPoseBuffer::interpolate( const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; if (target_ros_time < time_first) { + RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); return std::nullopt; } diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index bb32741067e65..ae6f0b61f063c 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -241,7 +241,7 @@ void output_pose_with_cov_to_log( rpy.y = rpy.y * 180.0 / M_PI; rpy.z = rpy.z * 180.0 / M_PI; - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 74d49e13a4c21..424a7051de883 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -60,6 +60,10 @@ One optional function is regularization. Please see the regularization chapter i {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} +#### Sensor Points + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/sensor_points.json") }} + #### Ndt {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 144449ce75084..241892e67b66c 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -11,6 +11,12 @@ map_frame: "map" + sensor_points: + # Required distance of input sensor points. [m] + # If the max distance of input sensor points is lower than this value, the scan matching will not be performed. + required_distance: 10.0 + + ndt: # The maximum difference between two consecutive # transformations in order to consider convergence diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index 2955e3cb9a5f7..ee1e2369c79bd 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -37,6 +37,11 @@ struct HyperParameters std::string map_frame; } frame; + struct SensorPoints + { + double required_distance; + } sensor_points; + pclomp::NdtParams ndt; bool ndt_regularization_enable; @@ -91,6 +96,9 @@ struct HyperParameters frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); frame.map_frame = node->declare_parameter("frame.map_frame"); + sensor_points.required_distance = + node->declare_parameter("sensor_points.required_distance"); + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); ndt.step_size = node->declare_parameter("ndt.step_size"); ndt.resolution = node->declare_parameter("ndt.resolution"); diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json index a42ee37858f46..74737a098e622 100644 --- a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json +++ b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json @@ -21,6 +21,9 @@ "covariance": { "$ref": "sub/covariance.json#/definitions/covariance" }, "dynamic_map_loading": { "$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading" + }, + "sensor_points": { + "$ref": "sub/sensor_points.json#/definitions/sensor_points" } }, "required": [ @@ -30,7 +33,8 @@ "validation", "score_estimation", "covariance", - "dynamic_map_loading" + "dynamic_map_loading", + "sensor_points" ], "additionalProperties": false } diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json new file mode 100644 index 0000000000000..68a0b40f8f94e --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -0,0 +1,18 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "sensor_points": { + "type": "object", + "properties": { + "required_distance": { + "type": "number", + "description": "Required distance of input sensor points [m]. If the max distance of input sensor points is lower than this value, the scan matching will not be performed.", + "default": "10.0" + } + }, + "required": ["required_distance"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 31d1c6588165f..286540a12ac72 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -78,7 +78,16 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) ndt_ptr_->setParams(param); - update_ndt(position, *ndt_ptr_); + const bool updated = update_ndt(position, *ndt_ptr_); + if (!updated) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 1000, + "update_ndt failed. If this happens with initial position estimation, make sure that" + "(1) the initial position matches the pcd map and (2) the map_loader is working properly."); + last_update_position_ = position; + ndt_ptr_mutex_->unlock(); + return; + } ndt_ptr_->setInputSource(input_source); ndt_ptr_mutex_->unlock(); need_rebuild_ = false; @@ -173,7 +182,7 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; - RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + RCLCPP_DEBUG(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); return true; // Updated } 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 cd637791f04b6..d8854757c2f8f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -346,6 +346,21 @@ void NDTScanMatcher::callback_sensor_points( transform_sensor_measurement( sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + + // check max distance of sensor points + double max_distance = 0.0; + for (const auto & point : sensor_points_in_baselink_frame->points) { + const double distance = std::hypot(point.x, point.y, point.z); + max_distance = std::max(max_distance, distance); + } + if (max_distance < param_.sensor_points.required_distance) { + RCLCPP_WARN_STREAM( + this->get_logger(), "Max distance of sensor points = " + << std::fixed << std::setprecision(3) << max_distance << " [m] < " + << param_.sensor_points.required_distance << " [m]"); + return; + } + ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; @@ -992,7 +1007,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); - RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score); return result_pose_with_cov_msg; } diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp index 801a08f83aab8..6e11f8fe74212 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp @@ -40,7 +40,6 @@ PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped RCLCPP_INFO(logger_, "Call NDT align server."); const auto res = cli_align_->async_send_request(req).get(); if (!res->success) { - RCLCPP_INFO(logger_, "NDT align server failed."); throw ServiceException( Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed."); } diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index fb7642356e8d9..0acba3d2dd58d 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -61,6 +63,11 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); + processing_time_publisher_ = + std::make_unique(node, "detection_by_tracker"); + stop_watch_ptr_ = + std::make_unique>(); + this->startStopWatch(); } ~Debugger() {} @@ -80,6 +87,19 @@ class Debugger { divided_objects_pub_->publish(removeFeature(input)); } + void startStopWatch() + { + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); } + void publishProcessingTime() + { + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); + } private: rclcpp::Publisher::SharedPtr @@ -90,6 +110,9 @@ class Debugger merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_auto_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 2595315e1f9b2..7afc3e41e4683 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -206,6 +206,7 @@ void DetectionByTracker::setMaxSearchRange() void DetectionByTracker::onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) { + debugger_->startMeasureProcessingTime(); autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = input_msg->header; @@ -250,6 +251,7 @@ void DetectionByTracker::onObjects( } objects_pub_->publish(detected_objects); + debugger_->publishProcessingTime(); } void DetectionByTracker::divideUnderSegmentedObjects( diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 980565fe2144b..d017d06929702 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -16,6 +16,7 @@ #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include @@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter // classified point label // (0: not classified, 1: ground, 2: not ground, 3: follow previous point, // 4: unkown(currently not used), 5: virtual ground) - enum class PointLabel { + enum class PointLabel : uint16_t { INIT = 0, GROUND, NON_GROUND, @@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter VIRTUAL_GROUND, OUT_OF_RANGE }; - struct PointRef + + struct PointData { - float grid_size; // radius of grid - uint16_t grid_id; // id of grid in vertical - float radius; // cylindrical coords on XY Plane - float theta; // angle deg on XY plane - size_t radial_div; // index of the radial division to which this point belongs to + float radius; // cylindrical coords on XY Plane PointLabel point_state{PointLabel::INIT}; - + uint16_t grid_id; // id of grid in vertical size_t orig_index; // index of this point in the source pointcloud - pcl::PointXYZ * orig_point; }; - using PointCloudRefVector = std::vector; + using PointCloudVector = std::vector; struct GridCenter { @@ -144,18 +141,39 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter pcl::PointIndices getIndices() { return pcl_indices; } std::vector getHeightList() { return height_list; } + + pcl::PointIndices & getIndicesRef() { return pcl_indices; } + std::vector & getHeightListRef() { return height_list; } }; void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const pointcloud_preprocessor::TransformInfo & transform_info); + tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + void set_field_offsets(const PointCloud2ConstPtr & input); + + void get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; float grid_size_rad_; + float tan_grid_size_rad_; float grid_size_m_; float low_priority_region_x_; uint16_t gnd_grid_buffer_size_; @@ -163,14 +181,17 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float grid_mode_switch_angle_rad_; float virtual_lidar_z_; float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance - double global_slope_max_angle_rad_; // radians - double local_slope_max_angle_rad_; // radians + float center_pcl_shift_; // virtual center of pcl to center mass + float grid_mode_switch_radius_; // non linear grid size switching distance + double global_slope_max_angle_rad_; // radians + double local_slope_max_angle_rad_; // radians + double global_slope_max_ratio_; + double local_slope_max_ratio_; double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions - double // minimum height threshold regardless the slope, - split_height_distance_; // useful for close points + double split_points_distance_tolerance_square_; + double // minimum height threshold regardless the slope, + split_height_distance_; // useful for close points bool use_virtual_ground_point_; bool use_recheck_ground_cluster_; // to enable recheck ground cluster size_t radial_dividers_num_; @@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter */ /*! - * Convert pcl::PointCloud to sorted PointCloudRefVector + * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector * @param[in] in_cloud Input Point Cloud to be organized in radial segments * @param[out] out_radial_ordered_points_manager Vector of Points Clouds, * each element will contain the points ordered */ void convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); void convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ void calcVirtualGroundOrigin(pcl::PointXYZ & point); + float calcGridSize(const PointData & p); + /*! * Classifies Points in the PointCloud as Ground and Not Ground * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud @@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter void initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids); - void checkContinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkDiscontinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkBreakGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkContinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkDiscontinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkBreakGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); void classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); /*! * Re-classifies point of ground cluster based on their height @@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * and the other removed as indicated in the indices * @param in_cloud_ptr Input PointCloud to which the extraction will be performed * @param in_indices Indices of the points to be both removed and kept - * @param out_object_cloud_ptr Resulting PointCloud with the indices kept + * @param out_object_cloud Resulting PointCloud with the indices kept */ void extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr); + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index f164e0102e0e9..7a0333d95075b 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -48,8 +48,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); @@ -60,6 +64,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division virtual_lidar_z_ = vehicle_info_.vehicle_height_m; grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + offset_initialized_ = false; } using std::placeholders::_1; @@ -77,92 +87,117 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } +inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstPtr & input) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } + offset_initialized_ = true; +} + +inline void ScanGroundFilterComponent::get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +{ + point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); + point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); + point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); +} + void ScanGroundFilterComponent::convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; - uint16_t back_steps_num = 1; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + const auto inv_grid_size_rad = 1.0f / grid_size_rad_; + const auto inv_grid_size_m = 1.0f / grid_size_m_; + + const auto grid_id_offset = + grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; + const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto x{ - in_cloud->points[i].x - vehicle_info_.wheel_base_m / 2.0f - - center_pcl_shift_}; // base on front wheel center - // auto y{in_cloud->points[i].y}; - auto radius{static_cast(std::hypot(x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(x, in_cloud->points[i].y), 0.0)}; + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto x{input_point.x - x_shift}; // base on front wheel center + auto radius{static_cast(std::hypot(x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; // divide by vertical angle - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; uint16_t grid_id = 0; - float curr_grid_size = 0.0f; if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius / grid_size_m_); - curr_grid_size = grid_size_m_; + grid_id = static_cast(radius * inv_grid_size_m); } else { - grid_id = grid_mode_switch_grid_id_ + (gamma - grid_mode_switch_angle_rad_) / grid_size_rad_; - if (grid_id <= grid_mode_switch_grid_id_ + back_steps_num) { - curr_grid_size = grid_size_m_; - } else { - curr_grid_size = std::tan(gamma) - std::tan(gamma - grid_size_rad_); - curr_grid_size *= virtual_lidar_z_; - } + auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset + gamma * inv_grid_size_rad; } current_point.grid_id = grid_id; - current_point.grid_size = curr_grid_size; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } + void ScanGroundFilterComponent::convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto radius{static_cast(std::hypot(in_cloud->points[i].x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(in_cloud->points[i].x, in_cloud->points[i].y), 0.0)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + // Point + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } - // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } @@ -173,6 +208,21 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) point.z = 0; } +inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) +{ + float grid_size = grid_size_m_; + uint16_t back_steps_num = 1; + + if ( + p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; +} + void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { @@ -193,96 +243,94 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; - float curr_gnd_slope_rad = 0.0f; + float curr_gnd_slope_ratio = 0.0f; float gnd_buff_z_mean = 0.0f; - float gnd_buff_z_max = 0.0f; float gnd_buff_radius = 0.0f; for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1; ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; - gnd_buff_z_max += it->max_height; } gnd_buff_radius /= static_cast(gnd_grid_buffer_size_ - 1); - gnd_buff_z_max /= static_cast(gnd_grid_buffer_size_ - 1); gnd_buff_z_mean /= static_cast(gnd_grid_buffer_size_ - 1); float tmp_delta_mean_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean; float tmp_delta_radius = gnd_grids_list.back().radius - gnd_buff_radius; - curr_gnd_slope_rad = std::atan(tmp_delta_mean_z / tmp_delta_radius); - curr_gnd_slope_rad = curr_gnd_slope_rad < -global_slope_max_angle_rad_ - ? -global_slope_max_angle_rad_ - : curr_gnd_slope_rad; - curr_gnd_slope_rad = curr_gnd_slope_rad > global_slope_max_angle_rad_ - ? global_slope_max_angle_rad_ - : curr_gnd_slope_rad; + curr_gnd_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; + curr_gnd_slope_ratio = curr_gnd_slope_ratio < -global_slope_max_ratio_ ? -global_slope_max_ratio_ + : curr_gnd_slope_ratio; + curr_gnd_slope_ratio = + curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = std::tan(curr_gnd_slope_rad) * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p.orig_point->z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; - float local_slope = std::atan(tmp_delta_mean_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p.orig_point->z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || - abs(local_slope) <= local_slope_max_angle_rad_) { + abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(local_slope_ratio) <= local_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (p.orig_point->z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_max_z = p.orig_point->z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( - abs(local_slope) < local_slope_max_angle_rad_ || + abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); - if (abs(local_slope) < global_slope_max_angle_rad_) { + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; + if (abs(local_slope_ratio) < global_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::recheckGroundCluster( PointsCentroid & gnd_cluster, const float non_ground_threshold, pcl::PointIndices & non_ground_indices) { const float min_gnd_height = gnd_cluster.getMinHeight(); - pcl::PointIndices gnd_indices = gnd_cluster.getIndices(); - std::vector height_list = gnd_cluster.getHeightList(); + const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); + const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } } + void ScanGroundFilterComponent::classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -299,42 +347,41 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // check the first point in ray auto * p = &in_radial_ordered_clouds[i][0]; - PointRef * prev_p; - prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point + 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; - - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - p = &in_radial_ordered_clouds[i][j]; - float global_slope_p = std::atan(p->orig_point->z / p->radius); + pcl::PointXYZ p_orig_point, prev_p_orig_point; + for (auto & point : in_radial_ordered_clouds[i]) { + prev_p = p; + 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); + float global_slope_ratio_p = p_orig_point.z / p->radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p->orig_point->x < low_priority_region_x_) { + if (p_orig_point.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_); + non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); } // classify first grid's point cloud if ( - !initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ && - p->orig_point->z > non_ground_height_threshold_local) { + !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && + p_orig_point.z > non_ground_height_threshold_local) { out_no_ground_indices.indices.push_back(p->orig_index); p->point_state = PointLabel::NON_GROUND; - prev_p = p; continue; } if ( - !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ && - abs(p->orig_point->z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(p_orig_point.z) < non_ground_height_threshold_local) { + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); p->point_state = PointLabel::GROUND; initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - prev_p = p; continue; } if (!initialized_first_gnd_grid) { - prev_p = p; continue; } @@ -366,53 +413,50 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( ground_cluster.initialize(); } // classify - if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) { + if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { p->point_state = PointLabel::OUT_OF_RANGE; - prev_p = p; continue; } - float points_xy_distance = std::hypot( - p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y); + float points_xy_distance_square = + (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + + (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); if ( prev_p->point_state == PointLabel::NON_GROUND && - points_xy_distance < split_points_distance_tolerance_ && - p->orig_point->z > prev_p->orig_point->z) { + points_xy_distance_square < split_points_distance_tolerance_square_ && + p_orig_point.z > prev_p_orig_point.z) { p->point_state = PointLabel::NON_GROUND; out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } - - if (global_slope_p > global_slope_max_angle_rad_) { + if (global_slope_ratio_p > global_slope_max_ratio_) { out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } // gnd grid is continuous, the last gnd grid is close uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; + float curr_grid_size = calcGridSize(*p); if ( p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkContinuousGndGrid(*p, gnd_grids); - - } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkDiscontinuousGndGrid(*p, gnd_grids); + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + } else if ( + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids); + checkBreakGndGrid(*p, p_orig_point, gnd_grids); } if (p->point_state == PointLabel::NON_GROUND) { out_no_ground_indices.indices.push_back(p->orig_index); } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); } - prev_p = p; } } } void ScanGroundFilterComponent::classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -430,16 +474,15 @@ void ScanGroundFilterComponent::classifyPointCloud( 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); + pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; // loop through each point in the radial div for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - const float global_slope_max_angle = global_slope_max_angle_rad_; const float local_slope_max_angle = local_slope_max_angle_rad_; + prev_p_orig_point = p_orig_point; auto * p = &in_radial_ordered_clouds[i][j]; - auto * p_prev = &in_radial_ordered_clouds[i][j - 1]; - + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); if (j == 0) { - bool is_front_side = (p->orig_point->x > virtual_ground_point.x); + bool is_front_side = (p_orig_point.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -449,22 +492,22 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(*p->orig_point, prev_gnd_point); + points_distance = calcDistance3d(p_orig_point, prev_gnd_point); } else { - points_distance = calcDistance3d(*p->orig_point, *p_prev->orig_point); + points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); } float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p->orig_point->z - prev_gnd_point.z; - float height_from_obj = p->orig_point->z - non_ground_cluster.getAverageHeight(); + float height_from_gnd = p_orig_point.z - prev_gnd_point.z; + float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope = std::atan2(p->orig_point->z, p->radius); + float global_slope_ratio = p_orig_point.z / p->radius; // check points which is far enough from previous point - if (global_slope > global_slope_max_angle) { + if (global_slope_ratio > global_slope_max_ratio_) { p->point_state = PointLabel::NON_GROUND; calculate_slope = false; } else if ( @@ -479,7 +522,7 @@ void ScanGroundFilterComponent::classifyPointCloud( calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p->orig_point->z - ground_cluster.getAverageHeight(); + height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { @@ -514,58 +557,66 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_point_label = p->point_state; if (p->point_state == PointLabel::GROUND) { prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p->orig_point->x, p->orig_point->y, p->orig_point->z); - ground_cluster.addPoint(p->radius, p->orig_point->z); + prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); + ground_cluster.addPoint(p->radius, p_orig_point.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p->orig_point->z); + non_ground_cluster.addPoint(p->radius, p_orig_point.z); } } } } void ScanGroundFilterComponent::extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr) + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud) { + size_t output_data_size = 0; for (const auto & i : in_indices.indices) { - out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]); + std::memcpy( + &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], + in_cloud_ptr->point_step * sizeof(uint8_t)); + *reinterpret_cast(&out_object_cloud.data[output_data_size + intensity_offset_]) = + 1; // set intensity to 1 + output_data_size += in_cloud_ptr->point_step; } } -void ScanGroundFilterComponent::filter( +void ScanGroundFilterComponent::faster_filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + PointCloud2 & output, + [[maybe_unused]] const pointcloud_preprocessor::TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input, *current_sensor_cloud_ptr); - std::vector radial_ordered_points; + if (!offset_initialized_) { + set_field_offsets(input); + } + std::vector radial_ordered_points; pcl::PointIndices no_ground_indices; - pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); - no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size()); if (elevation_grid_mode_) { - convertPointcloudGridScan(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloudGridScan(radial_ordered_points, no_ground_indices); + convertPointcloudGridScan(input, radial_ordered_points); + classifyPointCloudGridScan(input, radial_ordered_points, no_ground_indices); } else { - convertPointcloud(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloud(radial_ordered_points, no_ground_indices); + convertPointcloud(input, radial_ordered_points); + classifyPointCloud(input, radial_ordered_points, no_ground_indices); } - - extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr); - - auto no_ground_cloud_msg_ptr = std::make_shared(); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - - no_ground_cloud_msg_ptr->header = input->header; - output = *no_ground_cloud_msg_ptr; - + output.row_step = no_ground_indices.indices.size() * input->point_step; + output.data.resize(output.row_step); + output.width = no_ground_indices.indices.size(); + output.fields.assign(input->fields.begin(), input->fields.begin() + 3); + output.is_dense = true; + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + extractObjectPoints(input, no_ground_indices, output); if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); @@ -576,20 +627,62 @@ void ScanGroundFilterComponent::filter( } } +// TODO(taisa1): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. +void ScanGroundFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( const std::vector & p) { + if (get_param(p, "grid_size_m", grid_size_m_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } + if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting global_slope_max_angle_rad to: %f.", global_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting local_slope_max_angle_rad to: %f.", local_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { @@ -600,9 +693,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( get_logger(), "Setting split_points_distance_tolerance to: %f.", split_points_distance_tolerance_); + RCLCPP_DEBUG( + get_logger(), "Setting split_points_distance_tolerance_square to: %f.", + split_points_distance_tolerance_square_); } if (get_param(p, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 9df9c7e9c7433..444a38ea44754 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -131,7 +131,8 @@ class ScanGroundFilterTest : public ::testing::Test // wrapper function to test private function filter void filter(sensor_msgs::msg::PointCloud2 & out_cloud) { - scan_ground_filter_->filter(input_msg_ptr_, nullptr, out_cloud); + pointcloud_preprocessor::TransformInfo transform_info; + scan_ground_filter_->faster_filter(input_msg_ptr_, nullptr, out_cloud, transform_info); } void parse_yaml() diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 7ee5622414366..8fab65ef6b4d7 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -135,25 +135,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE lidar_centerpoint_node ) - ## single inference node ## - ament_auto_add_library(single_inference_lidar_centerpoint_component SHARED - src/single_inference_node.cpp - ) - - target_link_libraries(single_inference_lidar_centerpoint_component - centerpoint_lib - ) - - rclcpp_components_register_node(single_inference_lidar_centerpoint_component - PLUGIN "centerpoint::SingleInferenceLidarCenterPointNode" - EXECUTABLE single_inference_lidar_centerpoint_node - ) - - install(PROGRAMS - scripts/lidar_centerpoint_visualizer.py - DESTINATION lib/${PROJECT_NAME} - ) - ament_export_dependencies(ament_cmake_python) ament_auto_package( diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 2a9adedad9074..ed71349d5bd7f 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -68,18 +68,6 @@ You can download the onnx format of trained models by clicking on the links belo `Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. `Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. -## Standalone inference and visualization - -In addition to its use as a standard ROS node, `lidar_centerpoint` can also be used to perform inferences in an isolated manner. -To do so, execute the following launcher, where `pcd_path` is the path of the pointcloud to be used for inference. - -```bash -ros2 launch lidar_centerpoint single_inference_lidar_centerpoint.launch.xml pcd_path:=test_pointcloud.pcd detections_path:=test_detections.ply -``` - -`lidar_centerpoint` generates a `ply` file in the provided `detections_path`, which contains the detections as triangle meshes. -These detections can be visualized by most 3D tools, but we also integrate a visualization UI using `Open3D` which is launched alongside `lidar_centerpoint`. - ### Changelog #### v1 (2022/07/06) diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp deleted file mode 100644 index 45181d55ed46a..0000000000000 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp +++ /dev/null @@ -1,63 +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. - -#ifndef LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ -#define LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace centerpoint -{ - -class SingleInferenceLidarCenterPointNode : public rclcpp::Node -{ -public: - explicit SingleInferenceLidarCenterPointNode(const rclcpp::NodeOptions & node_options); - -private: - void detect(const std::string & pcd_path, const std::string & detections_path); - std::vector getVertices( - const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const; - void dumpDetectionsAsMesh( - const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, - const std::string & output_path) const; - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; - - float score_threshold_{0.0}; - std::vector class_names_; - bool rename_car_to_truck_and_bus_{false}; - bool has_twist_{false}; - - DetectionClassRemapper detection_class_remapper_; - - std::unique_ptr detector_ptr_{nullptr}; -}; - -} // namespace centerpoint - -#endif // LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md deleted file mode 100644 index 6a743b6e9d9c1..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md +++ /dev/null @@ -1,140 +0,0 @@ -# Run lidar_centerpoint and lidar_centerpoint-tiny simultaneously - -This tutorial is for showing `centerpoint` and `centerpoint_tiny`models’ results simultaneously, making it easier to visualize and compare the performance. - -## Setup Development Environment - -Follow the steps in the Source Installation ([link](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)) in Autoware doc. - -If you fail to build autoware environment according to lack of memory, then it is recommended to build autoware sequentially. - -Source the ROS 2 Galactic setup script. - -```bash -source /opt/ros/galactic/setup.bash -``` - -Build the entire autoware repository. - -```bash -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers=1 -``` - -Or you can use a constrained number of CPU to build only one package. - -```bash -export MAKEFLAGS="-j 4" && MAKE_JOBS=4 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1 --packages-select PACKAGE_NAME -``` - -Source the package. - -```bash -source install/setup.bash -``` - -## Data Preparation - -### Using rosbag dataset - -```bash -ros2 bag play /YOUR/ROSBAG/PATH/ --clock 100 -``` - -Don't forget to add `clock` in order to sync between two rviz display. - -You can also use the sample rosbag provided by autoware [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/). - -If you want to merge several rosbags into one, you can refer to [this tool](https://github.com/jerry73204/rosbag2-merge). - -### Using realtime LiDAR dataset - -Set up your Ethernet connection according to 1.1 - 1.3 in [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16). - -Download Velodyne ROS driver - -```bash -git clone -b ros2 https://github.com/ros-drivers/velodyne.git -``` - -Source the ROS 2 Galactic setup script. - -```bash -source /opt/ros/galactic/setup.bash -``` - -Compile Velodyne driver - -```bash -cd velodyne -rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -``` - -Edit the configuration file. Specify the LiDAR device IP address in `./velodyne_driver/config/VLP32C-velodyne_driver_node-params.yaml` - -```yaml -velodyne_driver_node: - ros__parameters: - device_ip: 192.168.1.201 //change to your LiDAR device IP address - gps_time: false - time_offset: 0.0 - enabled: true - read_once: false - read_fast: false - repeat_delay: 0.0 - frame_id: velodyne - model: 32C - rpm: 600.0 - port: 2368 -``` - -Launch the velodyne driver. - -```bash -# Terminal 1 -ros2 launch velodyne_driver velodyne_driver_node-VLP32C-launch.py -``` - -Launch the velodyne_pointcloud. - -```bash -# Terminal 2 -ros2 launch velodyne_pointcloud velodyne_convert_node-VLP32C-launch.py -``` - -Point Cloud data will be available on topic `/velodyne_points`. You can check with `ros2 topic echo /velodyne_points`. - -Check [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16) if there is any unexpected issue. - -## Launch file setting - -Several fields to check in `centerpoint_vs_centerpoint-tiny.launch.xml` before running lidar centerpoint. - -- `input/pointcloud` : set to the topic with input data you want to subscribe. -- `model_path` : set to the path of the model. -- `model_param_path` : set to the path of model's config file. - -## Run CenterPoint and CenterPoint-tiny simultaneously - -Run - -```bash -ros2 launch lidar_centerpoint centerpoint_vs_centerpoint-tiny.launch.xml -``` - -Then you will see two rviz window show immediately. On the left is the result for lidar centerpoint tiny, and on the right is the result for lidar centerpoint. - -![two rviz2 display centerpoint and centerpoint_tiny](https://github.com/autowarefoundation/autoware.universe/assets/58775300/2a89063a-8e0e-4f59-8d48-f339d4f7c0ff) - -## Troubleshooting - -### Bounding Box blink on rviz - -To avoid Bounding Boxes blinking on rviz, you can extend bbox marker lifetime. - -Set `marker_ptr->lifetime` and `marker.lifetime` to a longer lifetime. - -- `marker_ptr->lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp` -- `marker.lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp` - -Make sure to rebuild packages after any change. diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml deleted file mode 100644 index b9c056cfb5686..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz deleted file mode 100644 index 879837e101d47..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz +++ /dev/null @@ -1,1832 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Sensing1/LiDAR1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Perception1/ObjectRecognition1/Detection1 - - /ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.3668779730796814 - Tree Height: 434 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/SteeringAngle - Enabled: true - Left: 64 - Length: 128 - Name: SteeringAngle - Scale: 17 - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/steering_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Class: rviz_plugins/ConsoleMeter - Enabled: true - Left: 256 - Length: 128 - Name: ConsoleMeter - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Alpha: 0.9990000128746033 - Class: rviz_plugins/VelocityHistory - Color Border Vel Max: 3 - Constant Color: - Color: 255; 255; 255 - Value: true - Enabled: true - Name: VelocityHistory - Scale: 0.30000001192092896 - Timeout: 10 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Class: rviz_plugins/MaxVelocity - Enabled: true - Left: 298 - Length: 48 - Name: MaxVelocity - Text Color: 255; 255; 255 - Top: 140 - Topic: /planning/scenario_planning/current_max_velocity - Value: true - Value Scale: 0.25 - - Class: rviz_plugins/TurnSignal - Enabled: true - Height: 128 - Left: 98 - Name: TurnSignal - Top: 175 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/turn_indicators_status - Value: true - Width: 256 - Enabled: true - Name: Vehicle - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: false - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: false - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/temporary_merged_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: true - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: false - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/debug/rois - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: false - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Path: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate - Value: true - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: true - Value: true - Width: 2 - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Avoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: PullOver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift - Value: false - Enabled: false - Name: DebugMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: true - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: true - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: ModifiedGoal - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: false - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/lateral/predicted_trajectory - Value: true - View Path: - Alpha: 1 - Color: 255; 255; 255 - Constant Color: true - Value: true - Width: 0.05000000074505806 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/pure_pursuit/debug/markers - Value: false - Enabled: false - Name: Control - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.07999999821186066 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Name: centerpoint - Namespaces: - label: true - shape: true - twist: true - velocity: true - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /centerpoint/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/XYOrbit - Distance: 50.32217788696289 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6553983092308044 - Target Frame: base_link - Value: XYOrbit (rviz_default_plugins) - Yaw: 3.20540452003479 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: true - Displays: - collapsed: true - Height: 1028 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - Tool Properties: - collapsed: true - Views: - collapsed: true - Width: 928 - X: 992 - Y: 24 diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz deleted file mode 100644 index 539f89305e3c6..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz +++ /dev/null @@ -1,1833 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Sensing1/LiDAR1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Perception1/ObjectRecognition1/Detection1 - - /centerpoint_tiny1 - - /ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.3668779730796814 - Tree Height: 451 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/SteeringAngle - Enabled: true - Left: 64 - Length: 128 - Name: SteeringAngle - Scale: 17 - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/steering_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Class: rviz_plugins/ConsoleMeter - Enabled: true - Left: 256 - Length: 128 - Name: ConsoleMeter - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Alpha: 0.9990000128746033 - Class: rviz_plugins/VelocityHistory - Color Border Vel Max: 3 - Constant Color: - Color: 255; 255; 255 - Value: true - Enabled: true - Name: VelocityHistory - Scale: 0.30000001192092896 - Timeout: 10 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Class: rviz_plugins/MaxVelocity - Enabled: true - Left: 298 - Length: 48 - Name: MaxVelocity - Text Color: 255; 255; 255 - Top: 140 - Topic: /planning/scenario_planning/current_max_velocity - Value: true - Value Scale: 0.25 - - Class: rviz_plugins/TurnSignal - Enabled: true - Height: 128 - Left: 98 - Name: TurnSignal - Top: 175 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/turn_indicators_status - Value: true - Width: 256 - Enabled: true - Name: Vehicle - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: false - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: false - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/temporary_merged_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: true - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: false - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/debug/rois - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: false - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Path: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate - Value: true - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: true - Value: true - Width: 2 - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Avoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: PullOver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift - Value: false - Enabled: false - Name: DebugMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: true - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: true - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: ModifiedGoal - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: false - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/lateral/predicted_trajectory - Value: true - View Path: - Alpha: 1 - Color: 255; 255; 255 - Constant Color: true - Value: true - Width: 0.05000000074505806 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/pure_pursuit/debug/markers - Value: false - Enabled: false - Name: Control - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.07999999821186066 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Name: centerpoint_tiny - Namespaces: - label: true - shape: true - twist: true - velocity: true - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /centerpoint_tiny/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/XYOrbit - Distance: 50.32217788696289 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6553983092308044 - Target Frame: base_link - Value: XYOrbit (rviz_default_plugins) - Yaw: 3.20540452003479 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: true - Displays: - collapsed: true - Height: 1028 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - Tool Properties: - collapsed: true - Views: - collapsed: true - Width: 928 - X: 65 - Y: 24 diff --git a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml deleted file mode 100644 index 491abfbad7764..0000000000000 --- a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py deleted file mode 100755 index 5a1135379615f..0000000000000 --- a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python -# Copyright 2022 TIER IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import time - -import open3d as o3d -import rclpy -from rclpy.node import Node - - -def main(args=None): - rclpy.init(args=args) - - node = Node("lidar_centerpoint_visualizer") - node.declare_parameter("pcd_path", rclpy.Parameter.Type.STRING) - node.declare_parameter("detections_path", rclpy.Parameter.Type.STRING) - - pcd_path = node.get_parameter("pcd_path").get_parameter_value().string_value - detections_path = node.get_parameter("detections_path").get_parameter_value().string_value - - while not os.path.exists(pcd_path) and not os.path.exists(detections_path): - time.sleep(1.0) - - if not rclpy.ok(): - rclpy.shutdown() - return - - mesh = o3d.io.read_triangle_mesh(detections_path) - pcd = o3d.io.read_point_cloud(pcd_path) - - mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) - - detection_lines = o3d.geometry.LineSet.create_from_triangle_mesh(mesh) - detection_lines.paint_uniform_color([1.0, 0.0, 1.0]) - - o3d.visualization.draw_geometries([mesh_frame, pcd, detection_lines]) - - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/perception/lidar_centerpoint/src/single_inference_node.cpp b/perception/lidar_centerpoint/src/single_inference_node.cpp deleted file mode 100644 index 77099ad8226a2..0000000000000 --- a/perception/lidar_centerpoint/src/single_inference_node.cpp +++ /dev/null @@ -1,248 +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. - -#include "lidar_centerpoint/single_inference_node.hpp" - -#include -#include -#include -#include -#include - -#include - -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include - -namespace centerpoint -{ -SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( - const rclcpp::NodeOptions & node_options) -: Node("lidar_center_point", node_options), tf_buffer_(this->get_clock()) -{ - const float score_threshold = - static_cast(this->declare_parameter("score_threshold", 0.35)); - const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); - const auto yaw_norm_thresholds = - this->declare_parameter>("yaw_norm_thresholds"); - const std::string densification_world_frame_id = - this->declare_parameter("densification_world_frame_id", "map"); - const int densification_num_past_frames = - this->declare_parameter("densification_num_past_frames", 1); - const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); - const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); - const std::string encoder_engine_path = - this->declare_parameter("encoder_engine_path"); - const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); - const std::string head_engine_path = this->declare_parameter("head_engine_path"); - class_names_ = this->declare_parameter>("class_names"); - has_twist_ = this->declare_parameter("has_twist", false); - const std::size_t point_feature_size = - static_cast(this->declare_parameter("point_feature_size")); - const std::size_t max_voxel_size = - static_cast(this->declare_parameter("max_voxel_size")); - const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); - const auto voxel_size = this->declare_parameter>("voxel_size"); - const std::size_t downsample_factor = - static_cast(this->declare_parameter("downsample_factor")); - const std::size_t encoder_in_feature_size = - static_cast(this->declare_parameter("encoder_in_feature_size")); - const auto allow_remapping_by_area_matrix = - this->declare_parameter>("allow_remapping_by_area_matrix"); - const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); - const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - const auto pcd_path = this->declare_parameter("pcd_path"); - const auto detections_path = this->declare_parameter("detections_path"); - - detection_class_remapper_.setParameters( - allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); - - NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); - DensificationParam densification_param( - densification_world_frame_id, densification_num_past_frames); - - if (point_cloud_range.size() != 6) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("single_inference_lidar_centerpoint"), - "The size of point_cloud_range != 6: use the default parameters."); - } - if (voxel_size.size() != 3) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("single_inference_lidar_centerpoint"), - "The size of voxel_size != 3: use the default parameters."); - } - CenterPointConfig config( - class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_thresholds); - detector_ptr_ = - std::make_unique(encoder_param, head_param, densification_param, config); - - detect(pcd_path, detections_path); - exit(0); -} - -std::vector SingleInferenceLidarCenterPointNode::getVertices( - const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const -{ - std::vector vertices; - Eigen::Vector3d vertex; - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - return vertices; -} - -void SingleInferenceLidarCenterPointNode::detect( - const std::string & pcd_path, const std::string & detections_path) -{ - sensor_msgs::msg::PointCloud2 msg; - pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud()); - - pcl::io::loadPCDFile(pcd_path, *pc_ptr); - pcl::toROSMsg(*pc_ptr, msg); - msg.header.frame_id = "lidar_frame"; - - std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(msg, tf_buffer_, det_boxes3d); - if (!is_success) { - return; - } - - autoware_auto_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = msg.header; - for (const auto & box3d : det_boxes3d) { - autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - output_msg.objects.emplace_back(obj); - } - - detection_class_remapper_.mapClasses(output_msg); - - dumpDetectionsAsMesh(output_msg, detections_path); - - RCLCPP_INFO( - rclcpp::get_logger("single_inference_lidar_centerpoint"), - "The detection results were saved as meshes in %s", detections_path.c_str()); -} - -void SingleInferenceLidarCenterPointNode::dumpDetectionsAsMesh( - const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, - const std::string & output_path) const -{ - std::ofstream ofs(output_path, std::ofstream::out); - std::stringstream vertices_stream; - std::stringstream faces_stream; - int index = 0; - int num_detections = static_cast(objects_msg.objects.size()); - - ofs << "ply" << std::endl; - ofs << "format ascii 1.0" << std::endl; - ofs << "comment created by lidar_centerpoint" << std::endl; - ofs << "element vertex " << 8 * num_detections << std::endl; - ofs << "property float x" << std::endl; - ofs << "property float y" << std::endl; - ofs << "property float z" << std::endl; - ofs << "element face " << 12 * num_detections << std::endl; - ofs << "property list uchar uint vertex_indices" << std::endl; - ofs << "end_header" << std::endl; - - auto streamFace = [&faces_stream](int v1, int v2, int v3) { - faces_stream << std::to_string(3) << " " << std::to_string(v1) << " " << std::to_string(v2) - << " " << std::to_string(v3) << std::endl; - }; - - for (const auto & object : objects_msg.objects) { - Eigen::Affine3d pose_affine; - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, pose_affine); - - std::vector vertices = getVertices(object.shape, pose_affine); - - for (const auto & vertex : vertices) { - vertices_stream << vertex.x() << " " << vertex.y() << " " << vertex.z() << std::endl; - } - - streamFace(index + 1, index + 3, index + 4); - streamFace(index + 3, index + 5, index + 6); - streamFace(index + 0, index + 7, index + 5); - streamFace(index + 7, index + 2, index + 4); - streamFace(index + 5, index + 3, index + 1); - streamFace(index + 7, index + 0, index + 2); - streamFace(index + 2, index + 1, index + 4); - streamFace(index + 4, index + 3, index + 6); - streamFace(index + 5, index + 7, index + 6); - streamFace(index + 6, index + 7, index + 4); - streamFace(index + 0, index + 5, index + 1); - index += 8; - } - - ofs << vertices_stream.str(); - ofs << faces_stream.str(); - - ofs.close(); -} - -} // namespace centerpoint - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::SingleInferenceLidarCenterPointNode) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index a58e19db70304..7989a5fbf6af9 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -218,11 +218,13 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | -| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | -| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| `~/debug/processing_time_ms` | `std_msgs::msg::Float64` | processing time of this module. | +| `~/debug/cyclic_time_ms` | `std_msgs::msg::Float64` | cyclic time of this module. | ## Parameters diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 321342c3d8367..26dee3eacb08d 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include #include #include @@ -125,11 +125,14 @@ class MapBasedPredictionNode : public rclcpp::Node // ROS Publisher and Subscriber rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; - rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + // Object History std::unordered_map> objects_history_; std::map, rclcpp::Time> stopped_times_against_green_; @@ -196,9 +199,6 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; - // Stop watch - StopWatch stop_watch_; - // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 21697bb1b5f1a..549619950cf82 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -646,14 +646,6 @@ ObjectClassification::_label_type changeLabelForPrediction( } } -StringStamped createStringStamped(const rclcpp::Time & now, const double data) -{ - StringStamped msg; - msg.stamp = now; - msg.data = std::to_string(data); - return msg; -} - // NOTE: These two functions are copied from the route_handler package. lanelet::Lanelets getRightOppositeLanelets( const std::shared_ptr & lanelet_map_ptr, @@ -816,10 +808,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); - pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + processing_time_publisher_ = + std::make_unique(this, "map_based_prediction"); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); + + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( @@ -893,7 +890,7 @@ void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::Co void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { - stop_watch_.tic(); + stop_watch_ptr_->toc("processing_time", true); // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -1116,8 +1113,12 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); pub_debug_markers_->publish(debug_markers); - const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); - pub_calculation_time_->publish(calculation_time_msg); + const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); } bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 1d4d50c7eab4c..d955745bb8c47 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -148,7 +148,7 @@ void TrackerDebugger::publishProcessingTime() processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); processing_time_publisher_->publish( - "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3); } } diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 8a3a5629ec277..2d732db6bb709 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -140,6 +140,11 @@ BicycleTracker::BicycleTracker( } else { bounding_box_ = {1.0, 0.5, 1.7}; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + ekf_.init(X, P); // Set lf, lr @@ -400,10 +405,15 @@ bool BicycleTracker::measureWithShape( { // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bbox_object = object; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // convert cylinder to bbox + bbox_object.shape.dimensions.x = object.shape.dimensions.x; + bbox_object.shape.dimensions.y = object.shape.dimensions.x; + bbox_object.shape.dimensions.z = object.shape.dimensions.z; + } else { + utils::convertConvexHullToBoundingBox(object, bbox_object); } constexpr float gain = 0.9; @@ -415,6 +425,11 @@ bool BicycleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 80c0df7e5ffb1..8516e9a8d02c9 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -157,6 +157,11 @@ BigVehicleTracker::BigVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m @@ -455,6 +460,11 @@ bool BigVehicleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 7714c381894f0..18f73c8610f04 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -157,6 +157,11 @@ NormalVehicleTracker::NormalVehicleTracker( /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + // Set lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m @@ -455,6 +460,11 @@ bool NormalVehicleTracker::measureWithShape( last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + // update lf, lr lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 7562684b84220..b5b3bd5826e69 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -143,6 +143,13 @@ PedestrianTracker::PedestrianTracker( cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.3); + ekf_.init(X, P); } @@ -317,6 +324,13 @@ bool PedestrianTracker::measureWithShape( return false; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.3); + return true; } diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 8341ce490ca72..ba89a04553476 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -18,6 +18,8 @@ #include "object_merger/data_association/data_association.hpp" #include +#include +#include #include @@ -81,6 +83,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; + + // debug publisher + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 1ffc2791e8f4e..15b1939911394 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -118,6 +118,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Debug publisher + processing_time_publisher_ = + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void ObjectAssociationMergerNode::objectsCallback( @@ -128,6 +135,7 @@ void ObjectAssociationMergerNode::objectsCallback( if (merged_object_pub_->get_subscription_count() < 1) { return; } + stop_watch_ptr_->toc("processing_time", true); /* transform to base_link coordinate */ autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1; @@ -215,6 +223,11 @@ void ObjectAssociationMergerNode::objectsCallback( // publish output msg merged_object_pub_->publish(output_msg); + // publish processing time + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace object_association diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index b55755fb96f92..e976e342e3d9f 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -52,6 +52,12 @@ Autoware objects label is defined in [ObjectClassification.idl](https://gitlab.c ### Parameters +#### Parameter Summary + +{{ json_to_markdown("perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json") }} + +#### Parameter Description + - `update_rate_hz` (double) [hz] - Default parameter is 20.0 diff --git a/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json b/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json new file mode 100644 index 0000000000000..396dbcd413ee8 --- /dev/null +++ b/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json @@ -0,0 +1,57 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Tracks Msgs Converter node", + "type": "object", + "definitions": { + "radar_tracks_msgs_converter": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "description": "The update rate [hz] of the output topic", + "default": "20.0", + "minimum": 0.0 + }, + "new_frame_id": { + "type": "string", + "description": "The header frame_id of the output topic", + "default": "base_link" + }, + "use_twist_compensation": { + "type": "boolean", + "description": "Flag to enable the linear compensation of ego vehicle's twist", + "default": "false" + }, + "use_twist_yaw_compensation": { + "type": "boolean", + "description": "Flag to enable the compensation of yaw rotation of ego vehicle's twist", + "default": "false" + }, + "static_object_speed_threshold": { + "type": "number", + "description": "Threshold to treat detected objects as static objects", + "default": "1.0" + } + }, + "required": [ + "update_rate_hz", + "new_frame_id", + "use_twist_compensation", + "use_twist_yaw_compensation", + "static_object_speed_threshold" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/radar_tracks_msgs_converter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 624ca604d8fbf..22f31fffec21a 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -50,10 +50,17 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); + + processing_time_publisher_ = + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); // Guard if (pub_->get_subscription_count() < 1) { return; @@ -108,6 +115,11 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); + + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } #include diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 95ee4afe8d9e8..87df44f08f836 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -18,6 +18,8 @@ #include "shape_estimation/shape_estimator.hpp" #include +#include +#include #include #include @@ -33,6 +35,10 @@ class ShapeEstimationNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); std::unique_ptr estimator_; diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 0509fb2a07dc5..56843f4c5e1e0 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -20,6 +20,8 @@ #include "tracking_object_merger/utils/utils.hpp" #include +#include +#include #include #include @@ -87,6 +89,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b22b8d5fa2948..af0de2a02bbe7 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -151,6 +151,13 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("lidar-radar", data_association_map_); // radar-radar association matrix set3dDataAssociation("radar-radar", data_association_map_); + + // debug publisher + processing_time_publisher_ = + std::make_unique(this, "decorative_object_merger_node"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -182,6 +189,7 @@ void DecorativeTrackerMergerNode::set3dDataAssociation( void DecorativeTrackerMergerNode::mainObjectsCallback( const TrackedObjects::ConstSharedPtr & main_objects) { + stop_watch_ptr_->toc("processing_time", true); // try to merge sub object if (!sub_objects_buffer_.empty()) { // get interpolated sub objects @@ -214,6 +222,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( this->decorativeMerger(main_sensor_type_, main_objects); merged_object_pub_->publish(getTrackedObjects(main_objects->header)); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } /** diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index eed1f324727fb..384fd4a0d2fa3 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -321,10 +321,19 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; + using utils::traffic_light::calcDistanceToRedTrafficLight; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. constexpr double MARGIN = 10.0; - const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto forward_detection_range = [&]() { + const auto to_traffic_light = calcDistanceToRedTrafficLight( + data.current_lanelets, helper_->getPreviousReferencePath(), planner_data_); + if (!to_traffic_light.has_value()) { + return helper_->getForwardDetectionRange(); + } + return std::min(helper_->getForwardDetectionRange(), to_traffic_light.value()); + }(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, data, parameters_, forward_detection_range + MARGIN, debug); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 46b47de128bd9..35cc02e867557 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( const Pose & curent_pose, const double abort_return_dist, const LaneChangeParameters & lane_change_parameters, const Direction direction); -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length); - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2021630f1ae34..1e97017b24856 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -843,7 +843,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = utils::lane_change::getBackwardLanelets( + const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 48a5a8722b0e9..bae733610905b 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( return true; } -// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length) -{ - if (target_lanes.empty()) { - return {}; - } - - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); - - if (arc_length.length >= backward_length) { - return {}; - } - - const auto & front_lane = target_lanes.front(); - const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( - front_lane, std::abs(backward_length - arc_length.length), {front_lane}); - - lanelet::ConstLanelets backward_lanes{}; - const auto num_of_lanes = std::invoke([&preceding_lanes]() { - size_t sum{0}; - for (const auto & lanes : preceding_lanes) { - sum += lanes.size(); - } - return sum; - }); - - backward_lanes.reserve(num_of_lanes); - - for (const auto & lanes : preceding_lanes) { - backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); - } - - return backward_lanes; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index dcee7696933dd..22db593b45586 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane( lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length); + lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 6154d29855f7a..571a40859202f 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1481,6 +1481,10 @@ std::vector postProcess( const auto & vehicle_length = planner_data->parameters.vehicle_length; constexpr double overlap_threshold = 0.01; + if (original_bound.size() < 2) { + return original_bound; + } + const auto addPoints = [](const lanelet::ConstLineString3d & points, std::vector & bound) { for (const auto & bound_p : points) { diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 188c50993e4ae..ea9872f268678 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -78,7 +78,7 @@ PredictedObjects filterObjects( const std::shared_ptr & params) { // Guard - if (objects->objects.empty()) { + if (objects->objects.empty() || !params) { return PredictedObjects(); } diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 308cf9c4fed2c..6fcfad2209815 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath( return lanes; } +// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length) +{ + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto & front_lane = target_lanes.front(); + const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); + + lanelet::ConstLanelets backward_lanes{}; + const auto num_of_lanes = std::invoke([&preceding_lanes]() { + size_t sum{0}; + for (const auto & lanes : preceding_lanes) { + sum += lanes.size(); + } + return sum; + }); + + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : preceding_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); + } + + return backward_lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4b52aae91d032..ff0550e4d4867 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -222,9 +222,9 @@ This ordering is beneficial when the priority is to minimize the backward distan - **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) -- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles. +- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. -- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline. +- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and that the rear vehicle can pass through the gap between the ego vehicle and the lane border. ```plantuml @startuml @@ -235,7 +235,7 @@ if (Collision with dynamic objects detected?) then (yes) if (Before departure?) then (yes) :Deactivate module decision is registered; else (no) - if (Can stop within constraints \n && \n no crossing centerline?) then (yes) + if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes) :Stop; else (no) :Continue with caution; @@ -250,7 +250,7 @@ stop #### **example of safety check performed range for shift pull out** -Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled. +Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint does not leave enough space for a rear vehicle to drive through, the safety check against dynamic objects is disabled.
![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100} @@ -319,27 +319,28 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | -| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | -| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | -| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | -| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | -| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | -| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | -| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | -| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | +| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | +| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ### **Ego vehicle's velocity planning** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index fad29b84c9c76..b17db7f907471 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 + extra_width_margin_for_rear_obstacle: 0.5 th_moving_object_velocity: 1.0 object_types_to_check_for_path_generation: check_car: true diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 07c81d2edd050..1e32237ffd870 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -64,6 +64,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double extra_width_margin_for_rear_obstacle{0.0}; std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 1368124929367..14846a05a0929 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -70,8 +70,9 @@ struct PullOutStatus bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; std::optional stop_pose{std::nullopt}; - //! record the first time when the state changed from !isActivated() to isActivated() - std::optional first_engaged_time{std::nullopt}; + //! record the first time when ego started forward-driving (maybe after backward driving + //! completion) in AUTONOMOUS operation mode + std::optional first_engaged_and_driving_forward_time{std::nullopt}; PullOutStatus() {} }; @@ -207,7 +208,8 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; - bool isOverlapWithCenterLane() const; + bool isPreventingRearVehicleFromPassingThrough() const; + bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; @@ -278,7 +280,7 @@ class StartPlannerModule : public SceneModuleInterface const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; - bool needToPrepareBlinkerBeforeStart() const; + bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index beeb675efd3ae..050d591128a15 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_margin_from_front_object = @@ -371,6 +373,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam( + parameters, ns + "extra_width_margin_for_rear_obstacle", + p->extra_width_margin_for_rear_obstacle); + updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index f5674cfb288d0..274fe19e04618 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -110,7 +110,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); - + if (cropped_path.points.empty()) continue; shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 3c08d62f500ae..bb47182d953cc 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -39,6 +39,7 @@ #include using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -118,7 +119,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { updateData(); - if (!isActivated() || needToPrepareBlinkerBeforeStart()) { + if (!isActivated() || needToPrepareBlinkerBeforeStartDrivingForward()) { return planWaitingApproval(); } @@ -178,8 +179,8 @@ void StartPlannerModule::updateData() if ( planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && - !status_.first_engaged_time) { - status_.first_engaged_time = clock_->now(); + status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { + status_.first_engaged_and_driving_forward_time = clock_->now(); } status_.backward_driving_complete = hasFinishedBackwardDriving(); @@ -220,7 +221,7 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - !isOverlapWithCenterLane(); + !isPreventingRearVehicleFromPassingThrough(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -287,35 +288,140 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } -bool StartPlannerModule::isOverlapWithCenterLane() const +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); - for (const auto & current_lane : current_lanes) { - std::vector centerline_points; - for (const auto & point : current_lane.centerline()) { - geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); - centerline_points.push_back(center_point); - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - for (size_t i = 0; i < centerline_points.size() - 1; ++i) { - const auto & p1 = centerline_points.at(i); - const auto & p2 = centerline_points.at(i + 1); - for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { - const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); - const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); - const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + const auto target_lanes = utils::getCurrentLanes(planner_data_); + if (target_lanes.empty()) return false; + + // Define functions to get distance between a point and a lane's boundaries. + auto calc_absolute_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + std::vector boundary_path; + std::for_each( + boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { + const double x = boundary_point.x(); + const double y = boundary_point.y(); + boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + }); + + return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); + }; - if (intersection.has_value()) { - return true; - } + // Check from what side of the road the ego is merging + const auto centerline_path = + route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + const auto start_pose_nearest_segment_index = + motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); + if (!start_pose_nearest_segment_index) return false; + + const auto start_pose_point_msg = tier4_autoware_utils::createPoint( + start_pose.position.x, start_pose.position.y, start_pose.position.z); + const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( + centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); + if (std::isnan(starting_pose_lateral_offset)) return false; + + RCLCPP_DEBUG(getLogger(), "starting pose lateral offset: %f", starting_pose_lateral_offset); + const bool ego_is_merging_from_the_left = (starting_pose_lateral_offset > 0.0); + + // Get the ego's overhang point closest to the centerline path and the gap between said point and + // the lane's border. + auto get_gap_between_ego_and_lane_border = + [&]( + geometry_msgs::msg::Pose & ego_overhang_point_as_pose, + const bool ego_is_merging_from_the_left) -> std::optional { + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); + for (const auto & point : vehicle_footprint) { + geometry_msgs::msg::Pose point_pose; + point_pose.position.x = point.x(); + point_pose.position.y = point.y(); + point_pose.position.z = 0.0; + + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + + const lanelet::ConstLineString2d current_lane_bound = (ego_is_merging_from_the_left) + ? closest_lanelet_const.rightBound2d() + : closest_lanelet_const.leftBound2d(); + const double current_point_lateral_gap = + calc_absolute_lateral_offset(current_lane_bound, point_pose); + if (current_point_lateral_gap < smallest_lateral_gap_between_ego_and_border) { + smallest_lateral_gap_between_ego_and_border = current_point_lateral_gap; + ego_overhang_point_as_pose.position.x = point.x(); + ego_overhang_point_as_pose.position.y = point.y(); + ego_overhang_point_as_pose.position.z = 0.0; } } - } - return false; + if (smallest_lateral_gap_between_ego_and_border == std::numeric_limits::max()) { + return std::nullopt; + } + return smallest_lateral_gap_between_ego_and_border; + }; + + geometry_msgs::msg::Pose ego_overhang_point_as_pose; + const auto gap_between_ego_and_lane_border = + get_gap_between_ego_and_lane_border(ego_overhang_point_as_pose, ego_is_merging_from_the_left); + if (!gap_between_ego_and_lane_border) return false; + + // Get the lanelets that will be queried for target objects + const auto relevant_lanelets = std::invoke([&]() -> std::optional { + lanelet::Lanelet closest_lanelet; + const bool is_closest_lanelet = lanelet::utils::query::getClosestLanelet( + target_lanes, ego_overhang_point_as_pose, &closest_lanelet); + if (!is_closest_lanelet) return std::nullopt; + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + // Check backwards just in case the Vehicle behind ego is in a different lanelet + constexpr double backwards_length = 200.0; + const auto prev_lanes = behavior_path_planner::utils::getBackwardLanelets( + *route_handler, target_lanes, current_pose, backwards_length); + // return all the relevant lanelets + lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; + relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); + return relevant_lanelets; + }); + if (!relevant_lanelets) return false; + + // filtering objects with velocity, position and class + const auto filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, + objects_filtering_params_); + if (filtered_objects.objects.empty()) return false; + + // filtering objects based on the current position's lane + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + relevant_lanelets.value(), route_handler, filtered_objects, objects_filtering_params_); + if (target_objects_on_lane.on_current_lane.empty()) return false; + + // Get the closest target obj width in the relevant lanes + const auto closest_object_width = std::invoke([&]() -> std::optional { + double arc_length_to_closet_object = std::numeric_limits::max(); + double closest_object_width = -1.0; + std::for_each( + target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), + [&](const auto & o) { + const auto arc_length = motion_utils::calcSignedArcLength( + centerline_path.points, current_pose.position, o.initial_pose.pose.position); + if (arc_length > 0.0) return; + if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; + arc_length_to_closet_object = arc_length; + closest_object_width = o.shape.dimensions.y; + }); + if (closest_object_width < 0.0) return std::nullopt; + return closest_object_width; + }); + if (!closest_object_width) return false; + // Decide if the closest object does not fit in the gap left by the ego vehicle. + return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > + gap_between_ego_and_lane_border.value(); } bool StartPlannerModule::isCloseToOriginalStartPose() const @@ -1073,13 +1179,15 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const +bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const { - if (!status_.first_engaged_time) { - return true; + if (!status_.first_engaged_and_driving_forward_time) { + return false; } - const auto first_engaged_time = status_.first_engaged_time.value(); - const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds(); + const auto first_engaged_and_driving_forward_time = + status_.first_engaged_and_driving_forward_time.value(); + const double elapsed = + rclcpp::Duration(clock_->now() - first_engaged_and_driving_forward_time).seconds(); return elapsed < parameters_->prepare_time_before_start; } @@ -1148,14 +1256,14 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; - } + turn_signal.turn_signal.command = std::invoke([&]() { + if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE; + if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset) + return TurnIndicatorsCommand::ENABLE_RIGHT; + if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) + return TurnIndicatorsCommand::ENABLE_LEFT; + return TurnIndicatorsCommand::DISABLE; + }); turn_signal.desired_start_point = start_pose; turn_signal.required_start_point = start_pose; @@ -1375,7 +1483,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - break; + return; } default: { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1389,7 +1497,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons ? utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) : current_drivable_area_info; - break; + return; } } } diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index b323cdf4cb5da..bd2ad1935406b 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -389,6 +389,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_triangle) { + const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value(); + const auto color = debug_data_.static_occlusion ? green : red; + geometry_msgs::msg::Polygon poly; + poly.points.push_back( + geometry_msgs::build().x(p1.x).y(p1.y).z(p1.z)); + poly.points.push_back( + geometry_msgs::build().x(p2.x).y(p2.y).z(p2.z)); + poly.points.push_back( + geometry_msgs::build().x(p3.x).y(p3.y).z(p3.z)); + appendMarkerArray( + debug::createPolygonMarkerArray( + poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color), + std::get<1>(color), std::get<2>(color)), + &debug_marker_array, now); + } if (debug_data_.traffic_light_observation) { const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8917b4bac579b..fcb1ef3a1985a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -235,6 +235,10 @@ class IntersectionModule : public SceneModuleInterface std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + std::optional< + std::tuple> + nearest_occlusion_triangle{std::nullopt}; + bool static_occlusion{false}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; std::optional> diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 00ac333adeff1..2a66aedd36ac7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -334,13 +334,14 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( } return nearest; }; - struct NearestOcclusionPoint + struct NearestOcclusionInterval { int64 division_index{0}; int64 point_index{0}; double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; + geometry_msgs::msg::Point visible_end; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { @@ -370,6 +371,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( continue; } double acc_dist = 0.0; + bool found_min_dist_for_this_division = false; + bool is_prev_occluded = false; auto acc_dist_it = projection_it; for (auto point_it = projection_it; point_it != division.end(); point_it++) { const double dist = @@ -386,11 +389,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, + division_index, + std::distance(division.begin(), point_it), + acc_dist, tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + tier4_autoware_utils::createPoint( + projection_it->x(), projection_it->y(), + origin.z) /* initialize with projection point at first*/}; + found_min_dist_for_this_division = true; + } else if (found_min_dist_for_this_division && is_prev_occluded) { + // although this cell is not "nearest" cell, we have found the "nearest" cell on this + // division previously in this iteration, and the iterated cells are still OCCLUDED since + // then + nearest_occlusion_point.visible_end = + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } + is_prev_occluded = (pixel == OCCLUDED); } } @@ -400,16 +416,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - LineString2d ego_occlusion_line; - ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); - ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + debug_data_.nearest_occlusion_triangle = std::make_tuple( + current_pose.position, nearest_occlusion_point.point, nearest_occlusion_point.visible_end); + Polygon2d ego_occlusion_triangle; + ego_occlusion_triangle.outer().emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y); + bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); - if (bg::intersects(obj_poly, ego_occlusion_line)) { + if (bg::intersects(obj_poly, ego_occlusion_triangle)) { + debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; } } + debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index 092f1d32b27b3..1b483b95510ed 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -18,6 +18,7 @@ # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index 43dea2f2153ff..b61441132969c 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -27,6 +27,7 @@ tf2 tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index be72109db4548..e948bd74eba45 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -73,11 +73,13 @@ std::optional> object_time_to_range( if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; + const auto max_lon_deviation = object.shape.dimensions.x / 2.0; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + if (unique_path_points.size() < 2) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -121,7 +123,17 @@ std::optional> object_time_to_range( max_deviation); continue; } - // else we rely on the interpolation to estimate beyond the end of the predicted path + const auto is_last_predicted_path_point = + (exit_segment_idx + 2 == unique_path_points.size() || + enter_segment_idx + 2 == unique_path_points.size()); + const auto does_not_reach_overlap = + is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); + if (does_not_reach_overlap) { + RCLCPP_DEBUG( + logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", + std::min(exit_offset, enter_offset), max_lon_deviation); + continue; + } const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..4a76ef2078d63 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,139 @@ +// Copyright 2023-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 "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + auto stop_line_idx = 0UL; + bool found = false; + lanelet::BasicSegment2d path_segment; + path_segment.first.x() = predicted_path.path.front().position.x; + path_segment.first.y() = predicted_path.path.front().position.y; + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; + if (boost::geometry::intersects(stop_line, path_segment)) { + found = true; + break; + } + path_segment.first = path_segment.second; + } + if (found) { + auto cut_idx = stop_line_idx; + double arc_length = 0; + while (cut_idx > 0 && arc_length < object_front_overhang) { + arc_length += tier4_autoware_utils::calcDistance2d( + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); + --cut_idx; + } + predicted_path.path.resize(cut_idx); + } +} + +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) +{ + lanelet::ConstLanelets lanelets; + lanelet::BasicLineString2d query_line; + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); + const auto query_results = lanelet::geometry::findWithin2d( + planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line); + for (const auto & r : query_results) lanelets.push_back(r.second); + for (const auto & ll : lanelets) { + for (const auto & element : ll.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id()); + if ( + traffic_signal_stamped.has_value() && element->stopLine().has_value() && + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { + lanelet::BasicLineString2d stop_line; + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); + return stop_line; + } + } + } + return std::nullopt; +} + +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const PlannerData & planner_data, const double object_front_overhang) +{ + const auto stop_line = find_next_stop_line(predicted_path, planner_data); + if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang); +} + +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = planner_data.predicted_objects->header; + for (const auto & object : planner_data.predicted_objects->objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + if (no_overlap_path.size() <= 1) return true; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, planner_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); + } + + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 7bad4974ea9af..bb6b5f4d00005 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023-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. @@ -17,56 +17,41 @@ #include "types.hpp" -#include +#include -#include +#include namespace behavior_velocity_planner::out_of_lane { +/// @brief cut a predicted path beyond the given stop line +/// @param [inout] predicted_path predicted path to cut +/// @param [in] stop_line stop line used for cutting +/// @param [in] object_front_overhang extra distance to cut ahead of the stop line +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); + +/// @brief find the next red light stop line along the given path +/// @param [in] path predicted path to check for a stop line +/// @param [in] planner_data planner data with stop line information +/// @return the first red light stop line found along the path (if any) +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); + +/// @brief cut predicted path beyond stop lines of red lights +/// @param [inout] predicted_path predicted path to cut +/// @param [in] planner_data planner data to get the map and traffic light information +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const PlannerData & planner_data, const double object_front_overhang); + /// @brief filter predicted objects and their predicted paths -/// @param [in] objects predicted objects to filter +/// @param [in] planner_data planner data /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, - const PlannerParam & params) -{ - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = objects.header; - for (const auto & object : objects.objects) { - const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); - if (is_pedestrian) continue; - - auto filtered_object = object; - const auto is_invalid_predicted_path = [&](const auto & predicted_path) { - const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); - if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = - std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); - const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); - return is_low_confidence || is_crossing_ego; - }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - } - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) - filtered_objects.objects.push_back(filtered_object); - } - return filtered_objects; -} - + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 23de809e429ec..8055f5c9106ef 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -51,6 +51,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.objects_min_confidence = getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); + pp.objects_cut_predicted_paths_beyond_red_lights = + getOrDeclareParameter(node, ns + ".objects.cut_predicted_paths_beyond_red_lights"); pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index ef4434e235244..47020fb1cb623 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -109,13 +109,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); // Calculate stop and slowdown points - stopwatch.tic("calculate_decisions"); DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); + stopwatch.tic("filter_predicted_objects"); + inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_); + const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; + stopwatch.tic("calculate_decisions"); const auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); @@ -157,7 +159,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } if (point_to_insert) { prev_inserted_point_ = point_to_insert; - RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); + RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); debug_data_.slowdowns = {*point_to_insert}; auto path_idx = motion_utils::findNearestSegmentIndex( path->points, point_to_insert->point.point.pose.position) + @@ -187,12 +189,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto "\tcalculate_lanelets = %2.0fus\n" "\tcalculate_path_footprints = %2.0fus\n" "\tcalculate_overlapping_ranges = %2.0fus\n" + "\tfilter_pred_objects = %2.0fus\n" "\tcalculate_decisions = %2.0fus\n" "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", total_time_us, calculate_lanelets_us, calculate_path_footprints_us, - calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us, - insert_slowdown_points_us); + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); return true; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 34bd52634ce7b..d482870e5ce18 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -47,8 +47,10 @@ struct PlannerParam double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range bool objects_use_predicted_paths; // whether to use the objects' predicted paths - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path + bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red + // lights' stop lines + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in // the other lane diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f7d300d496e13..02a48580e0301 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -643,12 +643,22 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( return lanelet_sequence; } - lanelet::ConstLanelets lanelet_sequence_forward = - getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + const lanelet::ConstLanelets lanelet_sequence_forward = std::invoke([&]() { + if (only_route_lanes) { + return getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) { + return getShoulderLaneletSequenceAfter(lanelet, forward_distance); + } + return lanelet::ConstLanelets{}; + }); const lanelet::ConstLanelets lanelet_sequence_backward = 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); + if (only_route_lanes) { + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) { + return getShoulderLaneletSequenceUpTo(lanelet, backward_distance); + } } return lanelet::ConstLanelets{}; }); diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index eac423f94fa78..10f192696aabb 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - gyro_bias_threshold: 0.0015 # [rad/s] + gyro_bias_threshold: 0.003 # [rad/s] timer_callback_interval_sec: 0.5 # [sec] diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index f6407c532b5f8..6a26014b3e014 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -145,10 +145,10 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Subscribers { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); for (auto & input_topic : input_topics_) { - RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); } // Subscribe to the filters diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d5843be395adf..c4364ce1b06e3 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -77,7 +77,7 @@ pointcloud_preprocessor::Filter::Filter( latched_indices_ = static_cast(declare_parameter("latched_indices", false)); approximate_sync_ = static_cast(declare_parameter("approximate_sync", false)); - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( this->get_logger(), "Filter (as Component) successfully created with the following parameters:" << std::endl @@ -125,7 +125,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. std::set supported_nodes = { - "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index f8c6f9685f19a..f056892849fbc 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -59,7 +59,7 @@ topic_type: geometry_msgs/msg/PoseWithCovarianceStamped best_effort: false transient_local: false - warn_rate: 5.0 + warn_rate: 2.0 error_rate: 1.0 timeout: 1.0 diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index e10e31b5d3351..5c76d96e573ab 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -131,6 +131,8 @@ class EmergencyHandler : public rclcpp::Node // Heartbeat rclcpp::Time stamp_hazard_status_; + bool is_hazard_status_timeout_; + void checkHazardStatusTimeout(); // Algorithm void transitionTo(const int new_state); @@ -138,7 +140,7 @@ class EmergencyHandler : public rclcpp::Node void operateMrm(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); - bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); + bool isEmergency(); }; #endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index d591abad8c8ed..db3ad40249bfb 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -83,6 +83,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + is_hazard_status_timeout_ = false; // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -135,7 +136,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz HazardLightsCommand msg; // Check emergency - const bool is_emergency = isEmergency(hazard_status_stamped_->status); + const bool is_emergency = isEmergency(); if (hazard_status_stamped_->status.emergency_holding) { // turn hazard on during emergency holding @@ -309,22 +310,27 @@ bool EmergencyHandler::isDataReady() return true; } -void EmergencyHandler::onTimer() +void EmergencyHandler::checkHazardStatusTimeout() { - if (!isDataReady()) { - return; - } - const bool is_hazard_status_timeout = - (this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status; - if (is_hazard_status_timeout) { + if ((this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status) { + is_hazard_status_timeout_ = true; RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "heartbeat_hazard_status is timeout"); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; - publishControlCommands(); + } else { + is_hazard_status_timeout_ = false; + } +} + +void EmergencyHandler::onTimer() +{ + if (!isDataReady()) { return; } + // Check whether heartbeat hazard_status is timeout + checkHazardStatusTimeout(); + // Update Emergency State updateMrmState(); @@ -369,7 +375,7 @@ void EmergencyHandler::updateMrmState() using autoware_auto_vehicle_msgs::msg::ControlModeReport; // Check emergency - const bool is_emergency = isEmergency(hazard_status_stamped_->status); + const bool is_emergency = isEmergency(); // Get mode const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; @@ -412,7 +418,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre using autoware_auto_system_msgs::msg::HazardStatus; // Get hazard level - const auto level = hazard_status_stamped_->status.level; + auto level = hazard_status_stamped_->status.level; + if (is_hazard_status_timeout_) { + level = HazardStatus::SINGLE_POINT_FAULT; + } // State machine if (mrm_state_.behavior == MrmState::NONE) { @@ -435,10 +444,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre return mrm_state_.behavior; } -bool EmergencyHandler::isEmergency( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) +bool EmergencyHandler::isEmergency() { - return hazard_status.emergency || hazard_status.emergency_holding; + return hazard_status_stamped_->status.emergency || + hazard_status_stamped_->status.emergency_holding || is_hazard_status_timeout_; } bool EmergencyHandler::isStopped() diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index a0295fb07627c..e82ee36a7825a 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -4,6 +4,8 @@ ros__parameters: update_rate: 10 timeout_operation_mode_availability: 0.5 + timeout_call_mrm_behavior: 0.01 + timeout_cancel_mrm_behavior: 0.01 use_emergency_holding: false timeout_emergency_recovery: 5.0 use_parking_after_stopped: false diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg index 13a2956b6f15c..dbcc13918feff 100644 --- a/system/mrm_handler/image/mrm-state.svg +++ b/system/mrm_handler/image/mrm-state.svg @@ -8,302 +8,300 @@ width="958px" height="341px" viewBox="-0.5 -0.5 958 341" - content="<mxfile host="app.diagrams.net" modified="2024-02-07T09:48:58.193Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/118.0.0.0 Safari/537.36" etag="JzfBIbSx-UshJtjtpS4o" version="23.1.1" type="device"> <diagram id="C5RBs43oDa-KdzZeNtuy" name="Page-1"> <mxGraphModel dx="1364" dy="771" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="827" math="0" shadow="0"> <root> <mxCell id="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="WIyWlLk6GJQsqaUBKTNV-1" parent="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="6FWRLcRsoSDiZ6vq709X-284" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-281" target="6FWRLcRsoSDiZ6vq709X-3"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="230" y="360" /> <mxPoint x="230" y="360" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-285" value="not emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-284"> <mxGeometry x="0.02" relative="1" as="geometry"> <mxPoint x="1" y="-10" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-281" value="" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="280" y="140" width="720" height="340" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-3"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-2" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="50" y="305" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-19" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-3" target="6FWRLcRsoSDiZ6vq709X-281"> <mxGeometry relative="1" as="geometry"> <mxPoint x="270" y="290" as="targetPoint" /> <Array as="points"> <mxPoint x="260" y="290" /> <mxPoint x="260" y="290" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-21" value="emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-19"> <mxGeometry x="-0.0719" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-3" value="&lt;b&gt;NORMAL&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="120" y="260" width="60" height="120" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-274" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-272"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="260" /> <mxPoint x="810" y="260" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-275" value="succeeded" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-274"> <mxGeometry x="-0.0667" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-278" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-273"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="400" /> <mxPoint x="810" y="400" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-279" value="failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-278"> <mxGeometry x="0.2222" y="-3" relative="1" as="geometry"> <mxPoint x="-15" y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-5" value="MRM_OPERATING" style="swimlane;whiteSpace=wrap;html=1;gradientColor=none;swimlaneFillColor=default;fillColor=default;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="310" y="160" width="460" height="300" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-23" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-31" value="pull over unavailable&lt;br&gt;comfortable_stop available (ca)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-23"> <mxGeometry x="-0.12" y="-1" relative="1" as="geometry"> <mxPoint x="9" y="-24" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-24" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-13"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="80" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-26" value="pull over available (pa)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-24"> <mxGeometry x="0.1529" y="-1" relative="1" as="geometry"> <mxPoint x="13" y="-11" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-25" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-33" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop unavailable&lt;br&gt;emergency_stop available (ea)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-25"> <mxGeometry x="0.3217" relative="1" as="geometry"> <mxPoint x="-4" y="-26" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-12" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="20" y="143.75" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-27" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="420" y="80" /> <mxPoint x="420" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-13" value="pull_over" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="60" width="150" height="40" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-14" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-152" value="ea" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-28"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="9" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-156" value="ea" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-28"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="116" y="-36" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-15" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-13"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-17" value="emergency_stop" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="220" width="150" height="37.5" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-32" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry"> <mxPoint x="585" y="260" as="sourcePoint" /> <mxPoint x="585" y="380" as="targetPoint" /> <Array as="points" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-150" value="ca" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-32"> <mxGeometry x="-0.1" y="1" relative="1" as="geometry"> <mxPoint x="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-14" value="comfortable_stop" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="140" width="150" height="37.5" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-18" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=1;entryDx=0;entryDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-2"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-276" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-272" target="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="780" y="220" /> <mxPoint x="780" y="220" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-277" value="pa" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-276"> <mxGeometry x="-0.1556" y="-2" relative="1" as="geometry"> <mxPoint x="-6" y="-8" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-272" value="&lt;b&gt;MRM_SUCCEEDED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="850" y="200" width="130" height="80" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-273" value="&lt;b&gt;MRM_FAILED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="850" y="350" width="130" height="80" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + content="<mxfile host="app.diagrams.net" modified="2024-03-01T14:04:38.320Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/122.0.0.0 Safari/537.36" etag="jomwCx6bxG3LysB-HIk4" version="23.1.4" type="device"> <diagram id="C5RBs43oDa-KdzZeNtuy" name="Page-1"> <mxGraphModel dx="1364" dy="759" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="827" math="0" shadow="0"> <root> <mxCell id="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="WIyWlLk6GJQsqaUBKTNV-1" parent="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="6FWRLcRsoSDiZ6vq709X-281" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="280" y="140" width="720" height="340" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-5" value="MRM_OPERATING" style="swimlane;whiteSpace=wrap;html=1;gradientColor=none;swimlaneFillColor=default;fillColor=default;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="310" y="160" width="460" height="300" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-24" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="80" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-26" value="pull over available (pa)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-24" vertex="1" connectable="0"> <mxGeometry x="0.1529" y="-1" relative="1" as="geometry"> <mxPoint x="13" y="-11" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-25" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="250" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-33" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop unavailable&lt;br&gt;emergency_stop available (ea)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-25" vertex="1" connectable="0"> <mxGeometry x="0.3217" relative="1" as="geometry"> <mxPoint x="-4" y="-26" as="offset" /> </mxGeometry> </mxCell> <mxCell id="0iophzwyXz64g-DfP8aO-1" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="0iophzwyXz64g-DfP8aO-2" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop available (ca)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="0iophzwyXz64g-DfP8aO-1"> <mxGeometry x="-0.1263" y="-4" relative="1" as="geometry"> <mxPoint x="7" y="-24" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-12" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="20" y="145" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="420" y="80" /> <mxPoint x="420" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-13" value="pull_over" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="40" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-14" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="290" y="200" /> <mxPoint x="290" y="200" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-152" value="ea || failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-28" vertex="1" connectable="0"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="31" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-156" value="ea || failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-28" vertex="1" connectable="0"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="131" y="-36" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-15" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-17" value="emergency_stop" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="220" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-32" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="585" y="260" as="sourcePoint" /> <mxPoint x="585" y="380" as="targetPoint" /> <Array as="points"> <mxPoint x="290" y="110" /> <mxPoint x="290" y="110" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-150" value="ca" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-32" vertex="1" connectable="0"> <mxGeometry x="-0.1" y="1" relative="1" as="geometry"> <mxPoint x="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-14" value="comfortable_stop" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="130" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-284" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-281" target="6FWRLcRsoSDiZ6vq709X-3" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="230" y="360" /> <mxPoint x="230" y="360" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-285" value="not emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-284" vertex="1" connectable="0"> <mxGeometry x="0.02" relative="1" as="geometry"> <mxPoint x="1" y="-10" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-3" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-2" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="50" y="305" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-19" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-3" target="6FWRLcRsoSDiZ6vq709X-281" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="270" y="290" as="targetPoint" /> <Array as="points"> <mxPoint x="260" y="290" /> <mxPoint x="260" y="290" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-21" value="emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-19" vertex="1" connectable="0"> <mxGeometry x="-0.0719" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-3" value="&lt;b&gt;NORMAL&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="120" y="260" width="60" height="120" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-274" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-272" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="260" /> <mxPoint x="810" y="260" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-275" value="succeeded" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-274" vertex="1" connectable="0"> <mxGeometry x="-0.0667" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-278" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-17" target="6FWRLcRsoSDiZ6vq709X-273" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="740" y="414" /> <mxPoint x="740" y="414" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-279" value="failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-278" vertex="1" connectable="0"> <mxGeometry x="0.2222" y="-3" relative="1" as="geometry"> <mxPoint x="8" y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-18" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=1;entryDx=0;entryDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-2" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-276" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-272" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="780" y="220" /> <mxPoint x="780" y="220" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-277" value="pa" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-276" vertex="1" connectable="0"> <mxGeometry x="-0.1556" y="-2" relative="1" as="geometry"> <mxPoint x="13" y="-8" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-272" value="&lt;b&gt;MRM_SUCCEEDED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="850" y="200" width="130" height="80" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-273" value="&lt;b&gt;MRM_FAILED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="850" y="370" width="130" height="80" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " > - - + + + + -
-
+
+
- not emergency + MRM_OPERATING
- not emergency + MRM_OPERATING - - - - - - + + -
+
- emergency -
-
-
- - emergency - - - - - - -
-
-
- NORMAL + pull over available (pa)
- NORMAL + pull over available (pa) - - + + -
+
- succeeded + pull over unavailable +
+ comfortable_stop unavailable +
+ emergency_stop available (ea)
- succeeded + pull over unavailable... - - + + -
+
- failed + pull over unavailable +
+ comfortable_stop available (ca)
- failed + pull over unavailable... - - - + + + +
-
- MRM_OPERATING +
+ pull_over
- MRM_OPERATING + pull_over - - + + -
+
- pull over unavailable -
- comfortable_stop available (ca) + ea || failed
- pull over unavailable... + ea || failed - - -
+
- pull over available (pa) + ea || failed
- pull over available (pa) + ea || failed + + + + + + +
+
+
+ emergency_stop +
+
+
+ + emergency_stop - - + + -
+
- pull over unavailable -
- comfortable_stop unavailable -
- emergency_stop available (ea) + ca
- pull over unavailable... + ca - - - - - - +
- pull_over + comfortable_stop
- pull_over + comfortable_stop - - + + -
+
- ea + not emergency
- ea + not emergency + + + + + -
+
- ea + emergency
- ea + emergency - + -
+
- emergency_stop + NORMAL
- emergency_stop + NORMAL - - + + -
+
- ca + succeeded
- ca + succeeded - + + -
-
-
- comfortable_stop +
+
+
+ failed
- comfortable_stop + failed - - + + -
+
- pa + pa @@ -334,13 +332,13 @@ MRM_SUCCEEDED - +
@@ -349,7 +347,7 @@
- MRM_FAILED + MRM_FAILED diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index c706e1d773907..1dd0f6b081337 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -47,6 +47,8 @@ struct Param { int update_rate; double timeout_operation_mode_availability; + double timeout_call_mrm_behavior; + double timeout_cancel_mrm_behavior; bool use_emergency_holding; double timeout_emergency_recovery; bool use_parking_after_stopped; @@ -61,6 +63,9 @@ class MrmHandler : public rclcpp::Node MrmHandler(); private: + // type + enum RequestType { CALL, CANCEL }; + // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr @@ -120,10 +125,9 @@ class MrmHandler : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; rclcpp::Client::SharedPtr client_mrm_emergency_stop_; - void callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + bool requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; @@ -140,12 +144,15 @@ class MrmHandler : public rclcpp::Node // Heartbeat rclcpp::Time stamp_operation_mode_availability_; std::optional stamp_autonomous_become_unavailable_ = std::nullopt; + bool is_operation_mode_availability_timeout; + void checkOperationModeAvailabilityTimeout(); // Algorithm bool is_emergency_holding_ = false; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); + void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index aedb8076ef05c..9a50c6a326f01 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -16,6 +16,16 @@ "description": "If the input `operation_mode_availability` topic cannot be received for more than `timeout_operation_mode_availability`, vehicle will make an emergency stop.", "default": 0.5 }, + "timeout_call_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_call_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, + "timeout_cancel_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_cancel_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, "use_emergency_holding": { "type": "boolean", "description": "If this parameter is true, the handler does not recover to the NORMAL state.", @@ -56,6 +66,8 @@ "required": [ "update_rate", "timeout_operation_mode_availability", + "timeout_call_mrm_behavior", + "timeout_cancel_mrm_behavior", "use_emergency_holding", "timeout_emergency_recovery", "use_parking_after_stopped", diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 8ed9017a3ad3b..e8e692f755e2d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -24,6 +24,9 @@ MrmHandler::MrmHandler() : Node("mrm_handler") param_.update_rate = declare_parameter("update_rate", 10); param_.timeout_operation_mode_availability = declare_parameter("timeout_operation_mode_availability", 0.5); + param_.timeout_call_mrm_behavior = declare_parameter("timeout_call_mrm_behavior", 0.01); + param_.timeout_cancel_mrm_behavior = + declare_parameter("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); @@ -91,6 +94,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler") mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + is_operation_mode_availability_timeout = false; // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -209,18 +213,27 @@ void MrmHandler::operateMrm() if (mrm_state_.state == MrmState::NORMAL) { // Cancel MRM behavior when returning to NORMAL state const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handleFailedRequest(); } return; } if (mrm_state_.state == MrmState::MRM_OPERATING) { const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - callMrmBehavior(current_mrm_behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { + handleFailedRequest(); + } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handleFailedRequest(); } return; } @@ -236,88 +249,94 @@ void MrmHandler::operateMrm() RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); } -void MrmHandler::callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +void MrmHandler::handleFailedRequest() { using autoware_adapi_v1_msgs::msg::MrmState; - auto request = std::make_shared(); - request->operate = true; - - if (mrm_behavior == MrmState::NONE) { - RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); - return; - } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to operate"); - } - return; + if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { + if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); + } else { + transitionTo(MrmState::MRM_FAILED); } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); + mrm_state_.behavior = MrmState::EMERGENCY_STOP; } -void MrmHandler::cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +bool MrmHandler::requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const { using autoware_adapi_v1_msgs::msg::MrmState; auto request = std::make_shared(); - request->operate = false; - - if (mrm_behavior == MrmState::NONE) { - // Do nothing - return; + if (request_type == RequestType::CALL) { + request->operate = true; + } else if (request_type == RequestType::CANCEL) { + request->operate = false; + } else { + RCLCPP_ERROR(this->get_logger(), "invalid request type: %d", request_type); + return false; } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to cancel"); + const auto duration = std::chrono::duration>( + request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + std::shared_future> future; + + const auto behavior2string = [](const int behavior) { + if (behavior == MrmState::NONE) { + return "NONE"; } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to cancel"); + if (behavior == MrmState::PULL_OVER) { + return "PULL_OVER"; } - return; + if (behavior == MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + if (behavior == MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + const auto msg = "invalid behavior: " + std::to_string(behavior); + throw std::runtime_error(msg); + }; + + switch (mrm_behavior) { + case MrmState::NONE: + RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); + return true; + case MrmState::PULL_OVER: { + future = client_mrm_pull_over_->async_send_request(request).future.share(); + break; + } + case MrmState::COMFORTABLE_STOP: { + future = client_mrm_comfortable_stop_->async_send_request(request).future.share(); + break; + } + case MrmState::EMERGENCY_STOP: { + future = client_mrm_emergency_stop_->async_send_request(request).future.share(); + break; + } + default: + RCLCPP_ERROR(this->get_logger(), "invalid behavior: %d", mrm_behavior); + return false; } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + + if (future.wait_for(duration) == std::future_status::ready) { + const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); + RCLCPP_WARN( + this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", + behavior2string(mrm_behavior)); + return true; } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to cancel"); + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", + behavior2string(mrm_behavior)); + return false; } - return; + } else { + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", + behavior2string(mrm_behavior)); + return false; } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); } bool MrmHandler::isDataReady() @@ -358,24 +377,29 @@ bool MrmHandler::isDataReady() return true; } -void MrmHandler::onTimer() +void MrmHandler::checkOperationModeAvailabilityTimeout() { - if (!isDataReady()) { - return; - } - const bool is_operation_mode_availability_timeout = + if ( (this->now() - stamp_operation_mode_availability_).seconds() > - param_.timeout_operation_mode_availability; - if (is_operation_mode_availability_timeout) { + param_.timeout_operation_mode_availability) { + is_operation_mode_availability_timeout = true; RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "heartbeat operation_mode_availability is timeout"); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; - publishHazardCmd(); - publishGearCmd(); + "operation_mode_availability is timeout"); + } else { + is_operation_mode_availability_timeout = false; + } +} + +void MrmHandler::onTimer() +{ + if (!isDataReady()) { return; } + // Check whether operation_mode_availability is timeout + checkOperationModeAvailabilityTimeout(); + // Update Emergency State updateMrmState(); @@ -477,6 +501,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB // State machine if (mrm_state_.behavior == MrmState::NONE) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -493,6 +520,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -509,6 +539,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -525,6 +558,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -551,7 +587,8 @@ bool MrmHandler::isStopped() bool MrmHandler::isEmergency() const { - return !operation_mode_availability_->autonomous || is_emergency_holding_; + return !operation_mode_availability_->autonomous || is_emergency_holding_ || + is_operation_mode_availability_timeout; } bool MrmHandler::isArrivedAtGoal() diff --git a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index 1001696d96679..bce50a0b581c7 100644 --- a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -1,5 +1,8 @@ /**: ros__parameters: + csv_path_accel_map: $(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv + csv_path_brake_map: $(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv + csv_path_steer_map: $(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv convert_accel_cmd: true convert_brake_cmd: true convert_steer_cmd: true diff --git a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index a74cdcc56132d..0a9962a7c2a30 100644 --- a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -1,9 +1,5 @@ - - - - @@ -13,9 +9,6 @@ - - - diff --git a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index cc292c5a8c40f..4b562f401e09b 100644 --- a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -6,6 +6,21 @@ "raw_vehicle_cmd_converter": { "type": "object", "properties": { + "csv_path_accel_map": { + "type": "string", + "description": "path for acceleration map csv file", + "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv" + }, + "csv_path_brake_map": { + "type": "string", + "description": "path for brake map csv file", + "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv" + }, + "csv_path_steer_map": { + "type": "string", + "description": "path for steer map csv file", + "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv" + }, "convert_accel_cmd": { "type": "boolean", "description": "use accel or not", @@ -142,6 +157,9 @@ } }, "required": [ + "csv_path_accel_map", + "csv_path_brake_map", + "csv_path_steer_map", "convert_accel_cmd", "convert_brake_cmd", "convert_steer_cmd",