diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 5148d6a998f50..67b770aefb3d2 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -63,15 +63,30 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | -| include_right_lanes | bool | Flag for including right lanelet in borders | False | -| include_left_lanes | bool | Flag for including left lanelet in borders | False | -| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | -| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | -| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | +#### General Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | +| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | +| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +#### Parameters For Lane Departure + +| Name | Type | Description | Default value | +| :------------------------ | :--- | :------------------------------------------------ | :------------ | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | + +#### Parameters For Road Border Departure + +| Name | Type | Description | Default value | +| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | +| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 008832b1cab21..f0a8df21c425b 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,5 +1,10 @@ /**: ros__parameters: + # Enable feature + will_out_of_lane_checker: true + out_of_lane_checker: true + boundary_departure_checker: false + # Node update_rate: 10.0 visualize_lanelet: false @@ -7,7 +12,8 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - road_border_departure_checker: false + boundary_types_to_detect: [road_border] + # Core footprint_margin_scale: 1.0 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 573a38593fc59..c77503106e485 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 @@ -72,6 +72,7 @@ struct Input lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; }; struct Output @@ -79,7 +80,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; - bool will_cross_road_border{}; + bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -136,9 +137,10 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); - static bool willCrossRoadBorder( + static bool willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detects); static bool isCrossingRoadBorder( const lanelet::BasicLineString2d & road_border, const std::vector & footprints); diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index f7f7bcfda7d51..c146957bbd314 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -37,6 +37,7 @@ #include #include +#include #include namespace lane_departure_checker @@ -45,13 +46,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; struct NodeParam { + bool will_out_of_lane_checker; + bool out_of_lane_checker; + bool boundary_departure_checker; + double update_rate; bool visualize_lanelet; bool include_right_lanes; bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; - bool road_border_departure_checker; + std::vector boundary_types_to_detect; }; class LaneDepartureCheckerNode : public rclcpp::Node 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 f1d8d75452df1..9d413b2bf0a70 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 @@ -67,19 +67,19 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +bool isCrossingWithBoundary( + const lanelet::BasicLineString2d & boundary, const std::vector & footprints) { for (auto & footprint : footprints) { for (size_t i = 0; i < footprint.size() - 1; ++i) { auto footprint1 = footprint.at(i).to_3d(); auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < road_border.size() - 1; ++i) { - auto road_border1 = road_border.at(i); - auto road_border2 = road_border.at(i + 1); + for (size_t i = 0; i < boundary.size() - 1; ++i) { + auto boundary1 = boundary.at(i); + auto boundary2 = boundary.at(i + 1); if (tier4_autoware_utils::intersect( tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { return true; } } @@ -172,9 +172,9 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_road_border = - willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); - output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + output.will_cross_boundary = willCrossBoundary( + output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; } @@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossRoadBorder( +bool LaneDepartureChecker::willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detect) { for (const auto & candidate_lanelet : candidate_lanelets) { const std::string r_type = candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (r_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.rightBound().id() << std::endl; @@ -351,8 +354,10 @@ bool LaneDepartureChecker::willCrossRoadBorder( } const std::string l_type = candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (l_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.leftBound().id() << std::endl; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 20d535a82bfa1..f788e64316171 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -125,6 +125,11 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o { using std::placeholders::_1; + // Enable feature + node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); + node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); + node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); + // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); @@ -132,8 +137,10 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - node_param_.road_border_departure_checker = - declare_parameter("road_border_departure_checker"); + + // Boundary_departure_checker + node_param_.boundary_types_to_detect = + declare_parameter>("boundary_types_to_detect"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -338,6 +345,7 @@ void LaneDepartureCheckerNode::onTimer() input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; + input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); output_ = lane_departure_checker_->update(input_); @@ -377,12 +385,19 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( result.reason = "success"; try { + // Enable feature + update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker); + update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + // Node update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet); update_param(parameters, "include_right_lanes", node_param_.include_right_lanes); update_param(parameters, "include_left_lanes", node_param_.include_left_lanes); update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes); update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect); // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); @@ -409,19 +424,19 @@ void LaneDepartureCheckerNode::checkLaneDeparture( int8_t level = DiagStatus::OK; std::string msg = "OK"; - if (output_.will_leave_lane) { + if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) { level = DiagStatus::WARN; msg = "vehicle will leave lane"; } - if (output_.is_out_of_lane) { + if (output_.is_out_of_lane && node_param_.out_of_lane_checker) { level = DiagStatus::ERROR; msg = "vehicle is out of lane"; } - if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + if (output_.will_cross_boundary && node_param_.boundary_departure_checker) { level = DiagStatus::ERROR; - msg = "vehicle will cross road border"; + msg = "vehicle will cross boundary"; } stat.summary(level, msg); diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3bbc3a006c73a..73249095c6d5c 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -63,13 +63,14 @@ For the backward compatibility (to be removed): ## Parameters -| Name | Type | Description | Default value | -| :--------------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | -| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | -| `frequency_hz` | `double` | running hz | 10.0 | -| `check_engage_condition` | `double` | If false, autonomous transition is always available | 0.1 | -| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | -| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | +| `frequency_hz` | `double` | running hz | 10.0 | +| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 | +| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 | +| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | +| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | For `engage_acceptable_limits` related parameters: @@ -94,6 +95,21 @@ For `stable_check` related parameters: | `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | | `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | +## Engage check behavior on each parameter setting + +This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings: + +| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted | +| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- | +| x | x | x | Only when the vehicle is stationary. | +| x | x | o | Only when the vehicle is stationary. | +| x | o | x | When the vehicle is stationary and all engage conditions are met. | +| x | o | o | Only when the vehicle is stationary. | +| o | x | x | At any time (Caution: Not recommended). | +| o | x | o | At any time (Caution: Not recommended). | +| o | o | x | When all engage conditions are met, regardless of vehicle status. | +| o | o | o | When all engage conditions are met or the vehicle is stationary. | + ## Future extensions / Unimplemented parts - Need to remove backward compatibility interfaces. diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index a86443f5cabdb..67ce4b485e8c3 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -2,11 +2,16 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + + # set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. + enable_engage_on_driving: false + check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + allow_autonomous_in_stopped: true # if true, all engage check is skipped. dist_threshold: 1.5 yaw_threshold: 0.524 speed_upper_threshold: 10.0 diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 5062c596dce00..55b672aa48964 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -46,6 +46,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + enable_engage_on_driving_ = node->declare_parameter("enable_engage_on_driving"); nearest_dist_deviation_threshold_ = node->declare_parameter("nearest_dist_deviation_threshold"); nearest_yaw_deviation_threshold_ = @@ -218,6 +219,15 @@ bool AutonomousMode::isModeChangeAvailable() const auto target_control_speed = control_cmd_.longitudinal.speed; const auto & param = engage_acceptable_param_; + if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { + RCLCPP_INFO( + logger_, + "Engage unavailable: enable_engage_on_driving is false, and the vehicle is not " + "stationary."); + debug_info_ = DebugInfo{}; // all false + return false; + } + if (trajectory_.points.size() < 2) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 5000, "Engage unavailable: trajectory size must be > 2"); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index 9d384857bbe3d..de0dd8a543b15 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -73,6 +73,7 @@ class AutonomousMode : public ModeChangeBase rclcpp::Clock::SharedPtr clock_; bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + bool enable_engage_on_driving_ = false; // if false, engage is not permited on driving double nearest_dist_deviation_threshold_; // [m] for finding nearest index double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 6e7e194f7cf72..d5cd85aa2ed85 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -51,14 +51,7 @@ if(BUILD_TESTING) ) find_package(ament_cmake_gtest REQUIRED) - set(TEST_FILES - test/test_aged_object_queue.cpp - test/test_mahalanobis.cpp - test/test_measurement.cpp - test/test_numeric.cpp - test/test_state_transition.cpp - test/test_string.cpp - test/test_warning_message.cpp) + file(GLOB_RECURSE TEST_FILES test/*.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp deleted file mode 100644 index f249dffef6034..0000000000000 --- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// 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 "ekf_localizer/ekf_localizer.hpp" - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -using std::placeholders::_1; - -class EKFLocalizerTestSuite : public ::testing::Test -{ -protected: - void SetUp() { rclcpp::init(0, nullptr); } - void TearDown() { (void)rclcpp::shutdown(); } -}; // sanity_check - -class TestEKFLocalizerNode : public EKFLocalizer -{ -public: - TestEKFLocalizerNode(const std::string & node_name, const rclcpp::NodeOptions & node_options) - : EKFLocalizer(node_name, node_options) - { - sub_twist = this->create_subscription( - "/ekf_twist", 1, std::bind(&TestEKFLocalizerNode::testCallbackTwist, this, _1)); - sub_pose = this->create_subscription( - "/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1)); - - using std::chrono_literals::operator""ms; - test_timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TestEKFLocalizerNode::testTimerCallback, this)); - } - ~TestEKFLocalizerNode() {} - - std::string frame_id_a_ = "world"; - std::string frame_id_b_ = "base_link"; - - rclcpp::Subscription::SharedPtr sub_twist; - rclcpp::Subscription::SharedPtr sub_pose; - - rclcpp::TimerBase::SharedPtr test_timer_; - - geometry_msgs::msg::PoseStamped::SharedPtr test_current_pose_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr test_current_twist_ptr_; - - void testTimerCallback() - { - /* !!! this should be defined before sendTransform() !!! */ - static std::shared_ptr br = - std::make_shared(shared_from_this()); - geometry_msgs::msg::TransformStamped sent; - - rclcpp::Time current_time = this->now(); - - sent.header.stamp = current_time; - sent.header.frame_id = frame_id_a_; - sent.child_frame_id = frame_id_b_; - sent.transform.translation.x = -7.11; - sent.transform.translation.y = 0.0; - sent.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0.5); - sent.transform.rotation.x = q.x(); - sent.transform.rotation.y = q.y(); - sent.transform.rotation.z = q.z(); - sent.transform.rotation.w = q.w(); - - br->sendTransform(sent); - } - - void testCallbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) - { - test_current_pose_ptr_ = std::make_shared(*pose); - } - - void testCallbackTwist(geometry_msgs::msg::TwistStamped::SharedPtr twist) - { - test_current_twist_ptr_ = std::make_shared(*twist); - } - - void resetCurrentPoseAndTwist() - { - test_current_pose_ptr_ = nullptr; - test_current_twist_ptr_ = nullptr; - } -}; - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePose) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher("/in_pose", 1); - - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - in_pose.pose.orientation.x = 0.0; - in_pose.pose.orientation.y = 0.0; - in_pose.pose.orientation.z = 0.0; - in_pose.pose.orientation.w = 1.0; - - /* test for valid value */ - const double pos_x = 12.3; - in_pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_NE(ekf->test_current_pose_ptr_, nullptr); - ASSERT_NE(ekf->test_current_twist_ptr_, nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher("/in_twist", 1); - geometry_msgs::msg::TwistStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 20; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) - << "ekf vel x: " << ekf_vx << ", should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) -{ - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("use_pose_with_covariance", true); - rclcpp::sleep_for(std::chrono::milliseconds(200)); - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher( - "/in_pose_with_covariance", 1); - geometry_msgs::msg::PoseWithCovarianceStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.pose.position.x = 1.0; - in_pose.pose.pose.position.y = 2.0; - in_pose.pose.pose.position.z = 3.0; - in_pose.pose.pose.orientation.x = 0.0; - in_pose.pose.pose.orientation.y = 0.0; - in_pose.pose.pose.orientation.z = 0.0; - in_pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 36; ++i) { - in_pose.pose.covariance[i] = 0.1; - } - - /* test for valid value */ - const double pos_x = 99.3; - in_pose.pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher( - "/in_twist_with_covariance", 1); - geometry_msgs::msg::TwistWithCovarianceStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 36; ++i) { - in_twist.twist.covariance[i] = 0.1; - } - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; -} diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index fdc732ea6d34c..23458fb18a838 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -31,15 +31,15 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) P(2, 2) = 9.; std::array covariance = ekfCovarianceToPoseMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(1), 2.); - EXPECT_EQ(covariance(5), 3.); - EXPECT_EQ(covariance(6), 4.); - EXPECT_EQ(covariance(7), 5.); - EXPECT_EQ(covariance(11), 6.); - EXPECT_EQ(covariance(30), 7.); - EXPECT_EQ(covariance(31), 8.); - EXPECT_EQ(covariance(35), 9.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[1], 2.); + EXPECT_EQ(covariance[5], 3.); + EXPECT_EQ(covariance[6], 4.); + EXPECT_EQ(covariance[7], 5.); + EXPECT_EQ(covariance[11], 6.); + EXPECT_EQ(covariance[30], 7.); + EXPECT_EQ(covariance[31], 8.); + EXPECT_EQ(covariance[35], 9.); } // ensure other elements are zero @@ -62,10 +62,10 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) P(5, 5) = 4.; std::array covariance = ekfCovarianceToTwistMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(5), 2.); - EXPECT_EQ(covariance(30), 3.); - EXPECT_EQ(covariance(35), 4.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[5], 2.); + EXPECT_EQ(covariance[30], 3.); + EXPECT_EQ(covariance[35], 4.); } // ensure other elements are zero diff --git a/localization/ekf_localizer/test/test_ekf_localizer.test b/localization/ekf_localizer/test/test_ekf_localizer.test deleted file mode 100644 index 4eecfa6e094b7..0000000000000 --- a/localization/ekf_localizer/test/test_ekf_localizer.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 3e223dd3101df..5e7a5087efb84 100755 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -17,6 +17,7 @@ #include +#include #include namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 5253ac192c786..9f8ef7b94a123 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -133,7 +133,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); if (it != paint_class_names.end()) { int index = it - paint_class_names.begin(); - class_index_[cls] = index + 1; + class_index_[cls] = pow(2, index); // regard each class as a bit in binary } else { isClassTable_.erase(cls); } @@ -345,7 +345,8 @@ dc | dc dc dc dc ||zc| data = &painted_pointcloud_msg.data[0]; auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { - *p_class = cls.second(label2d) ? class_index_[cls.first] : *p_class; + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } } #if 0 diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index bd49d9e446f1f..f462b9b0ba17a 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -59,8 +59,10 @@ std::size_t VoxelGenerator::pointsToVoxels( point[1] = point_current.y(); point[2] = point_current.z(); point[3] = time_lag; + // decode the class value back to one-hot binary and assign it to point for (std::size_t i = 1; i <= config_.class_size_; i++) { - point[3 + i] = (*class_iter == i) ? 1 : 0; + auto decode = std::bitset<8>(*class_iter).to_string(); + point[3 + i] = decode[-i] == 1 ? 1 : 0; } out_of_range = false; diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index d487bb163c5e8..7631b47772862 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -151,23 +151,21 @@ If the target object is inside the road or crosswalk, this module outputs one or ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | -| `enable_delay_compensation` | bool | flag to enable the time delay compensation for the position of the object | -| `prediction_time_horizon` | double | predict time duration for predicted path [s] | -| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | -| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | -| `min_crosswalk_user_velocity` | double | minimum velocity use in path prediction for crosswalk users | -| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | -| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | -| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | -| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | -| `object_buffer_time_length` | double | Time span of object history to store the information [s] | -| `history_time_length` | double | Time span of object information used for prediction [s] | -| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | -| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | -| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | -| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| Parameter | Unit | Type | Description | +| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | +| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | +| `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | +| `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | +| `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | +| `max_crosswalk_user_delta_yaw_threshold_for_lanelet` | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users | +| `dist_threshold_for_searching_lanelet` | [m] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `delta_yaw_threshold_for_searching_lanelet` | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `sigma_lateral_offset` | [m] | double | Standard deviation for lateral position of objects | +| `sigma_yaw_angle_deg` | [deg] | double | Standard deviation yaw angle of objects | +| `object_buffer_time_length` | [s] | double | Time span of object history to store the information | +| `history_time_length` | [s] | double | Time span of object information used for prediction | +| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 9b7b32e686cfb..d7ff85adc9193 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -10,6 +10,7 @@ Yoshi Ri Takayuki Murooka Kyoichi Sugahara + Kotaro Uetake Apache License 2.0 ament_cmake diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 1021757e99a07..c2d53869fa3d5 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -22,6 +22,7 @@ sensor_msgs tf2 tf2_eigen + tf2_geometry_msgs tf2_ros tier4_autoware_utils tier4_perception_msgs diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index a46be77667bcb..f0a5d7b7b1fde 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -31,6 +31,12 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + namespace { cv::Point2d calcRawImagePointFromPoint3D( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp index 4ba2cb08b6341..55b8bdf777692 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -27,6 +27,7 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; /* @@ -38,6 +39,8 @@ struct StartGoalPlannerData PredictedObjects filtered_objects; TargetObjectsOnLane target_objects_on_lane; std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index bea671c266899..63b8676e2188b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" @@ -31,6 +32,7 @@ namespace behavior_path_planner::utils::start_goal_planner_common { using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; @@ -85,6 +87,13 @@ void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel); +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map); + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path); + std::pair getPairsTerminalVelocityAndAccel( const std::vector> & pairs_terminal_velocity_and_accel, const size_t current_path_idx); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index c2b372d2403e5..3b005c1b2242d 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1901,10 +1901,9 @@ bool AvoidanceModule::isSafePath( safe_count_ = 0; return false; - } else { - marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } } + marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } safe_count_++; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 37d44154789a6..cfdc968656353 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1494,13 +1494,12 @@ bool GoalPlannerModule::isSafePath() const *route_handler, left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); - const auto & common_param = planner_data_->parameters; const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); RCLCPP_DEBUG( - getLogger(), "pairs_terminal_velocity_and_accel: %f, %f", terminal_velocity_and_accel.first, - terminal_velocity_and_accel.second); + getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); @@ -1509,32 +1508,52 @@ bool GoalPlannerModule::isSafePath() const ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, ego_seg_idx); - const auto filtered_objects = utils::path_safety_checker::filterObjects( + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( dynamic_object, route_handler, pull_over_lanes, current_pose.position, objects_filtering_params_); - const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; - updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop for (const auto & obj_path : obj_predicted_paths) { - CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( - pull_over_path, ego_predicted_path, object, obj_path, common_param, - safety_check_params_->rss_params, hysteresis_factor, collision)) { - return false; + pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; } } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } } - return true; + return is_safe_dynamic_objects; } void GoalPlannerModule::setDebugData() diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 90078a50e6ac9..e8acc77113959 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -553,10 +553,15 @@ std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { const auto path_road_lanes = getPathRoadLanes(path); - if (!path_road_lanes.empty()) { - return utils::generateDrivableLanesWithShoulderLanes( - getPathRoadLanes(path), status_.pull_out_lanes); + lanelet::ConstLanelets shoulder_lanes; + const auto & rh = planner_data_->route_handler; + std::copy_if( + status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(), + std::back_inserter(shoulder_lanes), + [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); + + return utils::generateDrivableLanesWithShoulderLanes(getPathRoadLanes(path), shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes @@ -923,15 +928,6 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } -void StartPlannerModule::updateSafetyCheckTargetObjectsData( - const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, - const std::vector & ego_predicted_path) const -{ - start_planner_data_.filtered_objects = filtered_objects; - start_planner_data_.target_objects_on_lane = target_objects_on_lane; - start_planner_data_.ego_predicted_path = ego_predicted_path; -} - bool StartPlannerModule::isSafePath() const { // TODO(Sugahara): should safety check for backward path @@ -945,14 +941,20 @@ bool StartPlannerModule::isSafePath() const const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); + + // for ego predicted path const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); - const auto & common_param = planner_data_->parameters; const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); const auto ego_predicted_path = @@ -960,31 +962,51 @@ bool StartPlannerModule::isSafePath() const ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, ego_seg_idx); - const auto filtered_objects = utils::path_safety_checker::filterObjects( + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); - const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( current_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; - updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop for (const auto & obj_path : obj_predicted_paths) { - CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( - pull_out_path, ego_predicted_path, object, obj_path, common_param, - safety_check_params_->rss_params, hysteresis_factor, collision)) { - return false; + pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; } } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } } - return true; + return is_safe_dynamic_objects; } bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const @@ -1109,6 +1131,9 @@ void StartPlannerModule::setDebugData() const using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; using marker_utils::createPredictedPathMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1138,6 +1163,25 @@ void StartPlannerModule::setDebugData() const start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } + // safety check + { + add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); + add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); + add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + } + + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); { diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index cff6e9f81f02e..830860de484b6 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -142,6 +142,21 @@ void updatePathProperty( ego_predicted_path_params->acceleration = acceleration; } +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map) +{ + collision_check_debug_map.clear(); +} + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + std::pair getPairsTerminalVelocityAndAccel( const std::vector> & pairs_terminal_velocity_and_accel, const size_t current_path_idx) diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index f942d9a00c098..73e41b052cd90 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -17,8 +17,11 @@ import argparse from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathPoint +from autoware_auto_planning_msgs.msg import PathPointWithLaneId from autoware_auto_planning_msgs.msg import PathWithLaneId from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_planning_msgs.msg import TrajectoryPoint from autoware_auto_vehicle_msgs.msg import VelocityReport from geometry_msgs.msg import Pose from matplotlib import animation @@ -201,7 +204,7 @@ def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): - print("CallbackMotionVelOptTraj called") + self.get_logger().info("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) @@ -233,7 +236,7 @@ def CallBackTrajFinal(self, cmd): self.update_traj_final = True def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): - print("CallBackLaneDrivingTraj called") + self.get_logger().info("CallBackLaneDrivingTraj called") self.CallBackTrajFinal(cmd5) self.CallBackLBehaviorPathPlannerPath(cmd6) self.CallBackBehaviorVelocityPlannerPath(cmd7) @@ -306,9 +309,8 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: - print("plot start but self pose is not received") + self.get_logger().info("plot start but self pose is not received") return ( self.im1, self.im2, @@ -324,82 +326,9 @@ def plotTrajectoryVelocity(self, data): self.im11, self.im12, ) - print("plot start") - - # copy - behavior_path_planner_path = self.behavior_path_planner_path - behavior_velocity_planner_path = self.behavior_velocity_planner_path - obstacle_avoid_traj = self.obstacle_avoid_traj - obstacle_stop_traj = self.obstacle_stop_traj - trajectory_raw = self.trajectory_raw - trajectory_external_velocity_limited = self.trajectory_external_velocity_limited - trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered - trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered - trajectory_time_resampled = self.trajectory_time_resampled - trajectory_final = self.trajectory_final - - if self.update_behavior_path_planner_path: - x = self.CalcArcLengthPathWLid(behavior_path_planner_path) - y = self.ToVelListPathWLid(behavior_path_planner_path) - self.im1.set_data(x, y) - self.update_behavior_path_planner_path = False - if len(y) != 0: - self.max_vel = max(10.0, np.max(y)) - - if self.update_behavior_velocity_planner_path: - x = self.CalcArcLengthPath(behavior_velocity_planner_path) - y = self.ToVelListPath(behavior_velocity_planner_path) - self.im2.set_data(x, y) - self.update_behavior_velocity_planner_path = False - - if self.update_traj_ob_avoid: - x = self.CalcArcLength(obstacle_avoid_traj) - y = self.ToVelList(obstacle_avoid_traj) - self.im3.set_data(x, y) - self.update_traj_ob_avoid = False - - if self.update_traj_ob_stop: - x = self.CalcArcLength(obstacle_stop_traj) - y = self.ToVelList(obstacle_stop_traj) - self.im4.set_data(x, y) - self.update_traj_ob_stop = False - - if self.update_traj_raw: - x = self.CalcArcLength(trajectory_raw) - y = self.ToVelList(trajectory_raw) - self.im5.set_data(x, y) - self.update_traj_raw = False - - if self.update_ex_vel_lim: - x = self.CalcArcLength(trajectory_external_velocity_limited) - y = self.ToVelList(trajectory_external_velocity_limited) - self.im6.set_data(x, y) - self.update_ex_vel_lim = False - - if self.update_lat_acc_fil: - x = self.CalcArcLength(trajectory_lateral_acc_filtered) - y = self.ToVelList(trajectory_lateral_acc_filtered) - self.im7.set_data(x, y) - self.update_lat_acc_fil = False - - if self.update_steer_rate_fil: - x = self.CalcArcLength(trajectory_steer_rate_filtered) - y = self.ToVelList(trajectory_steer_rate_filtered) - self.im71.set_data(x, y) - self.update_steer_rate_fil = False - - if self.update_traj_resample: - x = self.CalcArcLength(trajectory_time_resampled) - y = self.ToVelList(trajectory_time_resampled) - self.im8.set_data(x, y) - self.update_traj_resample = False + self.get_logger().info("plot start") if self.update_traj_final: - x = self.CalcArcLength(trajectory_final) - y = self.ToVelList(trajectory_final) - self.im9.set_data(x, y) - self.update_traj_final = False - self.im10.set_data(0, self.localization_vx) self.im11.set_data(0, self.vehicle_vx) @@ -408,8 +337,33 @@ def plotTrajectoryVelocity(self, data): y = [self.velocity_limit, self.velocity_limit] self.im12.set_data(x, y) - if len(y) != 0: - self.min_vel = np.min(y) + if len(y) != 0: + self.min_vel = np.min(y) + + trajectory_data = [ + (self.behavior_path_planner_path, self.update_behavior_path_planner_path, self.im1), + ( + self.behavior_velocity_planner_path, + self.update_behavior_velocity_planner_path, + self.im2, + ), + (self.obstacle_avoid_traj, self.update_traj_ob_avoid, self.im3), + (self.obstacle_stop_traj, self.update_traj_ob_stop, self.im4), + (self.trajectory_raw, self.update_traj_raw, self.im5), + (self.trajectory_external_velocity_limited, self.update_ex_vel_lim, self.im6), + (self.trajectory_lateral_acc_filtered, self.update_lat_acc_fil, self.im7), + (self.trajectory_steer_rate_filtered, self.update_steer_rate_fil, self.im71), + (self.trajectory_time_resampled, self.update_traj_resample, self.im8), + (self.trajectory_final, self.update_traj_final, self.im9), + ] + + # update all trajectory plots + for traj, update_flag, image in trajectory_data: + if update_flag: + x = self.CalcArcLength(traj) + y = self.ToVelList(traj) + image.set_data(x, y) + update_flag = False return ( self.im1, @@ -427,107 +381,43 @@ def plotTrajectoryVelocity(self, data): self.im12, ) - def CalcArcLength(self, traj): - if len(traj.points) == 0: - return - - s_arr = [] - ds = 0.0 - s_sum = 0.0 - - closest_id = self.calcClosestTrajectory(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr - - def CalcArcLengthPathWLid(self, traj): - if len(traj.points) == 0: - return + def getPoint(self, path_point): + if isinstance(path_point, (TrajectoryPoint, PathPoint)): + return path_point + elif isinstance(path_point, PathPointWithLaneId): + return path_point.point + else: + raise TypeError("invalid path_point argument type") - s_arr = [] - ds = 0.0 - s_sum = 0.0 + def CalcDistance(self, point0, point1): + p0 = self.getPoint(point0).pose.position + p1 = self.getPoint(point1).pose.position + dx = p1.x - p0.x + dy = p1.y - p0.y + return np.sqrt(dx**2 + dy**2) - closest_id = self.calcClosestPathWLid(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr - - def CalcArcLengthPath(self, traj): + def CalcArcLength(self, traj): if len(traj.points) == 0: return s_arr = [] - ds = 0.0 s_sum = 0.0 - closest_id = self.calcClosestPath(traj) + closest_id = self.calcClosestIndex(traj) for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - + s_sum -= self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds + s_sum += self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) return s_arr def ToVelList(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list - - def ToVelListPathWLid(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.point.longitudinal_velocity_mps) - return v_list - - def ToVelListPath(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list + return [self.getPoint(p).longitudinal_velocity_mps for p in traj.points] def CalcAcceleration(self, traj): a_arr = [] @@ -606,8 +496,7 @@ def setPlotTrajectory(self): return self.im0, self.im1, self.im2, self.im3, self.im4 def plotTrajectory(self, data): - print("plot called") - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + self.get_logger().info("plot called") # copy trajectory_raw = self.trajectory_raw @@ -663,40 +552,16 @@ def plotTrajectory(self, data): return self.im0, self.im1, self.im2, self.im3, self.im4 - def calcClosestPath(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestPathWLid(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestTrajectory(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcSquaredDist2d(self, p1, p2): - dx = p1.position.x - p2.position.x - dy = p1.position.y - p2.position.y - return dx * dx + dy * dy + def calcClosestIndex(self, path): + points = np.array( + [ + [self.getPoint(p).pose.position.x, self.getPoint(p).pose.position.y] + for p in path.points + ] + ) + self_pose = np.array([self.self_pose.position.x, self.self_pose.position.y]) + dists_squared = np.sum((points - self_pose) ** 2, axis=1) + return np.argmin(dists_squared) def closeFigure(self): plt.close(self.fig)