From 5605b19a1d0d8fcb4d9afb45a08934dda7e595ec Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Mon, 10 Jul 2023 16:08:46 +0900 Subject: [PATCH 1/7] feat(roi_cluster_fusion): update default value of `only_allow_inside_cluster` is `true` (#4212) Signed-off-by: ktro2828 --- .../launch/roi_cluster_fusion.launch.xml | 2 +- .../src/roi_cluster_fusion/node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index a946da66f2488..d136cc9393703 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -74,7 +74,7 @@ - + diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index af3dd15d98abc..7b0a181e3dbd0 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -40,7 +40,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) use_iou_y_ = declare_parameter("use_iou_y", false); use_iou_ = declare_parameter("use_iou", false); use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", false); + only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); iou_threshold_ = declare_parameter("iou_threshold", 0.1); remove_unknown_ = declare_parameter("remove_unknown", false); From a58c94e15e28b6fe6231a3b838fc6624d01941c4 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Mon, 10 Jul 2023 18:16:08 +0900 Subject: [PATCH 2/7] feat(radar_object_clustering): add radar_object_clustering (#4144) * add radar_object_clustering Signed-off-by: scepter914 * apply pre-commit Signed-off-by: scepter914 * apply pre-commit Signed-off-by: scepter914 * update README Signed-off-by: scepter914 * add distance sort Signed-off-by: scepter914 * add README Signed-off-by: scepter914 * fix normalization of angle_diff Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- .../radar_object_clustering/CMakeLists.txt | 40 +++ perception/radar_object_clustering/README.md | 78 ++++++ .../docs/clustering.drawio.svg | 171 +++++++++++++ .../docs/radar_clustering.drawio.svg | 44 ++++ .../radar_object_clustering_node.hpp | 78 ++++++ .../launch/radar_object_clustering.launch.xml | 31 +++ .../radar_object_clustering/package.xml | 32 +++ .../radar_object_clustering_node.cpp | 233 ++++++++++++++++++ 8 files changed, 707 insertions(+) create mode 100644 perception/radar_object_clustering/CMakeLists.txt create mode 100644 perception/radar_object_clustering/README.md create mode 100644 perception/radar_object_clustering/docs/clustering.drawio.svg create mode 100644 perception/radar_object_clustering/docs/radar_clustering.drawio.svg create mode 100644 perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp create mode 100644 perception/radar_object_clustering/launch/radar_object_clustering.launch.xml create mode 100644 perception/radar_object_clustering/package.xml create mode 100644 perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt new file mode 100644 index 0000000000000..acb544bfe4f6a --- /dev/null +++ b/perception/radar_object_clustering/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_object_clustering) + +# Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(radar_object_clustering_node_component SHARED + src/radar_object_clustering_node/radar_object_clustering_node.cpp +) + +rclcpp_components_register_node(radar_object_clustering_node_component + PLUGIN "radar_object_clustering::RadarObjectClusteringNode" + EXECUTABLE radar_object_clustering_node +) + +# Tests +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# Package +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/radar_object_clustering/README.md b/perception/radar_object_clustering/README.md new file mode 100644 index 0000000000000..6463dd7157dea --- /dev/null +++ b/perception/radar_object_clustering/README.md @@ -0,0 +1,78 @@ +# radar_object_clustering + +This package contains a radar object clustering for [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) input. + +This package can make clustered objects from radar DetectedObjects, the objects which is converted from RadarTracks by [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_tracks_msgs_converter) and is processed by noise filter. +In other word, this package can combine multiple radar detections from one object into one and adjust class and size. + +![radar_clustering](docs/radar_clustering.drawio.svg) + +## Algorithm + +### Background + +In radars with object output, there are cases that multiple detection results are obtained from one object, especially for large vehicles such as trucks and trailers. +Its multiple detection results cause separation of objects in tracking module. +Therefore, by this package the multiple detection results are clustered into one object in advance. + +### Detail Algorithm + +- Sort by distance from `base_link` + +At first, to prevent changing the result from depending on the order of objects in DetectedObjects, input objects are sorted by distance from `base_link`. +In addition, to apply matching in closeness order considering occlusion, objects are sorted in order of short distance in advance. + +- Clustering + +If two radar objects are near, and yaw angle direction and velocity between two radar objects is similar (the degree of these is defined by parameters), then these are clustered. +Note that radar characteristic affect parameters for this matching. +For example, if resolution of range distance or angle is low and accuracy of velocity is high, then `distance_threshold` parameter should be bigger and should set matching that strongly looks at velocity similarity. + +![clustering](docs/clustering.drawio.svg) + +After grouping for all radar objects, if multiple radar objects are grouping, the kinematics of the new clustered object is calculated from average of that and label and shape of the new clustered object is calculated from top confidence in radar objects. + +- Fixed label correction + +When the label information from radar outputs lack accuracy, `is_fixed_label` parameter is recommended to set `true`. +If the parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. +If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to `VEHICLE`. + +- Fixed size correction + +When the size information from radar outputs lack accuracy, `is_fixed_size` parameter is recommended to set `true`. +If the parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. +If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to +`size_x`, `size_y`, `size_z`, as average of vehicle size. +Note that to use for [multi_objects_tracker](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/multi_object_tracker), the size parameters need to exceed `min_area_matrix` parameters of it. + +### Limitation + +For now, size estimation for clustered object is not implemented. +So `is_fixed_size` parameter is recommended to set `true`, and size parameters is recommended to set to value near to average size of vehicles. + +## Input + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | -------------- | +| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. | + +## Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | -------------- | +| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Output objects | + +## Parameters + +| Name | Type | Description | Default value | +| :------------------- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `angle_threshold` | double | Angle threshold to judge whether radar detections come from one object. [rad] | 0.174 | +| `distance_threshold` | double | Distance threshold to judge whether radar detections come from one object. [m] | 4.0 | +| `velocity_threshold` | double | Velocity threshold to judge whether radar detections come from one object. [m/s] | 2.0 | +| `is_fixed_label` | bool | If this parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. | false | +| `fixed_label` | string | If `is_fixed_label` is true, the label of a clustered object is overwritten by this parameter. | "UNKNOWN" | +| `is_fixed_size` | bool | If this parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. | false | +| `size_x` | double | If `is_fixed_size` is true, the x-axis size of a clustered object is overwritten by this parameter. [m] | 4.0 | +| `size_y` | double | If `is_fixed_size` is true, the y-axis size of a clustered object is overwritten by this parameter. [m] | 1.5 | +| `size_z` | double | If `is_fixed_size` is true, the z-axis size of a clustered object is overwritten by this parameter. [m] | 1.5 | diff --git a/perception/radar_object_clustering/docs/clustering.drawio.svg b/perception/radar_object_clustering/docs/clustering.drawio.svg new file mode 100644 index 0000000000000..64563a5f28ecd --- /dev/null +++ b/perception/radar_object_clustering/docs/clustering.drawio.svg @@ -0,0 +1,171 @@ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + distance_threshold +
+ +
+
+
+ + distance_t... + + + + + + + + + + + + + + + + +
+
+
+ + angle diffrence +
+ +
+
+
+ + angle diffr... + + + + + + + + + + + +
+
+
+ + velocity diffrence +
+ +
+
+
+ + velocity di... + + + + + + +
+
+
+ radar object +
+
+
+ + radar object + + + + + + +
+
+
+ clustered object +
+
+
+ + clustered object + + + + + + +
+
+
+ Text +
+
+
+ + Text + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/radar_object_clustering/docs/radar_clustering.drawio.svg b/perception/radar_object_clustering/docs/radar_clustering.drawio.svg new file mode 100644 index 0000000000000..4b512c52fdd38 --- /dev/null +++ b/perception/radar_object_clustering/docs/radar_clustering.drawio.svg @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp new file mode 100644 index 0000000000000..0e82c5388d297 --- /dev/null +++ b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp @@ -0,0 +1,78 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ +#define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include + +namespace radar_object_clustering +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + +class RadarObjectClusteringNode : public rclcpp::Node +{ +public: + explicit RadarObjectClusteringNode(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + double angle_threshold{}; + double distance_threshold{}; + double velocity_threshold{}; + bool is_fixed_label{}; + std::string fixed_label{}; + bool is_fixed_size{}; + double size_x{}; + double size_y{}; + double size_z{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_objects_{}; + + // Callback + void onObjects(const DetectedObjects::ConstSharedPtr msg); + + // Data Buffer + DetectedObjects::ConstSharedPtr objects_data_{}; + + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_{}; + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Parameter + NodeParam node_param_{}; + + // Core + bool isSameObject(const DetectedObject & object_1, const DetectedObject & object_2); +}; + +} // namespace radar_object_clustering + +#endif // RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml new file mode 100644 index 0000000000000..e216bec45798a --- /dev/null +++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/radar_object_clustering/package.xml b/perception/radar_object_clustering/package.xml new file mode 100644 index 0000000000000..388c034ff8001 --- /dev/null +++ b/perception/radar_object_clustering/package.xml @@ -0,0 +1,32 @@ + + + + radar_object_clustering + 0.1.0 + radar_object_clustering + Sathshi Tanaka + Shunsuke Miura + Yoshi Ri + + Apache License 2.0 + Sathshi Tanaka + + ament_cmake_auto + + autoware_auto_perception_msgs + geometry_msgs + perception_utils + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tier4_autoware_utils + + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp new file mode 100644 index 0000000000000..82b729f405647 --- /dev/null +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -0,0 +1,233 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "radar_object_clustering/radar_object_clustering_node.hpp" + +#include "perception_utils/perception_utils.hpp" + +#include + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace +{ +template +bool update_param( + const std::vector & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; + } + + value = itr->template get_value(); + return true; +} + +double get_distance(const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const auto & position = object.kinematics.pose_with_covariance.pose.position; + return std::hypot(position.x, position.y); +} + +} // namespace + +namespace radar_object_clustering +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + +RadarObjectClusteringNode::RadarObjectClusteringNode(const rclcpp::NodeOptions & node_options) +: Node("radar_object_clustering", node_options) +{ + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarObjectClusteringNode::onSetParam, this, std::placeholders::_1)); + + // Node Parameter + node_param_.angle_threshold = declare_parameter("angle_threshold", 0.174); + node_param_.distance_threshold = declare_parameter("distance_threshold", 4.0); + node_param_.velocity_threshold = declare_parameter("velocity_threshold", 2.0); + node_param_.is_fixed_label = declare_parameter("is_fixed_label", false); + node_param_.fixed_label = declare_parameter("fixed_label", "UNKNOWN"); + node_param_.is_fixed_size = declare_parameter("is_fixed_size", false); + node_param_.size_x = declare_parameter("size_x", 4.0); + node_param_.size_y = declare_parameter("size_y", 1.5); + node_param_.size_z = declare_parameter("size_z", 1.5); + + // Subscriber + sub_objects_ = create_subscription( + "~/input/objects", rclcpp::QoS{1}, + std::bind(&RadarObjectClusteringNode::onObjects, this, std::placeholders::_1)); + + // Publisher + pub_objects_ = create_publisher("~/output/objects", 1); +} + +void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr objects_data_) +{ + DetectedObjects output_objects; + output_objects.header = objects_data_->header; + std::vector objects = objects_data_->objects; + + std::vector used_flags(objects.size(), false); + + auto func = [](DetectedObject const & lhs, DetectedObject const & rhs) { + return get_distance(lhs) < get_distance(rhs); + }; + std::sort(objects.begin(), objects.end(), func); + + for (size_t i = 0; i < objects.size(); i++) { + if (used_flags.at(i) == true) { + continue; + } + + std::vector clustered_objects; + used_flags.at(i) = true; + clustered_objects.emplace_back(objects.at(i)); + + for (size_t j = i; j < objects.size(); ++j) { + if (used_flags.at(j) == false && isSameObject(objects.at(i), objects.at(j))) { + used_flags.at(j) = true; + clustered_objects.emplace_back(objects.at(j)); + } + } + + // clustering + DetectedObject clustered_output_object; + if (clustered_objects.size() == 1) { + clustered_output_object = clustered_objects.at(0); + } else { + auto func_max_confidence = [](const DetectedObject & a, const DetectedObject & b) { + return a.existence_probability < b.existence_probability; + }; + auto iter = std::max_element( + std::begin(clustered_objects), std::end(clustered_objects), func_max_confidence); + + // class label + clustered_output_object.existence_probability = iter->existence_probability; + clustered_output_object.classification = iter->classification; + + // kinematics + clustered_output_object.kinematics = iter->kinematics; + + auto & pose = clustered_output_object.kinematics.pose_with_covariance.pose; + auto func_sum_x = [](const double & a, const DetectedObject & b) { + return a + b.kinematics.pose_with_covariance.pose.position.x; + }; + pose.position.x = + std::accumulate( + std::begin(clustered_objects), std::end(clustered_objects), 0.0, func_sum_x) / + clustered_objects.size(); + auto func_sum_y = [](const double & a, const DetectedObject & b) { + return a + b.kinematics.pose_with_covariance.pose.position.y; + }; + pose.position.y = + std::accumulate( + std::begin(clustered_objects), std::end(clustered_objects), 0.0, func_sum_y) / + clustered_objects.size(); + pose.position.z = iter->kinematics.pose_with_covariance.pose.position.z; + + // Shape + clustered_output_object.shape = iter->shape; + } + + // Fixed label correction + if (node_param_.is_fixed_label) { + clustered_output_object.classification.at(0).label = + perception_utils::toLabel(node_param_.fixed_label); + } + + // Fixed size correction + if (node_param_.is_fixed_size) { + clustered_output_object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + clustered_output_object.shape.dimensions.x = node_param_.size_x; + clustered_output_object.shape.dimensions.y = node_param_.size_y; + clustered_output_object.shape.dimensions.z = node_param_.size_z; + } + output_objects.objects.emplace_back(clustered_output_object); + } + pub_objects_->publish(output_objects); +} + +bool RadarObjectClusteringNode::isSameObject( + const DetectedObject & object_1, const DetectedObject & object_2) +{ + const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - + tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); + const double velocity_diff = std::abs( + object_1.kinematics.twist_with_covariance.twist.linear.x - + object_2.kinematics.twist_with_covariance.twist.linear.x); + const double distance = tier4_autoware_utils::calcDistance2d( + object_1.kinematics.pose_with_covariance.pose.position, + object_2.kinematics.pose_with_covariance.pose.position); + + if ( + distance < node_param_.distance_threshold && angle_diff < node_param_.angle_threshold && + velocity_diff < node_param_.velocity_threshold) { + return true; + } else { + return false; + } +} + +rcl_interfaces::msg::SetParametersResult RadarObjectClusteringNode::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + + try { + // Node Parameter + { + auto & p = node_param_; + + // Update params + update_param(params, "angle_threshold", p.angle_threshold); + update_param(params, "distance_threshold", p.distance_threshold); + update_param(params, "velocity_threshold", p.velocity_threshold); + update_param(params, "is_fixed_label", p.is_fixed_label); + update_param(params, "fixed_label", p.fixed_label); + update_param(params, "is_fixed_size", p.is_fixed_size); + update_param(params, "size_x", p.size_x); + update_param(params, "size_y", p.size_y); + update_param(params, "size_z", p.size_z); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + + result.successful = true; + result.reason = "success"; + return result; +} +} // namespace radar_object_clustering + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_object_clustering::RadarObjectClusteringNode) From 3dfebadfe045697a4383c213bfeae99b76865e5c Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 10 Jul 2023 20:39:55 +0900 Subject: [PATCH 3/7] fix(behavior_path_planner): fix execution request condition (#4197) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/interface.cpp | 4 +++- .../src/scene_module/lane_change/normal.cpp | 12 ++---------- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 045484a7c8cd6..ca2ffec30146d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -237,7 +237,10 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); + path_reference_ = getPreviousModuleOutput().reference_path; + if (!module_type_->isValidPath()) { + removeRTCStatus(); path_candidate_ = std::make_shared(); return out; } @@ -245,7 +248,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; updateRTCStatus( candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); is_abort_path_approved_ = false; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index eed66650520e9..c744bd49f6012 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -77,6 +77,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) debug_valid_path_ = valid_paths; if (valid_paths.empty()) { + safe_path.reference_lanelets = current_lanes; return {false, false}; } @@ -100,16 +101,7 @@ bool NormalLaneChange::isLaneChangeRequired() const const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - if (target_lanes.empty()) { - return false; - } - - // find candidate paths - LaneChangePaths valid_paths{}; - [[maybe_unused]] const auto found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths, false); - - return !valid_paths.empty(); + return !target_lanes.empty(); } LaneChangePath NormalLaneChange::getLaneChangePath() const From 53cb3a44781731b0c1cb0006cbfc806310f96cac Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 10 Jul 2023 21:13:42 +0900 Subject: [PATCH 4/7] chore(behavior_path_planner): add message for invalid ego pose (#4218) Signed-off-by: Takamasa Horibe --- planning/behavior_path_planner/src/planner_manager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c9d4d2c191a4c..b09bba73d9128 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -70,6 +70,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da if (!is_any_module_running && is_out_of_route) { BehaviorModuleOutput output = utils::createGoalAroundPath(data); generateCombinedDrivableArea(output, data); + RCLCPP_WARN_THROTTLE( + logger_, clock_, 5000, + "Ego is out of route, no module is running. Skip running scene modules."); return output; } From 8d69caaf3711ba91b7b5ddd2cca585914e80bf58 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 10 Jul 2023 21:40:18 +0900 Subject: [PATCH 5/7] docs(behavior_velocity_planner_common): add readme (#4223) Signed-off-by: Takamasa Horibe --- planning/behavior_velocity_planner_common/README.md | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 planning/behavior_velocity_planner_common/README.md diff --git a/planning/behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner_common/README.md new file mode 100644 index 0000000000000..2abbb83575af5 --- /dev/null +++ b/planning/behavior_velocity_planner_common/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity Planner Common + +This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. From 463679ceb5f3f1c81e61ef8bf6ea8adec0adc97b Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 10 Jul 2023 22:39:17 +0900 Subject: [PATCH 6/7] docs(mrm_emergency_stop_operator): fix file name (#4226) Signed-off-by: Takamasa Horibe --- system/mrm_emergency_stop_operator/{READEME.md => README.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename system/mrm_emergency_stop_operator/{READEME.md => README.md} (100%) diff --git a/system/mrm_emergency_stop_operator/READEME.md b/system/mrm_emergency_stop_operator/README.md similarity index 100% rename from system/mrm_emergency_stop_operator/READEME.md rename to system/mrm_emergency_stop_operator/README.md From 079588284a7cc44a3445c2469594c4003d03c1f1 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 10 Jul 2023 23:57:39 +0900 Subject: [PATCH 7/7] docs(velodyne_monitor): rename readme to README (#4224) Signed-off-by: Takamasa Horibe --- system/velodyne_monitor/{Readme.md => README.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename system/velodyne_monitor/{Readme.md => README.md} (100%) diff --git a/system/velodyne_monitor/Readme.md b/system/velodyne_monitor/README.md similarity index 100% rename from system/velodyne_monitor/Readme.md rename to system/velodyne_monitor/README.md