From 5145bb5e6a3c9542130cc0b04a2aecae54a7239d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 23 May 2024 12:52:19 +0900 Subject: [PATCH 001/120] feat(avoidance): make it selectable output debug marker from yaml (#7097) Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 9 ++- .../data_structs.hpp | 9 ++- .../behavior_path_avoidance_module/debug.hpp | 8 ++- .../parameter_helper.hpp | 15 ++++- .../schema/avoidance.schema.json | 50 +++++++++++++++- .../src/debug.cpp | 59 ++++++++++++------- .../src/manager.cpp | 12 +++- .../src/scene.cpp | 7 +-- 8 files changed, 132 insertions(+), 37 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index dc42af0412254..884812d0db716 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -293,4 +293,11 @@ # for debug debug: - marker: false + enable_other_objects_marker: false + enable_other_objects_info: false + enable_detection_area_marker: false + enable_drivable_bound_marker: false + enable_safety_check_marker: false + enable_shift_line_marker: false + enable_lane_marker: false + enable_misc_marker: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 2e1daed38488f..7958a8a2dcbd4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -337,7 +337,14 @@ struct AvoidanceParameters bool enable_bound_clipping{false}; // debug - bool publish_debug_marker = false; + bool enable_other_objects_marker{false}; + bool enable_other_objects_info{false}; + bool enable_detection_area_marker{false}; + bool enable_drivable_bound_marker{false}; + bool enable_safety_check_marker{false}; + bool enable_shift_line_marker{false}; + bool enable_lane_marker{false}; + bool enable_misc_marker{false}; }; struct ObjectData // avoidance target diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 63cb54d2e3fa1..6a4f206ddf309 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -18,11 +18,13 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/type_alias.hpp" +#include #include namespace behavior_path_planner::utils::avoidance { +using behavior_path_planner::AvoidanceParameters; using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; @@ -42,10 +44,12 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info); +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 25101537850ae..461084c085ca7 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -383,7 +383,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // debug { const std::string ns = "avoidance.debug."; - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); + p.enable_other_objects_marker = + getOrDeclareParameter(*node, ns + "enable_other_objects_marker"); + p.enable_other_objects_info = + getOrDeclareParameter(*node, ns + "enable_other_objects_info"); + p.enable_detection_area_marker = + getOrDeclareParameter(*node, ns + "enable_detection_area_marker"); + p.enable_drivable_bound_marker = + getOrDeclareParameter(*node, ns + "enable_drivable_bound_marker"); + p.enable_safety_check_marker = + getOrDeclareParameter(*node, ns + "enable_safety_check_marker"); + p.enable_shift_line_marker = + getOrDeclareParameter(*node, ns + "enable_shift_line_marker"); + p.enable_lane_marker = getOrDeclareParameter(*node, ns + "enable_lane_marker"); + p.enable_misc_marker = getOrDeclareParameter(*node, ns + "enable_misc_marker"); } return p; diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index 7c0205d2473e5..e0f1156172932 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -1388,13 +1388,57 @@ "debug": { "type": "object", "properties": { - "marker": { + "enable_other_objects_marker": { "type": "boolean", - "description": "Publish debug marker.", + "description": "Publish other objects marker.", + "default": "false" + }, + "enable_other_objects_info": { + "type": "boolean", + "description": "Publish other objects detail information.", + "default": "false" + }, + "enable_detection_area_marker": { + "type": "boolean", + "description": "Publish detection area.", + "default": "false" + }, + "enable_drivable_bound_marker": { + "type": "boolean", + "description": "Publish drivable area boundary.", + "default": "false" + }, + "enable_safety_check_marker": { + "type": "boolean", + "description": "Publish safety check information.", + "default": "false" + }, + "enable_shift_line_marker": { + "type": "boolean", + "description": "Publish shift line information.", + "default": "false" + }, + "enable_lane_marker": { + "type": "boolean", + "description": "Publish lane information.", + "default": "false" + }, + "enable_misc_marker": { + "type": "boolean", + "description": "Publish misc markers.", "default": "false" } }, - "required": ["marker"], + "required": [ + "enable_other_objects_marker", + "enable_other_objects_info", + "enable_detection_area_marker", + "enable_drivable_bound_marker", + "enable_safety_check_marker", + "enable_shift_line_marker", + "enable_lane_marker", + "enable_misc_marker" + ], "additionalProperties": false } }, diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 65450c143ccda..4d3b4605ac956 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -130,7 +130,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: return msg; } -MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) +MarkerArray createObjectInfoMarkerArray( + const ObjectDataArray & objects, std::string && ns, const bool verbose) { MarkerArray msg; @@ -139,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { - { + if (verbose) { marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; @@ -160,6 +161,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st { marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; marker.pose.position.z += 2.0; std::ostringstream string_stream; string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); @@ -201,7 +203,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st createMarkerColor(1.0, 1.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); @@ -220,7 +222,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: createMarkerColor(1.0, 0.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); @@ -428,7 +430,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons return msg; } -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info) +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose) { const auto filtered_objects = [&objects, &info]() { ObjectDataArray ret{}; @@ -456,7 +459,8 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info"), &msg); + appendMarkerArray( + createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); appendMarkerArray( createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg); @@ -500,7 +504,8 @@ MarkerArray createDrivableBounds( } MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const std::shared_ptr & parameters) { using behavior_path_planner::utils::transformToLanelets; using lanelet::visualization::laneletsAsTriangleMarkerArray; @@ -534,7 +539,7 @@ MarkerArray createDebugMarkerArray( }; const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { - add(createOtherObjectsMarkerArray(objects, ns)); + add(createOtherObjectsMarkerArray(objects, ns, parameters->enable_other_objects_info)); }; const auto addShiftLength = @@ -549,7 +554,7 @@ MarkerArray createDebugMarkerArray( }; // ignore objects - { + if (parameters->enable_other_objects_marker) { addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_THRESHOLD); addObjects(data.other_objects, ObjectInfo::IS_NOT_TARGET_OBJECT); addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_GOAL); @@ -569,7 +574,7 @@ MarkerArray createDebugMarkerArray( } // shift line pre-process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); @@ -578,39 +583,39 @@ MarkerArray createDebugMarkerArray( } // merge process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); } // trimming process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); } // registering process - { + if (parameters->enable_shift_line_marker) { addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); } // safety check - { + if (parameters->enable_safety_check_marker) { add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); add(showPredictedPath(debug.collision_check, "ego_predicted_path")); add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); } // shift length - { + if (parameters->enable_shift_line_marker) { addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); } // shift grad - { + if (parameters->enable_shift_line_marker) { addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); @@ -618,15 +623,20 @@ MarkerArray createDebugMarkerArray( } // detection area - size_t i = 0; - for (const auto & detection_area : debug.detection_areas) { - add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + if (parameters->enable_detection_area_marker) { + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } } - // misc - { - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + // drivable bound + if (parameters->enable_drivable_bound_marker) { add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + } + + // lane + if (parameters->enable_lane_marker) { add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), createMarkerColor(0.16, 1.0, 0.69, 0.2))); @@ -636,6 +646,11 @@ MarkerArray createDebugMarkerArray( "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } + // misc + if (parameters->enable_misc_marker) { + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + } + return msg; } } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 7772bd9c2ad35..bdf6f4e822f7b 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -261,7 +261,17 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "marker", p->publish_debug_marker); + updateParam( + parameters, ns + "enable_other_objects_marker", p->enable_other_objects_marker); + updateParam(parameters, ns + "enable_other_objects_info", p->enable_other_objects_info); + updateParam( + parameters, ns + "enable_detection_area_marker", p->enable_detection_area_marker); + updateParam( + parameters, ns + "enable_drivable_bound_marker", p->enable_drivable_bound_marker); + updateParam(parameters, ns + "enable_safety_check_marker", p->enable_safety_check_marker); + updateParam(parameters, ns + "enable_shift_line_marker", p->enable_shift_line_marker); + updateParam(parameters, ns + "enable_lane_marker", p->enable_lane_marker); + updateParam(parameters, ns + "enable_misc_marker", p->enable_misc_marker); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 51fe19e4b3237..15b3e970df4b1 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1360,12 +1360,7 @@ void AvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { debug_marker_.markers.clear(); - - if (!parameters_->publish_debug_marker) { - return; - } - - debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug); + debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); } void AvoidanceModule::updateAvoidanceDebugData( From 7877192cb5700c1db15109dcd959785fc381951d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 23 May 2024 13:59:54 +0900 Subject: [PATCH 002/120] feat(autonomous_emergency_braking): add info about IMU vs Steering info for AEB doc (#7098) * add info about IMU vs Steering Signed-off-by: Daniel Sanchez * steering rate ->angle Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- control/autonomous_emergency_braking/README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index 7a7bf212320e0..23f4476d9659d 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -16,6 +16,24 @@ This module has following assumptions. ![aeb_range](./image/range.drawio.svg) +### IMU path generation: steering angle vs IMU's angular velocity + +Currently, the IMU-based path is generated using the angular velocity obtained by the IMU itself. It has been suggested that the steering angle could be used instead onf the angular velocity. + +The pros and cons of both approaches are: + +IMU angular velocity: + +- (+) Usually, it has high accuracy +- (-)Vehicle vibration might introduce noise. + +Steering angle: + +- (+) Not so noisy +- (-) May have a steering offset or a wrong gear ratio, and the steering angle of Autoware and the real steering may not be the same. + +For the moment, there are no plans to implement the steering angle on the path creation process of the AEB module. + ### Limitations - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. From 3aa84b00dd738414c0cbfc0f02c9a16b77003661 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 23 May 2024 15:27:17 +0900 Subject: [PATCH 003/120] feat(multi_object_tracker): multi object input (#6820) * refactor: frequently used types, namespace Signed-off-by: Taekjin LEE * test: multiple inputs Signed-off-by: Taekjin LEE * feat: check latest measurement time Signed-off-by: Taekjin LEE * feat: define input manager class Signed-off-by: Taekjin LEE * feat: interval measures Signed-off-by: Taekjin LEE * feat: store and sort inputs PoC Signed-off-by: Taekjin LEE * chore: rename classes Signed-off-by: Taekjin LEE * feat: object collector Signed-off-by: Taekjin LEE * impl input manager, no subscribe Signed-off-by: Taekjin LEE * fix: subscribe and trigger callback Signed-off-by: Taekjin LEE * fix: subscriber and callbacks are working Signed-off-by: Taekjin LEE * fix: callback object is fixed, tracker is working Signed-off-by: Taekjin LEE * fix: get object time argument revise Signed-off-by: Taekjin LEE * feat: back to periodic publish, analyze input latency and timings Signed-off-by: Taekjin LEE * fix: enable timing debugger Signed-off-by: Taekjin LEE * fix: separate object interval function Signed-off-by: Taekjin LEE * feat: prepare message triggered process Signed-off-by: Taekjin LEE * feat: trigger tracker by main message arrive Signed-off-by: Taekjin LEE * chore: clean-up, set namespace Signed-off-by: Taekjin LEE * feat: object lists with detector index Signed-off-by: Taekjin LEE * feat: define input channel struct Signed-off-by: Taekjin LEE * fix: define type for object list Signed-off-by: Taekjin LEE * feat: add channel wise existence probability Signed-off-by: Taekjin LEE * fix: relocate debugger Signed-off-by: Taekjin LEE * fix: total existence logic change Signed-off-by: Taekjin LEE * feat: publishing object debug info, need to fix marker id Signed-off-by: Taekjin LEE * feat: indexing marker step 1 Signed-off-by: Taekjin LEE * fix: uuid management Signed-off-by: Taekjin LEE * feat: association line fix Signed-off-by: Taekjin LEE * feat: print channel names Signed-off-by: Taekjin LEE * feat: association lines are color-coded Signed-off-by: Taekjin LEE * fix: association debug marker bugfix Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * feat: add option for debug marker Signed-off-by: Taekjin LEE * feat: skip time statistics update in case of outlier Signed-off-by: Taekjin LEE * feat: auto-tune latency band Signed-off-by: Taekjin LEE * feat: pre-defined channels, select on launcher Signed-off-by: Taekjin LEE * feat: add input channels Signed-off-by: Taekjin LEE * fix: remove marker idx map Signed-off-by: Taekjin LEE * fix: to do not miss the latest message of the target stream Signed-off-by: Taekjin LEE * fix: remove priority, separate timing optimization Signed-off-by: Taekjin LEE * fix: time interval bug fix Signed-off-by: Taekjin LEE * chore: refactoring timing state update Signed-off-by: Taekjin LEE * fix: set parameters optionally Signed-off-by: Taekjin LEE * feat: revise object time range logic Signed-off-by: Taekjin LEE * fix: launcher to set input channels Signed-off-by: Taekjin LEE * fix: exempt spell check 'pointpainting' Signed-off-by: Taekjin LEE * feat: remove expected interval Signed-off-by: Taekjin LEE * feat: implement spawn switch Signed-off-by: Taekjin LEE * fix: remove debug messages Signed-off-by: Taekjin LEE * chore: update readme Signed-off-by: Taekjin LEE * fix: change tentative object topic Signed-off-by: Taekjin LEE * Revert "fix: remove debug messages" This reverts commit 725a49ee6c382f73b54fe50bf9077aca6049e199. Signed-off-by: Taekjin LEE * fix: reset times when jumps to past Signed-off-by: Taekjin LEE * fix: check if interval is negative Signed-off-by: Taekjin LEE * fix: missing config, default value Signed-off-by: Taekjin LEE * fix: remove debug messages Signed-off-by: Taekjin LEE * fix: change no-object message level Signed-off-by: Taekjin LEE * Update perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: Taekjin LEE * chore: Update copyright to uppercase Signed-off-by: Taekjin LEE * chore: fix readme links to config files Signed-off-by: Taekjin LEE * chore: move and rename uuid functions Signed-off-by: Taekjin LEE * chore: fix debug topic to use node name Signed-off-by: Taekjin LEE * chore: express meaning of threshold Signed-off-by: Taekjin LEE * feat: revise decay rate, update function Signed-off-by: Taekjin LEE * fix: define constants with explanation Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- .../tracking/tracking.launch.xml | 2 + .../launch/perception.launch.xml | 1 + .../multi_object_tracker/CMakeLists.txt | 4 +- perception/multi_object_tracker/README.md | 45 +- .../config/input_channels.param.yaml | 82 ++++ .../multi_object_tracker_node.param.yaml | 1 + .../debugger/debug_object.hpp | 102 +++++ .../{ => debugger}/debugger.hpp | 76 +++- .../multi_object_tracker_core.hpp | 30 +- .../processor/input_manager.hpp | 158 +++++++ .../processor/processor.hpp | 16 +- .../tracker/model/bicycle_tracker.hpp | 3 +- .../tracker/model/big_vehicle_tracker.hpp | 3 +- .../model/multiple_vehicle_tracker.hpp | 3 +- .../tracker/model/normal_vehicle_tracker.hpp | 3 +- .../tracker/model/pass_through_tracker.hpp | 3 +- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +- .../tracker/model/pedestrian_tracker.hpp | 3 +- .../tracker/model/tracker_base.hpp | 53 ++- .../tracker/model/unknown_tracker.hpp | 3 +- .../launch/multi_object_tracker.launch.xml | 6 +- .../src/debugger/debug_object.cpp | 389 ++++++++++++++++++ .../src/{ => debugger}/debugger.cpp | 84 +++- .../src/multi_object_tracker_core.cpp | 205 ++++++--- .../src/multi_object_tracker_node.cpp | 2 +- .../src/processor/input_manager.cpp | 347 ++++++++++++++++ .../src/processor/processor.cpp | 47 ++- .../src/tracker/model/bicycle_tracker.cpp | 8 +- .../src/tracker/model/big_vehicle_tracker.cpp | 8 +- .../model/multiple_vehicle_tracker.cpp | 11 +- .../tracker/model/normal_vehicle_tracker.cpp | 8 +- .../tracker/model/pass_through_tracker.cpp | 8 +- .../model/pedestrian_and_bicycle_tracker.cpp | 11 +- .../src/tracker/model/pedestrian_tracker.cpp | 8 +- .../src/tracker/model/tracker_base.cpp | 77 +++- .../src/tracker/model/unknown_tracker.cpp | 8 +- 36 files changed, 1616 insertions(+), 205 deletions(-) create mode 100644 perception/multi_object_tracker/config/input_channels.param.yaml create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp rename perception/multi_object_tracker/include/multi_object_tracker/{ => debugger}/debugger.hpp (66%) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp create mode 100644 perception/multi_object_tracker/src/debugger/debug_object.cpp rename perception/multi_object_tracker/src/{ => debugger}/debugger.cpp (79%) create mode 100644 perception/multi_object_tracker/src/processor/input_manager.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index fed268084792a..cf11c65ac805c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -17,6 +17,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index d60f54c2e641a..e1a2260f3ee6d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 46af1f9b9a1de..fb2c4d73554a3 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,8 +22,10 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp - src/debugger.cpp + src/debugger/debugger.cpp + src/debugger/debug_object.cpp src/processor/processor.cpp + src/processor/input_manager.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/tracker/motion_model/motion_model_base.cpp diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index 311fe7b6da716..ad9e43355aebf 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -46,31 +46,38 @@ Example: ### Input -| Name | Type | Description | -| --------- | ----------------------------------------------------- | ----------- | -| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | obstacles | +Multiple inputs are pre-defined in the input channel parameters (described below) and the inputs can be configured + +| Name | Type | Description | +| ------------------------- | -------------------------- | ---------------------- | +| `selected_input_channels` | `std::vector` | array of channel names | + +- default value: `selected_input_channels:="['detected_objects']"`, merged DetectedObject message +- multi-input example: `selected_input_channels:="['lidar_centerpoint','camera_lidar_fusion','detection_by_tracker','radar_far']"` ### Output -| Name | Type | Description | -| ---------- | ---------------------------------------------------- | ------------------ | -| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | modified obstacles | +| Name | Type | Description | +| ---------- | ---------------------------------------------------- | --------------- | +| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracked objects | ## Parameters - +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------------- | ------------------------------------- | +| `` | | the name of channel | +| `.topic` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | +| `.can_spawn_new_tracker` | `bool` | a switch allow to spawn a new tracker | +| `.optional.name` | `std::string` | channel name for analysis | +| `.optional.short_name` | `std::string` | short name for visualization | ### Core Parameters -Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_object_tracker.param.yaml) and association parameters are defined in [data_association.param.yaml](config/data_association.param.yaml). +Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml). #### Node parameters @@ -80,6 +87,9 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob | `world_frame_id` | double | object kinematics definition frame | | `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | | `publish_rate` | double | Timer frequency to output with delay compensation | +| `publish_processing_time` | bool | enable to publish debug message of process time information | +| `publish_tentative_objects` | bool | enable to publish tentative tracked objects, which have lower confidence | +| `publish_debug_markers` | bool | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection | #### Association parameters @@ -93,13 +103,6 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob ## Assumptions / Known limits - - See the [model explanations](models.md). ## (Optional) Error detection and handling diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml new file mode 100644 index 0000000000000..b57f37675d4f1 --- /dev/null +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + input_channels: + detected_objects: + topic: "/perception/object_recognition/detection/objects" + can_spawn_new_tracker: true + optional: + name: "detected_objects" + short_name: "all" + # LIDAR - rule-based + lidar_clustering: + topic: "/perception/object_recognition/detection/clustering/objects" + can_spawn_new_tracker: true + optional: + name: "clustering" + short_name: "Lcl" + # LIDAR - DNN + lidar_centerpoint: + topic: "/perception/object_recognition/detection/centerpoint/objects" + can_spawn_new_tracker: true + optional: + name: "centerpoint" + short_name: "Lcp" + lidar_centerpoint_validated: + topic: "/perception/object_recognition/detection/centerpoint/validation/objects" + can_spawn_new_tracker: true + optional: + name: "centerpoint" + short_name: "Lcp" + lidar_apollo: + topic: "/perception/object_recognition/detection/apollo/objects" + can_spawn_new_tracker: true + optional: + name: "apollo" + short_name: "Lap" + lidar_apollo_validated: + topic: "/perception/object_recognition/detection/apollo/validation/objects" + can_spawn_new_tracker: true + optional: + name: "apollo" + short_name: "Lap" + # LIDAR-CAMERA - DNN + # cspell:ignore lidar_pointpainitng pointpainting + lidar_pointpainitng: + topic: "/perception/object_recognition/detection/pointpainting/objects" + can_spawn_new_tracker: true + optional: + name: "pointpainting" + short_name: "Lpp" + lidar_pointpainting_validated: + topic: "/perception/object_recognition/detection/pointpainting/validation/objects" + can_spawn_new_tracker: true + optional: + name: "pointpainting" + short_name: "Lpp" + # CAMERA-LIDAR + camera_lidar_fusion: + topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" + can_spawn_new_tracker: true + optional: + name: "camera_lidar_fusion" + short_name: "CLf" + # CAMERA-LIDAR+TRACKER + detection_by_tracker: + topic: "/perception/object_recognition/detection/detection_by_tracker/objects" + can_spawn_new_tracker: false + optional: + name: "detection_by_tracker" + short_name: "dbT" + # RADAR + radar: + topic: "/sensing/radar/detected_objects" + can_spawn_new_tracker: true + optional: + name: "radar" + short_name: "R" + radar_far: + topic: "/perception/object_recognition/detection/radar/far_objects" + can_spawn_new_tracker: true + optional: + name: "radar_far" + short_name: "Rf" diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index ca320c7f58442..09621a40c499f 100644 --- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -17,5 +17,6 @@ # debug parameters publish_processing_time: false publish_tentative_objects: false + publish_debug_markers: false diagnostics_warn_delay: 0.5 # [sec] diagnostics_error_delay: 1.0 # [sec] diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp new file mode 100644 index 0000000000000..6caa69181dd9d --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ + +#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +#include +#include + +#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +struct ObjectData +{ + rclcpp::Time time; + + // object uuid + boost::uuids::uuid uuid; + int uuid_int; + std::string uuid_str; + + // association link, pair of coordinates + // tracker to detection + geometry_msgs::msg::Point tracker_point; + geometry_msgs::msg::Point detection_point; + bool is_associated{false}; + + // existence probabilities + std::vector existence_vector; + + // detection channel id + uint channel_id; +}; + +class TrackerObjectDebugger +{ +public: + explicit TrackerObjectDebugger(std::string frame_id); + +private: + bool is_initialized_{false}; + std::string frame_id_; + visualization_msgs::msg::MarkerArray markers_; + std::unordered_set current_ids_; + std::unordered_set previous_ids_; + rclcpp::Time message_time_; + + std::vector object_data_list_; + std::list unused_marker_ids_; + int32_t marker_id_ = 0; + std::vector> object_data_groups_; + + std::vector channel_names_; + +public: + void setChannelNames(const std::vector & channel_names) + { + channel_names_ = channel_names; + } + void collect( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + + void reset(); + void draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const; + void process(); + void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; +}; + +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp similarity index 66% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 90291ae6fec18..1c632f5bebba0 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,11 +11,11 @@ // 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 MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ + +#include "multi_object_tracker/debugger/debug_object.hpp" #include #include @@ -27,7 +27,11 @@ #include #include +#include #include +#include +#include +#include /** * @brief Debugger class for multi object tracker @@ -36,40 +40,68 @@ class TrackerDebugger { public: - explicit TrackerDebugger(rclcpp::Node & node); - void publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; - void startMeasurementTime( - const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); - void endMeasurementTime(const rclcpp::Time & now); - void startPublishTime(const rclcpp::Time & now); - void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + explicit TrackerDebugger(rclcpp::Node & node, const std::string & frame_id); - void setupDiagnostics(); - void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); +private: struct DEBUG_SETTINGS { bool publish_processing_time; bool publish_tentative_objects; + bool publish_debug_markers; double diagnostics_warn_delay; double diagnostics_error_delay; } debug_settings_; - double pipeline_latency_ms_ = 0.0; - diagnostic_updater::Updater diagnostic_updater_; - bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } -private: - void loadParameters(); - bool is_initialized_ = false; + // ROS node, publishers rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; std::unique_ptr processing_time_publisher_; + rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; + + diagnostic_updater::Updater diagnostic_updater_; + // Object debugger + TrackerObjectDebugger object_debugger_; + + // Time measurement + bool is_initialized_ = false; + double pipeline_latency_ms_ = 0.0; rclcpp::Time last_input_stamp_; rclcpp::Time stamp_process_start_; rclcpp::Time stamp_process_end_; rclcpp::Time stamp_publish_start_; rclcpp::Time stamp_publish_output_; + + // Configuration + void setupDiagnostics(); + void loadParameters(); + +public: + // Object publishing + bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } + void publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + + // Time measurement + void startMeasurementTime( + const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); + void endMeasurementTime(const rclcpp::Time & now); + void startPublishTime(const rclcpp::Time & now); + void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Debug object + void setObjectChannels(const std::vector & channels) + { + object_debugger_.setChannelNames(channels); + } + void collectObjectInfo( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + void publishObjectsMarkers(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index f788dd3895216..3daeaa2a46322 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -20,7 +20,8 @@ #define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ #include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" +#include "multi_object_tracker/processor/input_manager.hpp" #include "multi_object_tracker/processor/processor.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" @@ -48,8 +49,16 @@ #include #include #include +#include #include +namespace multi_object_tracker +{ + +using DetectedObject = autoware_auto_perception_msgs::msg::DetectedObject; +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; +using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; + class MultiObjectTracker : public rclcpp::Node { public: @@ -57,10 +66,8 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr - tracked_objects_pub_; - rclcpp::Subscription::SharedPtr - detected_object_sub_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -78,15 +85,24 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; std::unique_ptr processor_; + // input manager + std::unique_ptr input_manager_; + + std::vector input_channels_{}; + size_t input_channel_size_{}; + // callback functions - void onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); + void onTrigger(); + void onMessage(const ObjectsList & objects_list); // publish processes + void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; +} // namespace multi_object_tracker + #endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp new file mode 100644 index 0000000000000..ad8c4ae6e5e24 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -0,0 +1,158 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include +#include +#include + +namespace multi_object_tracker +{ +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; +using ObjectsList = std::vector>; + +struct InputChannel +{ + std::string input_topic; // topic name of the detection, e.g. "/detection/lidar" + std::string long_name = "Detected Object"; // full name of the detection + std::string short_name = "DET"; // abbreviation of the name + bool is_spawn_enabled = true; // enable spawn of the object +}; + +class InputStream +{ +public: + explicit InputStream(rclcpp::Node & node, uint & index); + + void init(const InputChannel & input_channel); + + void setQueueSize(size_t que_size) { que_size_ = que_size; } + void setTriggerFunction(std::function func_trigger) + { + func_trigger_ = func_trigger; + } + + void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time); + + bool isTimeInitialized() const { return initial_count_ > 0; } + uint getIndex() const { return index_; } + void getObjectsOlderThan( + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + ObjectsList & objects_list); + void getNames(std::string & long_name, std::string & short_name) + { + long_name = long_name_; + short_name = short_name_; + } + bool isSpawnEnabled() const { return is_spawn_enabled_; } + + std::string getLongName() const { return long_name_; } + size_t getObjectsCount() const { return objects_que_.size(); } + void getTimeStatistics( + double & latency_mean, double & latency_var, double & interval_mean, + double & interval_var) const + { + latency_mean = latency_mean_; + latency_var = latency_var_; + interval_mean = interval_mean_; + interval_var = interval_var_; + } + void getLatencyStatistics(double & latency_mean, double & latency_var) const + { + latency_mean = latency_mean_; + latency_var = latency_var_; + } + bool getTimestamps( + rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; + rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; } + +private: + rclcpp::Node & node_; + uint index_; + + std::string input_topic_; + std::string long_name_; + std::string short_name_; + bool is_spawn_enabled_; + + size_t que_size_{30}; + std::deque objects_que_; + + std::function func_trigger_; + + // bool is_time_initialized_{false}; + int initial_count_{0}; + double expected_interval_; + double latency_mean_; + double latency_var_; + double interval_mean_; + double interval_var_; + + rclcpp::Time latest_measurement_time_; + rclcpp::Time latest_message_time_; +}; + +class InputManager +{ +public: + explicit InputManager(rclcpp::Node & node); + + void init(const std::vector & input_channels); + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + + void onTrigger(const uint & index) const; + + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const; + void optimizeTimings(); + bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); + bool isChannelSpawnEnabled(const uint & index) const + { + return input_streams_[index]->isSpawnEnabled(); + } + +private: + rclcpp::Node & node_; + std::vector::SharedPtr> sub_objects_array_{}; + + bool is_initialized_{false}; + rclcpp::Time latest_object_time_; + + size_t input_size_; + std::vector> input_streams_; + + std::function func_trigger_; + uint target_stream_idx_{0}; + double target_stream_latency_{0.2}; // [s] + double target_stream_latency_std_{0.04}; // [s] + double target_stream_interval_{0.1}; // [s] + double target_stream_interval_std_{0.02}; // [s] + double target_latency_{0.2}; // [s] + double target_latency_band_{1.0}; // [s] +}; + +} // namespace multi_object_tracker + +#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index 6d6e364d83a41..310871dfcb07a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,8 +11,6 @@ // 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 MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ #define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ @@ -34,7 +32,8 @@ class TrackerProcessor { public: - explicit TrackerProcessor(const std::map & tracker_map); + explicit TrackerProcessor( + const std::map & tracker_map, const size_t & channel_size); const std::list> & getListTracker() const { return list_tracker_; } // tracker processes @@ -42,11 +41,11 @@ class TrackerProcessor void update( const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment); + const std::unordered_map & direct_assignment, const uint & channel_index); void spawn( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment); + const std::unordered_map & reverse_assignment, const uint & channel_index); void prune(const rclcpp::Time & time); // output @@ -58,9 +57,12 @@ class TrackerProcessor const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void getExistenceProbabilities(std::vector> & existence_vectors) const; + private: std::map tracker_map_; std::list> list_tracker_; + const size_t channel_size_; // parameters float max_elapsed_time_; // [s] @@ -73,7 +75,7 @@ class TrackerProcessor void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const; + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; #endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 2d69334a2f43f..ead0d8dbf0e5a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -56,7 +56,8 @@ class BicycleTracker : public Tracker public: BicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 8d49138b94a24..3b509a3cb47d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -61,7 +61,8 @@ class BigVehicleTracker : public Tracker public: BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 5722566b4a633..fef8caaf20d8a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -35,7 +35,8 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 6ab7e63bce167..b7810d2471416 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -61,7 +61,8 @@ class NormalVehicleTracker : public Tracker public: NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 48a8702d182a0..0f892098a373a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -34,7 +34,8 @@ class PassThroughTracker : public Tracker public: PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index cf6a82088c7b6..490f6d93c6f6a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -34,7 +34,8 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index a56250390e34f..72f5a5a002f0f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -64,7 +64,8 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index ab6a747288d4a..348ba1f5d7383 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -35,33 +35,41 @@ class Tracker { -protected: - unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } - void setClassification( - const std::vector & classification) - { - classification_ = classification; - } - void updateClassification( - const std::vector & classification); - private: unique_identifier_msgs::msg::UUID uuid_; + + // classification std::vector classification_; + + // existence states int no_measurement_count_; int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; + std::vector existence_probabilities_; + float total_existence_probability_; public: Tracker( const rclcpp::Time & time, - const std::vector & classification); + const std::vector & classification, + const size_t & channel_size); virtual ~Tracker() {} + + void initializeExistenceProbabilities( + const uint & channel_index, const float & existence_probability); + bool getExistenceProbabilityVector(std::vector & existence_vector) const + { + existence_vector = existence_probabilities_; + return existence_vector.size() > 0; + } bool updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform); - bool updateWithoutMeasurement(); + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, + const uint & channel_index); + bool updateWithoutMeasurement(const rclcpp::Time & now); + + // classification std::vector getClassification() const { return classification_; @@ -70,6 +78,8 @@ class Tracker { return object_recognition_utils::getHighestProbLabel(classification_); } + + // existence states int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } int getTotalMeasurementCount() const { return total_measurement_count_; } @@ -77,13 +87,22 @@ class Tracker { return (current_time - last_update_with_measurement_time_).seconds(); } + +protected: + unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } + void setClassification( + const std::vector & classification) + { + classification_ = classification; + } + void updateClassification( + const std::vector & classification); + + // virtual functions +public: virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance( const rclcpp::Time & time) const; - /* - * Pure virtual function - */ - protected: virtual bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 73bf7849e13d8..3ef58773b408f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -49,7 +49,8 @@ class UnknownTracker : public Tracker public: UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index dcd080b851c20..526ce23efd33b 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -1,14 +1,16 @@ - + + - + + diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp new file mode 100644 index 0000000000000..ebbeab43a155b --- /dev/null +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -0,0 +1,389 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_object_tracker/debugger/debug_object.hpp" + +#include "autoware_auto_perception_msgs/msg/tracked_object.hpp" + +#include + +#include +#include +#include +#include + +namespace +{ +std::string uuidToString(const unique_identifier_msgs::msg::UUID & uuid_msg) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid_msg.uuid[i]; + } + return ss.str(); +} + +boost::uuids::uuid uuidToBoostUuid(const unique_identifier_msgs::msg::UUID & uuid_msg) +{ + const std::string uuid_str = uuidToString(uuid_msg); + boost::uuids::string_generator gen; + boost::uuids::uuid uuid = gen(uuid_str); + return uuid; +} + +int32_t uuidToInt(const boost::uuids::uuid & uuid) +{ + return boost::uuids::hash_value(uuid); +} +} // namespace + +TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) +{ + // set frame id + frame_id_ = frame_id; + + // initialize markers + markers_.markers.clear(); + current_ids_.clear(); + previous_ids_.clear(); + message_time_ = rclcpp::Time(0, 0); +} + +void TrackerObjectDebugger::reset() +{ + // maintain previous ids + previous_ids_.clear(); + previous_ids_ = current_ids_; + current_ids_.clear(); + + // clear markers, object data list + object_data_list_.clear(); + markers_.markers.clear(); +} + +void TrackerObjectDebugger::collect( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & /*reverse_assignment*/) +{ + if (!is_initialized_) is_initialized_ = true; + + message_time_ = message_time; + + int tracker_idx = 0; + for (auto tracker_itr = list_tracker.begin(); tracker_itr != list_tracker.end(); + ++tracker_itr, ++tracker_idx) { + ObjectData object_data; + object_data.time = message_time; + object_data.channel_id = channel_index; + + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + (*(tracker_itr))->getTrackedObject(message_time, tracked_object); + object_data.uuid = uuidToBoostUuid(tracked_object.object_id); + object_data.uuid_int = uuidToInt(object_data.uuid); + object_data.uuid_str = uuidToString(tracked_object.object_id); + + // tracker + bool is_associated = false; + geometry_msgs::msg::Point tracker_point, detection_point; + tracker_point.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; + tracker_point.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; + tracker_point.z = tracked_object.kinematics.pose_with_covariance.pose.position.z; + + // detection + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { + const auto & associated_object = + detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); + detection_point.x = associated_object.kinematics.pose_with_covariance.pose.position.x; + detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y; + detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z; + is_associated = true; + } else { + detection_point.x = tracker_point.x; + detection_point.y = tracker_point.y; + detection_point.z = tracker_point.z; + } + + object_data.tracker_point = tracker_point; + object_data.detection_point = detection_point; + object_data.is_associated = is_associated; + + // existence probabilities + std::vector existence_vector; + (*(tracker_itr))->getExistenceProbabilityVector(existence_vector); + object_data.existence_vector = existence_vector; + + object_data_list_.push_back(object_data); + } +} + +void TrackerObjectDebugger::process() +{ + if (!is_initialized_) return; + + // update uuid_int + for (auto & object_data : object_data_list_) { + current_ids_.insert(object_data.uuid_int); + } + + // sort by uuid, collect the same uuid object_data as a group, and loop for the groups + object_data_groups_.clear(); + { + // sort by uuid + std::sort( + object_data_list_.begin(), object_data_list_.end(), + [](const ObjectData & a, const ObjectData & b) { return a.uuid < b.uuid; }); + + // collect the same uuid object_data as a group + std::vector object_data_group; + boost::uuids::uuid previous_uuid = object_data_list_.front().uuid; + for (const auto & object_data : object_data_list_) { + // if the uuid is different, push the group and clear the group + if (object_data.uuid != previous_uuid) { + // push the group + object_data_groups_.push_back(object_data_group); + object_data_group.clear(); + previous_uuid = object_data.uuid; + } + // push the object_data to the group + object_data_group.push_back(object_data); + } + // push the last group + object_data_groups_.push_back(object_data_group); + } +} + +void TrackerObjectDebugger::draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const +{ + // initialize markers + marker_array.markers.clear(); + + constexpr int PALETTE_SIZE = 16; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{0.0, 0.0, 1.0}}, // Blue + {{0.0, 1.0, 0.0}}, // Green + {{1.0, 1.0, 0.0}}, // Yellow + {{1.0, 0.0, 0.0}}, // Red + {{0.0, 1.0, 1.0}}, // Cyan + {{1.0, 0.0, 1.0}}, // Magenta + {{1.0, 0.64, 0.0}}, // Orange + {{0.75, 1.0, 0.0}}, // Lime + {{0.0, 0.5, 0.5}}, // Teal + {{0.5, 0.0, 0.5}}, // Purple + {{1.0, 0.75, 0.8}}, // Pink + {{0.65, 0.17, 0.17}}, // Brown + {{0.5, 0.0, 0.0}}, // Maroon + {{0.5, 0.5, 0.0}}, // Olive + {{0.0, 0.0, 0.5}}, // Navy + {{0.5, 0.5, 0.5}} // Grey + }}; + + for (const auto & object_data_group : object_data_groups) { + if (object_data_group.empty()) continue; + const auto object_data_front = object_data_group.front(); + const auto object_data_back = object_data_group.back(); + + // set a reference marker + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = object_data_front.time; + marker.id = object_data_front.uuid_int; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; // white + marker.lifetime = rclcpp::Duration::from_seconds(0); + + // get marker - existence_probability + visualization_msgs::msg::Marker text_marker; + text_marker = marker; + text_marker.ns = "existence_probability"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose.position.z += 1.8; + text_marker.scale.z = 0.5; + text_marker.pose.position.x = object_data_front.tracker_point.x; + text_marker.pose.position.y = object_data_front.tracker_point.y; + text_marker.pose.position.z = object_data_front.tracker_point.z + 2.5; + + // show the last existence probability + // print existence probability with channel name + // probability to text, two digits of percentage + std::string existence_probability_text = ""; + for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) { + std::stringstream stream; + stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; + existence_probability_text += channel_names_[i] + stream.str() + ":"; + } + existence_probability_text = + existence_probability_text.substr(0, existence_probability_text.size() - 1); + existence_probability_text += "\n" + object_data_front.uuid_str.substr(0, 6); + + text_marker.text = existence_probability_text; + marker_array.markers.push_back(text_marker); + + // loop for each object_data in the group + // boxed to tracker positions + // and link lines to the detected positions + const double marker_height_offset = 1.0; + const double assign_height_offset = 0.6; + + visualization_msgs::msg::Marker marker_track_boxes; + marker_track_boxes = marker; + marker_track_boxes.ns = "track_boxes"; + marker_track_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_track_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_track_boxes.scale.x = 0.4; + marker_track_boxes.scale.y = 0.4; + marker_track_boxes.scale.z = 0.4; + + // make detected object markers per channel + std::vector marker_detect_boxes_per_channel; + std::vector marker_detect_lines_per_channel; + + for (size_t idx = 0; idx < channel_names_.size(); idx++) { + // get color - by channel index + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + color.r = color_array[idx % PALETTE_SIZE][0]; + color.g = color_array[idx % PALETTE_SIZE][1]; + color.b = color_array[idx % PALETTE_SIZE][2]; + + visualization_msgs::msg::Marker marker_detect_boxes; + marker_detect_boxes = marker; + marker_detect_boxes.ns = "detect_boxes_" + channel_names_[idx]; + marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_detect_boxes.scale.x = 0.2; + marker_detect_boxes.scale.y = 0.2; + marker_detect_boxes.scale.z = 0.2; + marker_detect_boxes.color = color; + marker_detect_boxes_per_channel.push_back(marker_detect_boxes); + + visualization_msgs::msg::Marker marker_lines; + marker_lines = marker; + marker_lines.ns = "association_lines_" + channel_names_[idx]; + marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; + marker_lines.action = visualization_msgs::msg::Marker::ADD; + marker_lines.scale.x = 0.15; + marker_lines.points.clear(); + marker_lines.color = color; + marker_detect_lines_per_channel.push_back(marker_lines); + } + + bool is_any_associated = false; + + for (const auto & object_data : object_data_group) { + int channel_id = object_data.channel_id; + + // set box + geometry_msgs::msg::Point box_point; + box_point.x = object_data.tracker_point.x; + box_point.y = object_data.tracker_point.y; + box_point.z = object_data.tracker_point.z + marker_height_offset; + marker_track_boxes.points.push_back(box_point); + + // set association marker, if exists + if (!object_data.is_associated) continue; + is_any_associated = true; + + // associated object box + visualization_msgs::msg::Marker & marker_detect_boxes = + marker_detect_boxes_per_channel.at(channel_id); + box_point.x = object_data.detection_point.x; + box_point.y = object_data.detection_point.y; + box_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset; + marker_detect_boxes.points.push_back(box_point); + + // association line + visualization_msgs::msg::Marker & marker_lines = + marker_detect_lines_per_channel.at(channel_id); + geometry_msgs::msg::Point line_point; + line_point.x = object_data.tracker_point.x; + line_point.y = object_data.tracker_point.y; + line_point.z = object_data.tracker_point.z + marker_height_offset; + marker_lines.points.push_back(line_point); + line_point.x = object_data.detection_point.x; + line_point.y = object_data.detection_point.y; + line_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset; + marker_lines.points.push_back(line_point); + } + + // add markers + marker_array.markers.push_back(marker_track_boxes); + if (is_any_associated) { + for (size_t i = 0; i < channel_names_.size(); i++) { + if (marker_detect_boxes_per_channel.at(i).points.empty()) continue; + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channel_names_.size(); i++) { + if (marker_detect_lines_per_channel.at(i).points.empty()) continue; + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); + } + } else { + for (size_t i = 0; i < channel_names_.size(); i++) { + marker_detect_boxes_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channel_names_.size(); i++) { + marker_detect_lines_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); + } + } + } + + return; +} + +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const +{ + if (!is_initialized_) return; + + // draw markers + draw(object_data_groups_, marker_array); + + // remove old markers + for (const auto & previous_id : previous_ids_) { + if (current_ids_.find(previous_id) != current_ids_.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = frame_id_; + delete_marker.header.stamp = message_time_; + delete_marker.ns = "existence_probability"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "track_boxes"; + marker_array.markers.push_back(delete_marker); + + for (size_t idx = 0; idx < channel_names_.size(); idx++) { + delete_marker.ns = "detect_boxes_" + channel_names_[idx]; + marker_array.markers.push_back(delete_marker); + delete_marker.ns = "association_lines_" + channel_names_[idx]; + marker_array.markers.push_back(delete_marker); + } + } + + return; +} diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp similarity index 79% rename from perception/multi_object_tracker/src/debugger.cpp rename to perception/multi_object_tracker/src/debugger/debugger.cpp index b5632a13e78fc..afb0ecdc22996 100644 --- a/perception/multi_object_tracker/src/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,14 +11,13 @@ // 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 "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) +: node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -31,7 +30,12 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_ = node_.create_publisher( - "debug/tentative_objects", rclcpp::QoS{1}); + "~/debug/tentative_objects", rclcpp::QoS{1}); + } + + if (debug_settings_.publish_debug_markers) { + debug_objects_markers_pub_ = node_.create_publisher( + "~/debug/objects_markers", rclcpp::QoS{1}); } // initialize timestamps @@ -46,14 +50,6 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod setupDiagnostics(); } -void TrackerDebugger::setupDiagnostics() -{ - diagnostic_updater_.setHardwareID(node_.get_name()); - diagnostic_updater_.add( - "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); - diagnostic_updater_.setPeriod(0.1); -} - void TrackerDebugger::loadParameters() { try { @@ -61,6 +57,7 @@ void TrackerDebugger::loadParameters() node_.declare_parameter("publish_processing_time"); debug_settings_.publish_tentative_objects = node_.declare_parameter("publish_tentative_objects"); + debug_settings_.publish_debug_markers = node_.declare_parameter("publish_debug_markers"); debug_settings_.diagnostics_warn_delay = node_.declare_parameter("diagnostics_warn_delay"); debug_settings_.diagnostics_error_delay = @@ -69,11 +66,32 @@ void TrackerDebugger::loadParameters() RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); debug_settings_.publish_processing_time = false; debug_settings_.publish_tentative_objects = false; + debug_settings_.publish_debug_markers = false; debug_settings_.diagnostics_warn_delay = 0.5; debug_settings_.diagnostics_error_delay = 1.0; } } +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); +} + +// Object publishing functions + +void TrackerDebugger::publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +// Time measurement functions + void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) { if (!is_initialized_) { @@ -97,14 +115,6 @@ void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & s stat.add("Detection delay", delay); } -void TrackerDebugger::publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const -{ - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_->publish(tentative_objects); - } -} - void TrackerDebugger::startMeasurementTime( const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp) { @@ -167,3 +177,33 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim } stamp_publish_output_ = now; } + +void TrackerDebugger::collectObjectInfo( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment) +{ + if (!debug_settings_.publish_debug_markers) return; + object_debugger_.collect( + message_time, list_tracker, channel_index, detected_objects, direct_assignment, + reverse_assignment); +} + +// ObjectDebugger +void TrackerDebugger::publishObjectsMarkers() +{ + if (!debug_settings_.publish_debug_markers) return; + + visualization_msgs::msg::MarkerArray marker_message; + // process data + object_debugger_.process(); + + // publish markers + object_debugger_.getMessage(marker_message); + debug_objects_markers_pub_->publish(marker_message); + + // reset object data + object_debugger_.reset(); +} diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d80a21813b28b..c3bf7023fb068 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -36,8 +36,6 @@ #include #include -using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - namespace { // Function to get the transform between two frames @@ -67,24 +65,79 @@ boost::optional getTransformAnonymous( } // namespace +namespace multi_object_tracker +{ +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), last_published_time_(this->now()) { - // Create publishers and subscribers - detected_object_sub_ = create_subscription( - "input", rclcpp::QoS{1}, - std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1)); - tracked_objects_pub_ = - create_publisher("output", rclcpp::QoS{1}); - // Get parameters double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + declare_parameter("selected_input_channels", std::vector()); + std::vector selected_input_channels = + get_parameter("selected_input_channels").as_string_array(); + + // ROS interface - Publisher + tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + + // ROS interface - Input channels + // Get input channels + std::vector input_topic_names; + std::vector input_names_long; + std::vector input_names_short; + std::vector input_is_spawn_enabled; + + if (selected_input_channels.empty()) { + RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); + return; + } + + for (const auto & selected_input_channel : selected_input_channels) { + // required parameters, no default value + const std::string input_topic_name = + declare_parameter("input_channels." + selected_input_channel + ".topic"); + input_topic_names.push_back(input_topic_name); + + // required parameter, but can set a default value + const bool spawn_enabled = declare_parameter( + "input_channels." + selected_input_channel + ".can_spawn_new_tracker", true); + input_is_spawn_enabled.push_back(spawn_enabled); + + // optional parameters + const std::string default_name = selected_input_channel; + const std::string name_long = declare_parameter( + "input_channels." + selected_input_channel + ".optional.name", default_name); + input_names_long.push_back(name_long); + + const std::string default_name_short = selected_input_channel.substr(0, 3); + const std::string name_short = declare_parameter( + "input_channels." + selected_input_channel + ".optional.short_name", default_name_short); + input_names_short.push_back(name_short); + } + + input_channel_size_ = input_topic_names.size(); + input_channels_.resize(input_channel_size_); + + for (size_t i = 0; i < input_channel_size_; i++) { + input_channels_[i].input_topic = input_topic_names[i]; + input_channels_[i].long_name = input_names_long[i]; + input_channels_[i].short_name = input_names_short[i]; + input_channels_[i].is_spawn_enabled = input_is_spawn_enabled[i]; + } + + // Initialize input manager + input_manager_ = std::make_unique(*this); + input_manager_->init(input_channels_); // Initialize input manager, set subscriptions + input_manager_->setTriggerFunction( + std::bind(&MultiObjectTracker::onTrigger, this)); // Set trigger function + // Create tf timer auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); @@ -119,7 +172,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracker_map.insert(std::make_pair( Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); - processor_ = std::make_unique(tracker_map); + processor_ = std::make_unique(tracker_map, input_channel_size_); } // Data association initialization @@ -138,35 +191,96 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } // Debugger - debugger_ = std::make_unique(*this); + debugger_ = std::make_unique(*this, world_frame_id_); + debugger_->setObjectChannels(input_names_short); published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) +void MultiObjectTracker::onTrigger() +{ + const rclcpp::Time current_time = this->now(); + // get objects from the input manager and run process + std::vector> objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); + if (!is_objects_ready) return; + + onMessage(objects_list); + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + + // Publish objects if the timer is not enabled + if (publish_timer_ == nullptr) { + // if the delay compensation is disabled, publish the objects in the latest time + publish(latest_time); + } else { + // Publish if the next publish time is close + const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period + if ((current_time - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(current_time); + } + } +} + +void MultiObjectTracker::onTimer() +{ + const rclcpp::Time current_time = this->now(); + + // Check the publish period + const auto elapsed_time = (current_time - last_published_time_).seconds(); + // If the elapsed time is over the period, publish objects with prediction + constexpr double maximum_latency_ratio = 1.11; // 11% margin + const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; + if (elapsed_time < maximum_publish_latency) return; + + // get objects from the input manager and run process + ObjectsList objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); + if (is_objects_ready) { + onMessage(objects_list); + } + + // Publish + checkAndPublish(current_time); +} + +void MultiObjectTracker::onMessage(const ObjectsList & objects_list) +{ + const rclcpp::Time current_time = this->now(); + const rclcpp::Time oldest_time(objects_list.front().second.header.stamp); + + // process start + debugger_->startMeasurementTime(this->now(), oldest_time); + // run process for each DetectedObjects + for (const auto & objects_data : objects_list) { + runProcess(objects_data.second, objects_data.first); + } + // process end + debugger_->endMeasurementTime(this->now()); +} + +void MultiObjectTracker::runProcess( + const DetectedObjects & input_objects_msg, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = - rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type()); + rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type()); - /* keep the latest input stamp and check transform*/ - debugger_->startMeasurementTime(this->now(), measurement_time); + // Get the transform of the self frame const auto self_transform = getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); if (!self_transform) { return; } - /* transform to world coordinate */ - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + + // Transform the objects to the world frame + DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( - *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } - ////// Tracker Process - //// Associate and update /* prediction */ processor_->predict(measurement_time); + /* object association */ std::unordered_map direct_assignment, reverse_assignment; { @@ -176,40 +290,22 @@ void MultiObjectTracker::onMeasurement( Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + + // Collect debug information - tracker list, existence probabilities, association results + debugger_->collectObjectInfo( + measurement_time, processor_->getListTracker(), channel_index, transformed_objects, + direct_assignment, reverse_assignment); } + /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment); + processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + /* tracker pruning */ processor_->prune(measurement_time); - /* spawn new tracker */ - processor_->spawn(transformed_objects, *self_transform, reverse_assignment); - - // debugger time - debugger_->endMeasurementTime(this->now()); - - // Publish objects if the timer is not enabled - if (publish_timer_ == nullptr) { - // Publish if the delay compensation is disabled - publish(measurement_time); - } else { - // Publish if the next publish time is close - const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(this->now()); - } - } -} -void MultiObjectTracker::onTimer() -{ - const rclcpp::Time current_time = this->now(); - // Check the publish period - const auto elapsed_time = (current_time - last_published_time_).seconds(); - // If the elapsed time is over the period, publish objects with prediction - constexpr double maximum_latency_ratio = 1.11; // 11% margin - const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; - if (elapsed_time > maximum_publish_latency) { - checkAndPublish(current_time); + /* spawn new tracker */ + if (input_manager_->isChannelSpawnEnabled(channel_index)) { + processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); } } @@ -234,7 +330,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; + TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -246,11 +342,14 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - autoware_auto_perception_msgs::msg::TrackedObjects tentative_objects_msg; + TrackedObjects tentative_objects_msg; tentative_objects_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_objects_msg); debugger_->publishTentativeObjects(tentative_objects_msg); } + debugger_->publishObjectsMarkers(); } -RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) +} // namespace multi_object_tracker + +RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp index c6eed297c02c4..273e6ac1a06bd 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; - auto multi_object_tracker = std::make_shared(options); + auto multi_object_tracker = std::make_shared(options); rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(multi_object_tracker); exec.spin(); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp new file mode 100644 index 0000000000000..e29b54e971491 --- /dev/null +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -0,0 +1,347 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_object_tracker/processor/input_manager.hpp" + +#include + +namespace multi_object_tracker +{ +/////////////////////////// +/////// InputStream /////// +/////////////////////////// +InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index) +{ +} + +void InputStream::init(const InputChannel & input_channel) +{ + // Initialize parameters + input_topic_ = input_channel.input_topic; + long_name_ = input_channel.long_name; + short_name_ = input_channel.short_name; + is_spawn_enabled_ = input_channel.is_spawn_enabled; + + // Initialize queue + objects_que_.clear(); + + // Initialize latency statistics + latency_mean_ = 0.2; // [s] (initial value) + latency_var_ = 0.0; + interval_mean_ = 0.0; // [s] (initial value) + interval_var_ = 0.0; + + latest_measurement_time_ = node_.now(); + latest_message_time_ = node_.now(); +} + +bool InputStream::getTimestamps( + rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const +{ + if (!isTimeInitialized()) { + return false; + } + latest_measurement_time = latest_measurement_time_; + latest_message_time = latest_message_time_; + return true; +} + +void InputStream::onMessage( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) +{ + const DetectedObjects objects = *msg; + objects_que_.push_back(objects); + if (objects_que_.size() > que_size_) { + objects_que_.pop_front(); + } + + // update the timing statistics + rclcpp::Time now = node_.now(); + rclcpp::Time objects_time(objects.header.stamp); + updateTimingStatus(now, objects_time); + + // trigger the function if it is set + if (func_trigger_) { + func_trigger_(index_); + } +} + +void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time) +{ + // Define constants + constexpr int SKIP_COUNT = 4; // Skip the initial messages + constexpr int INITIALIZATION_COUNT = 16; // Initialization process count + + // Update latency statistics + // skip initial messages for the latency statistics + if (initial_count_ > SKIP_COUNT) { + const double latency = (now - objects_time).seconds(); + if (initial_count_ < INITIALIZATION_COUNT) { + // set higher gain for the initial messages + constexpr double initial_gain = 0.5; + latency_mean_ = (1.0 - initial_gain) * latency_mean_ + initial_gain * latency; + } else { + constexpr double gain = 0.05; + latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; + const double latency_delta = latency - latency_mean_; + latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; + } + } + + // Calculate interval, Update interval statistics + if (initial_count_ > SKIP_COUNT) { + const double interval = (now - latest_message_time_).seconds(); + if (interval < 0.0) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, " + "latest_message_time_: %f", + long_name_.c_str(), now.seconds(), latest_message_time_.seconds()); + } else if (initial_count_ < INITIALIZATION_COUNT) { + // Initialization + constexpr double initial_gain = 0.5; + interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval; + } else { + // The interval is considered regular if it is within 0.5 and 1.5 times the mean interval + bool update_statistics = interval > 0.5 * interval_mean_ && interval < 1.5 * interval_mean_; + if (update_statistics) { + constexpr double gain = 0.05; + interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; + const double interval_delta = interval - interval_mean_; + interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + } + } + } + + // Update time + latest_message_time_ = now; + constexpr double delay_threshold = 3.0; // [s] + if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) { + // Reset the latest measurement time if the time difference is too large + latest_measurement_time_ = objects_time; + RCLCPP_WARN( + node_.get_logger(), + "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f", + long_name_.c_str(), objects_time.seconds()); + } else { + // Aware reversed message arrival + latest_measurement_time_ = + latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; + } + + // Update the initial count + if (initial_count_ < INITIALIZATION_COUNT) { + initial_count_++; + } +} + +void InputStream::getObjectsOlderThan( + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + ObjectsList & objects_list) +{ + assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); + + for (const auto & object : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); + + // remove objects older than the specified duration + if (object_time < object_oldest_time) { + objects_que_.pop_front(); + continue; + } + + // Add the object if the object is older than the specified latest time + if (object_latest_time >= object_time) { + std::pair object_pair(index_, object); + objects_list.push_back(object_pair); + // remove the object from the queue + objects_que_.pop_front(); + } + } +} + +//////////////////////////// +/////// InputManager /////// +//////////////////////////// +InputManager::InputManager(rclcpp::Node & node) : node_(node) +{ + latest_object_time_ = node_.now(); +} + +void InputManager::init(const std::vector & input_channels) +{ + // Check input sizes + input_size_ = input_channels.size(); + if (input_size_ == 0) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams"); + return; + } + + // Initialize input streams + sub_objects_array_.resize(input_size_); + bool is_any_spawn_enabled = false; + for (size_t i = 0; i < input_size_; i++) { + uint index(i); + InputStream input_stream(node_, index); + input_stream.init(input_channels[i]); + input_stream.setTriggerFunction( + std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); + input_streams_.push_back(std::make_shared(input_stream)); + is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled(); + + // Set subscription + RCLCPP_INFO( + node_.get_logger(), "InputManager::init Initializing %s input stream from %s", + input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); + std::function func = + std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); + } + + // Check if any spawn enabled input streams + if (!is_any_spawn_enabled) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No spawn enabled input streams"); + return; + } + is_initialized_ = true; +} + +void InputManager::onTrigger(const uint & index) const +{ + // when the target stream triggers, call the trigger function + if (index == target_stream_idx_ && func_trigger_) { + func_trigger_(); + } +} + +void InputManager::getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const +{ + object_latest_time = + now - rclcpp::Duration::from_seconds( + target_stream_latency_ - + 0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin + // check the target stream can be included in the object time interval + if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) { + rclcpp::Time latest_measurement_time = + input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); + + // if the object_latest_time is older than the latest measurement time, set it as the latest + // object time + object_latest_time = + object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; + } + + object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); + // if the object_oldest_time is older than the latest object time, set it to the latest object + // time + object_oldest_time = + object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; +} + +void InputManager::optimizeTimings() +{ + double max_latency_mean = 0.0; + uint selected_stream_idx = 0; + double selected_stream_latency_std = 0.1; + double selected_stream_interval = 0.1; + double selected_stream_interval_std = 0.01; + + { + // ANALYSIS: Get the streams statistics + // select the stream that has the maximum latency + double latency_mean, latency_var, interval_mean, interval_var; + for (const auto & input_stream : input_streams_) { + if (!input_stream->isTimeInitialized()) continue; + input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); + if (latency_mean > max_latency_mean) { + max_latency_mean = latency_mean; + selected_stream_idx = input_stream->getIndex(); + selected_stream_latency_std = std::sqrt(latency_var); + selected_stream_interval = interval_mean; + selected_stream_interval_std = std::sqrt(interval_var); + } + } + } + + // Set the target stream index, which has the maximum latency + // trigger will be called next time + // if no stream is initialized, the target stream index will be 0 and wait for the initialization + target_stream_idx_ = selected_stream_idx; + target_stream_latency_ = max_latency_mean; + target_stream_latency_std_ = selected_stream_latency_std; + target_stream_interval_ = selected_stream_interval; + target_stream_interval_std_ = selected_stream_interval_std; +} + +bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list) +{ + if (!is_initialized_) { + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); + return false; + } + + // Clear the objects + objects_list.clear(); + + // Get the time interval for the objects + rclcpp::Time object_latest_time, object_oldest_time; + getObjectTimeInterval(now, object_latest_time, object_oldest_time); + + // Optimize the target stream, latency, and its band + // The result will be used for the next time, so the optimization is after getting the time + // interval + optimizeTimings(); + + // Get objects from all input streams + // adds up to the objects vector for efficient processing + for (const auto & input_stream : input_streams_) { + input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list); + } + + // Sort objects by timestamp + std::sort( + objects_list.begin(), objects_list.end(), + [](const std::pair & a, const std::pair & b) { + return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < + 0; + }); + + // Update the latest object time + bool is_any_object = !objects_list.empty(); + if (is_any_object) { + latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + } else { + // check time jump + if (now < latest_object_time_) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f", + now.seconds(), latest_object_time_.seconds()); + latest_object_time_ = + now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago + } else { + RCLCPP_DEBUG( + node_.get_logger(), + "InputManager::getObjects No objects in the object list, object time band from %f to %f", + (now - object_oldest_time).seconds(), (now - object_latest_time).seconds()); + } + } + + return is_any_object; +} + +} // namespace multi_object_tracker diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 0d56e16b431e9..08df50fa66993 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,8 +11,6 @@ // 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 "multi_object_tracker/processor/processor.hpp" @@ -25,8 +23,9 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -TrackerProcessor::TrackerProcessor(const std::map & tracker_map) -: tracker_map_(tracker_map) +TrackerProcessor::TrackerProcessor( + const std::map & tracker_map, const size_t & channel_size) +: tracker_map_(tracker_map), channel_size_(channel_size) { // Set tracker lifetime parameters max_elapsed_time_ = 1.0; // [s] @@ -50,7 +49,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) void TrackerProcessor::update( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment) + const std::unordered_map & direct_assignment, const uint & channel_index) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -59,9 +58,10 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); + (*(tracker_itr)) + ->updateWithMeasurement(associated_object, time, self_transform, channel_index); } else { // not found - (*(tracker_itr))->updateWithoutMeasurement(); + (*(tracker_itr))->updateWithoutMeasurement(time); } } } @@ -69,7 +69,7 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment) + const std::unordered_map & reverse_assignment, const uint & channel_index) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -77,34 +77,43 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); + std::shared_ptr tracker = + createNewTracker(new_object, time, self_transform, channel_index); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); if (tracker == "bicycle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "big_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "multi_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "normal_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pass_through_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pedestrian_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); } - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); } void TrackerProcessor::prune(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 12c340516c6c1..0698e4651b6bf 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -44,13 +44,17 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty double r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 3c73b9f19cfbb..e8ca8a47bbc78 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,14 +44,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BigVehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 4f0fb4d7871f2..c925976f65e7e 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -22,11 +22,14 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), - normal_vehicle_tracker_(time, object, self_transform), - big_vehicle_tracker_(time, object, self_transform) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), + normal_vehicle_tracker_(time, object, self_transform, channel_size, channel_index), + big_vehicle_tracker_(time, object, self_transform, channel_size, channel_index) { + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 19fc5a2f71122..54d6e84dd6656 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,14 +44,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("NormalVehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 2084ac28e70f0..e1f4383cf11a3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -37,13 +37,17 @@ PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) { object_ = object; prev_observed_object_ = object; + + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index d61a9a02ccd80..e399dade880fa 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -22,11 +22,14 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), - pedestrian_tracker_(time, object, self_transform), - bicycle_tracker_(time, object, self_transform) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), + pedestrian_tracker_(time, object, self_transform, channel_size, channel_index), + bicycle_tracker_(time, object, self_transform, channel_size, channel_index) { + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index e1c07388836f6..0d4411b5266ad 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -44,13 +44,17 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance float r_stddev_x = 0.4; // [m] diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index a3320ff54afcb..4318da0569721 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -21,9 +21,18 @@ #include #include +namespace +{ +float updateProbability(float prior, float true_positive, float false_positive) +{ + return (prior * true_positive) / (prior * true_positive + (1 - prior) * false_positive); +} +} // namespace + Tracker::Tracker( const rclcpp::Time & time, - const std::vector & classification) + const std::vector & classification, + const size_t & channel_size) : classification_(classification), no_measurement_count_(0), total_no_measurement_count_(0), @@ -34,23 +43,81 @@ Tracker::Tracker( std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + + // Initialize existence probabilities + existence_probabilities_.resize(channel_size, 0.0); +} + +void Tracker::initializeExistenceProbabilities( + const uint & channel_index, const float & existence_probability) +{ + // The initial existence probability is modeled + // since the incoming object's existence probability is not reliable + // existence probability on each channel + constexpr float initial_existence_probability = 0.1; + existence_probabilities_[channel_index] = initial_existence_probability; + + // total existence probability + total_existence_probability_ = existence_probability; } bool Tracker::updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform) + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, + const uint & channel_index) { - no_measurement_count_ = 0; - ++total_measurement_count_; + // Update existence probability + { + no_measurement_count_ = 0; + ++total_measurement_count_; + + // existence probability on each channel + const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); + const double decay_rate = -log(0.5) / 0.3; // 50% decay in 0.3s + constexpr float probability_true_detection = 0.9; + constexpr float probability_false_detection = 0.2; + + // update measured channel probability + existence_probabilities_[channel_index] = updateProbability( + existence_probabilities_[channel_index], probability_true_detection, + probability_false_detection); + // decay other channel probabilities + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { + if (i == channel_index) { + continue; + } + existence_probabilities_[i] *= std::exp(decay_rate * delta_time); + } + + // update total existence probability + const float & existence_probability_from_object = object.existence_probability; + total_existence_probability_ = updateProbability( + total_existence_probability_, existence_probability_from_object, probability_false_detection); + } + last_update_with_measurement_time_ = measurement_time; + + // Update object measure(object, measurement_time, self_transform); + return true; } -bool Tracker::updateWithoutMeasurement() +bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) { + // Update existence probability ++no_measurement_count_; ++total_no_measurement_count_; + { + // decay existence probability + double const delta_time = (now - last_update_with_measurement_time_).seconds(); + const double decay_rate = -log(0.5) / 0.3; // 50% decay in 0.3s + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { + existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); + } + total_existence_probability_ *= std::exp(-decay_rate * delta_time); + } + return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 450bb2d94abb6..04638267f7ad8 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -38,13 +38,17 @@ UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // initialize params // measurement noise covariance constexpr double r_stddev_x = 1.0; // [m] From 3eed12c36903a3763f0ccd73f7bdce75c0bb3622 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 23 May 2024 21:00:08 +0900 Subject: [PATCH 004/120] perf(behavior_path_planner_common): remove unnecessary angle calculation in OccupancyGridMapBasedDetector (#7103) perf(behavior_path_planner_common): unnecessary angle calculation in OccupancyGridMapBasedDetector Signed-off-by: Mamoru Sobue --- .../occupancy_grid_based_collision_detector.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 1aab58ac2ff6e..df48b48d51241 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -110,6 +110,14 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG } } +static IndexXY position2index( + const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Point & position_local) +{ + const int index_x = position_local.x / costmap.info.resolution; + const int index_y = position_local.y / costmap.info.resolution; + return {index_x, index_y}; +} + void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( int theta_index, std::vector & indexes_2d) { @@ -131,12 +139,11 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; - geometry_msgs::msg::Pose pose_local; - pose_local.position.x = base_pose.position.x + offset_x; - pose_local.position.y = base_pose.position.y + offset_y; + geometry_msgs::msg::Point position_local; + position_local.x = base_pose.position.x + offset_x; + position_local.y = base_pose.position.y + offset_y; - const auto index = pose2index(costmap_, pose_local, param_.theta_size); - const auto index_2d = IndexXY{index.x, index.y}; + const auto index_2d = position2index(costmap_, position_local); indexes_2d.push_back(index_2d); }; From 4ffa82b896aac7383c4605489b4a38f2c27fb1f5 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Thu, 23 May 2024 16:55:31 +0200 Subject: [PATCH 005/120] build(behavior_path_external_request_lane_change_module): prefix package and namespace with autoware_ (#6636) Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- .../behavior_planning.launch.xml | 4 ++-- .../CMakeLists.txt | 2 +- .../package.xml | 4 ++-- .../plugins.xml | 4 ++++ .../src/manager.cpp | 17 +++++++++-------- .../src}/manager.hpp | 14 +++++++++----- .../src/scene.cpp | 8 +++++--- .../src}/scene.hpp | 14 +++++++++----- ...est_behavior_path_planner_node_interface.cpp | 8 +++++--- .../plugins.xml | 4 ---- .../plugins.xml | 4 ++-- 12 files changed, 49 insertions(+), 36 deletions(-) rename planning/{behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module}/CMakeLists.txt (88%) rename planning/{behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module}/package.xml (89%) create mode 100644 planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml rename planning/{behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module}/src/manager.cpp (75%) rename planning/{behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module/src}/manager.hpp (82%) rename planning/{behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module}/src/scene.cpp (84%) rename planning/{behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module/src}/scene.hpp (80%) rename planning/{behavior_path_external_request_lane_change_module => autoware_behavior_path_external_request_lane_change_module}/test/test_behavior_path_planner_node_interface.cpp (94%) delete mode 100644 planning/behavior_path_external_request_lane_change_module/plugins.xml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1ad3476333eee..44632c7bf99ce 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -156,13 +156,13 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index aa649710836da..865ab99e870e4 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -82,12 +82,12 @@ /> - behavior_path_external_request_lane_change_module + autoware_behavior_path_external_request_lane_change_module 0.1.0 - The behavior_path_external_request_lane_change_module package + The autoware_behavior_path_external_request_lane_change_module package Fumiya Watanabe Zulfaqar Azmi diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..d13dff53e0836 --- /dev/null +++ b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp similarity index 75% rename from planning/behavior_path_external_request_lane_change_module/src/manager.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 3cfe7aa51f0f1..130cbff29bcf3 100644 --- a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_external_request_lane_change_module/manager.hpp" +#include "manager.hpp" -#include "behavior_path_external_request_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/interface.hpp" +#include "scene.hpp" -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeInterface; std::unique_ptr ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() @@ -38,12 +39,12 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() std::make_unique(parameters_, direction_)); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp similarity index 82% rename from planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp index dfcc4f61d8241..a4f3dce188c73 100644 --- a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ #include "behavior_path_lane_change_module/manager.hpp" #include "route_handler/route_handler.hpp" @@ -23,8 +23,12 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeModuleManager; +using ::behavior_path_planner::LaneChangeModuleType; +using ::behavior_path_planner::SceneModuleInterface; + class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager { public: @@ -49,6 +53,6 @@ class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManage } std::unique_ptr createNewSceneModuleInstance() override; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 84% rename from planning/behavior_path_external_request_lane_change_module/src/scene.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp index 913266bf79fa3..2ba5a5aea34d4 100644 --- a/planning/behavior_path_external_request_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,17 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "scene.hpp" #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeModuleType; + ExternalRequestLaneChange::ExternalRequestLaneChange( const std::shared_ptr & parameters, Direction direction) : NormalLaneChange(parameters, LaneChangeModuleType::EXTERNAL_REQUEST, direction) { } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp similarity index 80% rename from planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp index c23d6f2f3996c..d2eb20b048a5d 100644 --- a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp @@ -12,15 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include "behavior_path_lane_change_module/scene.hpp" #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::Direction; +using ::behavior_path_planner::LaneChangeParameters; +using ::behavior_path_planner::NormalLaneChange; + class ExternalRequestLaneChange : public NormalLaneChange { public: @@ -33,6 +37,6 @@ class ExternalRequestLaneChange : public NormalLaneChange ExternalRequestLaneChange & operator=(ExternalRequestLaneChange &&) = delete; ~ExternalRequestLaneChange() override = default; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 94% rename from planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 1eb5118cd94b2..201d01e9b7fa0 100644 --- a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -22,7 +22,7 @@ #include #include -using behavior_path_planner::BehaviorPathPlannerNode; +using ::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -51,8 +51,10 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml deleted file mode 100644 index f3cc686837c38..0000000000000 --- a/planning/behavior_path_external_request_lane_change_module/plugins.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/planning/behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml index a598bc8176938..7df36919d8d54 100644 --- a/planning/behavior_path_lane_change_module/plugins.xml +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -1,6 +1,6 @@ - - + + From 8281d7f0d829b8c57fe5e7d022e5bb1ecdd69261 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 09:21:49 +0900 Subject: [PATCH 006/120] feat(stop_filter): componentize StopFilter (#7099) * remove unused main func file Signed-off-by: a-maumau * add and mod to use glog Signed-off-by: a-maumau * rm dependencies Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- localization/stop_filter/CMakeLists.txt | 10 +++++-- .../include/stop_filter/stop_filter.hpp | 2 +- .../stop_filter/launch/stop_filter.launch.xml | 2 +- localization/stop_filter/package.xml | 1 + localization/stop_filter/src/stop_filter.cpp | 7 +++-- .../stop_filter/src/stop_filter_node.cpp | 28 ------------------- 6 files changed, 15 insertions(+), 35 deletions(-) delete mode 100644 localization/stop_filter/src/stop_filter_node.cpp diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 97a0443195ae5..4776a1f4967b2 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -4,11 +4,15 @@ project(stop_filter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(stop_filter - src/stop_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/stop_filter.cpp ) -ament_target_dependencies(stop_filter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "StopFilter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 3c2b91590234f..24145a7920d91 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -37,7 +37,7 @@ class StopFilter : public rclcpp::Node { public: - StopFilter(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 0ea92d26c9677..c53b37efc9954 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index 83eaf1b9fa821..2e6d5c4f8bbe1 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tier4_debug_msgs diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index ac0960b8cb67b..4d6b2c6240867 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("stop_filter", node_options) { vx_threshold_ = declare_parameter("vx_threshold"); wz_threshold_ = declare_parameter("wz_threshold"); @@ -57,3 +57,6 @@ void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) pub_stop_flag_->publish(stop_flag_msg); pub_odom_->publish(odom_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(StopFilter) diff --git a/localization/stop_filter/src/stop_filter_node.cpp b/localization/stop_filter/src/stop_filter_node.cpp deleted file mode 100644 index 9748214828de2..0000000000000 --- a/localization/stop_filter/src/stop_filter_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2021 TierIV -// -// 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 "stop_filter/stop_filter.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("stop_filter", node_options); - - rclcpp::spin(node); - - return 0; -} From 2ac611624cdc0e6bcc7c005175ea22edc51cf942 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 10:21:52 +0900 Subject: [PATCH 007/120] feat(twist2accel): componentize Twist2Accel (#7101) * remove unusing main func file Signed-off-by: a-maumau * add and mod to use glog Signed-off-by: a-maumau * rm dependencies Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- localization/twist2accel/CMakeLists.txt | 10 +++++-- .../include/twist2accel/twist2accel.hpp | 2 +- .../twist2accel/launch/twist2accel.launch.xml | 2 +- localization/twist2accel/package.xml | 1 + localization/twist2accel/src/twist2accel.cpp | 7 +++-- .../twist2accel/src/twist2accel_node.cpp | 28 ------------------- 6 files changed, 15 insertions(+), 35 deletions(-) delete mode 100644 localization/twist2accel/src/twist2accel_node.cpp diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 59f23eacb2fb3..9178bf02288a8 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -4,11 +4,15 @@ project(twist2accel) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(twist2accel - src/twist2accel_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/twist2accel.cpp ) -ament_target_dependencies(twist2accel) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Twist2Accel" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/twist2accel/include/twist2accel/twist2accel.hpp index da5f2b4a3b228..0db69890fbfe8 100644 --- a/localization/twist2accel/include/twist2accel/twist2accel.hpp +++ b/localization/twist2accel/include/twist2accel/twist2accel.hpp @@ -40,7 +40,7 @@ class Twist2Accel : public rclcpp::Node { public: - Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit Twist2Accel(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index c4c9a3ed50bfc..c534a288aac3e 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 08dacf9185769..0dbce08f309ac 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components signal_processing tf2 tier4_debug_msgs diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index 68019f27e95fe..0af29445bbeb8 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("twist2accel", node_options) { sub_odom_ = create_subscription( "input/odom", 1, std::bind(&Twist2Accel::callbackOdometry, this, _1)); @@ -103,3 +103,6 @@ void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPt pub_accel_->publish(accel_msg); prev_twist_ptr_ = msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel) diff --git a/localization/twist2accel/src/twist2accel_node.cpp b/localization/twist2accel/src/twist2accel_node.cpp deleted file mode 100644 index ce8ed31db5c32..0000000000000 --- a/localization/twist2accel/src/twist2accel_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 TIER IV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "twist2accel/twist2accel.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("twist2accel", node_options); - - rclcpp::spin(node); - - return 0; -} From 11f010a28cb2c51cf8a9b410b18965d41f657a08 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 10:37:31 +0900 Subject: [PATCH 008/120] feat(localization_error_monitor): componentize LocalizationErrorMonitor (#7102) * remove unusing main func file Signed-off-by: a-maumau * add and mod to use glog Signed-off-by: a-maumau * change name to localization_error_monitor Signed-off-by: a-maumau * style(pre-commit): autofix * rm dependencies Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../localization_error_monitor/CMakeLists.txt | 17 +++++------ ...ode.hpp => localization_error_monitor.hpp} | 8 +++--- .../localization_error_monitor.launch.xml | 2 +- .../localization_error_monitor/package.xml | 1 + ...ode.cpp => localization_error_monitor.cpp} | 8 ++++-- .../localization_error_monitor/src/main.cpp | 28 ------------------- 6 files changed, 21 insertions(+), 43 deletions(-) rename localization/localization_error_monitor/include/localization_error_monitor/{node.hpp => localization_error_monitor.hpp} (87%) rename localization/localization_error_monitor/src/{node.cpp => localization_error_monitor.cpp} (94%) delete mode 100644 localization/localization_error_monitor/src/main.cpp diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 3528367486350..c27e51e6e0359 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,15 +4,16 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_error_monitor_module SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics.cpp + src/localization_error_monitor.cpp ) -ament_auto_add_executable(localization_error_monitor - src/main.cpp - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "LocalizationErrorMonitor" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor ) -target_link_libraries(localization_error_monitor localization_error_monitor_module) if(BUILD_TESTING) function(add_testcase filepath) @@ -20,17 +21,17 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} localization_error_monitor_module) + target_link_libraries(${test_name} ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() - find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_diagnostics.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE config launch ) diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp similarity index 87% rename from localization/localization_error_monitor/include/localization_error_monitor/node.hpp rename to localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 296b278004333..6016677d88136 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_ERROR_MONITOR__NODE_HPP_ -#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ +#define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #include #include @@ -58,7 +58,7 @@ class LocalizationErrorMonitor : public rclcpp::Node double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); public: - LocalizationErrorMonitor(); + explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); ~LocalizationErrorMonitor() = default; }; -#endif // LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml index eb2b5741b00fc..ad3e8beff92ab 100644 --- a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index a1b352d911a0d..0138451b069e4 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -23,6 +23,7 @@ diagnostic_msgs diagnostic_updater nav_msgs + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp similarity index 94% rename from localization/localization_error_monitor/src/node.cpp rename to localization/localization_error_monitor/src/localization_error_monitor.cpp index f47372a0158b2..67cdb78c942fb 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/localization_error_monitor.hpp" #include "localization_error_monitor/diagnostics.hpp" @@ -32,7 +32,8 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") +LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & options) +: Node("localization_error_monitor", options) { scale_ = this->declare_parameter("scale"); error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); @@ -143,3 +144,6 @@ double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); return d; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor) diff --git a/localization/localization_error_monitor/src/main.cpp b/localization/localization_error_monitor/src/main.cpp deleted file mode 100644 index f4fa5ab664005..0000000000000 --- a/localization/localization_error_monitor/src/main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2020 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 "localization_error_monitor/node.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} From 583cd8518bb9de6ebe334fe87c5e4cd8de62da3b Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 10:51:19 +0900 Subject: [PATCH 009/120] feat(localization_error_monitor): componentize EKFLocalizer (#7104) * remove unusing main func file Signed-off-by: a-maumau * add and mod to use glog Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- localization/ekf_localizer/CMakeLists.txt | 17 ++++++----- .../include/ekf_localizer/ekf_localizer.hpp | 2 +- .../launch/ekf_localizer.launch.xml | 2 +- localization/ekf_localizer/package.xml | 1 + .../ekf_localizer/src/ekf_localizer.cpp | 7 +++-- .../ekf_localizer/src/ekf_localizer_node.cpp | 28 ------------------- 6 files changed, 16 insertions(+), 41 deletions(-) delete mode 100644 localization/ekf_localizer/src/ekf_localizer_node.cpp diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 9944cbb84d97c..13e63ddf3c170 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -13,7 +13,7 @@ include_directories( ament_auto_find_build_dependencies() -ament_auto_add_library(ekf_localizer_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/ekf_localizer.cpp src/covariance.cpp src/diagnostics.cpp @@ -24,21 +24,20 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_module.cpp ) -target_link_libraries(ekf_localizer_lib Eigen3::Eigen) - -ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) - -target_compile_options(ekf_localizer PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "EKFLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(ekf_localizer ekf_localizer_lib) -target_include_directories(ekf_localizer PUBLIC include) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" ekf_localizer_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 6925e8b63c66b..8ed925c1bc228 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -102,7 +102,7 @@ class Simple1DFilter class EKFLocalizer : public rclcpp::Node { public: - EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit EKFLocalizer(const rclcpp::NodeOptions & options); private: const std::shared_ptr warning_; diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index b6a1bd06185c2..3c66fabe34650 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index e9d756e547f26..dad1b0e730711 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -29,6 +29,7 @@ kalman_filter nav_msgs rclcpp + rclcpp_components std_srvs tf2 tf2_ros diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f77481d45a534..687e812679574 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -40,8 +40,8 @@ using std::placeholders::_1; -EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), +EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), params_(this), ekf_dt_(params_.ekf_dt), @@ -479,3 +479,6 @@ void EKFLocalizer::serviceTriggerNode( res->success = true; return; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer) diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp deleted file mode 100644 index 0f75f19c5d50b..0000000000000 --- a/localization/ekf_localizer/src/ekf_localizer_node.cpp +++ /dev/null @@ -1,28 +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 - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("ekf_localizer", node_options); - - rclcpp::spin(node); - - return 0; -} From f2ae1af058d28edf1f856332070a6749f15b3395 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 16:16:32 +0900 Subject: [PATCH 010/120] feat(pose2twist): componentize Pose2Twist (#7113) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from log to both Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/pose2twist/CMakeLists.txt | 9 ++++-- .../include/pose2twist/pose2twist_core.hpp | 2 +- .../pose2twist/launch/pose2twist.launch.xml | 2 +- localization/pose2twist/package.xml | 1 + .../pose2twist/src/pose2twist_core.cpp | 5 +++- .../pose2twist/src/pose2twist_node.cpp | 29 ------------------- 6 files changed, 14 insertions(+), 34 deletions(-) delete mode 100644 localization/pose2twist/src/pose2twist_node.cpp diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index a2fbbddf7d120..2a586aa9cd049 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -4,11 +4,16 @@ project(pose2twist) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose2twist - src/pose2twist_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose2twist_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Pose2Twist" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index c2a39f7e2ed3d..459a62ea5cd13 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -24,7 +24,7 @@ class Pose2Twist : public rclcpp::Node { public: - Pose2Twist(); + explicit Pose2Twist(const rclcpp::NodeOptions & options); ~Pose2Twist() = default; private: diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml index a0fae57a163e6..57a41dbfcf017 100644 --- a/localization/pose2twist/launch/pose2twist.launch.xml +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 16c49bb51c218..07e445c72978c 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -19,6 +19,7 @@ geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tier4_debug_msgs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index a321a06122816..4b98ec6c81ad4 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -24,7 +24,7 @@ #include #include -Pose2Twist::Pose2Twist() : Node("pose2twist_core") +Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options) { using std::placeholders::_1; @@ -129,3 +129,6 @@ void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_ms angular_z_msg.data = twist_msg.twist.angular.z; angular_z_pub_->publish(angular_z_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist) diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/localization/pose2twist/src/pose2twist_node.cpp deleted file mode 100644 index 81f98461ecffd..0000000000000 --- a/localization/pose2twist/src/pose2twist_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2015-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 "pose2twist/pose2twist_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} From 27e1aa1dc32abe7a81ca54fabf1e67a544b5f412 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 24 May 2024 16:30:41 +0900 Subject: [PATCH 011/120] fix(perception_online_evaluator): fix range resolution (#7115) Signed-off-by: Fumiya Watanabe --- .../param/perception_online_evaluator.defaults.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 807f247807d98..16d4b6fe458cd 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -13,7 +13,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] stopped_velocity_threshold: 1.0 - detection_radius_list: [50.0, 100.0, 150.0, 200.0] + detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0] detection_height_list: [10.0] detection_count_purge_seconds: 36000.0 objects_count_window_seconds: 1.0 From 6d77841dbc0844f778bfa68139867f60ea2ddbd8 Mon Sep 17 00:00:00 2001 From: Motz <83898149+Motsu-san@users.noreply.github.com> Date: Fri, 24 May 2024 16:34:26 +0900 Subject: [PATCH 012/120] refactor(gyro_odometer): componentize node in gyro_odometer (#7090) * refactor: install glog to gyro_odometer Signed-off-by: Motsu-san * style(pre-commit): autofix * feat: change glog error setting --------- Signed-off-by: Motsu-san Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/gyro_odometer/CMakeLists.txt | 14 ++-- .../gyro_odometer/gyro_odometer_core.hpp | 11 ++- .../launch/gyro_odometer.launch.xml | 2 +- localization/gyro_odometer/package.xml | 5 +- .../gyro_odometer/src/gyro_odometer_core.cpp | 81 +++++++++++-------- .../gyro_odometer/src/gyro_odometer_node.cpp | 30 ------- .../test/test_gyro_odometer_pubsub.cpp | 6 +- 7 files changed, 68 insertions(+), 81 deletions(-) delete mode 100644 localization/gyro_odometer/src/gyro_odometer_node.cpp diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..a832383ff4761 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,12 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +20,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3a6358e62c21a..2926589bd2da9 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -36,14 +36,17 @@ #include #include -class GyroOdometer : public rclcpp::Node +namespace autoware::gyro_odometer +{ + +class GyroOdometerNode : public rclcpp::Node { private: using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); + ~GyroOdometerNode(); private: void callbackVehicleTwist( @@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node std::deque gyro_queue_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..3c979eb69ac8a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -19,14 +19,13 @@ fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs tier4_autoware_utils - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..bda7af74b8489 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -25,6 +27,42 @@ #include #include +namespace autoware::gyro_odometer +{ + +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), + vehicle_twist_arrived_(false), + imu_arrived_(false) +{ + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); + + vehicle_twist_sub_ = create_subscription( + "vehicle/twist_with_covariance", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + + imu_sub_ = create_subscription( + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + + twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); + twist_with_covariance_raw_pub_ = create_publisher( + "twist_with_covariance_raw", rclcpp::QoS{10}); + + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); + + // TODO(YamatoAndo) createTimer +} + +GyroOdometerNode::~GyroOdometerNode() +{ +} + std::array transformCovariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( return twist_with_cov; } -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame")), - message_timeout_sec_(declare_parameter("message_timeout_sec")), - vehicle_twist_arrived_(false), - imu_arrived_(false) -{ - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); - - vehicle_twist_sub_ = create_subscription( - "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); - - imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); - - twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); - twist_with_covariance_raw_pub_ = create_publisher( - "twist_with_covariance_raw", rclcpp::QoS{10}); - - twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); - twist_with_covariance_pub_ = create_publisher( - "twist_with_covariance", rclcpp::QoS{10}); - - // TODO(YamatoAndo) createTimer -} - -GyroOdometer::~GyroOdometer() -{ -} - -void GyroOdometer::callbackVehicleTwist( +void GyroOdometerNode::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { vehicle_twist_arrived_ = true; @@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist( gyro_queue_.clear(); } -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { imu_arrived_ = true; if (!vehicle_twist_arrived_) { @@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publishData( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +275,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-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 "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..b7849ef03bfc5 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -108,7 +108,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly) { Imu input_imu = generateSampleImu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); From fe3083303429d24ff259b860e362152255122caf Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 24 May 2024 16:47:50 +0900 Subject: [PATCH 013/120] feat(perception_online_evaluator): add metric_value not only stat (#7100) Signed-off-by: kosuke55 --- .../metrics/metric.hpp | 5 +++ .../metrics_calculator.hpp | 4 +- .../perception_online_evaluator_node.hpp | 2 + .../src/metrics_calculator.cpp | 16 ++++---- .../src/perception_online_evaluator_node.cpp | 39 +++++++++++++++---- .../test_perception_online_evaluator_node.cpp | 14 ++++++- 6 files changed, 61 insertions(+), 19 deletions(-) diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index 96b1c01ee2d16..a04819d4844ce 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index e5c41ff28c4da..070d938b8bd0a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 9c1e0667e4fef..cc455445ca9f8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 0fcdd77f4d515..be9ac2332f62c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 95d6f07cca5d9..53ef0fb99c8e3 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); From 98cc9f272e229a58fff4641223e53ca8859d5b14 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 17:06:48 +0900 Subject: [PATCH 014/120] feat(pose_instability_detector): componentize PoseInstabilityDetector (#7114) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from log to both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- .../pose_instability_detector/CMakeLists.txt | 15 +++++++---- .../pose_instability_detector.launch.xml | 2 +- .../pose_instability_detector/package.xml | 1 + .../pose_instability_detector/src/main.cpp | 26 ------------------- .../src/pose_instability_detector.cpp | 5 +++- 5 files changed, 16 insertions(+), 33 deletions(-) delete mode 100644 localization/pose_instability_detector/src/main.cpp diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt index 5270df636d791..c6f94ab7df16e 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -4,14 +4,19 @@ project(pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_instability_detector - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_instability_detector.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInstabilityDetector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_pose_instability_detector + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/pose_instability_detector.cpp ) @@ -19,6 +24,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE - launch - config + launch + config ) diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml index 4a390ee2854d7..5ebe7d7e429e0 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index bf57e5589b747..d658d1c2d437f 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -22,6 +22,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp deleted file mode 100644 index 34e679e86730f..0000000000000 --- a/localization/pose_instability_detector/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023- 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 "pose_instability_detector.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto pose_instability_detector = std::make_shared(); - rclcpp::spin(pose_instability_detector); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index afb7d6e007db2..c2bce6a3db288 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -23,7 +23,7 @@ #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) -: Node("pose_instability_detector", options), +: rclcpp::Node("pose_instability_detector", options), threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), @@ -174,3 +174,6 @@ void PoseInstabilityDetector::callback_timer() prev_odometry_ = latest_odometry_; twist_buffer_.clear(); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) From 02775ebd5305094058e31910195204dbdb131396 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 24 May 2024 18:26:43 +0900 Subject: [PATCH 015/120] fix(avoidance): improve avoidance safety check logic (#6974) * fix(avoidance): don't ignore stopped object unless it's a parked vehilce Signed-off-by: satoshi-ota * fix(avoidance): rebuild prepare distance calculation/validation logic Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/helper.hpp | 17 +++++++++++++---- .../src/scene.cpp | 9 +++++---- .../src/shift_line_generator.cpp | 8 ++++---- .../src/utils.cpp | 2 +- 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 8de39d00af130..55d14e424b7f6 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -97,16 +97,18 @@ class AvoidanceHelper return std::max(getEgoSpeed(), values.at(idx)); } - double getMinimumPrepareDistance() const + double getNominalPrepareDistance(const double velocity) const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); + const auto & values = p->velocity_map; + const auto idx = getConstraintsMapIndex(velocity, values); + return std::max(values.at(idx) * p->max_prepare_time, p->min_prepare_distance); } double getNominalPrepareDistance() const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); + return std::max(getAvoidanceEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const @@ -298,6 +300,13 @@ class AvoidanceHelper return std::numeric_limits::max(); } + bool isEnoughPrepareDistance(const double prepare_distance) const + { + const auto & p = parameters_; + return prepare_distance > + std::max(getEgoSpeed() * p->min_prepare_distance, p->min_prepare_distance); + } + bool isComfortable(const AvoidLineArray & shift_lines) const { const auto JERK_BUFFER = 0.1; // [m/sss] @@ -328,7 +337,7 @@ class AvoidanceHelper const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); - const auto prepare_distance = getMinimumPrepareDistance(); + const auto prepare_distance = getNominalPrepareDistance(0.0); const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 15b3e970df4b1..503438f9f7feb 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -409,9 +409,9 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto registered_lines = path_shifter_.getShiftLines(); if (!registered_lines.empty()) { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto to_shift_start_point = motion_utils::calcSignedArcLength( + const auto prepare_distance = motion_utils::calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); - if (to_shift_start_point < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(prepare_distance)) { RCLCPP_DEBUG( getLogger(), "Distance to shift start point is less than minimum prepare distance. The distance is not " @@ -1412,10 +1412,11 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant_distance = helper_->getFrontConstantDistance(object); + const auto prepare_distance = helper_->getNominalPrepareDistance(0.0); return object.longitudinal - std::min( - avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer, + avoidance_distance + constant_distance + prepare_distance + p->stop_buffer, p->stop_max_distance); } @@ -1444,7 +1445,7 @@ void AvoidanceModule::insertReturnDeadLine( const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); const auto min_return_distance = - helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); + helper_->getMinAvoidanceDistance(shift_length) + helper_->getNominalPrepareDistance(0.0); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 84b2b9b61a702..fc3b9e24ff9f6 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -929,7 +929,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(s.start_longitudinal)) { continue; } @@ -1173,13 +1173,13 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( std::max(nominal_prepare_distance - last_sl_distance, 0.0); double prepare_distance_scaled = std::max( - helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); + helper_->getNominalPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); prepare_distance_scaled = std::max( - helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); + helper_->getNominalPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1292,7 +1292,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(candidate.start_longitudinal)) { break; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 3559981fed38d..089571bc8392e 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1916,7 +1916,7 @@ std::vector getSafetyCheckTargetObjects( std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { // check only moving objects - if (filtering_utils::isMovingObject(object, parameters)) { + if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) { ret.objects.push_back(object.object); } } From d5e691ec9af579ceb7a156eff4c5ea5e2e86c92e Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 24 May 2024 18:45:26 +0900 Subject: [PATCH 016/120] refactor(obstacle_cruise): refactor a function checkConsistency() (#7105) refactor Signed-off-by: Yuki Takagi --- planning/obstacle_cruise_planner/src/node.cpp | 44 +++++-------------- 1 file changed, 11 insertions(+), 33 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f637911dae903..21e3ecb7bf758 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1378,7 +1378,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto current_closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - // If previous closest obstacle ptr is not set if (!prev_closest_stop_obstacle_ptr_) { if (current_closest_stop_obstacle) { prev_closest_stop_obstacle_ptr_ = @@ -1387,44 +1386,23 @@ void ObstacleCruisePlannerNode::checkConsistency( return; } - // Put previous closest target obstacle if necessary const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_stop_obstacle_ptr_->uuid; }); - - // If previous closest obstacle is not in the current perception lists - // just return the current target obstacles + // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { return; } - // Previous closest obstacle is in the perception lists - const auto obstacle_itr = std::find_if( + const auto is_disappeared_from_stop_obstacle = std::none_of( stop_obstacles.begin(), stop_obstacles.end(), [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - - // Previous closest obstacle is both in the perception lists and target obstacles - if (obstacle_itr != stop_obstacles.end()) { - if (current_closest_stop_obstacle) { - if ((current_closest_stop_obstacle->uuid == prev_closest_stop_obstacle_ptr_->uuid)) { - // prev_closest_obstacle is current_closest_stop_obstacle just return the target - // obstacles(in target obstacles) - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - // New obstacle becomes new stop obstacle - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } - } else { - // Previous closest stop obstacle becomes cruise obstacle - prev_closest_stop_obstacle_ptr_ = nullptr; - } - } else { - // prev obstacle is not in the target obstacles, but in the perception list + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); if ( predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < @@ -1433,13 +1411,13 @@ void ObstacleCruisePlannerNode::checkConsistency( stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); return; } + } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + if (current_closest_stop_obstacle) { + prev_closest_stop_obstacle_ptr_ = + std::make_shared(*current_closest_stop_obstacle); + } else { + prev_closest_stop_obstacle_ptr_ = nullptr; } } From bf575b6ef7ee2b23ca90fe35f46dae3bfe974848 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 24 May 2024 20:41:21 +0900 Subject: [PATCH 017/120] feat(default_ad_api): use diagnostic graph (#7043) Signed-off-by: Takagi, Isamu --- .../launch/system.launch.xml | 74 ++++++++++--------- system/default_ad_api/src/operation_mode.cpp | 40 +++------- system/default_ad_api/src/operation_mode.hpp | 8 +- .../system_diagnostic_monitor.launch.xml | 2 + 4 files changed, 53 insertions(+), 71 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index a1bbfc883de7c..e5f7aa4e18b33 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,11 +25,12 @@ - - - - - + + + + + + @@ -57,6 +58,13 @@ + + + + + + + @@ -70,8 +78,20 @@ + + + + + + + + + + + + - + @@ -85,52 +105,34 @@ - + + - - + + + + + + - - - - - - - - - - + + + - + - - - - - - - - - - - - - - - - diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 829585ed4b8b4..d4b430ecd0579 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); - const std::vector module_names = { - "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + const auto name = "/system/operation_mode/availability"; + const auto qos = rclcpp::QoS(1); + const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) { + mode_available_[OperationModeState::Message::STOP] = msg->stop; + mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous; + mode_available_[OperationModeState::Message::LOCAL] = msg->local; + mode_available_[OperationModeState::Message::REMOTE] = msg->remote; }; - - for (size_t i = 0; i < module_names.size(); ++i) { - const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; - const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[module_names[i]] = msg->available; - }; - sub_module_states_.push_back(create_subscription(name, qos, callback)); - } + sub_availability_ = create_subscription(name, qos, callback); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) mode_available_[OperationModeState::Message::UNKNOWN] = false; mode_available_[OperationModeState::Message::STOP] = true; mode_available_[OperationModeState::Message::AUTONOMOUS] = false; - mode_available_[OperationModeState::Message::LOCAL] = true; - mode_available_[OperationModeState::Message::REMOTE] = true; + mode_available_[OperationModeState::Message::LOCAL] = false; + mode_available_[OperationModeState::Message::REMOTE] = false; } template @@ -135,23 +132,6 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { - bool autonomous_available = true; - std::string unhealthy_components = ""; - for (const auto & state : module_states_) { - if (!state.second) { - unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; - } - autonomous_available &= state.second; - } - mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; - - if (!unhealthy_components.empty()) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 3000, - "%s component state is unhealthy. Autonomous is not available.", - unhealthy_components.c_str()); - } - update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 1830b7041b566..7cd609b5eb852 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -25,7 +25,7 @@ #include // TODO(Takagi, Isamu): define interface -#include +#include // This file should be included after messages. #include "utils/types.hpp" @@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; - using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; @@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node Sub sub_state_; Cli cli_mode_; Cli cli_control_; - - std::unordered_map module_states_; - std::vector::SharedPtr> sub_module_states_; + rclcpp::Subscription::SharedPtr sub_availability_; void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml index b00fcb8a26f68..8e2566a747abf 100644 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -1,6 +1,8 @@ + + From d827b1bd1f4bbacf0333eb14a62ef42e56caef25 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 24 May 2024 20:49:05 +0900 Subject: [PATCH 018/120] fix(perception_online_evaluator): revert "add metric_value not only stat (#7100)" (#7118) --- .../metrics/metric.hpp | 5 --- .../metrics_calculator.hpp | 4 +- .../perception_online_evaluator_node.hpp | 2 - .../src/metrics_calculator.cpp | 16 ++++---- .../src/perception_online_evaluator_node.cpp | 39 ++++--------------- .../test_perception_online_evaluator_node.cpp | 14 +------ 6 files changed, 19 insertions(+), 61 deletions(-) diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index eaa07f2317940..926fbb7435f3a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include namespace perception_diagnostics @@ -37,11 +36,7 @@ enum class Metric { SIZE, }; -// Each metric has a different return type. (statistic or just a one value etc). -// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; -using MetricValueMap = std::unordered_map; -using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index a04819d4844ce..96b1c01ee2d16 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricValueMap calcObjectsCountMetrics() const; + MetricStatMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index 070d938b8bd0a..e5c41ff28c4da 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,8 +61,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; - DiagnosticStatus generateDiagnosticStatus( - const std::string metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index cc455445ca9f8..9c1e0667e4fef 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,14 +455,15 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const +MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricValueMap metric_stat_map; + MetricStatMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( + count); } } // calculate the average number of objects in the detection area in the past @@ -471,8 +472,8 @@ MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map - ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; + metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] + .add(count); } } @@ -480,7 +481,8 @@ MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( + count); } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index be9ac2332f62c..0fcdd77f4d515 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,25 +69,16 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_result = metrics_calculator_.calculate(Metric(metric)); - if (!metric_result.has_value()) { + const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); + if (!metric_stat_map.has_value()) { continue; } - std::visit( - [&metrics_msg, this](auto && arg) { - using T = std::decay_t; - for (const auto & [metric, value] : arg) { - if constexpr (std::is_same_v) { - if (value.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); - } - } else if constexpr (std::is_same_v) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); - } - } - }, - metric_result.value()); + for (const auto & [metric, stat] : metric_stat_map.value()) { + if (stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); + } + } } // publish metrics @@ -120,22 +111,6 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } -DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( - const std::string metric, const double value) const -{ - DiagnosticStatus status; - - status.level = status.OK; - status.name = metric; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metric_value"; - key_value.value = std::to_string(value); - status.values.push_back(key_value); - - return status; -} - void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 53ef0fb99c8e3..95d6f07cca5d9 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,19 +141,7 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - const auto mean_it = std::find_if( - it->values.begin(), it->values.end(), - [](const auto & key_value) { return key_value.key == "mean"; }); - if (mean_it != it->values.end()) { - metric_value_ = boost::lexical_cast(mean_it->value); - } else { - const auto metric_value_it = std::find_if( - it->values.begin(), it->values.end(), - [](const auto & key_value) { return key_value.key == "metric_value"; }); - if (metric_value_it != it->values.end()) { - metric_value_ = boost::lexical_cast(metric_value_it->value); - } - } + metric_value_ = boost::lexical_cast(it->values[2].value); metric_updated_ = true; } }); From 7b51a43ccf224c33a4f83021824d043824332bfc Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 24 May 2024 22:58:08 +0900 Subject: [PATCH 019/120] feat(default_ad_api): add heratbeat api (#6969) * feat(default_ad_api): add heratbeat api Signed-off-by: Takagi, Isamu * fix node dying Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/system.hpp | 36 ++++++++++++++++ system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/src/heartbeat.cpp | 42 +++++++++++++++++++ system/default_ad_api/src/heartbeat.hpp | 40 ++++++++++++++++++ 5 files changed, 121 insertions(+) create mode 100644 common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp create mode 100644 system/default_ad_api/src/heartbeat.cpp create mode 100644 system/default_ad_api/src/heartbeat.hpp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..4dacc0b893714 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/fail_safe.cpp + src/heartbeat.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -23,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" "default_ad_api::FailSafeNode" + "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index cf8fbe7d001bf..f4f3986c678e9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -43,6 +43,7 @@ def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), + create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp new file mode 100644 index 0000000000000..4550302bb8bae --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "heartbeat.hpp" + +#include + +namespace default_ad_api +{ + +HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) +{ + // Move this function so that the timer no longer holds it as a reference. + const auto on_timer = [this]() { + autoware_ad_api::system::Heartbeat::Message heartbeat; + heartbeat.stamp = now(); + heartbeat.seq = ++sequence_; // Wraps at 65535. + pub_->publish(heartbeat); + }; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_); + + const auto period = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) diff --git a/system/default_ad_api/src/heartbeat.hpp b/system/default_ad_api/src/heartbeat.hpp new file mode 100644 index 0000000000000..d922b88489639 --- /dev/null +++ b/system/default_ad_api/src/heartbeat.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HEARTBEAT_HPP_ +#define HEARTBEAT_HPP_ + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class HeartbeatNode : public rclcpp::Node +{ +public: + explicit HeartbeatNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_; + uint16_t sequence_ = 0; +}; + +} // namespace default_ad_api + +#endif // HEARTBEAT_HPP_ From 9d71cceeedac4797b54433d617f86265580c171f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 27 May 2024 09:57:40 +0900 Subject: [PATCH 020/120] fix(multi_object_tracker): revert mot node implementation to composable node components (#7117) * Revert "chore(multi_object_tracker): change node and glog implementation (#6780)" This reverts commit 81c432e8110ca54acc5e6fc4a757eff87724c39b. Signed-off-by: Taekjin LEE * fix: follow executable naming rule Signed-off-by: Taekjin LEE * fix: update executable name Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 9 ++--- .../launch/multi_object_tracker.launch.xml | 2 +- .../src/multi_object_tracker_core.cpp | 6 ++++ .../src/multi_object_tracker_node.cpp | 36 ------------------- 4 files changed, 12 insertions(+), 41 deletions(-) delete mode 100644 perception/multi_object_tracker/src/multi_object_tracker_node.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index fb2c4d73554a3..4f9268583dd34 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -44,17 +44,18 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pass_through_tracker.cpp ) -ament_auto_add_library(multi_object_tracker_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED ${MULTI_OBJECT_TRACKER_SRC} ) -target_link_libraries(multi_object_tracker_node +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen glog::glog ) -ament_auto_add_executable(${PROJECT_NAME} - src/multi_object_tracker_node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "multi_object_tracker::MultiObjectTracker" + EXECUTABLE ${PROJECT_NAME}_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index 526ce23efd33b..b00ccd8fa623e 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index c3bf7023fb068..54c23d39f5357 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -75,6 +75,12 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tf_listener_(tf_buffer_), last_published_time_(this->now()) { + // glog for debug + if (!google::IsGoogleLoggingInitialized()) { + google::InitGoogleLogging("multi_object_tracker"); + google::InstallFailureSignalHandler(); + } + // Get parameters double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp deleted file mode 100644 index 273e6ac1a06bd..0000000000000 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2024 TIER IV, inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "multi_object_tracker/multi_object_tracker_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); // NOLINT - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto multi_object_tracker = std::make_shared(options); - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(multi_object_tracker); - exec.spin(); - rclcpp::shutdown(); - return 0; -} From 79e2a0001485c39db697a7573a37af17d09f09c7 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Mon, 27 May 2024 11:14:05 +0900 Subject: [PATCH 021/120] fix(ndt_scan_matcher): modify diagnostic (#7112) * Fixed diagnostic Signed-off-by: Shintaro Sakoda * Fixed README.md Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 34 +++++++++---------- .../src/ndt_scan_matcher_core.cpp | 5 +-- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index fbc2fa3c9a3f3..a44db7bbaa4bf 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,23 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | -| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -------------------------------- | ------------------------------------------------------------------------ | -| `topic_time_stamp` | the time stamp of input topic | none | none | no | -| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | -| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | -| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | -| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | yes | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | -| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | -| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | -| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | none | the number of times is 5 or more | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | +| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | no | +| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | +| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8692027e9634a..69349e75e786d 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -275,13 +275,14 @@ void NDTScanMatcher::callback_sensor_points( // check skipping_publish_num static size_t skipping_publish_num = 0; const size_t error_skipping_publish_num = 5; - skipping_publish_num = is_succeed_scan_matching ? 0 : (skipping_publish_num + 1); + skipping_publish_num = + ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); if (skipping_publish_num >= error_skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; diagnostics_scan_points_->updateLevelAndMessage( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } diagnostics_scan_points_->publish(); From 65c2c515e9f288c1d3c72b41a62bbd5dec16372a Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 27 May 2024 13:59:00 +0900 Subject: [PATCH 022/120] feat(tier4_autoware_utils): faster sin and cos (#7120) * feat(tier4_autoware_utils): faster sin and cos Signed-off-by: veqcc --- .../math/trigonometry.hpp | 4 ++++ .../src/math/trigonometry.cpp | 23 +++++++++++++++++++ .../test/src/math/test_trigonometry.cpp | 10 ++++++++ 3 files changed, 37 insertions(+) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp index 0c53a9e3941dd..4901e28472acb 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp @@ -15,6 +15,8 @@ #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#include + namespace tier4_autoware_utils { @@ -22,6 +24,8 @@ float sin(float radian); float cos(float radian); +std::pair sin_and_cos(float radian); + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 15f5c71012722..0ce65c7aa5bc8 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -49,4 +49,27 @@ float cos(float radian) return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); } +std::pair sin_and_cos(float radian) +{ + constexpr float tmp = + (180.f / static_cast(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + const float degree = radian * tmp; + size_t idx = + (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % + discrete_arcs_num_360; + + if (idx < discrete_arcs_num_90) { + return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { + idx = 2 * discrete_arcs_num_90 - idx; + return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { + idx = idx - 2 * discrete_arcs_num_90; + return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 + idx = 4 * discrete_arcs_num_90 - idx; + return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } +} + } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp index 379418539841c..d7106fd823682 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp @@ -40,3 +40,13 @@ TEST(trigonometry, cos) tier4_autoware_utils::cos(x * static_cast(i))) < 10e-5); } } + +TEST(trigonometry, sin_and_cos) +{ + float x = 4.f * tier4_autoware_utils::pi / 128.f; + for (int i = 0; i < 128; i++) { + const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast(i)); + EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); + EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); + } +} From 49f91a2d7e93c1249e7fc9d07683fc63424510ab Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 27 May 2024 14:35:37 +0900 Subject: [PATCH 023/120] refactor(blind_spot): devide scene into Categories and act accordingly (#7110) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 1 + .../src/debug.cpp | 54 +-- .../src/decisions.cpp | 154 ++++++++ .../src/manager.cpp | 11 +- .../src/scene.cpp | 366 +++++++++--------- .../src/scene.hpp | 82 +++- 6 files changed, 400 insertions(+), 268 deletions(-) create mode 100644 planning/behavior_velocity_blind_spot_module/src/decisions.cpp diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt index 682da3ae58d7e..2ad4fa495ffce 100644 --- a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp src/scene.cpp + src/decisions.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 5cc90afb0043d..b733a1ea1e79c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -71,55 +71,16 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( return msg; } -visualization_msgs::msg::MarkerArray createPoseMarkerArray( - const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns, - const int64_t id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - if (state == StateMachine::State::STOP) { - visualization_msgs::msg::Marker marker_line{}; - marker_line.header.frame_id = "map"; - marker_line.ns = ns + "_line"; - marker_line.id = id; - marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); - marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker_line.action = visualization_msgs::msg::Marker::ADD; - marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); - marker_line.color = createMarkerColor(r, g, b, 0.999); - - const double yaw = tf2::getYaw(pose.orientation); - - const double a = 3.0; - geometry_msgs::msg::Point p0; - p0.x = pose.position.x - a * std::sin(yaw); - p0.y = pose.position.y + a * std::cos(yaw); - p0.z = pose.position.z; - marker_line.points.push_back(p0); - - geometry_msgs::msg::Point p1; - p1.x = pose.position.x + a * std::sin(yaw); - p1.y = pose.position.y - a * std::cos(yaw); - p1.z = pose.position.z; - marker_line.points.push_back(p1); - - msg.markers.push_back(marker_line); - } - - return msg; -} - } // namespace motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() { motion_utils::VirtualWalls virtual_walls; - if (!isActivated() && !is_over_pass_judge_line_) { + if (debug_data_.virtual_wall_pose) { motion_utils::VirtualWall wall; wall.text = "blind_spot"; - wall.pose = debug_data_.virtual_wall_pose; + wall.pose = debug_data_.virtual_wall_pose.value(); wall.ns = std::to_string(module_id_) + "_"; virtual_walls.push_back(wall); } @@ -130,19 +91,8 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - createPoseMarkerArray( - debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), - &debug_marker_array, now); - - appendMarkerArray( - createPoseMarkerArray( - debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5), - &debug_marker_array, now); - appendMarkerArray( createLaneletPolygonsMarkerArray( debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp new file mode 100644 index 0000000000000..d3e439b3663f8 --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -0,0 +1,154 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene.hpp" + +#include + +namespace behavior_velocity_planner +{ + +/* + * for default + */ +template +void BlindSpotModule::setRTCStatusByDecision( + const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + static_assert("Unsupported type passed to setRTCStatus"); + return; +} + +template +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const T & decision, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + static_assert("Unsupported type passed to reactRTCApprovalByDecision"); +} + +/* + * for InternalError + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * For OverPassJudge + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * for Unsafe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(false); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +/* + * for Safe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(true); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index faa10c1fbe9b4..0843199cac089 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -63,14 +63,17 @@ void BlindSpotModuleManager::launchNewModules( } // Is turning lane? - const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (turn_direction != "left" && turn_direction != "right") { + const std::string turn_direction_str = ll.attributeOr("turn_direction", "else"); + if (turn_direction_str != "left" && turn_direction_str != "right") { continue; } + const auto turn_direction = turn_direction_str == "left" + ? BlindSpotModule::TurnDirection::LEFT + : BlindSpotModule::TurnDirection::RIGHT; registerModule(std::make_shared( - module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"), - clock_)); + module_id, lane_id, turn_direction, planner_data_, planner_param_, + logger_.get_child("blind_spot_module"), clock_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 7a3ea3160cb24..22e54464d400e 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -44,165 +44,120 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -[[maybe_unused]] geometry_msgs::msg::Polygon toGeomPoly(const lanelet::CompoundPolygon3d & poly) -{ - geometry_msgs::msg::Polygon geom_poly; - - for (const auto & p : poly) { - geometry_msgs::msg::Point32 geom_point; - geom_point.x = p.x(); - geom_point.y = p.y(); - geom_point.z = p.z(); - geom_poly.points.push_back(geom_point); - } - - return geom_poly; -} -} // namespace - BlindSpotModule::BlindSpotModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), planner_param_{planner_param}, - turn_direction_(TurnDirection::INVALID), + turn_direction_(turn_direction), is_over_pass_judge_line_(false) { velocity_factor_.init(PlanningBehavior::REAR_CHECK); - - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("left")) { - turn_direction_ = TurnDirection::LEFT; - } else if (!turn_direction.compare("right")) { - turn_direction_ = TurnDirection::RIGHT; - } + sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data); } -bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +void BlindSpotModule::initializeRTCStatus() { - debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); - - const auto input_path = *path; - - const auto current_state = state_machine_.getState(); - RCLCPP_DEBUG( - logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); - - /* get current pose */ - const auto & current_pose = planner_data_->current_odometry->pose; + setSafe(true); + setDistance(std::numeric_limits::lowest()); +} - /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); +BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + const auto & input_path = *path; /* set stop-line and stop-judgement-line for base_link */ const auto interpolated_path_info_opt = generateInterpolatedPathInfo(input_path); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] failed to interpolate path"); - return false; + return InternalError{"Failed to interpolate path"}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!sibling_straight_lanelet_) { - sibling_straight_lanelet_ = getSiblingStraightLanelet(); - } - const auto stoplines_idx_opt = generateStopLine(interpolated_path_info, path); if (!stoplines_idx_opt) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] setStopLineIdx fail"); - return false; + return InternalError{"generateStopLine fail"}; } const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); - - if (default_stopline_idx <= 0) { - RCLCPP_DEBUG( - logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); - *path = input_path; // reset path - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; + if (default_stopline_idx == 0) { + return InternalError{"stop line or pass judge line is at path[0], ignore planning."}; } /* calc closest index */ + const auto & current_pose = planner_data_->current_odometry->pose; const auto closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const auto stop_line_idx = closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; - - /* set judge line dist */ - const double current_vel = planner_data_->current_velocity->twist.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double pass_judge_line_dist = - planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); - const auto stop_point_pose = path->points.at(stop_line_idx).point.pose; - const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - const size_t stop_point_segment_idx = - motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); - const auto distance_until_stop = motion_utils::calcSignedArcLength( - input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, - stop_point_segment_idx); - - /* get debug info */ const auto stop_line_pose = planning_utils::getAheadPose( - stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); - debug_data_.virtual_wall_pose = stop_line_pose; - const auto stop_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.stop_point_pose = stop_pose; + stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, input_path); - /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ - if (planner_param_.use_pass_judge_line) { - const double eps = 1e-1; // to prevent hunting - if ( - current_state == StateMachine::State::GO && - distance_until_stop + eps < pass_judge_line_dist) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - *path = input_path; // reset path - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; // no plan needed. - } + const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose); + if (is_over_pass_judge) { + return is_over_pass_judge.value(); } - /* get dynamic object */ - const auto objects_ptr = planner_data_->predicted_objects; + const auto areas_opt = generateBlindSpotPolygons(input_path, closest_idx, stop_line_pose); + if (!areas_opt) { + return InternalError{"Failed to generate BlindSpotPolygons"}; + } /* calculate dynamic collision around detection area */ - bool has_obstacle = checkObstacleInBlindSpot( - lanelet_map_ptr, routing_graph_ptr, *path, objects_ptr, closest_idx, stop_line_pose); + const auto collision_obstacle = isCollisionDetected(areas_opt.value()); state_machine_.setStateWithMarginTime( - has_obstacle ? StateMachine::State::STOP : StateMachine::State::GO, + collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); - /* set stop speed */ - setSafe(state_machine_.getState() != StateMachine::State::STOP); - setDistance(motion_utils::calcSignedArcLength( - path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position)); - if (!isActivated()) { - constexpr double stop_vel = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx, stop_vel, path); - - /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); - planning_utils::appendStopReason(stop_factor, stop_reason); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); - } else { - *path = input_path; // reset path + if (state_machine_.getState() == StateMachine::State::STOP) { + return Unsafe{stop_line_idx, collision_obstacle}; } + + return Safe{stop_line_idx}; +} + +// template-specification based visitor pattern +// https://en.cppreference.com/w/cpp/utility/variant/visit +template +struct VisitorSwitch : Ts... +{ + using Ts::operator()...; +}; +template +VisitorSwitch(Ts...) -> VisitorSwitch; + +void BlindSpotModule::setRTCStatus( + const BlindSpotDecision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, + decision); +} + +void BlindSpotModule::reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { + reactRTCApprovalByDecision(sub_decision, path, stop_reason); + }}, + decision); +} + +bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); + + initializeRTCStatus(); + const auto decision = modifyPathVelocityDetail(path, stop_reason); + const auto & input_path = *path; + setRTCStatus(decision, input_path); + reactRTCApproval(decision, path, stop_reason); + return true; } @@ -259,10 +214,11 @@ std::optional BlindSpotModule::generateInterpolatedPathInf return interpolated_path_info; } -std::optional BlindSpotModule::getSiblingStraightLanelet() const +std::optional BlindSpotModule::getSiblingStraightLanelet( + const std::shared_ptr planner_data) const { - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map_ptr = planner_data->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data->route_handler_->getRoutingGraphPtr(); const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { @@ -326,7 +282,7 @@ static std::optional insertPointIndex( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) - int insert_idx = closest_idx; + size_t insert_idx = closest_idx; autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { @@ -349,6 +305,7 @@ std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * path) const { + // NOTE: this is optionallly int for later subtraction const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interpolated_path_info.ds); @@ -368,7 +325,9 @@ std::optional> BlindSpotModule::generateStopLine( if (!first_conflict_idx_ip_opt) { return std::nullopt; } - const int first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); + + // NOTE: this is optionallly int for later subtraction + const auto first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); stop_idx_default_ip = static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); stop_idx_critical_ip = static_cast(first_conflict_idx_ip); @@ -429,80 +388,90 @@ void BlindSpotModule::cutPredictPathWithDuration( } } -bool BlindSpotModule::checkObstacleInBlindSpot( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) +std::optional BlindSpotModule::isOverPassJudge( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const geometry_msgs::msg::Pose & stop_point_pose) const { - /* get detection area */ - if (turn_direction_ == TurnDirection::INVALID) { - RCLCPP_WARN(logger_, "blind spot detector is running, turn_direction_ = not right or left."); - return false; - } + const auto & current_pose = planner_data_->current_odometry->pose; - const auto areas_opt = generateBlindSpotPolygons( - lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); - if (areas_opt) { - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; - const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; - debug_data_.detection_areas = detection_areas; - debug_data_.conflict_areas = conflict_areas; - debug_data_.detection_areas.insert( - debug_data_.detection_areas.end(), opposite_detection_areas.begin(), - opposite_detection_areas.end()); - debug_data_.conflict_areas.insert( - debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), - opposite_conflict_areas.end()); + /* set judge line dist */ + const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit( + planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, + planner_data_->delay_response_time); + const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + const size_t stop_point_segment_idx = + motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = motion_utils::calcSignedArcLength( + input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, + stop_point_segment_idx); - autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; - cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); + /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ + if (planner_param_.use_pass_judge_line) { + const double eps = 1e-1; // to prevent hunting + if (const auto current_state = state_machine_.getState(); + current_state == StateMachine::State::GO && + distance_until_stop + eps < pass_judge_line_dist) { + return OverPassJudge{"over the pass judge line. no plan needed."}; + } + } + return std::nullopt; +} - // check objects in blind spot areas - for (const auto & object : objects.objects) { - if (!isTargetObjectType(object)) { - continue; - } +std::optional +BlindSpotModule::isCollisionDetected(const BlindSpotPolygons & areas) +{ + // TODO(Mamoru Sobue): only do this for target object + autoware_auto_perception_msgs::msg::PredictedObjects objects = + *(planner_data_->predicted_objects); + cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); + + // check objects in blind spot areas + for (const auto & object : objects.objects) { + if (!isTargetObjectType(object)) { + continue; + } - // right direction - const bool exist_in_right_detection_area = - std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - // opposite direction - const bool exist_in_opposite_detection_area = std::any_of( - opposite_detection_areas.begin(), opposite_detection_areas.end(), - [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - const bool exist_in_detection_area = - exist_in_right_detection_area || exist_in_opposite_detection_area; - if (!exist_in_detection_area) { - continue; - } - const bool exist_in_right_conflict_area = - isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_opposite_conflict_area = isPredictedPathInArea( - object, opposite_conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_conflict_area = - exist_in_right_conflict_area || exist_in_opposite_conflict_area; - if (exist_in_detection_area && exist_in_conflict_area) { - debug_data_.conflicting_targets.objects.push_back(object); - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - return true; - } + const auto & detection_areas = areas.detection_areas; + const auto & conflict_areas = areas.conflict_areas; + const auto & opposite_detection_areas = areas.opposite_detection_areas; + const auto & opposite_conflict_areas = areas.opposite_conflict_areas; + + // right direction + const bool exist_in_right_detection_area = + std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + // opposite direction + const bool exist_in_opposite_detection_area = std::any_of( + opposite_detection_areas.begin(), opposite_detection_areas.end(), + [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + const bool exist_in_detection_area = + exist_in_right_detection_area || exist_in_opposite_detection_area; + if (!exist_in_detection_area) { + continue; + } + const bool exist_in_right_conflict_area = + isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_opposite_conflict_area = + isPredictedPathInArea(object, opposite_conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_conflict_area = + exist_in_right_conflict_area || exist_in_opposite_conflict_area; + if (exist_in_detection_area && exist_in_conflict_area) { + debug_data_.conflicting_targets.objects.push_back(object); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); + return object; } - return false; - } else { - return false; } + return std::nullopt; } bool BlindSpotModule::isPredictedPathInArea( @@ -621,11 +590,13 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( } std::optional BlindSpotModule::generateBlindSpotPolygons( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const { + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + std::vector lane_ids; lanelet::ConstLanelets blind_spot_lanelets; lanelet::ConstLanelets blind_spot_opposite_lanelets; @@ -744,6 +715,15 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( blind_spot_polygons.opposite_detection_areas.emplace_back( std::move(detection_area_opposite_adj)); } + debug_data_.detection_areas = blind_spot_polygons.detection_areas; + debug_data_.conflict_areas = blind_spot_polygons.conflict_areas; + debug_data_.detection_areas.insert( + debug_data_.detection_areas.end(), blind_spot_polygons.opposite_detection_areas.begin(), + blind_spot_polygons.opposite_detection_areas.end()); + debug_data_.conflict_areas.insert( + debug_data_.conflict_areas.end(), blind_spot_polygons.opposite_conflict_areas.begin(), + blind_spot_polygons.opposite_conflict_areas.end()); + return blind_spot_polygons; } else { return std::nullopt; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index fd93661b33d6b..005984085fcd8 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner @@ -58,16 +59,40 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; +/** + * @brief represent action + */ +struct InternalError +{ + const std::string error; +}; + +struct OverPassJudge +{ + const std::string report; +}; + +struct Unsafe +{ + const size_t stop_line_idx; + const std::optional collision_obstacle; +}; + +struct Safe +{ + const size_t stop_line_idx; +}; + +using BlindSpotDecision = std::variant; + class BlindSpotModule : public SceneModuleInterface { public: - enum class TurnDirection { LEFT = 0, RIGHT, INVALID }; + enum class TurnDirection { LEFT, RIGHT }; struct DebugData { - geometry_msgs::msg::Pose virtual_wall_pose; - geometry_msgs::msg::Pose stop_point_pose; - geometry_msgs::msg::Pose judge_point_pose; + std::optional virtual_wall_pose{std::nullopt}; std::vector conflict_areas; std::vector detection_areas; std::vector opposite_conflict_areas; @@ -91,9 +116,9 @@ class BlindSpotModule : public SceneModuleInterface }; BlindSpotModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -105,18 +130,39 @@ class BlindSpotModule : public SceneModuleInterface std::vector createVirtualWalls() override; private: + // (semi) const variables const int64_t lane_id_; const PlannerParam planner_param_; - TurnDirection turn_direction_{TurnDirection::INVALID}; - bool is_over_pass_judge_line_{false}; + const TurnDirection turn_direction_; std::optional sibling_straight_lanelet_{std::nullopt}; + // state variables + bool is_over_pass_judge_line_{false}; + // Parameter + void initializeRTCStatus(); + BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + // setDafe(), setDistance() + void setRTCStatus( + const BlindSpotDecision & decision, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + template + void setRTCStatusByDecision( + const Decision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + // stop/GO + void reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); + template + void reactRTCApprovalByDecision( + const Decision & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); + std::optional generateInterpolatedPathInfo( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const; - std::optional getSiblingStraightLanelet() const; + std::optional getSiblingStraightLanelet( + const std::shared_ptr planner_data) const; /** * @brief Generate a stop line and insert it into the path. @@ -131,6 +177,10 @@ class BlindSpotModule : public SceneModuleInterface const InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; + std::optional isOverPassJudge( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & stop_point_pose) const; + /** * @brief Check obstacle is in blind spot areas. * Condition1: Object's position is in broad blind spot area. @@ -141,12 +191,8 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return true when an object is detected in blind spot */ - bool checkObstacleInBlindSpot( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose); + std::optional isCollisionDetected( + const BlindSpotPolygons & area); /** * @brief Create half lanelet @@ -168,9 +214,7 @@ class BlindSpotModule : public SceneModuleInterface * @return Blind spot polygons */ std::optional generateBlindSpotPolygons( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & pose) const; /** From 92b51143028ad113a77abcd8fb5e7f424c629769 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 27 May 2024 14:46:10 +0900 Subject: [PATCH 024/120] feat(behavior_path_planner_common): update RTC state to SUCCEEDED (#7119) Signed-off-by: Fumiya Watanabe --- .../interface/scene_module_interface.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index bd7949a83a1d7..1035aff5f7093 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -186,6 +187,10 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); + if (getCurrentStatus() == ModuleStatus::SUCCESS) { + updateRTCStatusForSuccess(); + } + clearWaitingApproval(); unlockNewModuleLaunch(); unlockOutputPath(); @@ -499,6 +504,17 @@ class SceneModuleInterface } } + void updateRTCStatusForSuccess() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + } + } + void setObjectsOfInterestData( const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) From cbaf62a36685b35110afc46097f9d5bba59f27fa Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 27 May 2024 15:39:05 +0900 Subject: [PATCH 025/120] chore(tensorrt_yolox): add processing time and cyclic time debug topic (#7126) fix: add processing time topic Signed-off-by: badai-nguyen --- .../tensorrt_yolox/tensorrt_yolox_node.hpp | 4 +++ .../src/tensorrt_yolox_node.cpp | 25 +++++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index 9b440f1dbdfec..fc396ae4b3473 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include @@ -61,6 +63,8 @@ class TrtYoloXNode : public rclcpp::Node LabelMap label_map_; std::unique_ptr trt_yolox_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index af435ce6efe83..93380b95680c2 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -29,6 +29,14 @@ namespace tensorrt_yolox TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) : Node("tensorrt_yolox", node_options) { + { + stop_watch_ptr_ = + std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "tensorrt_yolox"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } using std::placeholders::_1; using std::chrono_literals::operator""ms; @@ -132,6 +140,7 @@ void TrtYoloXNode::onConnect() void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) { + stop_watch_ptr_->toc("processing_time", true); tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; cv_bridge::CvImagePtr in_image_ptr; @@ -173,6 +182,22 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) out_objects.header = msg->header; objects_pub_->publish(out_objects); + + if (debug_publisher_) { + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - out_objects.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } } bool TrtYoloXNode::readLabelFile(const std::string & label_path) From 54dfc057650c3cbab433844ae83172d63382d34e Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 27 May 2024 16:23:01 +0900 Subject: [PATCH 026/120] feat(map_projection_loader): componentize MapProjectionLoader (#7108) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau * change executable name to match CMakelists Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- map/map_projection_loader/CMakeLists.txt | 20 ++++++++-------- .../map_projection_loader.hpp | 2 +- .../launch/map_projection_loader.launch.xml | 2 +- map/map_projection_loader/package.xml | 1 + .../src/map_projection_loader.cpp | 6 ++++- .../src/map_projection_loader_node.cpp | 23 ------------------- ...load_local_cartesian_utm_from_yaml.test.py | 2 +- .../test_node_load_local_from_yaml.test.py | 2 +- .../test_node_load_mgrs_from_yaml.test.py | 2 +- ...load_transverse_mercator_from_yaml.test.py | 2 +- 10 files changed, 22 insertions(+), 40 deletions(-) delete mode 100644 map/map_projection_loader/src/map_projection_loader_node.cpp diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index f6102a1efa795..87f519ab22572 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -6,25 +6,24 @@ autoware_package() ament_auto_find_build_dependencies() -ament_auto_add_library(map_projection_loader_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_projection_loader.cpp src/load_info_from_lanelet2_map.cpp ) -target_link_libraries(map_projection_loader_lib yaml-cpp) - -ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) - -target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapProjectionLoader" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(map_projection_loader map_projection_loader_lib) -target_include_directories(map_projection_loader PUBLIC include) +target_link_libraries(${PROJECT_NAME} yaml-cpp) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" map_projection_loader_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() @@ -57,7 +56,8 @@ if(BUILD_TESTING) add_testcase(test/test_load_info_from_lanelet2_map.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 54e794e2742bf..05bc6e64e1675 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -29,7 +29,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( class MapProjectionLoader : public rclcpp::Node { public: - MapProjectionLoader(); + explicit MapProjectionLoader(const rclcpp::NodeOptions & options); private: using MapProjectorInfo = map_interface::MapProjectorInfo; diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index a6570b69d3498..13418a7e97423 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 475881577bd58..7a930085cd7b1 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -21,6 +21,7 @@ component_interface_utils lanelet2_extension rclcpp + rclcpp_components tier4_map_msgs yaml-cpp diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 5966baaed8383..383051e8f67a5 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -82,7 +82,8 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) +: rclcpp::Node("map_projection_loader", options) { const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); const std::string lanelet2_map_filename = @@ -96,3 +97,6 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") adaptor.init_pub(publisher_); publisher_->publish(msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapProjectionLoader) diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp deleted file mode 100644 index 1d9336be0d9dd..0000000000000 --- a/map/map_projection_loader/src/map_projection_loader_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "map_projection_loader/map_projection_loader.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index b8540550ce9da..0d0b5cb31afba 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index c7697038cc253..6a17ff340b19f 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index f75beddc6827c..37cfd9936bf20 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index 765f3cde04679..7bccdc7875454 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { From 6cb807ddc7342c7c8085ee009968151a266e54d6 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 27 May 2024 17:02:53 +0900 Subject: [PATCH 027/120] feat(ndt_scan_matcher): componentize NDTScanMatcher (#7130) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from log to both Signed-off-by: a-maumau * remove unnecessary Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- localization/ndt_scan_matcher/CMakeLists.txt | 22 +++++------- .../launch/ndt_scan_matcher.launch.xml | 2 +- localization/ndt_scan_matcher/package.xml | 1 + .../src/ndt_scan_matcher_core.cpp | 3 ++ .../src/ndt_scan_matcher_node.cpp | 35 ------------------- 5 files changed, 13 insertions(+), 50 deletions(-) delete mode 100644 localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 7ac78514c49fe..5e4db9571c404 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -22,20 +22,24 @@ else() endif() endif() -find_package(glog REQUIRED) find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) -ament_auto_add_executable(ndt_scan_matcher +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics_module.cpp src/map_update_module.cpp src/ndt_scan_matcher_core.cpp - src/ndt_scan_matcher_node.cpp src/particle.cpp ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) +target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "NDTScanMatcher" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) if(BUILD_TESTING) add_launch_test( @@ -46,19 +50,9 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation test/test_cases/standard_sequence_for_initial_pose_estimation.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) endif() diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index cbcb0a9f74bc4..671e2ace56cad 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b9a1f7dbad1bd..a37e7c370adf5 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -29,6 +29,7 @@ ndt_omp pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs std_srvs diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 69349e75e786d..624eb89671a41 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -1070,3 +1070,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( return result_pose_with_cov_msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp deleted file mode 100644 index 785055eef3700..0000000000000 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2015-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 "ndt_scan_matcher/ndt_scan_matcher_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); // NOLINT - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto ndt_scan_matcher = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(ndt_scan_matcher); - exec.spin(); - rclcpp::shutdown(); - return 0; -} From 177db15fcd3e3e9252e7eae2b33a41120c5fb4a0 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Mon, 27 May 2024 11:36:33 +0200 Subject: [PATCH 028/120] refactor(behavior_velocity_planner)!: prefix package and namespace with autoware_ (#6693) Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- .../behavior_planning.launch.xml | 2 +- launch/tier4_planning_launch/package.xml | 2 +- planning/.pages | 2 +- .../CMakeLists.txt | 4 ++-- .../README.md | 0 .../config/behavior_velocity_planner.param.yaml | 0 ...ehaviorVelocityPlanner-Architecture.drawio.svg | 0 .../docs/set_stop_velocity.drawio.svg | 0 .../launch/behavior_velocity_planner.launch.xml | 2 +- .../package.xml | 4 ++-- .../schema/behavior_velocity_planner.schema.json | 0 .../src/node.cpp | 15 ++++++++------- .../src/node.hpp | 13 +++++++------ .../src/planner_manager.cpp | 6 +++--- .../src/planner_manager.hpp | 7 +++++-- .../srv/LoadPlugin.srv | 0 .../srv/UnloadPlugin.srv | 0 .../test/src/test_node_interface.cpp | 4 ++-- .../package.xml | 2 +- .../test/test_static_centerline_generator.test.py | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- 37 files changed, 52 insertions(+), 47 deletions(-) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/CMakeLists.txt (90%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/README.md (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/config/behavior_velocity_planner.param.yaml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/docs/BehaviorVelocityPlanner-Architecture.drawio.svg (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/docs/set_stop_velocity.drawio.svg (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/launch/behavior_velocity_planner.launch.xml (97%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/package.xml (96%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/schema/behavior_velocity_planner.schema.json (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/node.cpp (96%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/node.hpp (93%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/planner_manager.cpp (95%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/planner_manager.hpp (91%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/srv/LoadPlugin.srv (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/srv/UnloadPlugin.srv (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/test/src/test_node_interface.cpp (98%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 44632c7bf99ce..2eb1bdcdfcb33 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -157,6 +157,7 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @@ -179,7 +180,6 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 865ab99e870e4..57d0280330641 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -220,7 +220,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index d04a405a61c7b..9d39c397f9417 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,9 +57,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_remaining_distance_time_calculator behavior_path_planner - behavior_velocity_planner costmap_generator external_cmd_selector external_velocity_limit_selector diff --git a/planning/.pages b/planning/.pages index 645a32b4b05fa..e355639eb8da5 100644 --- a/planning/.pages +++ b/planning/.pages @@ -18,7 +18,7 @@ nav: - 'Side Shift': planning/behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module - 'Behavior Velocity Planner': - - 'About Behavior Velocity': planning/behavior_velocity_planner + - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/behavior_velocity_template_module - 'Available Module': - 'Blind Spot': planning/behavior_velocity_blind_spot_module diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/autoware_behavior_velocity_planner/CMakeLists.txt similarity index 90% rename from planning/behavior_velocity_planner/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner/CMakeLists.txt index ee7dd7c07a77a..37a02b844dfe9 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner) +project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -19,7 +19,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_lib - PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" + PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md similarity index 100% rename from planning/behavior_velocity_planner/README.md rename to planning/autoware_behavior_velocity_planner/README.md diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml rename to planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml diff --git a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg diff --git a/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml similarity index 97% rename from planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml rename to planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index c1631fdb739de..d4478f87f0610 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -30,7 +30,7 @@ - + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml similarity index 96% rename from planning/behavior_velocity_planner/package.xml rename to planning/autoware_behavior_velocity_planner/package.xml index 1357b555f0ba4..573d862f1725b 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner + autoware_behavior_velocity_planner 0.1.0 - The behavior_velocity_planner package + The autoware_behavior_velocity_planner package Mamoru Sobue Takayuki Murooka diff --git a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json similarity index 100% rename from planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json rename to planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/node.cpp rename to planning/autoware_behavior_velocity_planner/src/node.cpp index 1801db4658a72..d4ae3897bca65 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -52,7 +52,7 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -436,14 +436,15 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = + ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = - interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = filterStopPathPoint(interpolated_path_msg); + output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); @@ -477,7 +478,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( } debug_viz_pub_->publish(output_msg); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include -RCLCPP_COMPONENTS_REGISTER_NODE(behavior_velocity_planner::BehaviorVelocityPlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp similarity index 93% rename from planning/behavior_velocity_planner/src/node.hpp rename to planning/autoware_behavior_velocity_planner/src/node.hpp index dd6edf6ae0bc0..6e74203460542 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -18,8 +18,8 @@ #include "planner_manager.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include #include @@ -44,11 +44,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; -using behavior_velocity_planner::srv::LoadPlugin; -using behavior_velocity_planner::srv::UnloadPlugin; +using autoware_behavior_velocity_planner::srv::LoadPlugin; +using autoware_behavior_velocity_planner::srv::UnloadPlugin; +using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node @@ -134,6 +135,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/planner_manager.cpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index cb491920c87d4..f939f67a06f9e 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -50,7 +50,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { } @@ -128,4 +128,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe { return stop_reason_diag_; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp similarity index 91% rename from planning/behavior_velocity_planner/src/planner_manager.hpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 7485e5faba8bf..f9c1d0253de62 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -36,8 +36,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PlannerData; +using ::behavior_velocity_planner::PluginInterface; + class BehaviorVelocityPlannerManager { public: @@ -56,6 +59,6 @@ class BehaviorVelocityPlannerManager pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/srv/LoadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv diff --git a/planning/behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 98% rename from planning/behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index f70259b6a1f80..06c1bc48cf529 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -23,7 +23,7 @@ #include #include -using behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -50,7 +50,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 96e17595d49ee..56d30a715c006 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -43,9 +43,9 @@ ament_cmake_gmock ament_lint_auto + autoware_behavior_velocity_planner autoware_lint_common behavior_path_planner - behavior_velocity_planner ros_testing rosidl_interface_packages diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 3011abccd48ca..dc3c26798b12d 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -59,7 +59,7 @@ def generate_test_description(): "config/behavior_path_planner.param.yaml", ), os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("autoware_behavior_velocity_planner"), "config/behavior_velocity_planner.param.yaml", ), os.path.join( diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt index 2ad4fa495ffce..7ff2419eed919 100644 --- a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_blind_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 69dd1ced695ec..f21d5ab786cad 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_crosswalk_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt index 2a16ed95ba655..e6990d2d28642 100644 --- a/planning/behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_detection_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 20f4b3aa8f4d3..481ac245cb432 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_dynamic_obstacle_stop_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 07847a08c1209..c02b5a3852722 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_intersection_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) diff --git a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt index ef3073df56c22..265b5b637e7ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_drivable_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt index 4d14228c4685c..ba3fd0e4f71dd 100644 --- a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_stopping_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt index 7a062ee854421..8b327291a60f1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_occlusion_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 8ce1a334c4b35..e8191f9b632d5 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_out_of_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 21758c4d0198c..3ee589ba69740 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_run_out_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt index a96eb3398e5c0..f5d3a7078be9c 100644 --- a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_speed_bump_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt index 3c8cc6a4f9626..bef98aafe6f75 100644 --- a/planning/behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_stop_line_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_template_module/CMakeLists.txt b/planning/behavior_velocity_template_module/CMakeLists.txt index e0beb42daf50d..103a3d0a4ceb3 100644 --- a/planning/behavior_velocity_template_module/CMakeLists.txt +++ b/planning/behavior_velocity_template_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_template_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/manager.cpp diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt index cf9392fa0568f..5737802408996 100644 --- a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index 48ffa35cbae5e..a1c00cf49db29 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_virtual_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_walkway_module/CMakeLists.txt index cd41dec991bc6..351a240743402 100644 --- a/planning/behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_walkway_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_walkway_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp From 6dd23398da3c31c5d489dab39bc26f6a9b585520 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 27 May 2024 18:42:42 +0900 Subject: [PATCH 029/120] fix(crosswalk): fix calclation distance from the null polygons (#7122) Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.hpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e9fdc38304603..faba8730aa6d9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -182,7 +182,7 @@ class CrosswalkModule : public SceneModuleInterface const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, const bool is_ego_yielding, const std::optional & collision_point, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const bool is_object_on_ego_path) + const bool is_object_away_from_path) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -202,7 +202,7 @@ class CrosswalkModule : public SceneModuleInterface planner_param.timeout_set_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; - if (is_ego_yielding && !intent_to_cross && !is_object_on_ego_path) { + if (is_ego_yielding && !intent_to_cross && is_object_away_from_path) { collision_state = CollisionState::IGNORE; return; } @@ -261,15 +261,16 @@ class CrosswalkModule : public SceneModuleInterface // update current uuids current_uuids_.push_back(uuid); - const bool is_object_on_ego_path = - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) < - 0.5; + const bool is_object_away_from_path = + !attention_area.outer().empty() && + boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) > + 0.5; // add new object if (objects.count(uuid) == 0) { if ( has_traffic_light && planner_param.disable_yield_for_new_stopped_object && - !is_object_on_ego_path) { + is_object_away_from_path) { objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); } else { objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); @@ -279,7 +280,7 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon, - is_object_on_ego_path); + is_object_away_from_path); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification; From 965fdf4708d775f247a942d52b069671367322a4 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 27 May 2024 20:15:14 +0900 Subject: [PATCH 030/120] feat(tier4_system_launch): modify diagnostic_graph_aggregator_graph argument (#7133) Signed-off-by: Takagi, Isamu --- launch/tier4_system_launch/launch/system.launch.xml | 6 +----- system/system_diagnostic_monitor/config/map.yaml | 2 +- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index e5f7aa4e18b33..6253dce864ddf 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -29,8 +29,7 @@ - - + @@ -113,9 +112,6 @@ - - - diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml index 45c1db294119f..231ac6eb5fa44 100644 --- a/system/system_diagnostic_monitor/config/map.yaml +++ b/system/system_diagnostic_monitor/config/map.yaml @@ -13,4 +13,4 @@ units: - path: /autoware/map/topic_rate_check/pointcloud_map type: diag node: topic_state_monitor_pointcloud_map - name: map_topic_status" + name: map_topic_status From f2a3fd379abd71aa91a404c2137edce344a2f3e8 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 27 May 2024 20:30:41 +0900 Subject: [PATCH 031/120] feat(default_ad_api): add diagnostics api (#7052) Signed-off-by: Takagi, Isamu --- system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/package.xml | 1 + system/default_ad_api/src/diagnostics.cpp | 81 +++++++++++++++++++ system/default_ad_api/src/diagnostics.hpp | 46 +++++++++++ .../include/diagnostic_graph_utils/graph.hpp | 11 ++- .../diagnostic_graph_utils/src/lib/graph.cpp | 2 +- 7 files changed, 142 insertions(+), 2 deletions(-) create mode 100644 system/default_ad_api/src/diagnostics.cpp create mode 100644 system/default_ad_api/src/diagnostics.hpp diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 4dacc0b893714..7975937f5b2c1 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/diagnostics.cpp src/fail_safe.cpp src/heartbeat.cpp src/interface.cpp @@ -23,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" + "default_ad_api::DiagnosticsNode" "default_ad_api::FailSafeNode" "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index f4f3986c678e9..ca1575a528db2 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -42,6 +42,7 @@ def get_default_config(): def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), + create_api_node("diagnostics", "DiagnosticsNode"), create_api_node("fail_safe", "FailSafeNode"), create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 4e691f7ab3192..f9b5a167bdab1 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -21,6 +21,7 @@ autoware_planning_msgs component_interface_specs component_interface_utils + diagnostic_graph_utils geographic_msgs geography_utils motion_utils diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp new file mode 100644 index 0000000000000..f4e58b5a4d441 --- /dev/null +++ b/system/default_ad_api/src/diagnostics.cpp @@ -0,0 +1,81 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "diagnostics.hpp" + +#include +#include + +namespace default_ad_api +{ + +DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("diagnostics", options) +{ + using std::placeholders::_1; + + pub_struct_ = create_publisher( + "/api/system/diagnostics/struct", rclcpp::QoS(1).transient_local()); + pub_status_ = create_publisher( + "/api/system/diagnostics/status", rclcpp::QoS(1)); + + sub_graph_.register_create_callback(std::bind(&DiagnosticsNode::on_create, this, _1)); + sub_graph_.register_update_callback(std::bind(&DiagnosticsNode::on_update, this, _1)); + sub_graph_.subscribe(*this, 10); +} +void DiagnosticsNode::on_create(DiagGraph::ConstSharedPtr graph) +{ + const auto & units = graph->units(); + const auto & links = graph->links(); + + std::unordered_map unit_indices_; + for (size_t i = 0; i < units.size(); ++i) { + unit_indices_[units[i]] = i; + } + + autoware_adapi_v1_msgs::msg::DiagGraphStruct msg; + msg.stamp = graph->created_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + msg.links.reserve(links.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().path = unit->path(); + } + for (const auto & link : links) { + msg.links.emplace_back(); + msg.links.back().parent = unit_indices_.at(link->parent()); + msg.links.back().child = unit_indices_.at(link->child()); + } + pub_struct_->publish(msg); +} + +void DiagnosticsNode::on_update(DiagGraph::ConstSharedPtr graph) +{ + const auto & units = graph->units(); + + autoware_adapi_v1_msgs::msg::DiagGraphStatus msg; + msg.stamp = graph->updated_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().level = unit->level(); + } + pub_status_->publish(msg); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::DiagnosticsNode) diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp new file mode 100644 index 0000000000000..302ffe39159df --- /dev/null +++ b/system/default_ad_api/src/diagnostics.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DIAGNOSTICS_HPP_ +#define DIAGNOSTICS_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include +#include + +namespace default_ad_api +{ + +class DiagnosticsNode : public rclcpp::Node +{ +public: + explicit DiagnosticsNode(const rclcpp::NodeOptions & options); + +private: + using DiagGraph = diagnostic_graph_utils::DiagGraph; + using DiagUnit = diagnostic_graph_utils::DiagUnit; + using DiagLink = diagnostic_graph_utils::DiagLink; + void on_create(DiagGraph::ConstSharedPtr graph); + void on_update(DiagGraph::ConstSharedPtr graph); + rclcpp::Publisher::SharedPtr pub_struct_; + rclcpp::Publisher::SharedPtr pub_status_; + diagnostic_graph_utils::DiagGraphSubscription sub_graph_; +}; + +} // namespace default_ad_api + +#endif // DIAGNOSTICS_HPP_ diff --git a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp index b2c6fc752e463..c5b386dbe2c86 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp +++ b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp @@ -55,12 +55,20 @@ class DiagLink public: using DiagLinkStruct = tier4_system_msgs::msg::DiagLinkStruct; using DiagLinkStatus = tier4_system_msgs::msg::DiagLinkStatus; - explicit DiagLink(const DiagLinkStruct & msg) : struct_(msg) {} + DiagLink(const DiagLinkStruct & msg, DiagUnit * parent, DiagUnit * child) : struct_(msg) + { + parent_ = parent; + child_ = child; + } void update(const DiagLinkStatus & msg) { status_ = msg; } + DiagUnit * parent() const { return parent_; } + DiagUnit * child() const { return child_; } private: DiagLinkStruct struct_; DiagLinkStatus status_; + DiagUnit * parent_; + DiagUnit * child_; }; class DiagNode : public DiagUnit @@ -114,6 +122,7 @@ class DiagGraph bool update(const DiagGraphStatus & msg); rclcpp::Time created_stamp() const { return created_stamp_; } rclcpp::Time updated_stamp() const { return updated_stamp_; } + std::string id() const { return id_; } std::vector units() const; std::vector nodes() const; std::vector diags() const; diff --git a/system/diagnostic_graph_utils/src/lib/graph.cpp b/system/diagnostic_graph_utils/src/lib/graph.cpp index f1177c1047bdb..bd4c215a346ed 100644 --- a/system/diagnostic_graph_utils/src/lib/graph.cpp +++ b/system/diagnostic_graph_utils/src/lib/graph.cpp @@ -54,7 +54,7 @@ void DiagGraph::create(const DiagGraphStruct & msg) for (const auto & data : msg.links) { DiagNode * parent = nodes_.at(data.parent).get(); DiagUnit * child = get_child(data.is_leaf, data.child); - const auto link = links_.emplace_back(std::make_unique(data)).get(); + const auto link = links_.emplace_back(std::make_unique(data, parent, child)).get(); parent->add_child({link, child}); } } From 1093f3a9dcf65719e92c88709baa4e7de6d16db4 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 27 May 2024 12:11:15 +0000 Subject: [PATCH 032/120] chore: update CODEOWNERS (#7088) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions --- .github/CODEOWNERS | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 2eb1bdcdfcb33..5f5d5008c0f1c 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -21,7 +21,6 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/mission_planner_rviz_plugin/** isamu.takagi@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @@ -29,27 +28,18 @@ common/path_distance_calculator/** isamu.takagi@tier4.jp common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp -common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp -common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp -common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp -common/tier4_screen_capture_rviz_plugin/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp -common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp -common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp -common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp From 967f4a76142f2a3af073d7908a7e18c4b85dc092 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Mon, 27 May 2024 22:17:15 +0900 Subject: [PATCH 033/120] fix(behavior_velocity_blind_spot_module): fix typo in behavior_velocity_blind_spot_module (#7136) fix typo Signed-off-by: a-maumau --- planning/behavior_velocity_blind_spot_module/src/scene.cpp | 4 ++-- planning/behavior_velocity_blind_spot_module/src/scene.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 22e54464d400e..c591a240029ff 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -305,7 +305,7 @@ std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * path) const { - // NOTE: this is optionallly int for later subtraction + // NOTE: this is optionally int for later subtraction const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interpolated_path_info.ds); @@ -326,7 +326,7 @@ std::optional> BlindSpotModule::generateStopLine( return std::nullopt; } - // NOTE: this is optionallly int for later subtraction + // NOTE: this is optionally int for later subtraction const auto first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); stop_idx_default_ip = static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 005984085fcd8..2f4ccc37ec51e 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -143,7 +143,7 @@ class BlindSpotModule : public SceneModuleInterface void initializeRTCStatus(); BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); - // setDafe(), setDistance() + // setSafe(), setDistance() void setRTCStatus( const BlindSpotDecision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); From f28a7576588f03b33d7d76e7b14a3df2ef52e412 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Tue, 28 May 2024 09:14:40 +0900 Subject: [PATCH 034/120] feat(geo_pose_projector): componentize GeoPoseProjector (#7138) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- .../geo_pose_projector/CMakeLists.txt | 10 +++++-- .../launch/geo_pose_projector.launch.xml | 2 +- localization/geo_pose_projector/package.xml | 1 + .../src/geo_pose_projector.cpp | 7 +++-- .../src/geo_pose_projector.hpp | 2 +- .../src/geo_pose_projector_node.cpp | 29 ------------------- 6 files changed, 15 insertions(+), 36 deletions(-) delete mode 100644 localization/geo_pose_projector/src/geo_pose_projector_node.cpp diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt index 6e2cdf32fb6c5..f274e7d23dbab 100644 --- a/localization/geo_pose_projector/CMakeLists.txt +++ b/localization/geo_pose_projector/CMakeLists.txt @@ -4,11 +4,15 @@ project(geo_pose_projector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(geo_pose_projector - src/geo_pose_projector_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/geo_pose_projector.cpp ) -ament_target_dependencies(geo_pose_projector) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "GeoPoseProjector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml index d840add1ed1c7..de6785a016858 100644 --- a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml +++ b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index c0e2db59deb64..36b2aec8384ac 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -24,6 +24,7 @@ geography_utils geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tf2_ros diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp index d147888bb743b..80bb407958de2 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -25,8 +25,8 @@ #include -GeoPoseProjector::GeoPoseProjector() -: Node("geo_pose_projector"), publish_tf_(declare_parameter("publish_tf")) +GeoPoseProjector::GeoPoseProjector(const rclcpp::NodeOptions & options) +: rclcpp::Node("geo_pose_projector", options), publish_tf_(declare_parameter("publish_tf")) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -102,3 +102,6 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m tf_broadcaster_->sendTransform(transform_stamped); } } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(GeoPoseProjector) diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp index b0611fec36984..738e805ef8101 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.hpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -36,7 +36,7 @@ class GeoPoseProjector : public rclcpp::Node using MapProjectorInfo = map_interface::MapProjectorInfo; public: - GeoPoseProjector(); + explicit GeoPoseProjector(const rclcpp::NodeOptions & options); private: void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg); diff --git a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp b/localization/geo_pose_projector/src/geo_pose_projector_node.cpp deleted file mode 100644 index 97367d6b9f774..0000000000000 --- a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2023 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 "geo_pose_projector.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} From d4829240bd881aff1e41a5660bf29cfaa0e8df3b Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Tue, 28 May 2024 11:29:20 +0900 Subject: [PATCH 035/120] test(detected_object_validation): add unit tests of utils (#7144) test: add unit tests Signed-off-by: ktro2828 --- .../detected_object_validation/CMakeLists.txt | 6 ++ .../test/test_utils.cpp | 67 +++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 perception/detected_object_validation/test/test_utils.cpp diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index f882c6dd165fe..d023c6c9df328 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -85,6 +85,12 @@ rclcpp_components_register_node(occupancy_grid_based_validator EXECUTABLE occupancy_grid_based_validator_node ) +if(BUILD_TESTING) + ament_auto_add_gtest(detection_object_validation_tests + test/test_utils.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp new file mode 100644 index 0000000000000..ace4dd10a191f --- /dev/null +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -0,0 +1,67 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "detected_object_validation/utils/utils.hpp" + +#include + +#include + +using AutowareLabel = autoware_auto_perception_msgs::msg::ObjectClassification; + +utils::FilterTargetLabel createFilterTargetAll() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = true; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = true; + filter.BICYCLE = true; + filter.PEDESTRIAN = true; + return filter; +} + +utils::FilterTargetLabel createFilterTargetVehicle() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = false; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = false; + filter.BICYCLE = false; + filter.PEDESTRIAN = false; + return filter; +} + +TEST(IsTargetTest, AllTarget) +{ + auto filter = createFilterTargetAll(); + auto label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(label)); +} + +TEST(IsTargetTest, VehicleTarget) +{ + auto filter = createFilterTargetVehicle(); + + auto car_label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(car_label)); + + auto unknown_label = AutowareLabel::UNKNOWN; + EXPECT_FALSE(filter.isTarget(unknown_label)); +} From 84e33d8dc6b87da8b1c75f1b40bf8d7b618bb376 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 28 May 2024 14:46:55 +0900 Subject: [PATCH 036/120] refactor(planning_test_utils): separate hpp and cpp (#7082) Signed-off-by: Muhammad Zulfaqar Azmi --- planning/planning_test_utils/CMakeLists.txt | 3 + .../planning_test_utils.hpp | 598 +++++++++--------- .../src/planning_test_utils.cpp | 313 +++++++++ 3 files changed, 617 insertions(+), 297 deletions(-) create mode 100644 planning/planning_test_utils/src/planning_test_utils.cpp diff --git a/planning/planning_test_utils/CMakeLists.txt b/planning/planning_test_utils/CMakeLists.txt index 56e985c7fa2b7..ce746352bb89c 100644 --- a/planning/planning_test_utils/CMakeLists.txt +++ b/planning/planning_test_utils/CMakeLists.txt @@ -4,6 +4,9 @@ project(planning_test_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(planning_test_utils SHARED + src/planning_test_utils.cpp) + ament_auto_add_library(mock_data_parser SHARED src/mock_data_parser.cpp) diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index 6af1ce165d7ab..43c3e029d4437 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -79,20 +79,248 @@ using tier4_autoware_utils::createQuaternionFromRPY; using tier4_planning_msgs::msg::Scenario; using unique_identifier_msgs::msg::UUID; +/** + * @brief Creates a Pose message with the specified position and orientation. + * + * This function initializes a geometry_msgs::msg::Pose message with the + * given position (x, y, z) and orientation (roll, pitch, yaw). + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position. + * @param roll The roll component of the orientation (in radians). + * @param pitch The pitch component of the orientation (in radians). + * @param yaw The yaw component of the orientation (in radians). + * @return A geometry_msgs::msg::Pose message with the specified position + * and orientation. + */ geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); - return p; -} - -geometry_msgs::msg::Pose createPose(const std::array & pose3d) -{ - return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); -} - + double x, double y, double z, double roll, double pitch, double yaw); + +/** + * @brief Creates a Pose message from a 4-element array representing a 3D pose. + * + * This function initializes a geometry_msgs::msg::Pose message using a + * std::array of four doubles, where the first three elements represent the + * position (x, y, z) and the fourth element represents the yaw orientation. + * The roll and pitch orientations are assumed to be zero. + * + * @param pose3d A std::array of four doubles representing the 3D pose. The + * first element is the x-coordinate, the second is the y-coordinate, the + * third is the z-coordinate, and the fourth is the yaw orientation. + * @return A geometry_msgs::msg::Pose message with the specified position + * and yaw orientation, with roll and pitch set to zero. + */ +geometry_msgs::msg::Pose createPose(const std::array & pose3d); + +/** + * @brief Creates a LaneletSegment with the specified ID. + * + * Initializes a LaneletSegment containing a single LaneletPrimitive with the + * given ID and a primitive type of "lane". + * + * @param id The ID for the LaneletPrimitive and preferred primitive. + * @return A LaneletSegment with the specified ID. + */ +LaneletSegment createLaneletSegment(int id); + +/** + * @brief Loads a Lanelet map from a specified file. + * + * This function loads a Lanelet2 map using the given filename. It uses the MGRS + * projector and checks for any errors during the loading process. If errors + * are found, they are logged, and the function returns nullptr. + * + * @param lanelet2_filename The filename of the Lanelet2 map to load. + * @return A LaneletMapPtr to the loaded map, or nullptr if there were errors. + */ +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); + +/** + * @brief Converts a Lanelet map to a HADMapBin message. + * + * This function converts a given Lanelet map to a HADMapBin message. It also + * parses the format and map versions from the specified filename and includes + * them in the message. The timestamp for the message is set to the provided time. + * + * @param map The Lanelet map to convert. + * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. + * @param now The current time to set in the message header. + * @return A HADMapBin message containing the converted map data. + */ +HADMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +/** + * @brief Creates a normal Lanelet route with predefined start and goal poses. + * + * This function initializes a LaneletRoute with a predefined start and goal pose. + * + * @return A LaneletRoute with the specified start and goal poses. + */ +LaneletRoute makeNormalRoute(); + +/** + * @brief Creates an OccupancyGrid message with the specified dimensions and resolution. + * + * This function initializes an OccupancyGrid message with given width, height, and resolution. + * All cells in the grid are initialized to zero. + * + * @param width The width of the occupancy grid. + * @param height The height of the occupancy grid. + * @param resolution The resolution of the occupancy grid. + * @return An OccupancyGrid message with the specified dimensions and resolution. + */ +OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); + +/** + * @brief Creates a HADMapBin message from a Lanelet map file. + * + * This function loads a Lanelet map from the given file, overwrites the + * centerline with the specified resolution, and converts the map to a HADMapBin message. + * + * @param absolute_path The absolute path to the Lanelet2 map file. + * @param center_line_resolution The resolution for the centerline. + * @return A HADMapBin message containing the map data. + */ +HADMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution = 5.0); + +/** + * @brief Creates a HADMapBin message using a predefined Lanelet2 map file. + * + * This function loads a lanelet2_map.osm from the test_map folder in the + * planning_test_utils package, overwrites the centerline with a resolution of 5.0, + * and converts the map to a HADMapBin message. + * + * @return A HADMapBin message containing the map data. + */ +HADMapBin makeMapBinMsg(); + +/** + * @brief Creates an Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount at y + * direction. x pose, z pose, and yaw angle remains zero. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeOdometry(const double shift = 0.0); + +/** + * @brief Creates an initial Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount, + * accounting for a specific yaw angle. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeInitialPose(const double shift = 0.0); + +/** + * @brief Creates a TFMessage with the specified frame IDs. + * + * This function initializes a TFMessage with a transform containing the given frame IDs. + * The transform includes a specific translation and rotation. + * + * @param target_node The node to use for obtaining the current time. + * @param frame_id The ID of the parent frame. + * @param child_frame_id The ID of the child frame. + * @return A TFMessage containing the transform. + */ +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id = "", + std::string && child_frame_id = ""); + +/** + * @brief Creates a Scenario message with the specified scenario. + * + * This function initializes a Scenario message with the current scenario and a list of activating + * scenarios. + * + * @param scenario The name of the current scenario. + * @return A Scenario message with the specified scenario. + */ +Scenario makeScenarioMsg(const std::string & scenario); + +/** + * @brief Combines two sets of consecutive RouteSections. + * + * This function combines two sets of RouteSections, removing the overlapping end section from the + * first set. + * + * @param route_sections1 The first set of RouteSections. + * @param route_sections2 The second set of RouteSections. + * @return A combined set of RouteSections. + */ +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2); + +/** + * @brief Creates a predefined behavior Lanelet route. + * + * This function initializes a LaneletRoute with predefined start and goal poses, + * a list of lanelet segment IDs, and a fixed UUID. + * this is for the test lanelet2_map.osm + * file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 + * + * @return A LaneletRoute with the specified attributes. + */ +LaneletRoute makeBehaviorNormalRoute(); + +/** + * @brief Spins multiple ROS nodes a specified number of times. + * + * This function spins the given test and target nodes for the specified number of iterations. + * + * @param test_node The test node to spin. + * @param target_node The target node to spin. + * @param repeat_count The number of times to spin the nodes. + */ +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, + const int repeat_count = 1); + +/** + * @brief Updates node options with parameter files. + * + * This function updates the given node options to include the specified parameter files. + * + * @param node_options The node options to update. + * @param params_files A vector of parameter file paths to add to the node options. + */ +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files); + +/** + * @brief Loads a PathWithLaneId message from a YAML file. + * + * This function loads a PathWithLaneId message from a YAML file located in the + * planning_test_utils package. + * + * @return A PathWithLaneId message containing the loaded data. + */ +PathWithLaneId loadPathWithLaneIdInYaml(); + +/** + * @brief Generates a trajectory with specified parameters. + * + * This function generates a trajectory of type T with a given number of points, + * point interval, velocity, initial theta, delta theta, and optionally an + * overlapping point index. + * + * @tparam T The type of the trajectory. + * @param num_points The number of points in the trajectory. + * @param point_interval The distance between consecutive points. + * @param velocity The longitudinal velocity for each point. + * @param init_theta The initial theta angle. + * @param delta_theta The change in theta per point. + * @param overlapping_point_index The index of the point to overlap (default is max size_t value). + * @return A trajectory of type T with the specified parameters. + */ template T generateTrajectory( const size_t num_points, const double point_interval, const double velocity = 0.0, @@ -121,197 +349,17 @@ T generateTrajectory( return traj; } -LaneletSegment createLaneletSegment(int id) -{ - LaneletPrimitive primitive; - primitive.id = id; - primitive.primitive_type = "lane"; - LaneletSegment segment; - segment.preferred_primitive.id = id; - segment.primitives.push_back(primitive); - return segment; -} - -lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) -{ - lanelet::ErrorMessages errors{}; - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); - } - return nullptr; -} - -HADMapBin convertToMapBinMsg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) -{ - std::string format_version{}, map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); - - HADMapBin map_bin_msg; - map_bin_msg.header.stamp = now; - map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; - lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); - - return map_bin_msg; -} - -LaneletRoute makeNormalRoute() -{ - const std::array start_pose{5.5, 4., 0., M_PI_2}; - const std::array goal_pose{8.0, 26.3, 0, 0}; - LaneletRoute route; - route.header.frame_id = "map"; - route.start_pose = createPose(start_pose); - route.goal_pose = createPose(goal_pose); - return route; -} - -OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2) -{ - nav_msgs::msg::OccupancyGrid costmap_msg; - - // create info - costmap_msg.header.frame_id = "map"; - costmap_msg.info.width = width; - costmap_msg.info.height = height; - costmap_msg.info.resolution = resolution; - - // create data - const size_t n_elem = width * height; - for (size_t i = 0; i < n_elem; ++i) { - costmap_msg.data.push_back(0.0); - } - return costmap_msg; -} - -HADMapBin make_map_bin_msg( - const std::string & absolute_path, const double center_line_resolution = 5.0) -{ - const auto map = loadMap(absolute_path); - if (!map) { - return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; - } - - // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); - - // create map bin msg - const auto map_bin_msg = - convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); - return map_bin_msg; -} - -HADMapBin makeMapBinMsg() -{ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; - double center_line_resolution = 5.0; - - return make_map_bin_msg(lanelet2_path, center_line_resolution); -} - -Odometry makeOdometry(const double shift = 0.0) -{ - Odometry odometry; - const std::array start_pose{0.0, shift, 0.0, 0.0}; - odometry.pose.pose = createPose(start_pose); - odometry.header.frame_id = "map"; - return odometry; -} - -Odometry makeInitialPose(const double shift = 0.0) -{ - Odometry current_odometry; - const auto yaw = 0.9724497591854532; - const auto shift_x = shift * std::sin(yaw); - const auto shift_y = shift * std::cos(yaw); - const std::array start_pose{ - 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; - current_odometry.pose.pose = test_utils::createPose(start_pose); - current_odometry.header.frame_id = "map"; - return current_odometry; -} - -TFMessage makeTFMsg( - rclcpp::Node::SharedPtr target_node, std::string frame_id = "", std::string child_frame_id = "") -{ - TFMessage tf_msg; - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = 0.; - quaternion.y = 0.; - quaternion.z = 0.23311256049418302; - quaternion.w = 0.9724497591854532; - - TransformStamped tf; - tf.header.stamp = target_node->get_clock()->now(); - tf.header.frame_id = frame_id; - tf.child_frame_id = child_frame_id; - tf.transform.translation.x = 3722.16015625; - tf.transform.translation.y = 73723.515625; - tf.transform.translation.z = 0; - tf.transform.rotation = quaternion; - - tf_msg.transforms.emplace_back(std::move(tf)); - return tf_msg; -} - -Scenario makeScenarioMsg(const std::string scenario) -{ - Scenario scenario_msg; - scenario_msg.current_scenario = scenario; - scenario_msg.activating_scenarios = {scenario}; - return scenario_msg; -} - -RouteSections combineConsecutiveRouteSections( - const RouteSections & route_sections1, const RouteSections & route_sections2) -{ - RouteSections route_sections; - route_sections.reserve(route_sections1.size() + route_sections2.size()); - if (!route_sections1.empty()) { - // remove end route section because it is overlapping with first one in next route_section - route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); - } - if (!route_sections2.empty()) { - route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); - } - return route_sections; -} - -// this is for the test lanelet2_map.osm -// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 -LaneletRoute makeBehaviorNormalRoute() -{ - LaneletRoute route; - route.header.frame_id = "map"; - route.start_pose = - createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); - route.goal_pose = - createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); - - std::vector primitive_ids = {9102, 9178, 54, 112}; - for (int id : primitive_ids) { - route.segments.push_back(createLaneletSegment(id)); - } - - std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, - 252, 221, 230, 92, 122, 170, 46, 6}; - route.uuid.uuid = uuid_bytes; - - route.allow_modification = false; - return route; -} - +/** + * @brief Creates a publisher with appropriate QoS settings. + * + * This function creates a publisher for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be created. + */ template void createPublisherWithQoS( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -329,6 +377,16 @@ void createPublisherWithQoS( } } +/** + * @brief Sets up a publisher for a given topic. + * + * This function sets up a publisher for a given topic using createPublisherWithQoS. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be set. + */ template void setPublisher( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -337,6 +395,18 @@ void setPublisher( createPublisherWithQoS(test_node, topic_name, publisher); } +/** + * @brief Creates a subscription with appropriate QoS settings. + * + * This function creates a subscription for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param callback The callback function to call when a message is received. + * @param subscriber A reference to the subscription to be created. + */ template void createSubscription( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -350,6 +420,17 @@ void createSubscription( } } +/** + * @brief Sets up a subscriber for a given topic. + * + * This function sets up a subscriber for a given topic using createSubscription. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param subscriber A reference to the subscription to be set. + * @param count A reference to a counter that increments on message receipt. + */ template void setSubscriber( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -359,32 +440,32 @@ void setSubscriber( test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber); } -void spinSomeNodes( - rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, - const int repeat_count = 1) -{ - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(test_node); - executor.add_node(target_node); - for (int i = 0; i < repeat_count; i++) { - executor.spin_some(std::chrono::milliseconds(100)); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } -} - +/** + * @brief Publishes data to a target node. + * + * This function publishes data to a target node on a given topic, ensuring that the topic has + * subscribers and retrying a specified number of times. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param target_node The target node to publish the message to. + * @param topic_name The name of the topic to publish to. + * @param publisher A shared pointer to the publisher. + * @param data The data to publish. + * @param repeat_count The number of times to retry publishing. + */ template void publishToTargetNode( rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) { if (topic_name.empty()) { - int status; + int status = 1; char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); if (status == 0) { throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); - } else { - throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); } + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); } test_utils::setPublisher(test_node, topic_name, publisher); @@ -396,83 +477,6 @@ void publishToTargetNode( test_utils::spinSomeNodes(test_node, target_node, repeat_count); } -void updateNodeOptions( - rclcpp::NodeOptions & node_options, const std::vector & params_files) -{ - std::vector arguments; - arguments.push_back("--ros-args"); - arguments.push_back("--params-file"); - for (const auto & param_file : params_files) { - arguments.push_back(param_file.c_str()); - arguments.push_back("--params-file"); - } - arguments.pop_back(); - - node_options.arguments(std::vector{arguments.begin(), arguments.end()}); -} - -PathWithLaneId loadPathWithLaneIdInYaml() -{ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto yaml_path = planning_test_utils_dir + "/config/path_with_lane_id_data.yaml"; - YAML::Node yaml_node = YAML::LoadFile(yaml_path); - PathWithLaneId path_msg; - - // Convert YAML data to PathWithLaneId message - // Fill the header - path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); - path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); - path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); - - // Fill the points - for (const auto & point_node : yaml_node["points"]) { - PathPointWithLaneId point; - // Fill the PathPoint data - point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); - point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); - point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); - point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); - point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); - point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); - point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); - point.point.longitudinal_velocity_mps = - point_node["point"]["longitudinal_velocity_mps"].as(); - point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); - point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); - point.point.is_final = point_node["point"]["is_final"].as(); - // Fill the lane_ids - for (const auto & lane_id_node : point_node["lane_ids"]) { - point.lane_ids.push_back(lane_id_node.as()); - } - - path_msg.points.push_back(point); - } - - // Fill the left_bound - for (const auto & point_node : yaml_node["left_bound"]) { - Point point; - // Fill the Point data (left_bound) - point.x = point_node["x"].as(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.left_bound.push_back(point); - } - - // Fill the right_bound - for (const auto & point_node : yaml_node["right_bound"]) { - Point point; - // Fill the Point data - point.x = point_node["x"].as(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.right_bound.push_back(point); - } - return path_msg; -} - } // namespace test_utils #endif // PLANNING_TEST_UTILS__PLANNING_TEST_UTILS_HPP_ diff --git a/planning/planning_test_utils/src/planning_test_utils.cpp b/planning/planning_test_utils/src/planning_test_utils.cpp new file mode 100644 index 0000000000000..efed22855cf0f --- /dev/null +++ b/planning/planning_test_utils/src/planning_test_utils.cpp @@ -0,0 +1,313 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planning_test_utils/planning_test_utils.hpp" +namespace test_utils +{ + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +geometry_msgs::msg::Pose createPose(const std::array & pose3d) +{ + return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); +} + +LaneletSegment createLaneletSegment(int id) +{ + LaneletPrimitive primitive; + primitive.id = id; + primitive.primitive_type = "lane"; + LaneletSegment segment; + segment.preferred_primitive.id = id; + segment.primitives.push_back(primitive); + return segment; +} + +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; +} + +HADMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + HADMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.format_version = format_version; + map_bin_msg.map_version = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +LaneletRoute makeNormalRoute() +{ + const std::array start_pose{5.5, 4., 0., M_PI_2}; + const std::array goal_pose{8.0, 26.3, 0, 0}; + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = createPose(start_pose); + route.goal_pose = createPose(goal_pose); + return route; +} + +OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) +{ + nav_msgs::msg::OccupancyGrid costmap_msg; + + // create info + costmap_msg.header.frame_id = "map"; + costmap_msg.info.width = width; + costmap_msg.info.height = height; + costmap_msg.info.resolution = static_cast(resolution); + + // create data + const size_t n_elem = width * height; + for (size_t i = 0; i < n_elem; ++i) { + costmap_msg.data.push_back(0.0); + } + return costmap_msg; +} + +HADMapBin make_map_bin_msg(const std::string & absolute_path, const double center_line_resolution) +{ + const auto map = loadMap(absolute_path); + if (!map) { + return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; +} + +HADMapBin makeMapBinMsg() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; + double center_line_resolution = 5.0; + + return make_map_bin_msg(lanelet2_path, center_line_resolution); +} + +Odometry makeOdometry(const double shift) +{ + Odometry odometry; + const std::array start_pose{0.0, shift, 0.0, 0.0}; + odometry.pose.pose = createPose(start_pose); + odometry.header.frame_id = "map"; + return odometry; +} + +Odometry makeInitialPose(const double shift) +{ + Odometry current_odometry; + const auto yaw = 0.9724497591854532; + const auto shift_x = shift * std::sin(yaw); + const auto shift_y = shift * std::cos(yaw); + const std::array start_pose{ + 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; + current_odometry.pose.pose = test_utils::createPose(start_pose); + current_odometry.header.frame_id = "map"; + return current_odometry; +} + +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id, std::string && child_frame_id) +{ + TFMessage tf_msg; + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = 0.; + quaternion.y = 0.; + quaternion.z = 0.23311256049418302; + quaternion.w = 0.9724497591854532; + + TransformStamped tf; + tf.header.stamp = target_node->get_clock()->now(); + tf.header.frame_id = frame_id; + tf.child_frame_id = child_frame_id; + tf.transform.translation.x = 3722.16015625; + tf.transform.translation.y = 73723.515625; + tf.transform.translation.z = 0; + tf.transform.rotation = quaternion; + + tf_msg.transforms.emplace_back(std::move(tf)); + return tf_msg; +} + +Scenario makeScenarioMsg(const std::string & scenario) +{ + Scenario scenario_msg; + scenario_msg.current_scenario = scenario; + scenario_msg.activating_scenarios = {scenario}; + return scenario_msg; +} + +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2) +{ + RouteSections route_sections; + route_sections.reserve(route_sections1.size() + route_sections2.size()); + if (!route_sections1.empty()) { + // remove end route section because it is overlapping with first one in next route_section + route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); + } + if (!route_sections2.empty()) { + route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); + } + return route_sections; +} + +// this is for the test lanelet2_map.osm +// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 +LaneletRoute makeBehaviorNormalRoute() +{ + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = + createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); + route.goal_pose = + createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); + + std::vector primitive_ids = {9102, 9178, 54, 112}; + for (int id : primitive_ids) { + route.segments.push_back(createLaneletSegment(id)); + } + + std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, + 252, 221, 230, 92, 122, 170, 46, 6}; + route.uuid.uuid = uuid_bytes; + + route.allow_modification = false; + return route; +} + +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, const int repeat_count) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); + for (int i = 0; i < repeat_count; i++) { + executor.spin_some(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files) +{ + std::vector arguments; + arguments.push_back("--ros-args"); + arguments.push_back("--params-file"); + for (const auto & param_file : params_files) { + arguments.push_back(param_file.c_str()); + arguments.push_back("--params-file"); + } + arguments.pop_back(); + + node_options.arguments(std::vector{arguments.begin(), arguments.end()}); +} + +PathWithLaneId loadPathWithLaneIdInYaml() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto yaml_path = planning_test_utils_dir + "/config/path_with_lane_id_data.yaml"; + YAML::Node yaml_node = YAML::LoadFile(yaml_path); + PathWithLaneId path_msg; + + // Convert YAML data to PathWithLaneId message + // Fill the header + path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); + path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); + path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); + + // Fill the points + for (const auto & point_node : yaml_node["points"]) { + PathPointWithLaneId point; + // Fill the PathPoint data + point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); + point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); + point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); + point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); + point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); + point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); + point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); + point.point.longitudinal_velocity_mps = + point_node["point"]["longitudinal_velocity_mps"].as(); + point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); + point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); + point.point.is_final = point_node["point"]["is_final"].as(); + // Fill the lane_ids + for (const auto & lane_id_node : point_node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + + path_msg.points.push_back(point); + } + + // Fill the left_bound + for (const auto & point_node : yaml_node["left_bound"]) { + Point point; + // Fill the Point data (left_bound) + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.left_bound.push_back(point); + } + + // Fill the right_bound + for (const auto & point_node : yaml_node["right_bound"]) { + Point point; + // Fill the Point data + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.right_bound.push_back(point); + } + return path_msg; +} + +} // namespace test_utils From 6744b1a8a749543d5f80bbb359727320cf801fe7 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 28 May 2024 15:05:31 +0900 Subject: [PATCH 037/120] fix(ndt_scan_matcher): changed the type of timestamp from double to int in ndt diag (#7128) Changed the type of timestamp from double to int in ndt diag Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher/src/map_update_module.cpp | 2 +- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 43174a39b797e..ae68dfc44c2c9 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -52,7 +52,7 @@ void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().seconds()); + diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().nanoseconds()); // check is_activated diagnostics_ptr->addKeyValue("is_activated", is_activated); diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 624eb89671a41..4da1c6865dbb7 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -214,7 +214,8 @@ void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { diagnostics_initial_pose_->addKeyValue( - "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).seconds()); + "topic_time_stamp", + static_cast(initial_pose_msg_ptr->header.stamp).nanoseconds()); // check is_activated diagnostics_initial_pose_->addKeyValue("is_activated", static_cast(is_activated_)); @@ -255,7 +256,7 @@ void NDTScanMatcher::callback_regularization_pose( diagnostics_regularization_pose_->clear(); diagnostics_regularization_pose_->addKeyValue( - "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).seconds()); + "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).nanoseconds()); regularization_pose_buffer_->push_back(pose_conv_msg_ptr); @@ -295,7 +296,7 @@ bool NDTScanMatcher::callback_sensor_points_main( // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.seconds()); + diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.nanoseconds()); // check sensor_points_size const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; @@ -867,7 +868,7 @@ void NDTScanMatcher::service_trigger_node( std_srvs::srv::SetBool::Response::SharedPtr res) { diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().seconds()); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -905,7 +906,7 @@ void NDTScanMatcher::service_ndt_align_main( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().seconds()); + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame; From b6c548f229c789136d8df6d295020bb086b027ea Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 28 May 2024 17:06:40 +0900 Subject: [PATCH 038/120] fix(landmark_manager): adjust volume threshold (#7147) adjust volume threshold Signed-off-by: Yamato Ando --- .../landmark_manager/src/landmark_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 57bfcde461af6..e977dbf6cf74a 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -50,7 +50,7 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - const double volume_threshold = 1e-5; + const double volume_threshold = 1e-3; if (volume > volume_threshold) { continue; } From 405ec7083bfc650866b38b0b471053cfbf8efdd4 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 28 May 2024 18:02:35 +0900 Subject: [PATCH 039/120] feat(pose_initializer): add a method type to the `/localization/initialize` service. (#7048) * change message type of InitializeLocalization in component_interface_specs Signed-off-by: Yamato Ando * change message type of InitializeLocalizaion in pose_initializer Signed-off-by: Yamato Ando * modify readme Signed-off-by: Yamato Ando * style(pre-commit): autofix * delete redundant line Signed-off-by: Yamato Ando * fix typo Signed-off-by: Yamato Ando * Update localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * add description in readme Signed-off-by: Yamato Ando * style(pre-commit): autofix * add bash Signed-off-by: Yamato Ando --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../localization.hpp | 4 +- common/component_interface_specs/package.xml | 1 + localization/pose_initializer/README.md | 89 ++++++++++++++++++- .../pose_initializer_core.cpp | 71 ++++++++++----- .../pose_initializer_core.hpp | 3 +- system/default_ad_api/CMakeLists.txt | 1 + system/default_ad_api/src/localization.cpp | 12 ++- system/default_ad_api/src/localization.hpp | 4 + .../src/utils/localization_conversion.cpp | 40 +++++++++ .../src/utils/localization_conversion.hpp | 44 +++++++++ 10 files changed, 241 insertions(+), 28 deletions(-) create mode 100644 system/default_ad_api/src/utils/localization_conversion.cpp create mode 100644 system/default_ad_api/src/utils/localization_conversion.hpp diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index 70c590c837927..e00d8cef1840d 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -18,16 +18,16 @@ #include #include -#include #include #include +#include namespace localization_interface { struct Initialize { - using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using Service = tier4_localization_msgs::srv::InitializeLocalization; static constexpr char name[] = "/localization/initialize"; }; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 1ad5f410a814a..13f135c925258 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -21,6 +21,7 @@ rclcpp rosidl_runtime_cpp tier4_control_msgs + tier4_localization_msgs tier4_map_msgs tier4_planning_msgs tier4_system_msgs diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 7006becf00c2f..36cc92e1d235b 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -17,9 +17,9 @@ This node depends on the map height fitter library. ### Services -| Name | Type | Description | -| -------------------------- | --------------------------------------------------- | --------------------- | -| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api | +| Name | Type | Description | +| -------------------------- | ---------------------------------------------------- | --------------------- | +| `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api | ### Clients @@ -46,3 +46,86 @@ This node depends on the map height fitter library. This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md). drawing + +## Initialize pose via CLI + +### Using the GNSS estimated position + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization +``` + +The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. + +### Using the input position + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " +pose_with_covariance: + - header: + frame_id: map + pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: -3.1565165519714355 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 + covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467] +method: 0 +" +``` + +The input position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. + +### Direct initial position set + +```bash +ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " +pose_with_covariance: + - header: + frame_id: map + pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: -3.1565165519714355 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 + covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467] +method: 1 +" +``` + +The initial position is set directly by the input position without going through localization algorithm. + +### Via ros2 topic pub + +```bash +ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " +header: + frame_id: map +pose: + pose: + position: + x: 89571.1640625 + y: 42301.1875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.28072773940524687 + w: 0.9597874433062874 +" +``` + +It behaves the same as "initialpose (from rviz)". +The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them. diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index bc86b5476dcca..dd368960d33db 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -23,6 +23,7 @@ #include "yabloc_module.hpp" #include +#include #include PoseInitializer::PoseInitializer() : Node("pose_initializer") @@ -79,7 +80,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") initial_pose.orientation.z = initial_pose_array[5]; initial_pose.orientation.w = initial_pose_array[6]; - set_user_defined_initial_pose(initial_pose); + set_user_defined_initial_pose(initial_pose, true); } } } @@ -114,11 +115,12 @@ void PoseInitializer::change_node_trigger(bool flag, bool need_spin) } } -void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose) +void PoseInitializer::set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin) { try { change_state(State::Message::INITIALIZING); - change_node_trigger(false, true); + change_node_trigger(false, need_spin); PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -127,7 +129,7 @@ void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Po pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - change_node_trigger(true, true); + change_node_trigger(true, need_spin); change_state(State::Message::INITIALIZED); RCLCPP_INFO(get_logger(), "Set user defined initial pose"); @@ -147,25 +149,52 @@ void PoseInitializer::on_initialize( Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped."); } try { - change_state(State::Message::INITIALIZING); - change_node_trigger(false, false); - - auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); - if (ndt_) { - pose = ndt_->align_pose(pose); - } else if (yabloc_) { - // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more - // accuracy pose. - pose = yabloc_->align_pose(pose); - } - pose.pose.covariance = output_pose_covariance_; - pub_reset_->publish(pose); + if (req->method == Initialize::Service::Request::AUTO) { + change_state(State::Message::INITIALIZING); + change_node_trigger(false, false); + + auto pose = + req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front(); + if (ndt_) { + pose = ndt_->align_pose(pose); + } else if (yabloc_) { + // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more + // accuracy pose. + pose = yabloc_->align_pose(pose); + } + pose.pose.covariance = output_pose_covariance_; + pub_reset_->publish(pose); + + change_node_trigger(true, false); + res->status.success = true; + change_state(State::Message::INITIALIZED); + + } else if (req->method == Initialize::Service::Request::DIRECT) { + if (req->pose_with_covariance.empty()) { + std::stringstream message; + message << "No input pose_with_covariance. If you want to use DIRECT method, please input " + "pose_with_covariance."; + RCLCPP_ERROR(get_logger(), message.str().c_str()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); + } + auto pose = req->pose_with_covariance.front().pose.pose; + set_user_defined_initial_pose(pose, false); + res->status.success = true; - change_node_trigger(true, false); - res->status.success = true; - change_state(State::Message::INITIALIZED); + } else { + std::stringstream message; + message << "Unknown method type (=" << std::to_string(req->method) << ")"; + RCLCPP_ERROR(get_logger(), message.str().c_str()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); + } } catch (const ServiceException & error) { - res->status = error.status(); + autoware_adapi_v1_msgs::msg::ResponseStatus respose_status; + respose_status = error.status(); + res->status.success = respose_status.success; + res->status.code = respose_status.code; + res->status.message = respose_status.message; change_state(State::Message::UNINITIALIZED); } } diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 623cfe50307f5..c7f1c5eec6db0 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -60,7 +60,8 @@ class PoseInitializer : public rclcpp::Node double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); - void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose); + void set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin = false); void change_state(State::Message::_state_type state); void on_initialize( const Initialize::Service::Request::SharedPtr req, diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 7975937f5b2c1..799a73508a061 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -18,6 +18,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/vehicle.cpp src/vehicle_info.cpp src/vehicle_door.cpp + src/utils/localization_conversion.cpp src/utils/route_conversion.cpp src/compatibility/autoware_state.cpp ) diff --git a/system/default_ad_api/src/localization.cpp b/system/default_ad_api/src/localization.cpp index 468e02b37644f..97544a7868d9c 100644 --- a/system/default_ad_api/src/localization.cpp +++ b/system/default_ad_api/src/localization.cpp @@ -14,6 +14,8 @@ #include "localization.hpp" +#include "utils/localization_conversion.hpp" + namespace default_ad_api { @@ -23,7 +25,15 @@ LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options) const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.relay_message(pub_state_, sub_state_); - adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_); + adaptor.init_cli(cli_initialize_); + adaptor.init_srv(srv_initialize_, this, &LocalizationNode::on_initialize, group_cli_); +} + +void LocalizationNode::on_initialize( + const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req, + const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res) +{ + res->status = localization_conversion::convert_call(cli_initialize_, req); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/localization.hpp b/system/default_ad_api/src/localization.hpp index 71517c4c6c769..a24e2110fabd1 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/default_ad_api/src/localization.hpp @@ -36,6 +36,10 @@ class LocalizationNode : public rclcpp::Node Pub pub_state_; Cli cli_initialize_; Sub sub_state_; + + void on_initialize( + const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req, + const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res); }; } // namespace default_ad_api diff --git a/system/default_ad_api/src/utils/localization_conversion.cpp b/system/default_ad_api/src/utils/localization_conversion.cpp new file mode 100644 index 0000000000000..3ddf259a4a590 --- /dev/null +++ b/system/default_ad_api/src/utils/localization_conversion.cpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_conversion.hpp" + +#include + +namespace default_ad_api::localization_conversion +{ + +InternalInitializeRequest convert_request(const ExternalInitializeRequest & external) +{ + auto internal = std::make_shared(); + internal->pose_with_covariance = external->pose; + internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO; + return internal; +} + +ExternalResponse convert_response(const InternalResponse & internal) +{ + // TODO(Takagi, Isamu): check error code correspondence + ExternalResponse external; + external.success = internal.success; + external.code = internal.code; + external.message = internal.message; + return external; +} + +} // namespace default_ad_api::localization_conversion diff --git a/system/default_ad_api/src/utils/localization_conversion.hpp b/system/default_ad_api/src/utils/localization_conversion.hpp new file mode 100644 index 0000000000000..85b6e81e6cd91 --- /dev/null +++ b/system/default_ad_api/src/utils/localization_conversion.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__LOCALIZATION_CONVERSION_HPP_ +#define UTILS__LOCALIZATION_CONVERSION_HPP_ + +#include + +#include +#include + +namespace default_ad_api::localization_conversion +{ + +using ExternalInitializeRequest = + autoware_adapi_v1_msgs::srv::InitializeLocalization::Request::SharedPtr; +using InternalInitializeRequest = + tier4_localization_msgs::srv::InitializeLocalization::Request::SharedPtr; +InternalInitializeRequest convert_request(const ExternalInitializeRequest & external); + +using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus; +using InternalResponse = autoware_common_msgs::msg::ResponseStatus; +ExternalResponse convert_response(const InternalResponse & internal); + +template +ExternalResponse convert_call(ClientT & client, RequestT & req) +{ + return convert_response(client->call(convert_request(req))->status); +} + +} // namespace default_ad_api::localization_conversion + +#endif // UTILS__LOCALIZATION_CONVERSION_HPP_ From 4a9bb742ba465c1ff85a0729af6997979eb87116 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Tue, 28 May 2024 18:35:28 +0900 Subject: [PATCH 040/120] feat(yabloc_common): componentize yabloc_common nodes (#7143) * make executables component Signed-off-by: Kento Yabuuchi * log output changes to both Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * add default param path Signed-off-by: Kento Yabuuchi * add glog as depend package Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../yabloc/yabloc_common/CMakeLists.txt | 33 ++++++++----------- .../ground_server/ground_server.hpp | 2 +- .../ll2_decomposer/ll2_decomposer.hpp | 2 +- .../launch/yabloc_common.launch.xml | 6 ++-- localization/yabloc/yabloc_common/package.xml | 2 +- .../src/ground_server/ground_server_core.cpp | 7 ++-- .../src/ground_server/ground_server_node.cpp | 29 ---------------- .../ll2_decomposer/ll2_decomposer_core.cpp | 5 ++- .../ll2_decomposer/ll2_decomposer_node.cpp | 23 ------------- .../yabloc/yabloc_particle_filter/package.xml | 1 + 10 files changed, 31 insertions(+), 79 deletions(-) delete mode 100644 localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp delete mode 100644 localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp diff --git a/localization/yabloc/yabloc_common/CMakeLists.txt b/localization/yabloc/yabloc_common/CMakeLists.txt index c8474a85ad963..ca2f28bae7cda 100644 --- a/localization/yabloc/yabloc_common/CMakeLists.txt +++ b/localization/yabloc/yabloc_common/CMakeLists.txt @@ -23,9 +23,6 @@ find_package(Sophus REQUIRED) # because it rewrite CMAKE_NO_SYSTEM_FROM_IMPORTED to TRUE. set(CMAKE_NO_SYSTEM_FROM_IMPORTED FALSE) -# glog -find_package(glog REQUIRED) - # =================================================== # GeographicLib find_package(PkgConfig) @@ -45,7 +42,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/static_tf_subscriber.cpp src/extract_line_segments.cpp src/transform_line_segments.cpp - src/color.cpp) + src/color.cpp + src/ground_server/ground_server_core.cpp + src/ground_server/polygon_operation.cpp + src/ll2_decomposer/ll2_decomposer_core.cpp) target_include_directories( ${PROJECT_NAME} PUBLIC include ) @@ -63,23 +63,18 @@ target_link_libraries(${PROJECT_NAME} Geographic ${PCL_LIBRARIES} Sophus::Sophus # =================================================== # Executables # ground_server -set(TARGET ground_server_node) -ament_auto_add_executable(${TARGET} - src/ground_server/ground_server_core.cpp - src/ground_server/ground_server_node.cpp - src/ground_server/polygon_operation.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus glog::glog) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ground_server::GroundServer" + EXECUTABLE yabloc_ground_server_node + EXECUTOR SingleThreadedExecutor +) # ll2_decomposer -set(TARGET ll2_decomposer_node) -ament_auto_add_executable(${TARGET} - src/ll2_decomposer/ll2_decomposer_core.cpp - src/ll2_decomposer/ll2_decomposer_node.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ll2_decomposer::Ll2Decomposer" + EXECUTABLE yabloc_ll2_decomposer_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_export_dependencies(PCL Sophus) diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index 154507f8195d9..4cc4128ad5a70 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -57,7 +57,7 @@ class GroundServer : public rclcpp::Node using String = std_msgs::msg::String; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Point = geometry_msgs::msg::Point; - GroundServer(); + explicit GroundServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const bool force_zero_tilt_; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp index f6e6de6b38fdd..784b3997faffd 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp @@ -39,7 +39,7 @@ class Ll2Decomposer : public rclcpp::Node using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; - Ll2Decomposer(); + explicit Ll2Decomposer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Publisher::SharedPtr pub_road_marking_; diff --git a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml index 48299e880cc9c..29d3da71bce7f 100644 --- a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml +++ b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml @@ -1,5 +1,7 @@ + + @@ -7,7 +9,7 @@ - + @@ -25,7 +27,7 @@ - + diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 213057f428c38..caf044660257e 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -19,11 +19,11 @@ autoware_auto_mapping_msgs cv_bridge geometry_msgs - glog lanelet2_core lanelet2_extension pcl_conversions rclcpp + rclcpp_components sensor_msgs signal_processing sophus diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index 749aa39318923..75d5f8e1f9cbc 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -30,8 +30,8 @@ namespace yabloc::ground_server { -GroundServer::GroundServer() -: Node("ground_server"), +GroundServer::GroundServer(const rclcpp::NodeOptions & options) +: Node("ground_server", options), force_zero_tilt_(declare_parameter("force_zero_tilt")), R(declare_parameter("R")), K(declare_parameter("K")) @@ -248,3 +248,6 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) } } // namespace yabloc::ground_server + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ground_server::GroundServer) diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp deleted file mode 100644 index 4e032ff72c998..0000000000000 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_common/ground_server/ground_server.hpp" - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index 5cf3e905e447d..df56f43b9287c 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -24,7 +24,7 @@ namespace yabloc::ll2_decomposer { -Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") +Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options) { using std::placeholders::_1; const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local(); @@ -263,3 +263,6 @@ void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lan } } // namespace yabloc::ll2_decomposer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ll2_decomposer::Ll2Decomposer) diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp deleted file mode 100644 index 692f6a5da0913..0000000000000 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 5db4aa0c29e88..66c0d8af8865c 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -18,6 +18,7 @@ rosidl_default_generators geometry_msgs + glog rclcpp sensor_msgs sophus From 9dfca093ef6407fa3f785e8e37f031e1ce6c66dc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 28 May 2024 19:09:09 +0900 Subject: [PATCH 041/120] docs(planning_test_utils): update purpose of the package and add lanelet map images (#7077) * docs(planning_test_utils): Add explanation Signed-off-by: Muhammad Zulfaqar Azmi * remove autoware prefix from autoware planning test manager Signed-off-by: Muhammad Zulfaqar Azmi * fix document Signed-off-by: Muhammad Zulfaqar Azmi * remove implemented test part Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- planning/.pages | 1 + .../autoware_planning_test_manager/README.md | 2 +- planning/planning_test_utils/README.md | 92 +--- .../planning_test_utils/images/2km-test.png | Bin 0 -> 5585 bytes .../planning_test_utils/images/2km-test.svg | 505 ++++++++++++++++++ .../planning_test_utils/images/common.png | Bin 0 -> 54710 bytes 6 files changed, 527 insertions(+), 73 deletions(-) create mode 100644 planning/planning_test_utils/images/2km-test.png create mode 100644 planning/planning_test_utils/images/2km-test.svg create mode 100644 planning/planning_test_utils/images/common.png diff --git a/planning/.pages b/planning/.pages index e355639eb8da5..28a791801b805 100644 --- a/planning/.pages +++ b/planning/.pages @@ -80,5 +80,6 @@ nav: - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils + - 'Planning Test Manager': planning/autoware_planning_test_manager - 'Planning Topic Converter': planning/planning_topic_converter - 'Planning Validator': planning/planning_validator diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index 63f1bf53a4954..b72625be999dd 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -1,4 +1,4 @@ -# Planning Interface Test Manager +# Autoware Planning Test Manager ## Background diff --git a/planning/planning_test_utils/README.md b/planning/planning_test_utils/README.md index b72625be999dd..c064e5791a487 100644 --- a/planning/planning_test_utils/README.md +++ b/planning/planning_test_utils/README.md @@ -1,92 +1,40 @@ -# Autoware Planning Test Manager +# Test Utils ## Background -In each node of the planning module, when exceptional input, such as unusual routes or significantly deviated ego-position, is given, the node may not be prepared for such input and could crash. As a result, debugging node crashes can be time-consuming. For example, if an empty trajectory is given as input and it was not anticipated during implementation, the node might crash due to the unaddressed exceptional input when changes are merged, during scenario testing or while the system is running on an actual vehicle. +Several Autoware's components and modules have already adopted unit testing, so a common library to ease the process of writing unit tests is necessary. ## Purpose -The purpose is to provide a utility for implementing tests to ensure that node operates correctly when receiving exceptional input. By utilizing this utility and implementing tests for exceptional input, the purpose is to reduce bugs that are only discovered when actually running the system, by requiring measures for exceptional input before merging PRs. +The objective of the `test_utils` is to develop a unit testing library for the Autoware components. This library will include -## Features +- commonly used functions +- input/mock data parser +- maps for testing +- common routes and mock data for testing. -### Confirmation of normal operation +## Available Maps -For the test target node, confirm that the node operates correctly and publishes the required messages for subsequent nodes. To do this, test_node publish the necessary messages and confirm that the node's output is being published. +The following maps are available [here](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/planning_test_utils/test_map) -### Robustness confirmation for special inputs +### Common -After confirming normal operation, ensure that the test target node does not crash when given exceptional input. To do this, provide exceptional input from the test_node and confirm that the node does not crash. +The common map contains multiple types of usable inputs, including shoulder lanes, intersections, and some regulatory elements. The common map is named `lanelet2_map.osm` in the folder. -(WIP) +![common](./images/common.png) -## Usage +### 2 km Straight -```cpp +The 2 km straight lanelet map consists of two lanes that run in the same direction. The map is named `2km_test.osm`. -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); +![two_km](./images/2km-test.png) - // instantiate test_manager with PlanningInterfaceTestManager type - auto test_manager = std::make_shared(); +The following illustrates the design of the map. - // get package directories for necessary configuration files - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto target_node_dir = - ament_index_cpp::get_package_share_directory("target_node"); +![straight_diagram](./images/2km-test.svg) - // set arguments to get the config file - node_options.arguments( - {"--ros-args", "--params-file", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", - planning_validator_dir + "/config/planning_validator.param.yaml"}); +## Example use cases - // instantiate the TargetNode with node_options - auto test_target_node = std::make_shared(node_options); +### Autoware Planning Test Manager - // publish the necessary topics from test_manager second argument is topic name - test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); - test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); - - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); - - // test with normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); - - // make sure target_node is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with trajectory input with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); - - // shutdown ROS context - rclcpp::shutdown(); -} -``` - -## Implemented tests - -| Node | Test name | exceptional input | output | Exceptional input pattern | -| -------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| motion_velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_avoidance_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | - -## Important Notes - -During test execution, when launching a node, parameters are loaded from the parameter file within each package. Therefore, when adding parameters, it is necessary to add the required parameters to the parameter file in the target node package. This is to prevent the node from being unable to launch if there are missing parameters when retrieving them from the parameter file during node launch. - -## Future extensions / Unimplemented parts - -(WIP) +The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. diff --git a/planning/planning_test_utils/images/2km-test.png b/planning/planning_test_utils/images/2km-test.png new file mode 100644 index 0000000000000000000000000000000000000000..297dc5a43865ea0d71a04be8bd19bed6ef102cfe GIT binary patch literal 5585 zcmeI0e^gRg9>A@3veB)a6=TxWv=TE%vL9MWxY;zMX-3(Yt72n6X0;p=e*lWmOf_ww zQcz2&VMQ6`Bw3TBJwaze=8dIOW@IQ7VCGK-98v;x@3ZloIcIQspiFk|Dzy~9@A(HC2s6lMX@{~X!>^n*~8Q_mKsCBw6i@vL7*!1k7IhhmbPSugM9 zC{pp|M$5d0;)=H3j*gC>mL8s6(UzvJ8DHVF&wR=GSZQkyIFw4|M#pI(!yUIUnSQF!R>o6Ki%Ca9kc4(a0os0Feeu}NL%#%FmY1$D)ItNSLb8hOJ z$1hdVt}M}}pT>~VlmZhqh#j9pi3Lw37Cj#uoih<8!0duxu6KAbm#f0oLr~IqPrQV5 z%%!EJubkxx5R8BPr&y#F)0E(jYEB*_n?~A@f;P2 zkzQ7y{Vov->r;y%?KKyjLSfg|>Nfd}+~-7lSw&*d_1ZLL2^W6%c)F(j(lM)X*;FWQTZlg+@C z<3>Y`ffHx>e0~vYgN@wYExbBx}8EWR( z+dCr!A}}OQFOK2}me6VLivIECZdiZP5K%}=T+%ukhJJT@I!9Fy(`2}422&Cq6^hcA zYHmBK?quXsD33Gc-TQ)7-)7u{4oKUVloYFTw88miO>;z&8Qc)%?dX2%;!8$nb(l*u zaTc$XdU)d<1;_ge?LEJ_Y=Ld`c}-VA+%{;!qrEiBgfso!mxH!mQiDE<47-B0bbbJW zZ4f0f=q$zT+Ca)_T{{UXRQ8-eCqP5M!JOIBG5=VcVh2vaiFM10fffCpS7>ZBD=Ze` zv99lgl-oT&x>{CVe%xg!yh4#$&AjUJoz+S=5^C3_7qrxf@+9=1Tg&JJ#cIE~KAoMN z13X5~lcob+g#jz+4?q-UxdCG<#aDMg#YFa1nh8FF(Jhop(N-fX?xV~c5pUwEGFO~%N1 zud*bi}9kKQY80?$T zz8GQVlfUs*`3wuPQk`M)VLhQ}O|72KIB8&GtP5ecSn~$nGNIwf;&>eF7>SQF`BACV z{`gMp^=$3IkkTB-zXZ+`2n1ve)GN$lDTkU%DtgBXefdgmUyRqv^pl49gfoAKq%@zm zqbPDnsRCEvWOqSfpCmzfTw?=VT2Wh9*EiM-BCvQnGz#1L+j$@8r9KP1n4a*ZqV(Z@AD!`9kqzu*Qqa8Sx<=2bP1XRXXhtHzd6OR( z8fZ`?%3cf4pU+9Uz4LlC;(v>ON4yLQ3Q8U4x-=}b`-4giDlcfr zb;)p;U9!P|?gDio<^wedj*MCj`!`rQ9zJ7|!Jp%sz(t^M+H5S#&anez{Pz zLQ~Z+cLNCP+Ajo|arH=zm|tXD*$Vvpx8ko1CK078NH5L{Kl+3EPDbg0xWY`BaGFlNq8MYM6Ub10uBuPyZ5Ix6cndJohf+K*`EELK6mn3^mHtOT%&GWJz@IE!gDcfWm`w>DkB`jjYQ z7Hizv1DxWX3q-lm!b)1BX_IzHEhPg0qo%=t9L&=> z@%zDvnxAh|rLdNgG$40@nw_SkPFzY%HeaCC^3h1fYTXX?_xEE{Wc~=7b;k(%M*+%( zl(%jP!z-NkU_q6C&Lxi+!dc_?o^lqY{`Dlacm&Xk6oADNjX)hx6L@r!U&?sPl?MRc zbhOFh;E_0hSuZ2KQlia{ zDijZ?+1Q&jpzxctm^Q}SU(!~bJ9q|%gLQ*QRvS7YmFp9^aa+t5B3ru0e999fFYm%U zm!*4vB6vnz)I0K2f()$(#Ptrr+7Sfv*SK!>>RYNAC%wz1$MXP#q0cb{fLVk{A@aXK z>jxB+Z-)EAZ3gBqO*c+i)Dn3&?ry=Xm(^?4#UTDjti2eF(d!P>l~*m?4G_!|j+a-@ z4?mg&FGA9CFubQ~ErX))C@}pPP1i_4%B#ypV4y({X6jtw6Ouu z^kmv5&?m)rxQ2Rcox+d1;0moC&L>0)mj~#LNnRWj3cJHBYM;2Z;8D$>)oN`@s8mH? zZBtgAYwc+cw*s>$t4<)k9}F9@iOjo;wtCSS3HYNLmS7mq?imGDD@jCStHRP3z|Mgr z0_b+7)QXg+P9kj8F}jcsjh6-?Yb z4frJUsky}EJM*pX*Omhjvzm$E58wDGTiYjAR@HjDMVx)O-&0f*B{QA$>a0pD$;24f z%QVD#*r*akJe~X5h~Z`00^nam2fmJ~qS5*2UC;{TRFNiQj@9ik|GbJV)dT16bR{?F z`wJd@6oQz-14$1IwQke6|8?5MLS*THAoG`}>jiP(uac6x9j6qWWK{1uXeVZi4I50t zPfSr;yqtnGjZ+@>6=LRd(4y_t@DW{m3gpf|?!#8CdN>r$o~cyP?}^%%dAYJ;o>lle zdbH<1?1PH}>2`4*SE4Z}5c94qcF#1YB14df*8FFDk&GZkKRo?DJV^>lu6rg$`7d~- zipHA@qU@G-=(VL^fFh3o}fCA~c zk^1efz#uGED=WAsPjt8kjMLWp%fP_F*9#kF# + + + + + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2000.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1.75 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (0.0, 0.0) +
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1000.0 meter +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ 7.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, -3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, 3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (-1000.0, 3.5) +
+
+
+
+
+ + + + + + + diff --git a/planning/planning_test_utils/images/common.png b/planning/planning_test_utils/images/common.png new file mode 100644 index 0000000000000000000000000000000000000000..340437b53c0f59ac418f47e4819b6b2a451db0fa GIT binary patch literal 54710 zcmeFZ2UL?=w=T@yZc#x|lqS`UiUI;k?^tM36$pV4f*`&3-YiH55drBc0zyatA+*qu z-b*NfP^3xkz1@}Qcg8t){AY~wpE2%t&mH4?w+^$(`@Uk)8g2_D}GWOyP&$z(42g6?L7+$Qb6q-?~2zEJAX~$gYt+mVKb* zp0r5lwo2amqhW0<(VS_RL#+9s8j709B)Qdra4r2~UrqUNY-=r=eA)Eu0r$Sb*$US4 zu2IS>A{lL;DEVW1Kjo!dh;X{~Cf?cA(nt~;RQ3)h?4h!sawD}d0(jN&QljACni7Qm`rw~5L!2GB-uA-(`1Q## zf&15o`)W?7e|>8D=f2GU4@_ZUVM&S34pF#qX`;0Lf8gev@k_1*?q&4A0uK)lu7Wrj z@|VO3RHx`y`BwDK_5Hh_5hefUOzPS_2?@QOzrc75XGyyA>Rs5HPaz6$O7@?j!e{mv z7^>_RaloBV#5F$u)fKD1^BTM0icNnXA0J;92)?Sm-}TR3;u{I=ri3eC((x^P=^!{g#*%zvDgIVr*-!vv@KAzAYrNye)GBnY}MAQkwgbqO+~dybRne z>7GI~BL?$!Q>Psk2VY-bkvRPdPnS56#VyYhzt#P%h2eX>v-*SfjX=%et{X{GCf3EO9NO45{{@-`yL@EAz zdL=&jU$772ZU34k8QK39mTWpz2Cp`p#MErChGNnI3h+A@!y34udmBQRD)Ui$oB+~v2w{B%ox3}K)q||-x z4BzqM>_uO26PW_;7j`o4_ahL#vc$GhiQPrUb%wpyywz$jy5GJ~<+pUr7XTdolg&;< z#Nq}W9bIL|fZqXjK)ATbm6=)xT;!&2;=kTnM`TnJIDCZ!CiafskXsgEl!hl zrs(wdBFCEzEoZo$)Z3-@cB2nO$MYLm51Q5e);fko-XPce0>my77a}i`lvq;rkJ*kc zj+Wemw=BiPv};sWbETif#P+v$==n{L^b zh1OE#`MPSUVs5T8Bj11ij8TTRm~t|udu#9}RXy3UC%%LBB$YG+WVg8>uR$fIXj_*v zT>kmN_^$h80KM<-Orl+m-{wkiaBycXv(+8Au8y98RhY>9FeP}}D~+hm)oWowuBLrg zZpm}p$}9qNHt}|J^mcYWC>bXr!4p3Swcy^q$-_OT^!HJL>M6-$P^GI9K4r^t;OB2w zlZXOrF)&FG%vHj}&POJR-V+nn-XSXGtVaiNig?NWpPNlh!Df8=3A;b)iLbN&Ny@$Z zENqvDN4d^}aX!ruY7agRED$EG3x>R(DTAA+;zH@GN{=Si3d|Ptfuv;lX}xf-3PkZR z;do@u`CB5$5K4tnwP2k;a#uhoK8_*z4>RoF96?o-K`@DRar92#heUk6D;-wk4d1rWOIvGR)2?!SjY|4k)||JYA8ub0AmnHF zefZc4)?L|I>t<_V=)2a!n;PuO{n!%vNSBbA#HNi}Cq*BbyIlM(?z>OEFiX$w9G-NR z{x~24qGbN!7`0!Oyeb!6s9aii&+x`xxz=*ymGk7GwjSGyLv}JJDCc12=9OK)S(VPK z)I&oa+k3NBOF!SHUBAk>pFBD$!2WGxacH*c2&HTcor>Xxii=AuFs|0tz6@$>YwL5# zCI8Bw*WCQr)tA(x(D_m?0Cfosd-vYGr~4?d#!C5Cw`m-)1UV7k%^`(U@9pM|szM(Z>_N_K&tOw`)Z%i> z;PjXmP|)PD6M=nUVF4M*x@2cZpJHt0mbuvMR0_XC$Ia5bySv-f^W0@fZeN*Z<}VsW;*pcYYreLyLRZGVNFj z57#kV%GNzaPF%E)9)y%CoBk}x3RW>1F`A(`#2iGV3|?_2(9!v}mzU%5;oG1jF{g~o zF(>bjNo)Ou=~fmL!_F=JRV1B1gZso=amAE*Y|ON@gvh1cQ=}BNx+rUZSw~%1EnE~>``}pn2V5?)86F~g~+S@kr~QCSIV`_ zx-&jh^I+xKMEGnup8<1OU$56p0F#05oYnKV4hAZ?Kzvw@z|W8Pv-DY&N2vAX+)>Ok zQKF{&dpkZIl-k-_2PvMECWymvHyO@r@AuOic&}O8DaLq16|#x z2c)oipSt%Hl=><&bIwgq$#&DR)QWut=n^aNra-Uhf`U$jCfhFp7Gm#|Unt|l*?cx1 zm}R<4zN)M1v{M)M)fPaPBsqa8;sYF z`}?Ee%guFxZYv{u_X)eJ%LLvi-YD0oz*L|9JnD#u2+z(Wu|3lC?>2;>YaxRgd-GeK zOeY_OKG)qP&Y>Fa$v@yOiJnBbtq@{(qo|_PN)t3ffGSnDyD4z8>``>Qt0n`M%{+-;VU*3V&PA;~bsC;+o#rn=flk&oy{=JxchhsEB=slNv#k-_gQ&s%K_K-tVNe_EjLC zeotJ{JcADBO1wb4S*Prms;aT^EaGdD)KYnPo!vr$6cKm|I_%{V@s?YZsqmvTq-!K; z6LxJf4(jPl_54g%HmLe?8lCXD8p1|6T@oP4+F4!g&*ieTv_wmtJ8W}UNPM4K;>nEq z$>ORaRiyEWB5%0*SEl6twtlDigZ(Z;(3}GOD~ey1&A!q=}k zoGe_AL-;z#ZRAp-UX2t1kA5EeyHa8^XJ3D!!H<=Tw_nEl*R^< z{8}k0bAZ#)r38pK(7W&tP31V7qJw2?M;t!XzkiXnUCYidENFaaO?;P{7&mM{IAhK> zs-~ub*3~9k8mmnEN$}u1p+po7wJMB8o9C zc74eb3WcgNH)nk-D$2;M$osu2A45CZV$6wMRI+S=9*B-eD8+r3Rd-3(iw`b}pC?6N zXLF1=l=^YHhU-O=2;ZMUCsa6|+mrFewffcMR*tq=OT&FXl*C<%f(!51?-?3#Ms?0! zkbeDqZny{X>DRXZh~SASC4cqc_cmo`Z?L);^zJ%Q;$<4d5=zqMeeRH5xJTIwlWhWFwvplbB#4*%33yK`{_4EW7N0z(D`lGUtgrO30*oHq}lrTRn0R=)?zQ3 z`lP)AqFZ^!UQk96+B83(eaZ#pzSl$OCwE@o-Wc72n)~Wb7&+KY4ULURHou3b&Gee#ozr*%0b;PcD0yP!d1SL+59*kU=I1OwL4wQ{ksvE*mY zxQr1d*WQz(*}oMOTxVxuVv>-Mi$}KZ8o?pTma8|k?Xd*M5yT+PZ~eYcuPV+H$U3eF zmSv}*7?O&m7qwRe1Ke*tu$SImI5}*Ma21ohOubiO7k?OEK8mirr4b&$ZIfvE{rLE} zX2LMQu87Hw53V$w+_1E~Yo2w*|B?Az?;g>;p|aaul5WkU zqg1^sDLGO`Hqo{0-wF$FG;qnfZEv@ZBy@&0b&T3hAK(NPu6fo^tmDqj)Rjed=>rMT z+t9PzyirB-*s|;j(^f_{Ht37|bNFFpE4^Yjq}I>=?<4y9a#P!fOK}L#xx;FYE1g+w zjYh@C$8k$Gj_>1yTT%@q>E4l zc_=(pLfrFsc6D_xH7%{9XuF`ojEag%R`v)wx3na6tl^i1#QXREj79T8V@zx1i=0v4 zpHWyn4V)<2o|U-dMogD(ZHW9$atAs!UB2j!5n)J7aYuXmkkngx6*V=y6p@WX#-XF0 zs|TIRu5D#Urso1{K<>h5pH#fT}$I6Wvk#(V_5UM)A)#PTcRGy%w zi6}T@>S{df^97=yc|pf*xEI42bRmtC(b3TnjERtsIR86yz%=bC!l zF)gO(mZqjAJRV;cc<~ko2N&~td990;)vf4eQ-0mt0nMaxR#qixJ{A*>?1o}zOw;eg zk|U+7Of`s&f+jgd=TjRjqQn@QBX}x-0=4eiR%wslrxEM7b8{EIzuV*(T z^;fhm;q!RK&M4Mx1tG{tOKmhj_sjx1K|VY<_|?kjGb3f-!%sSQw71DXU=|hC*%sid@H_{oSg3C;+UUvRHaPn0$C z%%6WWas+i)Pc5`P)m;jdIsCMC_x>4(!2v*x)AL{AQApi%aw3 z;>UW=i=5l19ZiZ+EI{LzC3}CBC~5+aT+rE;z43>>RifWM3xXWxY&~&HWj+ClO-Z4P zG8cpicfSw-xu=eD)B;TG0s~^HOu5gI52rJ@u+{IF7XriVmMUp@+2`>&_H}m`p**yG zLv?c-C%4@D2ACkBsx}J0l0Q*QSbu3q6972UEFaU_V0ANTFMYl1ZfncSOf~##H4{2# zvA#$9ZEY%oj4_ixprX25G5aDX+hcu>#&&LYbK!3E%i3b1d>+&eEcE`i(?!Q~A}3nz zI*4^Cf)saZE=>v?3!P{#c56Kon*ex);DV(D@dI@n5JD?~oohS0QBx0jN%t*P0LVBZ zqNAfD0)>he(Bi2f;794AG7Y>Pe&_?VJj6EZyYVqHm^!X;(Krw-5HjAi-Rd`8CdVVZ-hrckM>FSqReOi3er!k z$H|mg)?0wwN;)<5!-uasA7IPo>@iCHLT>^BPRF%EoS#dl%Iwv21P8qzg^KmMaHMux zjUDaR=UQdrZ13zN0_uZ+L#Fq-(6X zmoHL%zt!;Qj+HpPQh$>fj_-1_v{Y*J!Kq~4)7IABB;>bvn^{|%yYIf7br0=lcR7uoZ)7<8 zmvdOx>_(QsePJjm>M@uJI$?~8Qk$n_r)pj)kawniRuzypmTsJ>0g{;Cpv8tL-njht zu9%cYmX;tRkDerq5;BVaF!Z{TY7jCb!DSCIxOo0N1MA12ps|$!w?<&{=gHH$Z-=sy z=0YB?IcvFWEtA|4@;Du>;o^Y_zwH8wZ~w~_0U4Q@ayJ;mFJn_KmxU?_SqdArJj?FV z4@0)Q73%f1;A^WPvhoTFq06Ge!rB9;vJ?juBTZuKNQyYeoZTop7I}gi5wSVSadmdy}Rn>OjlUoA=sk&+P zb%*mL&k}a`QwL~NEg191x0{CA-pR3noX(ZkVLZnqn?=M?1*KE@n5nK{Q^@zrwqi}n z5k=xL7ZwsBv_kjQG7F&SFnShXlp%OZD1Hiary!*=B=C1heYf^CsjL~L?Z3z>dArQS zNdUS#V7#6`>L4{c1&=0G@H7;Gb--73n>(ADHZ12?UoOLL(LD_|1@NMw3N1tDG3BgL zZq>2U(DQOc?zT%scmS?fBn5$d=wxU~TF1N9uiGF7nVi(R2n2fK1H|XnfKIZNY4N6= zxH4%1A+H~P%F4=emy{L|sLT9L_go2T7>?6CH@Q_3;B+rCGk5Wxi_jI`%aM)YnFAr6 zv%S76Dq@CNT3lN{qq+&;ECIM8Odif;_c2*!qig4!%L*&(sb8g|V~gl33kn6RO3ZJ# zO5IFH?<^AvNX4z{qfq_pQD^h5PI<4^oblUXJ3;>;V|fKj5n8W#PIKPDO7}Tz5aI6b zUNTCZaGQogw=935rKM%;aLjLGqLB^e;7A#=n3!l{*Jwx*Q@15dr`<-8M{A>Qvhe5b*kP~T=<(igXeuwvnbaoq*Kf;m zf@B%$yEPm0&xd~I(eIt(H_08!Iew@imNJ`o8;eOljb?w>VH*CvEPJajR&5a&RG0I^ zPjB(h`H9vIGnA^3J~&=n+8e&$e#Z$*VfC`+W020h$R}w+z9}#zeFX&tmn1R9@a#-G zULlV#S@w+7(k^iKcM?6l%c!jxj*CURsBZPnId{`TB;B8x4>MuNMshOrhES-z`ufdK zo+vPSOBTuG?C}ga4>tF_2XYOd3mZZD?U>!UAJ{5SkgQS20NzbtDhV;!IO{ZK zeT%ARaxDnmQ2XM3_3F=%EO}XWBSHhZ1q@G9PTXntx$oU_LW%ULa2S*nezKW^Wu*uu zcQD>@5kgkjudhw+)$ov@TNxW@co3?*5AVyNk2CX$G7>{SI&x>U4KvPgh2k+( zTRSZ2#@_DQx@CR4Nq8YW9SLFj$j|c^yQ|RGsA*IPEZ*{mFj}rprSbdY*K-FQN*B`6 zF0`dB4qVnOHm?0224h~5a5D2hGE9hyjEjq-Jb@{D8Qv>GB4K(qb8REfybOO~PF%f4 z156^ec}nVmTbujCGJaUYX_V3(y3oeS47*8K(@40M?2RbJprM;zz$E(BlE0TR=$#9! z3%6cSts%M!GC_%6erS4ZmhaQ&hBx)P65XCvryZ9c?{m+0;*g)Xp$$?g($}ABCLS@w zPhB)9u@|CYWMbqxAUF$K<@LpoYTK_5>(T6&gJpNW>%5%JBQ*OK@t-+=zTW*huGPC_NoZ!P&o##Xe~viB#~HnU_h z!k*~Q-IZ=;tF-^5v1nP|hl!HP=~`^zH88&lQs{49XS?S5QF=UwGvzJ|+Ey|(sYi=u z^I~|ZO9BLO)1CV&+YL;1hntx)Hu)k`D81rQ_sMWNFO3Y!fQP-ky*<^;Ux%{R`{jkN zXlsoFUS5fihp_m%dZs9g=9OdrN(Ye3agKbd{v$=E(P3NMZ5B9l1= zS^s);YEV<6UFFbtV8`r+UHRG@{Ycv^ubt(i@iQ`)qEZj1ZpHB#pxv(A2|NI|-(lz3 zs|{|t=)3yLY5-lajkiMW)HbhK-9jC#ul5iHVxcny1(+W`quottWd)rag~)n!1EC?az6@-6k|`oepx}$+g{)k)$=YP+WL` z(EI=oSV;CLkis{pbiG41Pg$;q4~)4_C*}%{PmrdcaaX_zWkR7p_^9NGcyfnn0c&LA z_qF%@4)6Hb>*#rd6rV2ck9VhWPfn6|<}?g7>H3_a(OFxD|u04hA23R!%wOIkR(eZX@ z!`A8ZAY<`7rLc6Oln)NN7$O_>vgAT+g=33Dd|n`Fa(Ho4?G`HSg*A}8EUoN6YV39K zD+vr%SC3XF9#OA(RshIRXf)urzoFnTLvwZK__l%GN9jdaoD%)@<&jd&X~eBJH7S*q zU`4aXT|8VM7#3j=0dHOo*6iWy7*+m?N{*r-zro{bQ+Bg}E&XQnJNEf$t7Zdnxr&#> z!uF!yZe73Ep6t|QqzqkDzn4>XT-)-~4~u|x18lv>i-tq9$_RW?7PV?t3qYQ42>zIFCt7$8Q-{^<$%;;f$peqI^w>vSuE&F7c5ZtS{W4E;E+p> z;)oJ$AIqLHzNA&W(5G1MX4v}U$I2(E33A@jV_*z3Q&ZTA)AO4}am68ucjz=_?1xW5 z#Zp88GTTS{7k>Rsd`e1$EIX*rG0Dt(Lmu+q1i23ck%z&A!WfMO8F3kntdVBV zo_#Fr!;KQmXj3~gc^}KBJ*^sB8T|7HY*5GBgK=J3Z#U~PH^SZycVF3JZ%ou;ZUDXI zI|p08NnDk})srkCc~NGnzy1V_m?%Zjy;G$R)Nb3+E}R|lFI5;G5Y307KM$H9rP6U ztHp|!C!q$1wdla2K7h;U?Pfbu$_QC(nGIxDIkW(Oz6ZYj3`tE6I{c|1k!Qn)(v-MT3)D%{KOT`SMk`qr1Di zr*8k?fJr>3MQf3ZJxWm)wQkD}&?P+Ryzy`bv`&9p_=FOclsp8`5oSvBN)(;1WzT09 ziv;bX@ZQ4hf>T}mz`nB2f{c#pLv%3s}!iD0m>n5Jv{h)ZmIw=jXB&0w->FHU~ za*xf2;oxT-N1_}lS98k0;e^9e>Nz-Gq1BHB{kZhT_Z8i|u*GaH=OtDuFySQRU00j% z$OsSzDi4ruHY>N1l+tZSN~j|nfk((OUy3H+6Aa)qphb(CT!wXyUMbkt;xaj)%1Wm+ zY<1ja0#>1}rY4`(4RWL;(Y`0Qoww$3!NKR88sEZIiLa!W(Z6#*@H_I;_gh)<+au!q zRmTGZ?LH**%fI2>FT?Ow^_klW`b^MkY-m_yfC_QBLg+b|edma)vP}{25S4G#DK?GD z*ST{!I4FjxB)b|H#Hg%~`(vKPC=DiWr% z=zZlx6TAjg?524^_HTNodI72D4iM_10u z@))ZO=WU*yzRPFBtJgr{TcA3#+aQTzdd|KDH0`UQ;^!nhdMoWkC5432XEuJdo;yGv z9&J#CmV4lOF_Q`ezhb}RqANZ9T3qQi+V5b@&U@2PMYht_p$T0>6zYC!!CfcFhBr^E zbI#`lYD6y71g%;lw&4N36NOKNNQH(7sph93{C1!#cqt0%C@n4_5fS>xM$jA_TJ50$ znJ^~+@GRRFxJEd7Y!}ZPoC|me5-rB?{iEaB7eXJk1hJUuxdnlpq)%X1Q)Cq)Od4F< z`+lW12OtIeSWZfBa!et1hBq&r)f$s;`kNwAcWY+{$&@?-^3X@y7aVgyBm=lG5GwO* zz{70guF;0fn4HW3+%n4bqO9x|T_)m$dr!CpaiOJiqO)QRcD^dz z4Al~54wj`jW4+a*feEyAwQ3ECM#d+IJ``1(bqEKjBCy@h_ZEv0SzFwUk!mV{3FsFnQQ9B5b{ZxE3F1R<1Vs|vN;MBJ* zBqxdxTb6B-F@u@6sd6%#Yl(4)?%)6!I`Cjjc`ckMt2;TTM=!EUmEHJg*ZB`F0KYB1 z;Pw|h2l6t4x>*^~c%Uge(%`A8{Get5EMWaF`UjbbZepo6(3316E{tF8%RvYgT&4yD zg4#SrZW@ZcqhrMzKMGrkB-}hjt{9lEF6RM?E0sKXyVOCd;<&l?R>tD!{^~>>1|!7c zws~Ry%QYKXYUD5}K+J_eHML66K6Cq}BWwgo#ILjmkH@DE#ER;x$&_bw_;9 zyK-S5SHf`Y8W2p-pY9UfIr3LHnKRj);UJ>A)T1L=QPSRT?MtUFR&MC~oZ>Z6@<4JH zvSr|aUf&V$*)$~y%88Qz3WU+*^YP_|1nK4@i}J15qn@10=gj1Z@jfhB&5dn9J03ni zaXUF4({(<^KV2pDW$E%IopAly5;9IaFo?~L{U-M!y}~;9%3rAv!51w1<9p;n)gIr; z?#c1un5$d!@rygWqepBfnI}WzX^A?cmM_Z%KgQ=Z^bWWQ(R{NNfQAdRX!;3B)Jkfd zJhxqBqzVMp2B0K@Xk^X~oHy4|b7sNN)MO*a@oI9|uQc?ozTPR;>j0JpYhqdb{3;Da zz{?r{klTcGYg1DM5FHV37Dy2=SBUOHE%;k~u1P@M1PLzk1!(1b>Wet(ieUn${!&lx z&9y-k&e70t3G{lr!QK?VX-`WX1@5&M9r3S*2!MKRu8xT(!dc6?G2O%woTDf^9B zEe&}1a!m{LOp~Fi#sC4)y>(|D6gA(9`Et>8xT>N&up7pHK`)@<1agp>Q-?Rfaa36V z`vkzrWZ%tT)2lWv0QmIugwd{y0^pNWpQM8z8&U$|bBubJ4KH;Zpm5#vJ4`2fRZu#! z^r-K0qi1_#$(btcCoFGkeJD==7Gi2*auf8jbdLX;ouzLN(NF9C+ByvyU)m~$?;_ct z7K`55Mo{WfDL=36qoJ+MvR^DW|BQj4Hpqe*j;1~hv{dDK4;laxjF%I9M}!yNLw1An zdm8Kgwku9tSMIxBGwUtFy5?w^FSl$@jV_l!d(`%@s z*=PBIqyRa!FnvA*^S;4JWYzJ?aedXk!EtBR@zIKR@#sdQQGW55$HBnL5&m_4(QTaN z;K-3INH?cMtE}6&1|{5L&!_y&7#^YsTx4LKc3W%1n>T7<#k2JE>JX6ixGj(TrmA#} zh%;{7g=H;(Z1=jGVvmiZBR^|oXM1}1B~)@*rPz9)ore&uPxvOy#>@F znEvGU)Xu^6s31B|T9&(lf`Y~$={C|Lr5Tyn1O&fFhmkIt(Rb~pa>f>O9+yNxW6jw= zNYZ<^tLG`MW29P$F0_8q0iYjaE5$}H1@4YN&NTSho2X{CGNN1NXaXNQhhc|qLAhs4 z<^yw%k@&N5C~u$DpWQJI$(^4=*47-ZB=FR0 zvAh8iU%OW#~)Yuy!Z=~Sm3#i%_1Hv^AP zD3deMH!0xG9Y}1~8W}HmTyJy_)1=|kVK>D!=qwKU0IaP2(QNe7N7|jOg@-~|3?{zS z#8J!!Ip8epWoFU5If&^=e8PDctAzN)Hz^p(iJB%RYh?c9=I|N3b2|Fd6GhxB5Z5vo z?DhLjQ&x>tef#zqZS|F#pJ&e)q)boZNF9>T-R6?4sVOP8pF0E7pCY-erEE-Ia@ohw z(`&=>Df$;&=y9c7t*uCQ4ZB5{)>O9n27kU|^4HAV&fWp2)}f``BCJ*KX+09uut+3Q zSy`FOTOS6?{$^WUvW&LnR-+^em8mev549-LfQpU25xyKOI1rP8-}9CRbp|69(zb5i zwH9m!e@(Rjy>7rrkUl@_SZXbt*leuk(U<*{Z80-5lhUrxh}PQAuRe8kecjTpYIt%E zd1M{)5+dYRDDH}TFU(dHIzNLN9fj?3u}@A;z9;#U>_yC7Z(2MRSmh+8fP7^C=YBEA(j+0%a!-(OkzgPOah&1SzYjkk2%l?KLA^htAECOwp z(0WThAuBr?$-CoMSt@j(ec57bQW^52*@;zs4=D`*GY@PS3kw??)h*`O$%yY{{u#X= zvaeO2YeT8MYEUc#2UFgt+#|N^Tc>**#z0lHZD0A>h{FdoDmkC$Ig9HW%vgHgR)x-U zH8PzA1S3FhJ21&;xJ_XoC%ZIrlW!(BbK3~Cc>N!J&A4-m)pQD$GPaVMkkDfUeHEQ# zUDP(3dW)-Fmljizz6}Ll)mE*NTx*j1YriVs>{X2!O!L+}%UeY1?qAU{=#=(&2N~N> z9=l2_)>WB4>aMID+PEX`xi@`$~ckga+41cKGRTAJO9(xO6=yZ-R7j#9j_ z{toJp#R-^L7!+dKy$M>wM2? zdXmN3K2qXvo#A|u_tj6QgvvS~33+`G#>y9OoxZL@WLsu(8F>O?-kiyAHtu=<++V&7 z^!zDh;2Zn9laWiTFWY^$3sOPqm$-H%j}Ym*c^w=0Ou9fiP^&Z~J2p4A%we)2xKUAR z0kN@n*q$h2C0SC-DLeeOhlV4i-EA~;DR<)bJSJ3@9mB>P*{EK=h(@$CB9031(AL%+ z*=^Zv3N~)wcfk>a5N2M!0se-NNrzH6CJe5|2sV>}j*h+c>a*os|KM99yOe0V(^6gQ zcX+PkA5A1g1rN0Up&ZZDRxRFlT}&51)ADJmG!H)s4ZbZEex8M7R;bS`jgDzC(9G)F z+$?0~`wSg}=@754&W+yECs4mF=mka-G5Djv-oXQ=KWMr&(BGoPhfmJg7@r+rd@*K~ zTai&ID%E<{+Al7pX=Mi*>1SE`5H|OL%ZwLJo9ScCeck{fuaS;;@|4AZ!mhhE=E{nm za|*F}VkuME4d)jUnyzlu195xJ9q#jI?n56@w^BGR@^JjsMX-v15vWZqAtt6fH$FWb zv$gf3XJTWP@cj8P{){%IOE%u`7_ahWQGaG;M)pg~#<$jyk&)h=RJ>G3h?@6?mc~k} z8Map}8JMMLvPKpS+ew$AFo=pt>1gRxb(?E6!LlHsvE8x1^Tp+FO$Z8h<_i~eGlWiv zUjItlwBx?33l7b=P*hmBS#yRBj&*Gx_aM7cvC$zhcD!Be;^wOfP5@c=yMI(yw$(=Q z#?T!ZZ0OllnDp8|>jS8S#m*(h?-MH;$b&sVn^MBX2VBf%(cI5p`p&_N&wLQqnjS)J zRb@x5V217(6;0c(*2sGA84P?Lk~v<5gJ8QeI+_Ag$?DnUqBQh77>?=TaopK9w&TlP zTCzO6<@$rXFum^;h@Z0TPv;O+-khgiRD%-LHS#l~kBlI0Vx+lBn&^YO7`Bbks<8R_ zjlDjEHb`Q(GwO#>Jb9H}h`PU0Z`wtRuaHM7eqdnWWK;cQa})0T zW+PoAYhq#o6z|z(?}3wC0O$wK!`%{e_DUgJ0eQ_+lS*!Igs_c*hH3>{c;6bAh3)5n za^lGTbf)SvV9(EUopw3^{^GeYe^rU7u%IBw`+^nvQ&3>%$7E+KLRvKwZ_|K9B2$a0 zt={@R*Sx;|E$B<{oVD=6ntL|iYZPmaDSy3G;MP;GJ2>1I*rP>KzHCo3%s;@v<3bzt z>EBuy#Rd3SjQK2J%XR3FpYjBau>2$e3i7dz&XaFilQ6@QS* zQGX5-y%{c5y?HM7341`#8D_v*#sUZXrFHspRB5RgXO9te0@OIH@38*Mc1ix1em=%$HwSlPEgZm%HmS<+DZ?=r@H8f zYz2#Y zlTV-7p}oD@^&=MV`;A4T6hAmVWTEKoe|A^L;v!4nAq9=a{1_>fwLhCE0HuDKj?UL< z>lx%vB;ZHd;(hb)>!;+2yZRXV^`m*`=EePv%Y(wgB0_?Om$U0s9~7tR~kZYmJpwo%B*7p3@pkICt$6=!*AsjhQqKSD>=TiW0*Ll}uL^!t>m@Gx&@6XYRRehE8wK$r)eU zZ13bV{&J%8T4XKpcBLYXR3zf~TEzxirNXYsqhuT7#s zDn0vZ7q(0Vh5$-qFPfut7e{`3o-*)tE?)3~0!;UP9#_Cpz_iE7r<20G)wQa_xRct? zg^>J0<+reIY+l}_VA-4?ml($3vqY=$*Z;9rk{-;|;wU=iC#SH-fkxRuLd zHWpUw*_VBTO$L4R`d0%I(`pQ;n4J+->mixB>g`H}hmb`ogDnh=_Pb`|A%MnGhBoc) za@UO4xgSO^E%BjR;i#Wna3vctYtIrrJ0?RgCRugX4^V6|_868_o$NI{<#H7&){UjZ z%9C1Y_|%6yPmvj8OTI@jP)o5TQ{K!O4v%m*%(I~|{B}UPJK0Yr%+xJly zCA(j@d*rLfDiWi#_P3^HLIIsq@J{_^{`<~ZP^4xrLLzp&F@>|M;Orvt2qSPi`eRqQ zgy^)vz@hxm1rC$&IWBk;Uk4B zn`<^3rlNu{q}dquF>f|6p(<#IsnXKv3`Qu+7No^TM@KFMxy1Lu9uFLWTC9b!U@On< z9v}O5KBjgL*XqkQHN<>JYtt8@(P(X%@u7~6?NP+`ZlKfgh?tl-zXO>G3Wf4H`Z0j| zr9X37fvUrseX|?jw=)9A%fT29LKgMo8<&4AJE(dt_>>n3 z&xu;JM0d6xS7xTPLDD>_0_!GvE*OM=iOJGZ0vIi**xRo=_fjJ)r$3pZ0(cU~wJkmG zumg&hsR?M^6Yv ziU}Ae_>84R;Dpjv4zWZtG6JyH6~NR~#}m;a&5jiEFA55d?zfkga1ldeBFCa!-a_fK zc_DVwC46dtC?qY(q5`MXD?-|DDZ043501a0dz$+)Hk7{Nc9At6_H?Mlbc@y2F0Ss; zmFR&HHCw%+biMC4)vLVAq6nh1VrKo#L@~w-9pE!ubinF?!2y5?)+aBT+Ua+4FqRoj zL?OG(aRLLcPF-Qk2CK(R??&glAPXZw)+uOYRA0F-u2b%nER9I~V|!!LxM*V!4@}jVjlJD~78eGT zx%voz)>-~(x9m$iJwRsZV*jMFZnfrxoP-IT?_#c$M{;~Tpo9W?e$$QHiR}rvn1T1< z`p6L&|XqbsqbXhk4z z<>JZFQ6*_!eQ(1^9rJ@qYOKe}9Uw*eFJsE2=G&uD3`d+lm z-jzpnM9!DLX6L7Pq)xwKiFGz$85-vy^IWf{FJ7Lzf9(=gAb@y+h@{1?KiBsTYinx- z<`^l@>$}gTgMH&$Jlt#c)i4Z0b_~0Pg{j*8#8#SZtQe%DmPU z6iSoL9G&~dyj-6PfQ?zb->^Skt*qXw&R6a{vDVwTmC{dGaNQ}Bn*t}Vuh;46>N*^E zk}$tJ(<5z=I}3N=CF#huLQGycW*;uaOoi0-A;OiqQefW99V%W^s=n5ffhj-q!ATqzlWN`bE=P zA+>G{;T5w8l2}Fy!FoNAMHNg49z?mJx)tYOA(M#XO;5U`_Z}; zG5g?0@Amf5E%lVeMakYZXE13pGDcAI7^wzMba#hpTN7ZDK9pu#M40A)7)(2y^W4Cn z!lUWq<$_n+9Yx7}Rhu3aqj3hj*{)mek{qUAxAyVl&)lGdEpNG+l(ki%7^Rz+&rt)~ zdk~w|R7l%r$5B|yQ~eEojL6dF=4Nmvj}2cA7Fpo=B7{bDq{VbtIyE=%dK8ZD_3)B3 z*h`-In5_nRbY`ZWthhM-fIygUNPBgPTGzl}$CG#llt3V`J38A>xnidkM~-Z%J@)H0 z-KzA~dvJuU!X=A}DmYVordg+}vx|!{|2vgq;Y7l!dq@1bAjwP8IC{=-v0EBh|KOF7 z@6i^sSm=k*bj&k(dl1_>UzM+2dJ+W*-%e#Iax2=}++LKH_+K<#c|6qn_g1dDw}tDL z7DQ2$Y>_2Gw~*}H7|YlqGWLBZb%i8J2%)mim_dwfY$^Lz7|YoAjC~oh|IX<9n^%AI zhxyF=v%Swb&v~BbWMQrH4=L040F8)s5lec`%Kl6|QM2HQlO(FkzQkPTSx`_A^qk3& zxz5vZ&|qYa2b|@G7A8I4iG^|=BniIS`yvLo*kW^rTm=sg4??aqY!6aW^516X&d+#B5$1LP!=f1L!tF2w zwZ8F@BS*R(97~DfyAUhI>!2X?pRtMsoPt7HYHDU`s)g^ildtdKEHzX~L!&rw0^6); zF}%Z6`NMS}smHU>|KQ9hUgRB3P7VWVVF&O8#OI|a1)ojA1THlQ$ zI5p(Z*vH>9Q*pS0id{R?;gRY)o}L@kPwHpgH#Ug1PM+sy=jP@fiS*_A+vh3IVWi*X zq-B*8)}DOI4tiI1wZKj|`x$Jkb3TW~e);f<1>eI|IJ%|_*yv>I%BGx+f_}Guvn9zy z7?PZm$*JycZtH9BnVAuzhF7CE(N&{ELkr2t@DI;=_M~o4zi-{_1v0tm0=t`OnOp6z zzYdkJdQz-&RnzOu~QgR`EuO}^T%)5>yuk1Ql1NmbCs(>ZE7QwyGuc0k!YitUD)*~lALt05 z>$;JoWmJYS`}K_*dz;&SO#H?TbIY+Q01ledT7P^?ut}gC@dHxQkjcjS`a)QJc&JoP zX8nKFh-Jp|i3?sf7OMu<*3*sm52{B#STxSr1ouM^vAr9hBkm!iW!!WB<%a{-huXJE zm#_wcoW$Ed$q!vuRpw_^dHVbNN&WrAs;a;|Utw`^tA(Qh{;e)A4p;ffE@E7()H=i6 z!m}hH9RACu4^Lm;1k}2(PGQz-U~BF;#4DRx6J})G&xF*Hyx-|?W3mNaT<8?rei7i} zGSwqK13~J#2gviq)5{4Bpg4->)lii?P;kB%FM0$ZfVOni!dcucSqWtd8^NFH9KA$ zUU+tJZ6qj^{nrswk=Yx^ig|8GCTb=HK>23TKbloxJic7=C7(p1dtekzeivq>HJKlE z_Mxw*FSvIMcuFZK$B9fRFeTGdEH+B*J?w?*&Q&Y>vo`ae=Lda17(N#y09*&=*P|ij{8$|uAP<78! zeF%1*r@ql50?X;6`1+RY0fOLRUD1n~S8_39-sa}!4UshZ{N*>yCF&`bLQd>4xxq*^TjM&jS zUBoI_{^O-9_FKWfHdvR8;ev_^0j~R8M6TV_2fBNKOMn0QjyQV^Z5ilj?ynncdy|#A zy_?Z8e?0e%q;-mdyZx2FLlsgB02z*rTSO?Eh|$w0Lmbs59;TzzX5OH~;n5IoxK>xg zse&e5Q_K(jzdX`fLHmBOl}!O?HE=U7@H~xVs$|iPN+N3$a?Vo`&zS`AZ>nIG%i(hiu@<7MXn@} zf@O;@I@3Xh$4-~czzhS42_PCS1Dr|(*&-z)8V-LW0`|O;#2i&A^(KF=1#9;HWc=*ZlogoWILu%k zyZZWbJ9~NC8U*Yk8{svm00%G~oncomn%@(hq^kLz&sDCmH_OYTm6g+Cf$^&9%CN>7 zJzoAkUEgrP&ZF!=v zRcM*|n%_$9huWcFNE)-5cM2Z^0FQfSrzz>EY3T|jWw$S|qOh>xEiLk?Svoms%7av7 zkls5e;iFdthrP+jQXyh|b+cwPkIxH`KfT2)3_Som_BMBxWCG>@%u^U&`9YLPrnZ|$^&hPmxHC{H~;d$v{Xyc+R`M*5(=Gu zXhD4@Fb8b8Zd@(@2^8scPLtBNm%W6a%*`C4kk8$8MX**iVzqCoor(ehRjV=&4-f0D zcdXfi-n6ZgPVuIcs~S)ABS($Q&MoqBTd1naPP>c^m@wegkmn5F!NZub7AAk!Hikcf zB0wvltbw88Y$nyq|KgZYlALE=S-X?Ka;Eq{%waUHaCL)lVT2jx>>@zNa2Cq>!2UI{ z7q(B};X?Nh`pF7$L6a0(r9~$iKZI;7I^uEOM~{Me%4c7G+HywZ#k)!ktpQZu8mL^q z&Y%t?d=QqIOwx&^Be9tz!e33mL6rLL{>L3-*>k}Wv!2gZ=Fb#oXqxQO~;r;q*b}w|DluKMWyzbUNbHvD^X@_ALY^%=P9ga!}C)LM17l&gUfc>G8Y?;9STuW;fWX1^cg!H28 z-7d`IAZ%!;Teq*L*ja$#;qC%|si6|1*o{Lg6l=TAVYWw=Rg5pNl$>Eb_!t@+$BZZR zfb{`}G1w>+6XdJNA_fL&Y0?b3$4p5Y zJb(YGxkE#ieEheYiWhkoht&rB`?gmko(|{vO0s}C*Fo3*7+V%d>R-Klnf1AbL?YQ` z>^%Q)(qp?`heniAQ7cyq8)ZKod8)3s8Gnl0fSAa3Y7%7k)_PGxS(#@FT zC8*q+H%z*|I|&r!h=!|IF)`I&lU^2miR>6WJcKUnaAGcfee$8_9vs(v%uc(;dG1haufv z?bBBjKUm&B*Ow@+Ov+Fzt8{ble6BWYGmuD3>NB5SXX*CVLd!|corl!kf7dJ8&_Ju!WFOT;)iB|ILPB(dIk8 z;ZINFYy)?5_o@46+vwOcw{E`$9|y3)Dw|U3CinG0Dpfevlo=b>$s)V+Yk3@{9kRXEAeSY7^N+-Szi0t0Qp{0KrbT~Joq8lv(Mli_`e@VK;G8F!)H6#!=-1dh9 zs!S_F_@1&b16xA-U%Sb~KUo~=->uQB43^~gx0>cpzIOY(^U9;s2Ctwf8QZj57vDU| zAIARc#?vNBGIzYhBzC1Z4$L#3ilJ-Q_8)2r@-*(Rf#*%vvoXhFbmgH7d^VeFi(q2X z-#8I6KR@QAB*>eX__@MgH_dXkq475%no)~mDdkZvIE4Fw7ylK_4$E&+|=27BhH1&7Fnwo@S=;{559|5wul1$3zG9#}i5A-EY=qi>U>be}STK6!&va<42 zzYg!2@E@NGmtP?uKhA7oLd8TyMWHdq$BWzmz{)tOx0tLNk*4fdn1G~OE(<){A&S>- zV#RrdDoZVf29FXN2KPKA)DIg8xQWbUD8{UQmFKzvwK#<<)#9rIgI{Y_0)OU6h~-xY zKek^h+W?Dkg>kY1n>fMsYBP5TbX+&dT-o>N7_@ zqo0{6Wj$t`vqZKO*___rY^S0IQLp}q{!?d=C{ahq+1ECX-u>ucaa8k52$P&$F9Ge|Rkhc!y|~XGZ2*t|&=6Ke<}6{8 zAWrS%@sSE_qiwEtqNViWZA(FE;~@-`%SoOfhWTJ1SKZ=fVRkN+S{zJV_j=FO(cm3$ z3vmRiEXm%z5DDm$%|`g@@FwH}#xf12LPGZk^?&r$;P$a6ZY1N*r`;ugEJZ>L%3| zwbD*Ulf2xBaxH!Tb@Utu2i_@DKit?NU%%0}3C=2GhsOi?Lb>g2dWYi9C8F@=E!%(w^n*O|x%%+}_~pEU7P6s_J7M}EA>a1$ zKh9D~8H^9)p?k*@%?#Q_oB0PR?bFL%hlB1cFGGtg@=at5M+`@Jc=PiXX4qB=D@7o3 z#3+Y_cFx?)99q*kb{MIUm=qRu(uPT%@j9ou`H0WbSTDp&X% zI~&_|Z|jdAD)RcebWIuXFV^+`@VB)J3{(kmaSl}l z?uMJX^=BMun{mUGD__Zh^WYMZkx5cc2P=Z>|K)m+WTlA{)H1v&=Xcjwz+ee_2>E}) zzR}o%5EiLas-=hxdd^V939;hKd>%xcxi$3;1Lq3!+Vqq0e|2D)jze z!DXE@a};#O$Yx*P6?aD<73y(T@;#I8*7V?DUoRSiN9ilCp;`sQn zQgXLW!dar;pQxCo@Q1VC2mRY--C5f6z8wj|`mClu_@t+X7P~?r%J7b5Wq6CyZpC}a z)}&HQHMAE?O5Uz2L1b{x<$KplL-mT!7DhI#qRh9A`pJ~oOa-Mem2v+%x!+jS4vGxtx&&$t;f;0;ClRpJC~ z*uq2MX1fsw;lzSQ^Jk-iu%J#f^_>@UU;6e9WPe$02C9wcV}pZiMdEcu#bP`pw8)rW zw<}{>S)4;B_gBy9N+0(qT1OpTbZ?PW*32CU3cp}u>T-tw^@Tq66u1q#Za>$4X zd*DQzNRR!WgZ#;D{k&j4Fl6U)NGg8Rt zz$BY4FSQ76I@98?uneJ|SIwHlBJ-Af2)w5FgFN!HMR4pZbzI^2VP z!7pS|-a9n3*CxFfsx(-I?>bw;$Kd~bb`SYya2a&tSlLUGt2YzAj%r>xpsqR95OC?* zjLgi9VUqkDs)#p@Y3GaO3VR6yJ`~#Pu^Wv+I`m(nK+?gBLn6rdgkfPRdFE~vGN+R zl(LFXf*%cS9*HD!iOB71PJZCqzKK|_!_UvJJmcJv<3)eG3E##@Kxbve+jp-{5=3Lv zzm;cQ{Lvg~8EoXaJMyds>zJ7QSgZ%m<;KsAs{LK=?Z4s*h(xeT4+amb*(1fz|G>YX zh~9?%$8(OHSB=<~0wV+T*Mv7AKoA7B^}FA;ORPDqBoa%x4*}S4vxH3^RZI;+)x?4$ zOrAr|BybkzX^}6txLEaRF!ARi(@m>7y!{*uSv-3tgHtZxDlnW%x)U$JVkQ5Yi^p-W zZO)3Y^*KS5)X5HsKztG;eP!C0LFx_v>xD$GTo#uT5)5}|DhJPdf{fE05A^UL$dY;7?c?1Z~; zlP`YKW(xD8rssXAKFhb{8A*#qciQC}k+d<=e`lm$5=eQ=&fYLypFN!t_=R#Q#;ije zY+cmO%HtTGei_+Z#TO54HA0d>Pu9;49<9lsmCR2J4BhGsH=~~(@nmBRE?bT6d8&;R zvmp-+fr?nF+2}a?c_Kz7<@(LPhm_ZR$S=uj%=>s3MQs(>S&N6?dtqZM7zeLzAud3%Yy0GBj>+9=(c79=@GH^fO-wq%NlSHB0PE9IeWN`GQA978y zwKT@Ju}K^WTn_i-<4`!jm+4w@xXkI53g7EoU^(aSkHm3kd<3Bj^LY*q@$9wENy?g! z@fGP4!zDM~?IXYT=%f@Dip4eGVz(NchX?bCed}P=@TCr}4jz~Fu59Q$r<$g=*K<0C zOBTrv3C0&Cua+K52A1hIaogP#fAKJ68?gOFJTTecTO9V=`SmgM<*Qd#1$~e=HwpFi z_5#_3Y7A+WNrexh7EeC>8D78}u9bTK?)}kPK7mLi%*-iGxVYo-JPO}tm*3w0@T>WA z)IXd&{N+-#d(O^eFNo|R*qWkDP3I{qWvF#N7kGM4_GV@lZ`^gBA`f#ql}a5?ZmEj@ zw5op0sQ54-AbyMVbn1r}H&+v{U#BdWu1-VG$GZp0Y-6M5WU0x&qjVNJuSEX!hv01) zFKb`l{&|n^hSUk}q`0;e{w!0fT2d_pxEN1RZo-}d<7nYrfD-^8XsE{e#z=iw&_)V) zAma3}d}>L#k3`}j|I@sxyOPRaQ+#eW*>)1A=>JMKrDuHE!G6AwK$w(UTrL?{_`Huk z`GMCJeBzG0qRB?-5|D|ZC4no>d_0HJqG;=1H`=H zEl8NHsboTK6s3nqXvXBo?CkFD@9g#K%kFmj&a4KL z5oOqpt0JL$9rMd+?`x%szjVL!>@3@i+mw^%eLe?^{3=N6`};W;7Z=0kx@DfDf&R$y zBmT#_c%a1(DCgi-g6-37uchkO(a6RbXefC&*x9|8^(R;7E~)Zxa~XCSUbnZg6O2m) zszvA9hH3+&0IwWpDQS(A1=Y-ymc-i}*NMlF*P{DSrtKg{m7hRzs%G|dvh}%RkJ2`) z4>uPS^waGf0u}+9DDwwDxPl-dYJt8>xown=N! zTMNCRvdUzVHsy_nM{7ki!%Z>jsf!$u5>zs%tI%V&JQrHxF%-?_<*jmCtSu$Kc~Y7G zvZ0}o;2%PT1j)48{1xGu<&r{nmA>KO;h{Z!c`&zMNxuEq)|Mpt27PX5T`y7dy|gC~ zU+QPvfNUDxFgEru##+-IF9oz)uy|HV*QP8(o4xJPXTF%zQCpw7{U|blyVM?FhWIa* zUMeU;*H|EW4VNa-*U>TaJ3IH7H-Dza5C~m+U`DR!v1k^!KT=KkK?@mAE)deYwL7>z zE!-+>G_=AsQ;dz-3L8{_C@^IGn(+cgojMCt`cq zptYxmM^Cp%pPiM3MdxeV8Lijl-7*)>K{r7G za`V8!E~c#PR180zSQF5)#qVSHWO@vS3~jw~sHcj|EzHG!^!N3d7^&FXLvzY0uY!2* zAa|yVA%xxgcInM2tq3!m!%-|xS*&F=;RrqfP7Nx5N)T8!ljkOJ49DIeJ7^72HK64B z{OOTjB3YcD-|^#;FELW$vw9TA?)1PA`Ak&BV8yG>3&zR#-HP4!6O2quK)sON`pL~~ zV{1D@ae2{DH&xH&;`L7Ywq#e};MQY%M_YkPco`v0KX8qHT8MzuD`4m6k5s9;RBII^ z%V(fPgV2^X83NLP48=;|l`ISrsW6Z^!cG~cmP>+vWBoblS9qd7jqhHqVAQFJxMFOsFO3qM`2@Pb6?Dy?CezKL117ZoZD3&c6{EA{w;x=Ii8E#GqT5&l>U3@MSzjS)UFP_-tZUn- zQ(+SN6my8Ap`js-pylfuef_$Tv@{!A-|jANjCZEAlzMToqoYx*qXHLn_#`1OeYBh& zdw2@OmYdJyemw(#YR4?MyIQQO&AoWY{|*43@)I5DD(aa!GBDWXw|p|4@&G0-l=*KL zp~Fz7x(4Y_Gvp@~vjR7|4m zv8>gj0}cuMM_luu*iQdf+jz0Yu*o2-;myGYd(_Fes^`XooX(Q$5EJzxoB88gTQ@_P zkfQOg3g$UdbmTj-b3m;rFUely^XDBKKj*`A>Wq=oJ5y6lqaWEhnrZ+l8>&oQ{V$wT zrtgcJxy@*!j)1_JQwwjLygd6W4aU~VwG3ww&K-FE{s{MwLS(p%eM~ zcC_~H%Cm}u!#!iK39kfWP%(jK5`=eK$9`yDP1^(BAWU(AG4*M( zUu#t?CTFA_8y0Y^^P-r)D;@0ZvvZ40_g3Okw(bDXzvB{U%7_zw8=Cz8T!8F4+0Nzc zidf^Mq>8hGUhmbJ=zqztdawuz%gSdCW;IP<7B}HtSig{_mSMs>yu(Qpk9FWyj)^D#8S`rRf>uO=Q z3Y5>pFvY$#UBalb7bd7MpTBScr|Ei2R5bd4D@C}AAW2A8`8ox?amSOG^cg_YmMiIm zc&|R4t`avCux-zr&Fw+b)!{Rgz>Of}b#=I)Prw+`nwY`zt&!%3ro!Gu-pW>>&s1+5 zN`5f@*YqpQ~3XbNYr6(HCd(^90PZ%J(@ns(P?gyXg69d?lk&(s}9FA}$ z*W-<;_xZ?r1lec8&)mvN(H@hq2=2L*c9#d}#%Zjb(EC;^sO;n|A3x5jF-KeU6b}b% zkJW|?RsI@_%Ea+k>FQ2rAA6jSr6`>auU*M>oYG3s`Zy+J7IM$f(2gMg;T>O)@m~y! z75mAd6LVVs9tfmk3lydjRC#D39^;1tbd8oSuuUlsUY(hp)iEkVH`=QCUg&#`7LD1z zBjDFCR5cb6wJyetd@iwLtO1YV`}xv7jw`w{{RYdoQ~pT#aLt#jn|qr@Q!97k&nB z*^l#Vm+Rq=WCg+ren;A3$ds`4cIY%67XeSynC*@@>^HLS#SMTLUuJLp!^iftH`R;vvl9 zr%4J5g8KF1vwHm{CS`8f1LFAFt~xw~XJ|fb>@6|vf0xb2itqDnuA_}A#095^qb-TjU;Z&QAs!?d2`{d#T2q0C*QaAmdnn(pCa+#kyzX+<^B~*1~;R1eXyr2vaxk0(`=nCpF8m?A|mEJXKPE##0D4M zq_mpA3Wk*wdZYpX8OrIEU~j&^u(*f6@UW*T2%B-E5!*i9KQPD=l3H^k$+|5xbZ(K& zcjlOQL898A=K*bDu6nFRj5VUEQ;^uI)PFLoa)Cf-qb#avm~x)`e#3;e(jS6>>!0}> z3|(;Jc@{q@nt*q3=T=tcJC{{dq>k}?{vXI*;^WLVItJ981K5)Q(DBn=cVA!Pia4I7 z5XKnD_!hl7i`g}$sHj3q4FV#7@d3ZSa>h#EJsY?Ql z@h0&Q3D-2?4rr}d^d%35S0RQfiqQWyF*Y_9a;x~>Ik>tSxan;)8H?d@>WPu0AWI{h z7yw#D`@#kuFx!oT_C5}`#U=uCO^`o1Q0fLu}sO-lkgP z$#VZ*u(dos^_cQtSEmZ<-LYz_>izq1o!7V`A`NM;5Z#f_BChrne9d#mdqyJOG36W9 z9ySl;Lyd#R|BKy%s!|T9%&cMzn#9-ObEKnapyh8IylM6S znC%W_k$7k-obvxSTm1ehX8VLQj3L`wTWcEhh%V)}gPU6a{87BEjwg%wZ6{KeK}DHb zs)&vI$NG5H?M~u92WG8#;1h~Wju%@D7--|?_V)Vl3wwkz&gGQQ@;@Zx^W*~$+b z8mg*qRjyW;EK(@2G0s*NDh5J%VBNrE3NY+4YhRN{Vj|4@2|YN+kzb#AXr!iAqX~@t zpO!--Fk2H`^x?zXn$wod61AriHvk=-naduuf@H3`H%rX)@;{)M30yuKm|t8B>c}MZ z%sp3*2Z6}Zoob-K!Vd1DQ#CX+^d=r#Kc2nVk&vBz4S<+JNOq^2e zui8Ydcc$CUOJ1g3>?i*E(WRL5&Q|>iAdW>C4CXeO1hPi7UHJ#{Yxqamgr$6TC=Ttt z<#7`1P=_tg!nYnjd8365Jx8>CGQ7yi$_h5~G?hI(>-6i-dioHOa!v?`!LoGBU2LQr z_2wBcEX2I;dYK{5ij&p?lR1~KJrKI~Tho5exSRX(w1re&S8iqNG=k}4fwKg|fJZPn zqfA(?YG-K_889xvdg~nBb4>|Luw(Arz5EB=$Twx4Xs(VYS%fEFM@wKhoJQc5&X3}H zN|YPKb4C;?8sHX?Z2>a3OD-rWVL#?h^Go3CF)5KHbKn8dJ^72Jd2wTVcW>;ZsV(K! z0b^29oWH-FFr&{>A}AnKj)n3w_5*q|EW50RNW->>Xm4l(cbkbNt@ko#0idUwWL~8;_$dFy8mK&N2#U zH^;?&K2v$7@PdhSJtzf{2UIk7~6?G%LeK5f?(GvS}^rIqXbRWiFk z0ajI2&3bMO;A?KKhCbpXe9H(LKiRrX6s@=4CnyVkXuiAeZ-MZ0qJvTHoKOYOPDa&E_4kGa6k!Mhf(Qr=JcH`DS z#2znDy{SrwJD>Jc@R_SJ*@Ia|Kz=1711Lq+{vno^-IQt@?f*&9@yi4%SGL zLI^qmsGXc7fRghWuOo2oIhlji&i+Qq*m@1A!1Xu&Cuzf)P%3U51=w^%?ZeoYx2hTl zO@)Mau~ki-)kMoZKCbmD0u6q8*Yq5Ln=D^xx(+&~#ma5`&sWB?3w#4)vd{Wl-`Mo= zCZp9|a;}1ul;y*RpWs#Ta~-#Pp>4z){QWdPFlS(Sa99hyY$uk>%ms!#!o4 z-4AOwZ3FhrPhO*u9nAuVf7MCI%UjPGdLqq%d0bUxDjbvby6A;cVBj4ZF&TVm-o0x` zamYHL_HP~fyW{{MpUZHLw{?yJiffccu5D4IrAT&>Ga+q$dTix$kd1Br&fZt9H-L$* zcv$G@Q~9VPVE@R@%G%Lp@qL;W1Ky)(KRh)(J-cv%pX$M)=UakN)4vI5b)1S(jAoXR zObK`y^Pg#66wVCIkG%&@ap1KeP}m zqh`0Ze58^p19p8Z>NHQ;Gdhy!@JuIe&KW|>GdLTOkq5Md*m5((V0I(ZBd|6qqyH4Z zV*AO&UXo+(_R)rYQo)Q5-G{KQWj0HIm)^g>X(3$QXgRX+)}P0s6t&PlyjSFN8?2DT z#1PHlt>>ctbegY_+LYqH!cm0_Gl+>EB@uxY)?taNsPm+SMoHXO&PeFw6 z!nnY}g~-6$A9t_BH3RXU#SgLnt6mQ8a)Q#q7{bjFd3L`ZRE2%h+vx@qmp5RjNcUFa zv#pS;d$#{_Fx|Ws@CM$P6K_={_&;*RxCDx!_F@GezdP~>kFhEo;gn2Lmzbguw3$PL z!!zy;4#uL8I=6yj$~h5N0?mLL2Ns;AWT#&uMXK*G z4D0yt=uWBC6<%>W>9slRx9A8h9qQyuWvL{PQ%=fCnSIl)b(5;7BIZ&(vva=KeuCg9 z7>8YAaPso{)tR!VE3>}oM@liMz=~C*6&HKj+pEZz5oOYkbbl^Km30w_M5pNPd3{Om}u7$f{yFtSM^A(#OMEkXAoAbr|Hc1K-Bu;>c*g9>k}y*T zDc#BWb2{~OiI0~qhYYit{}o`(U}l722(BLo)J3d9Q(88$^@S3&>NU*4@Iygfp%iYw zj9a%v`TfTGi1kM{J2x+Z6Y9|N1hjbuJS$f(FoVxe_R0kSv%yMuD+#h=CNf#ww_=d8 z)L+bf#!vNs!R_;$n}M^ z-jK2ju}YT?!g}m#~zCY4XCm?V(&5+O>v~Ftnv|5Tx7wx>n#n<5h_TSs#Ew$k( zFjz5*!>-mve0kbSkOcoYARf}X(C=Q9VNakSBG!y9{q;Tvx}<<<&4;6cb3pgAvUZ>( zq%*tKDca+rfKS-m^xn3c!5r@S@k0xHwBs?KHDW#xQG;1{l692ajoXxXq=bMy!t7FJ zXLbh<%NIcbfv^TJ#KHKnu>F_o$$9(lsJ%OPT83pNVEF(SG_g6B=jY`)Szejk+A0eL z;U7X)H62GJ9!*M0`ly-J6m|L=es}j#bAsbw8A~{A$_+MKk)&5o#<-E}(B?twSLWe# zg&xtU7V}O;Dg!P$2uLNnG=|+Y6h`StyUMa)GHiT+%GfYH-Vj^#0WxUpdh?udGaf>k54j zm1-Bm=&YuTpSMZD=VX-f;!i?j#|)dJl5k@rai@{VbRm{BZtPE zfvFQPf7ts>#@6!t*QESS8(WKGMkMvdgGD8^w4zAQKh&;TlUkH5Le^{0thwS>Wbj}W z0S1fLzbRY>{cme@&IMXDGCM-@la%uVH3vM6^z`&6RZNaQL`KlU-qcsc$=ri5Gyw~9 zCl1yc7$2bBfQ(_Lba!_F5_9w@a#}*Tlu8-nQ{}ix=3bz-Ma3mOz6n0B&qG2YcPYJf znY4B$9UXmqVkxH7d3kllO=>?kw)m{^JxYOEL%%fJ( z)YbL2`xW8+PSMu{!5-$s#BPDEK`?Vi$_>u&rYKu((;rGjn^f&g3RP_{5W!a&Sit{z zK<%ro5QPl|h)>9G^tgtiJ~*?T{NL-K-}vAKxsKY~(xsi=)M$n=j8!3GkO z@qZJ2j0Nc>y#3evtoMIL?)@YV?=FBu91!t!Kdz|1qtGuSiLebeaOL44?E+WDu zu}z!^jY(V{AvZU2n`UW|_w38{XIw5&9St2n%V&w_NHuC0D)-iruiPG7R99280fP$# z{ALh1a6|j3@9BGY@6CLzQ~?7DAU&j)pHc1R=eSP)Ik?whkNrK1xjx7s9-B@3@z0^> z0MIHhLA=;X`>%$E#P@cL#C(AtT<7y@eE=>AixSpI>* zwL)zwG2{h=pg*2{;R{EF(<3Lhs6oCP84&`Gtw2YuY6JuH#9wYx~m#*AH$|LYx*uE=*z=U+&t%{7zLk?$hw1Pj7la zZK?PLdxlt+q;AoUYU?YohTg;HzMO(C76@Q0aoB0+0?|N`brCw-lXg zbxb1FUVmlFGQv(>-V#jOyW`lz*u*S0x;KV4e~4b_p#1IJ&>pzXEph;=^Tsd7(}-;w zJjwxU>+}7DWGCRtd3apgkJoH?a?SbQs9D$+_}EePd2B!^02u49*8gZ*IR8%}FC39D zlRA;QQ-vAEw_f);>F!BPQb%SUV$0CY=%|DRc|UwO24e`T_GYV0obIRr&nv@1N2f~I z?yUw)B0vdFJ4x~_N*jehCFR1YJzD%=|IDVX<>KBBzJ0nNZ5!`REDqQU9xmS>ioDEw zG-Tyy?%q*R!Gpc2wdNP05i~Z{1uRGLCb@^g{lddh6()|Q&uHL|=?^EUwi)p7HvK_k zTS#&o$vsiI)r6IB94Pbwd4xO-bp?YmeSVlOm|^wz5%irA479W13z-@z$q5Nfft9bS z5x|us5@nt{&Pd(30l=!frHe~PuPAcFy;OCpSW`!wz~v1xvzIEm#tvu zy9bvQ)n^%k+;sbbeV5mm)d%vkE@hpn;r#(%1n=){8-|RQ(f4Go%QK!}%}^RD>jh7& z@Y&jR3)Gq!Fll+7NM#D2q4pOp&r(fb{qY~5f7KbWkNTiwz zp7P)3yAw-e*)4-*T)^J8?K-PcVnwG0%>%6dotAVtC9di#_JwfbBWmWrB^6d`i~=j-Wu>iudCC_%qJFRS4zv@@DrOeM7$n-XY_xr08F?nL?Ucm z&o|DCKUn^D-~AjAUW*VTd?_55;YFs9uh656S7misd6QV0aJqzSe*B&0#Wl+H+MBz+10+?4WO(CV#FW4Sd~b6kA#c7g+!D|eilgO6YruZ~ zxxcZ$_bgPBedYfCg2wLN=b1R%F_ULsM!<|#XYIV;t>wl@(fNY=50*6{H+$rjZ7aKa zxCb-`$9Df?6DFqp&*}T2Wzz408Q|+9$(>GwAx^OMv|`LOG#o*Xy4cVjgsb~&&jKGi z#=H`@v5yM$w_Rx%x7S{xY;50GRD=`(b}6th#Br!!ndFh>tJNjE^LRas(D&`=)$Zv_ zPv5B;I>&yK+X)C`$e>vmH+aQceDdImn+c5j9zXi*Ul}3KQmI^1&a23F)?+gXY{(&p z?nxIG&wucF?Y3Vhj^fu1B51vhF5Y-l8;yL#&S(QHB#aLU0vXcOo76ku4yKp@7?{PnYFz>AG)Vr z!+;iWnO|I#@StwZ^txXI|?h2(kJ zf+E$J&}*DK=007Im*VbivaN_28ZwcI6J`&hMbTI3ar^U&!+Y~E3W9a3u4=n;IwjYB z@r39xlUeXD)BW-jCMV+_A`!O@y~ixHS!~plJy_bTM=n+f_E3Wki5^DU7C-s;`_Y*D zn%6=~Bc3>NrW_+O&?GwgRJ{gFclX_AGsMVT2j{uGx--qazrGQ$UtZNVcyBK6EBFxy z&J%i|-}3uc;)fTJ^HBoq>ruv`h1y6NB(zE?On*Sy_4C$)2YgNdi^jI}Ty;En>4`VI zjNi?N9XY{b&wDd9Nn0P*7{BMdTR+`5`Z`8p7Q;9`}3Ukg%3~L z>#!NeqizehQ`$*vXCg*x z=l}pOB3X@;z8q{J>67l)82XNm)zuty6}#u%SSy({583~aIJhxRX^?O|LmuJWp?s0{ z9L`6d;0wTtDBsjjqlao>*~{AY3hnBZSQQUPeR7gyxCET~!$H*@9UVX zpWWDILn#^B3g<-RExV^PtR2NeVFJS0Zsh;2o7$ImQb>1=FGae2kJW@=#}T_AM=Z2d z`f5&9$D|Ly!RaVw_Fu3F63w>qRaoTBqSu9$8M5gAz?rtt`9kZPW60lc#fPwSuS+r6 zx%LvSxN-FmFqHFf$7u)7V`IYeBWUl0Stwlk+{zUUq7l*)aSc^}gF3NYk?2^nR-GK{NHc*AYuR2>p$YtWznNVlg=l|UU!z;H%z}RlI#W)k&8Ufs`JcNDGJ+LB1cmreDC7F<^?_qkhudT0xG#1$gYB?opYKx z({Jyq1fRUn^`ow?E=_B@O7r*gtET2Sh3Wpcry`kb_xx<6Px!hw$#en}tKNyAMONX+ zD-5|XBmrJC=bMpx!Fh&v?X?itNmm5l?F%EiKg3 z4)&^RRqD6Q$ISGu##Or>4X6G-Z5% z)qYp<5rxb*!*%%LYL;YY`Igph5H`q@lXlFmv?zvLvP1TBiScYW?AX{Rb_4~2rW)po z*yXCO=b8JuELmY`O`d9>nv7<64|*u|VTWLZSNu>SuH+qJkXu-&ZI0=WwUqz3Xt{5- zA#5O3ThgZfdz;g(m9LBPvFCBl*c2U5Yc(Cs%DDkz*9+01AZ&d=O6z%D(AYVX={+Az zaL?xv82bMUdfhnQmVW(5cB0nCpT|m4*3V=#&_D!94%z*IY17?QWUKA#)ElW{4r&p> z3YPBf1Q%a(gbF4$gkt@xyyfLWJ!?U+^KM6OVA%hzE8gu9o@X-Qh=>hF^}sgniQ)Mn zpt{x7#JF}J=3^7L48x5UaIpNv=3aDos`bZCz<%JC++Js{|7g#NgNK%`n&-q%0n|WG z593S2IOBg5XbCX9{)!=^Xpx#7d(`yw^mWc03~&;_xkE z9FfhPf0g3i6~|+-1)Z`|JN+c1)S))$+fp{D3zAPX5 z2$l#%K-j#EgAX$F`eZoNneg*sjQ`i(l>jxFWovA^by^U$Tfqgz#ujW5q(uZ-L|YVr zW>E-X2LX)`Hd#fMpp2-XpwhBQ2)0UqKmZ|uunUSTvJ>`IfygdP!WwwzhHhWgtD1Um z=1tYi^vo|+rvBu=|6R^K+dbzyHqq@fMnJ$#u5M^|7GJeCxZO9ri(i;hXFoUIwcaUP zpCIF4@sM)+3>o7~70q37h}=Pqb)DREnpE6-1J&SGq2ZMCqel=O}A)x4n`eYhZt)nA+)hPaVY z;{eIa<8u7aP`vTSL*Gk%)QWBEOmT6UTt|iYR!GSc7LJ765J1iH*mf^Hv`_9bJ+Y`D zy%NHJWhAY6?J3fReEp#6lC8+Pm)>ir#7uiM-Z$<}`6hr*WP7V3Bv?6QJ;c9U5nZ^X zyv#Tv;SKSIm6e*4(|LJ$tQw-v^>+)EmBWp!0w15~N^IKFcXGH_8jn&(u(KRr?q$TBOz8j;9NK>ucV}OIrg{&<1s0vbH0MO zVf~$QOKxP5;?#V$|J1%MSMNOQ)Cjg)FD&wa@F=!_d8%mX(xhG0#-A6j4Ag2B-PCdz zbFdSIf-AS>h{??#B=a`sSe4oxLBlC_Kxgd1R*T?#Ztrgn`YLDV>Wj7eS6y16S8PmQ zrZ=#r0rqRFYEgRn1I-t~q{9|5#^dLy(lSwBwABWh3b@3ZhI?2b69(~S5x+}O2!I&LBRiO(NO@)-9;;vSl zuay;;MO#%>;Y4Jd_`~+FW6vM0?D8Re^(Mmf1=Zm9NHpuI|MAbYnp?%_HUvN_Un*E6m9W1mY%Jfv#yCr z|EKic-q6s{o4EE4CBaSd&(nk0dTM6U*jHA`q8$*FP+WYHMml}^N}v0h(zhC`r*0Av z)b0{arz{hu>#Hk=5EnZd2@JBTV&Kh-H?HejM{o`-qPq%=>j24l&lUg;@S-I&*M|*$oNkPFZiB2c4<;kI$Qi%Ny%wMaf z7VPQop-6<_v4x8e`>^GQwv9Q(>Y{$`{XZHeeXx|>`ls{#Je5wuIE1-YCTc_Am~jA} zm4WC#aWr-|w(ZuvHizc&MjBlckFN;(K0K@{KOucz#v{!a$s(oz4q|H4x8X?iz4PSU zWA4?7`n-)3a~?X59EcEM^3h5le%)nJPNyL{oISbQ0aKoMP*ZOA*cdL@(?M!S9yQI` z!Eqa%lNwJZtE&NYOsMk9%CZu%hWh%egYTk;-&(oBiYv2JaO?xn0w14Y1BPYaak4(> zVmhkI9o)t7<_X&nP#Pb3OQeanq_mN3T8P9h_gwmWz~UP->@mu@{#E_UjrBbF&bQYC zhooZb)z~iqb$a+a)N0*8?5x>=%}0j%nc1JDk|<^hkP_+Xo;Ay{_;k(eQq82TYRk=p zJb;$JpW5FIs12GvdVV&2fTDRYEQ{ep$X-4D<};ee8Dk}vSak^X29-htIBHwOw4#4X zb*ZaonYA)ao9zyX@p{~0nd{hH!Tu%JGb=liYlI(JC=24qXD_B`S`=8!lEWkmNO-xi zTNM1EPO*lU1qFGTnRx{T+3=TNw<+_1uapbXJvC=An_&mY}#(Fo+I?p-PK$7bX1=fy@mNh*E|zm(r7 zKQ+*3*l01ASXM$-Yd-_{BZgR6QNLd_$+C-!3@$ei{^;}=T*96(a&u(Vjn%W^_$Eu1 zeeGNw{h|vm;u+ptd^O@{>FjKckkck24F{(vmO9a1QsIgMl(Ky0wtLO*drxvWmDpFw z?>w-!MJ2}#eEK##{B$1t*spWJw?lO~vzP~4;kQ|BJ);s$sh-_W!);FiJiOngRo9;| z+kNBCX+byFsPMFxm59S7}5_ZtBI- zgr&*Bu{p0RI@;US`e%mp)kEI}eLTohMasQWIBu9HjP__p`545ILvN%ljkf@X98PyZ zdXvhc14BH!OaX}TZ1l5(5`cKlE?5e;HhfGUAFJpKPtP28JNUSRAL3fHUX+#SmKiMl z5^R3PcIA_v4)IZQv+i)qQYf)W)Ua>cqt0BxTs&QNw*3Pl?!?NMNQrY8`XjzAohc z>S*=kOO*t$(h{jxkxTmFy;QO(_m^io`6br_x@iGXJ^-SJYVK@sciaDs@fqER6lF(0hpnsG z>L)X-9@w|&tLM&>0Ep(XCj zQ0P9(4@pt%Js?3eYODC8z^5KYrnI?o-uRju!>)iS(QDAUu!!_m=^(dBep;Q{>{eFN?p>Oc^LOHm|i z{YsjsB?K;(a5Y(b5SPH&^!;XmIc}G@IWLXdDo9Zl+d;3Oxvin9tj=M4_^`}UO&1)} zbPu_AgWxJzbKl{x!<|aU+p1&wKGkt2y1MJ@se2Y{h+3&>!XjPX3t4eDlEhkEmFk;w zPp5j{7^$5&YijabTkhA&3iVBqpD0Fp@P|AY3aN!ok)s4RO;BZ4rvJr zWvkSJPxj>TQLp26rO7C;UHInj-$KhoQ43!3Ad*jcYx z^PS)#>RQWCkeOMU-ZD+)c|l|X-*_O!Y<65F-IKQ!<1JHn2TAG!#Bf6DM!k`MENI>Ez*6_h z)S_gC>G3VO8_+a?G0Lcc{I|QO_CLwY%oLldSEq=*EOdIObB5j}%dGire6PX71|2R> zvhT8QGNC!Cbco#!ZI+M>5S2b5z`k`|nseuzsn$s8%(L4=>IYqz#w;k+C?yu;zU>4{ zg*^Q|Q(m*m=Zjnen?5!>6^h+$(IDTiFmvoEioAarL}fsDapKK$z^pj< zy$%N7j1oBccNENtVlVSD^Aw7mqY3K0?hTh(+oUAMClMV+zwdvB9xc63M#UAF61DIP^32B6{|uc1nfWhsph^<(MCn3({5E7(+jz`{DU;S~!#6IpDfO?$L&K zBUWZkTJ7)Ua&8F<&RCkW^3+!~%*4B2MUT9<9$GdT8EIP=h72;kch3|BOjUv}KT9je z=u1zguKSG_(~=3Q2umW@5|RM*mij9zZ`YnM%_+&Plrn+n$F4v-J|*X)$P_A82Jy%& zpittPKK;3|@$)zpn@9{eBv`O8aw0UQrUL9lLm~yeGGG7>HoNk#P80Jcnr2-|wSbh^ zL@E;;y~h9L7am0stI}U@J)YynBlmBuKUMDC9kffMR*?3am}w%{pb+SYMZNI@+jM;T z@gqt+CEe&_d!3xD&XelP)e5N?sJ%s78&5$ZkT2dz4o8KMmZ_#DRBA_qWBlUj)9+Sg zez7Lh+habvVz@CR;%gpbB-`!Wv)J;ZiMM4u`I8hQWFwp z4YDS(X&U00k95dXOTsC&K_QmmF>L?6PTfx37y5_IAe}k`F{CR_Xh9+w?cl6lr=D>ahK~@DlF^?i-)o0PsMS>w3 z>y>b-#ns*guaz-poNAP-LVvbD3tvk(VO^1k-o;m)_+}WfT7cN&ZO;u`0Iti1y*cIl z(lUUixjS7?F2Lx}D#JkeHhfeSA;1%Eb@x`Kdhr+F-8y~a0rOdU!0v;VTAtf1jH2)! z=d?ny6kb0JfAM0!RvAxOy8;Ct(OazUgYB2Hr{rslNKSCMu>V-|@>7vP+Z8>*Gtx)% zxADMP;=Ld8&}IYVVDl2+UBE)2km1o;FT%W}Wk+j7x7O*Nxk1tbw`=1bo(&!EP2&OT z1ATxYGsLkhnrtx)d;jGRZ5ShT)&Zk!D4#@c=L=mwyb8FjiClsU7Eu+p_jccGsA}921kixZ1a){w*YyA~`LPe?@%l)m!h}HW-5RM9 zX)eozmA5N#?^YvC*R39VryKEN@g@B&s#^f_r&Qhj&x-3H#7)w7rms{@TH?k@Jsj~4S6`8TLBj`ElnHBLtpABw-`SU+lAbNfrBWuJLn2Pa{tEgHplNdaMuWnV+i zBx6?DbcEuj!|ZQrx<=0Q`xHjV2N0hNjZLp#H_crmCl8(=Jt;V0T8as?x3^A~?Vf+w zvjT8WL{)-?DU8A@%$p<$^Y7C)&0yd&Gt&%qCz(VM$5PX<%#xDMu?{Wu(5Ajp5AK3P zTDAp7&Opvg4!}|_tCGxnjGVnua{cHz#G^3eFP+B}*;9tkGYv5qqKc1`TPB@u zkEa}y$6NoZk70?|Fb(0~<@;P$2Y1`7t^s&!dS)LT?XpiL=G^}9if{>GA*axLOH-{G zr0kb{tsg(age;AYUS_6|6T}sX$$<2-Ta*M=4&>1%CAoFrLig+H>r_T zrSKjVXVESfh-chlwB2fZ(V;~;RP8tQ zh6L%prkH~6smfvYn>X`^3!}JmA0#_)((kIfj7=0x*l$KhnUYu&b2J}^_gBM?v*MFV zJJh1b)RUqfQdt^&GDcW#HY`#;n&rV7FDvkwZE21;cC49V@8rqR!{{>&QV&bf@^``G z7C*(H(eE9`vPZ_d7JHOiQxcLbF&;A?GyIE=$%|qzMV9;aJ;QkTIyfv&ge4c+c_9v! zAG^AMGf#?^t6OW|j&sI)Oga_VA&u$;4h|!C zV+c^6FO`aDGqS;@14#hEU(*WnCoNazMZ*@@key2r2<-X7lGu#Tl;fzJT9o88cGHzPw=b@5Oka!`pF@kV5^I8}C^FwY_= zDw|>0^(#xWyS~MNaXp<94!Mi(`Qfx_LR4hW319ZGx+uX{ZrKE$?bYB>N;(AGJyC6H zdiYfFh=F?0$O3JiE{sIPao&QB((YDdYU`VFll>i?n2k?*ue1{k2<+yb0&ccaK`r~MuUba&q6B;Jppunm=GH@$qwp-+ zZ=Zbakera38|#?3S9FzbXMbCs;e?MF^uE;6)%_y3)ZA8|y-4asKi26V1)v`jI@k{EK?VJP(`Yq?h8nmPg#&hZ&sqkB8Rc6?-onn3nMKpztSh`#41d- zcZVTX$D5?qwY3{w=q@ibU0GVsCO3D|CS`!T9n)6)bTA$K!9lcznhluH*Hl!EPsDyh+6&I z)HExv-&fE|wEka(b--xmigDtBYXXKhcx_jRVG?(j#c)4$+KQX^!^I1 zR7L?n8#<7#U3}x&zmYB{v-fumNa4<3!3C^b0N`%bEdPHxPL{L%l;~GCZ;0QU7~Z+| z>#u)2({Si(Oz+No#i5R+=nWQy*I`W{ycgL&#W;?iDDnmH72f&X9;*B7vg7>({q`Sq zOZhe3@tFj4KxW_X93%d-gHZ7=+zL=RkJpj$ z+&KBWTk+%a_~M=dM7(hIV(0(hiw%Q3`G=z3f7t;!Z>?VQ^_#&SGemeAXLQu4KUv-U EFEHc!>;M1& literal 0 HcmV?d00001 From 39d406ec7e011a43431ddc3625ca24a21b04f356 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 28 May 2024 20:19:58 +0900 Subject: [PATCH 042/120] fix(avoidance): fix RTC state transition (#7142) * fix(avoidance): fix RTC state transition Signed-off-by: satoshi-ota * fix(rtc_interface): check if the cooperate status is registered Signed-off-by: satoshi-ota * refactor(avoidance): rebuild avoidance module state Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../data_structs.hpp | 12 +-- .../behavior_path_avoidance_module/scene.hpp | 36 ++++---- .../src/scene.cpp | 90 ++++++++----------- .../interface/scene_module_interface.hpp | 9 +- 4 files changed, 68 insertions(+), 79 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 7958a8a2dcbd4..2e432e093583f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -501,11 +501,9 @@ using AvoidOutlines = std::vector; * avoidance state */ enum class AvoidanceState { - NOT_AVOID = 0, - AVOID_EXECUTE, - YIELD, - AVOID_PATH_READY, - AVOID_PATH_NOT_READY, + RUNNING = 0, + CANCEL, + SUCCEEDED, }; /* @@ -514,7 +512,7 @@ enum class AvoidanceState { struct AvoidancePlanningData { // ego final state - AvoidanceState state{AvoidanceState::NOT_AVOID}; + AvoidanceState state{AvoidanceState::RUNNING}; // un-shifted pose (for current lane detection) Pose reference_pose; @@ -566,8 +564,6 @@ struct AvoidancePlanningData bool ready{false}; - bool success{false}; - bool comfortable{false}; bool avoid_required{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 194e5ecdaf86c..da5e29ef4d5e9 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -64,7 +65,14 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr get_debug_msg_array() const; private: - bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const; + /** + * @brief return the result whether the module can stop path generation process. + * @param avoidance data. + * @return it will return AvoidanceState::RUNNING when there are obstacles ego should avoid. + * it will return AvoidanceState::CANCEL when all obstacles have gone. + * it will return AvoidanceState::SUCCEEDED when the ego avoid all obstacles. + */ + AvoidanceState getCurrentModuleState(const AvoidancePlanningData & data) const; bool canTransitSuccessState() override; @@ -188,14 +196,6 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRTCData(); - // ego state check - - /** - * @brief update ego status based on avoidance path and surround condition. - * @param ego status. (NOT_AVOID, AVOID, YIELD, AVOID_EXECUTE, AVOID_PATH_NOT_READY) - */ - AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - // ego behavior update /** @@ -359,7 +359,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief reset registered shift lines. * @details reset only when the base offset is zero. Otherwise, sudden steering will be caused; */ - void removeRegisteredShiftLines() + void removeRegisteredShiftLines(const uint8_t state) { constexpr double threshold = 0.1; if (std::abs(path_shifter_.getBaseOffset()) > threshold) { @@ -370,15 +370,19 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); for (const auto & left_shift : left_shift_array_) { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - left_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), clock_->now()); + if (rtc_interface_ptr_map_.at("left")->isRegistered(left_shift.uuid)) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, state, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } } for (const auto & right_shift : right_shift_array_) { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - right_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), clock_->now()); + if (rtc_interface_ptr_map_.at("right")->isRegistered(right_shift.uuid)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, state, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } } if (!path_shifter_.getShiftLines().empty()) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 503438f9f7feb..9247d9b3d200a 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -115,14 +115,14 @@ bool AvoidanceModule::isExecutionReady() const return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } -bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const +AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningData & data) const { const bool has_avoidance_target = std::any_of( data.target_objects.begin(), data.target_objects.end(), [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); }); if (has_avoidance_target) { - return false; + return AvoidanceState::RUNNING; } // If the ego is on the shift line, keep RUNNING. @@ -133,7 +133,7 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & }; for (const auto & shift_line : path_shifter_.getShiftLines()) { if (within(shift_line, idx)) { - return false; + return AvoidanceState::RUNNING; } } } @@ -142,17 +142,21 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_execution_threshold; + if (has_base_offset) { + return AvoidanceState::RUNNING; + } + // Nothing to do. -> EXIT. - if (!has_shift_point && !has_base_offset) { - return true; + if (!has_shift_point) { + return AvoidanceState::SUCCEEDED; } // Be able to canceling avoidance path. -> EXIT. if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { - return true; + return AvoidanceState::CANCEL; } - return false; + return AvoidanceState::RUNNING; } bool AvoidanceModule::canTransitSuccessState() @@ -183,7 +187,7 @@ bool AvoidanceModule::canTransitSuccessState() } } - return data.success; + return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED; } void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) @@ -502,7 +506,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - data.success = isSatisfiedSuccessCondition(data); + data.state = getCurrentModuleState(data); /** * Find the nearest object that should be avoid. When the ego follows reference path, @@ -633,27 +637,6 @@ void AvoidanceModule::fillDebugData( } } -AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const -{ - if (data.yield_required) { - return AvoidanceState::YIELD; - } - - if (!data.avoid_required) { - return AvoidanceState::NOT_AVOID; - } - - if (!data.found_avoidance_path) { - return AvoidanceState::AVOID_PATH_NOT_READY; - } - - if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { - return AvoidanceState::AVOID_PATH_READY; - } - - return AvoidanceState::AVOID_EXECUTE; -} - void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) { if (parameters_->disable_path_update) { @@ -663,29 +646,30 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif insertPrepareVelocity(path); insertAvoidanceVelocity(path); - switch (data.state) { - case AvoidanceState::NOT_AVOID: { - break; - } - case AvoidanceState::YIELD: { + const auto insert_velocity = [this, &data, &path]() { + if (data.yield_required) { insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + return; } - case AvoidanceState::AVOID_PATH_NOT_READY: { - insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + + if (!data.avoid_required) { + return; } - case AvoidanceState::AVOID_PATH_READY: { + + if (!data.found_avoidance_path) { insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + return; } - case AvoidanceState::AVOID_EXECUTE: { - insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); - break; + + if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); + return; } - default: - throw std::domain_error("invalid behavior"); - } + + insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); + }; + + insert_velocity(); insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); @@ -869,12 +853,16 @@ BehaviorModuleOutput AvoidanceModule::plan() updatePathShifter(data.safe_shift_line); - if (data.success) { - removeRegisteredShiftLines(); + if (data.state == AvoidanceState::SUCCEEDED) { + removeRegisteredShiftLines(State::SUCCEEDED); + } + + if (data.state == AvoidanceState::CANCEL) { + removeRegisteredShiftLines(State::FAILED); } if (data.yield_required) { - removeRegisteredShiftLines(); + removeRegisteredShiftLines(State::FAILED); } // generate path with shift points that have been inserted. @@ -941,8 +929,6 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, parameters_->resample_interval_for_output); } - avoid_data_.state = updateEgoState(data); - // update output data { updateEgoBehavior(data, spline_shift_path); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 1035aff5f7093..b6dc2997c11e8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -508,9 +508,12 @@ class SceneModuleInterface { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { - ptr->updateCooperateStatus( - uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), clock_->now()); + if (ptr->isRegistered(uuid_map_.at(module_name))) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), true, State::SUCCEEDED, + std::numeric_limits::lowest(), std::numeric_limits::lowest(), + clock_->now()); + } } } } From 0e2f26d68ff44b3c2b84dfdf739c9fa7cca0f1e4 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 28 May 2024 13:49:38 +0200 Subject: [PATCH 043/120] build(freespace_planning_algorithms): add pybind11_vendor dependency (#7150) Signed-off-by: Esteve Fernandez --- planning/freespace_planning_algorithms/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 2fb3e066f0bdd..9b19d8a81bb51 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -19,6 +19,7 @@ geometry_msgs nav_msgs nlohmann-json-dev + pybind11_vendor rosbag2_cpp std_msgs tf2 From f3f853cf1898b62973b325f044078fdc38d33161 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 28 May 2024 20:51:39 +0900 Subject: [PATCH 044/120] feat(goal_planner): allow shift pull over in parking lot outside lane (#7140) Signed-off-by: kosuke55 --- .../util.hpp | 6 +++ .../src/shift_pull_over.cpp | 44 ++++++++++++++----- .../src/util.cpp | 13 ++++++ 3 files changed, 52 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index b6f671cfc94f8..be3dd9ba96e38 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -41,6 +42,7 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using Polygon2d = tier4_autoware_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, @@ -85,6 +87,10 @@ PathWithLaneId extendPath( const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, const Pose & extend_pose); +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width); + // debug MarkerArray createPullOverAreaMarkerArray( const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 8fba7f760ab9d..376adb178708a 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -258,17 +258,39 @@ std::optional ShiftPullOver::generatePullOverPath( road_lane_reference_path_to_shift_end.points.back().point.pose); pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); - // check if the parking path will leave lanes - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); - if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { + // check if the parking path will leave drivable area and lanes + const bool is_in_parking_lots = std::invoke([&]() -> bool { + const auto & p = planner_data_->parameters; + const auto parking_lot_polygons = + lanelet::utils::query::getAllParkingLots(planner_data_->route_handler->getLaneletMapPtr()); + const auto path_footprints = goal_planner_utils::createPathFootPrints( + pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width); + const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { + return std::any_of( + parking_lot_polygons.begin(), parking_lot_polygons.end(), + [&footprint](const auto & polygon) { + return lanelet::geometry::within(footprint, lanelet::utils::to2D(polygon).basicPolygon()); + }); + }; + return std::all_of( + path_footprints.begin(), path_footprints.end(), + [&is_footprint_in_any_polygon](const auto & footprint) { + return is_footprint_in_any_polygon(footprint); + }); + }); + const bool is_in_lanes = std::invoke([&]() -> bool { + const auto drivable_lanes = + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto expanded_lanes = utils::expandLanelets( + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); + return !lane_departure_checker_.checkPathWillLeaveLane( + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); + }); + if (!is_in_parking_lots && !is_in_lanes) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 72ae0143783ea..e94ab74a3bbed 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -433,4 +433,17 @@ PathWithLaneId extendPath( return extendPath(target_path, reference_path, extend_distance); } +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width) +{ + std::vector footprints; + for (const auto & point : path.points) { + const auto & pose = point.point.pose; + footprints.push_back( + tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + } + return footprints; +} + } // namespace behavior_path_planner::goal_planner_utils From c696905fec4e3053e33ca149926e30ffd21f4b7f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 28 May 2024 22:24:34 +0900 Subject: [PATCH 045/120] docs(common): adding .pages file (#7148) * docs(common): adding .pages file Signed-off-by: Muhammad Zulfaqar Azmi * fix naming Signed-off-by: Muhammad Zulfaqar Azmi * fix naming Signed-off-by: Muhammad Zulfaqar Azmi * fix naming Signed-off-by: Muhammad Zulfaqar Azmi * include main page plus explanation to autoware tools Signed-off-by: Muhammad Zulfaqar Azmi * style(pre-commit): autofix --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/.pages | 62 +++++++++++++++++++ common/README.md | 16 +++++ common/autoware_point_types/README.md | 1 + .../Readme.md | 2 +- 4 files changed, 80 insertions(+), 1 deletion(-) create mode 100644 common/.pages create mode 100644 common/README.md create mode 100644 common/autoware_point_types/README.md diff --git a/common/.pages b/common/.pages new file mode 100644 index 0000000000000..f1ecfc71f96b6 --- /dev/null +++ b/common/.pages @@ -0,0 +1,62 @@ +nav: + - 'Introduction': common + - 'Testing Libraries': + - 'autoware_testing': common/autoware_testing/design/autoware_testing-design + - 'fake_test_node': common/fake_test_node/design/fake_test_node-design + - 'Common Libraries': + - 'autoware_auto_common': + - 'comparisons': common/autoware_auto_common/design/comparisons + - 'autoware_auto_geometry': + - 'interval': common/autoware_auto_geometry/design/interval + - 'polygon intersection 2d': common/autoware_auto_geometry/design/polygon_intersection_2d-design + - 'spatial hash': common/autoware_auto_geometry/design/spatial-hash-design + - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design + - 'autoware_point_types': common/autoware_point_types + - 'Cuda Utils': common/cuda_utils + - 'Geography Utils': common/geography_utils + - 'Global Parameter Loader': common/global_parameter_loader/Readme + - 'Glog Component': common/glog_component + - 'grid_map_utils': common/grid_map_utils + - 'interpolation': common/interpolation + - 'Kalman Filter': common/kalman_filter + - 'Motion Utils': common/motion_utils + - 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle + - 'Object Recognition Utils': common/object_recognition_utils + - 'OSQP Interface': common/osqp_interface/design/osqp_interface-design + - 'Perception Utils': common/perception_utils + - 'QP Interface': common/qp_interface/design/qp_interface-design + - 'Signal Processing': + - 'Introduction': common/signal_processing + - 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter + - 'TensorRT Common': common/tensorrt_common + - 'tier4_autoware_utils': common/tier4_autoware_utils + - 'traffic_light_utils': common/traffic_light_utils + - 'TVM Utility': + - 'Introduction': common/tvm_utility + - 'YOLOv2 Tiny Example': common/tvm_utility/tvm-utility-yolo-v2-tiny-tests + - 'RVIZ2 Plugins': + - 'autoware_auto_perception_rviz_plugin': common/autoware_auto_perception_rviz_plugin + - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin + - 'autoware_mission_details_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin + - 'bag_time_manager_rviz_plugin': common/bag_time_manager_rviz_plugin + - 'polar_grid': common/polar_grid/Readme + - 'tier4_adapi_rviz_plugin': common/tier4_adapi_rviz_plugin + - 'tier4_api_utils': common/tier4_api_utils + - 'tier4_camera_view_rviz_plugin': common/tier4_camera_view_rviz_plugin + - 'tier4_datetime_rviz_plugin': common/tier4_datetime_rviz_plugin + - 'tier4_localization_rviz_plugin': common/tier4_localization_rviz_plugin + - 'tier4_perception_rviz_plugin': common/tier4_perception_rviz_plugin + - 'tier4_planning_rviz_plugin': common/tier4_planning_rviz_plugin + - 'tier4_state_rviz_plugin': common/tier4_state_rviz_plugin + - 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin + - 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin + - 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin + - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme + - 'Node': + - 'Goal Distance Calculator': common/goal_distance_calculator/Readme + - 'Path Distance Calculator': common/path_distance_calculator/Readme + - 'Others': + - 'autoware_ad_api_specs': common/autoware_ad_api_specs + - 'component_interface_specs': common/component_interface_specs + - 'component_interface_tools': common/component_interface_tools + - 'component_interface_utils': common/component_interface_utils diff --git a/common/README.md b/common/README.md new file mode 100644 index 0000000000000..95b8973a66b2b --- /dev/null +++ b/common/README.md @@ -0,0 +1,16 @@ +# Common + +## Getting Started + +The Autoware.Universe Common folder consists of common and testing libraries that are used by other Autoware components, as well as useful plugins for visualization in RVIZ2. + +!!! note + + In addition to the ones listed in this folder, users can also have a look at some of the add-ons in the `autoware_tools/common` documentation [page](https://autowarefoundation.github.io/autoware_tools/main/common/mission_planner_rviz_plugin/). + +## Highlights + +Some of the commonly used libraries are: + +1. `tier4_autoware_utils` +2. `motion_utils` diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md new file mode 100644 index 0000000000000..92f19d2bc353a --- /dev/null +++ b/common/autoware_point_types/README.md @@ -0,0 +1 @@ +# Autoware Point Types diff --git a/common/traffic_light_recognition_marker_publisher/Readme.md b/common/traffic_light_recognition_marker_publisher/Readme.md index 1cafc619ff6fb..6a51499c4e5da 100644 --- a/common/traffic_light_recognition_marker_publisher/Readme.md +++ b/common/traffic_light_recognition_marker_publisher/Readme.md @@ -1,4 +1,4 @@ -# path_distance_calculator +# Traffic Light Recognition Marker Publisher ## Purpose From be9a2b80d16ab1e45e7888ff29c168bf04233f0d Mon Sep 17 00:00:00 2001 From: Samrat Thapa <38401989+SamratThapa120@users.noreply.github.com> Date: Wed, 29 May 2024 13:51:48 +0900 Subject: [PATCH 046/120] test(image_projection_based_fusion): add test for image_projection_based_fusion's IoU calculation functions (#7087) * added test for geometry utils iou calculation Signed-off-by: samrat.thapa * style(pre-commit): autofix * fix pre-commit ci Signed-off-by: samrat.thapa * fixed spelling errors Signed-off-by: samrat.thapa --------- Signed-off-by: samrat.thapa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 3 + .../test/test_calc_iou_functions.cpp | 122 ++++++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 7baed86088811..825c4888ac7ea 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -162,6 +162,9 @@ endif() if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_calc_iou_functions + test/test_calc_iou_functions.cpp + ) ament_auto_add_gtest(test_utils test/test_utils.cpp ) diff --git a/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp b/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp new file mode 100644 index 0000000000000..1cddef44c0bac --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/utils/geometry.hpp" + +#include + +using image_projection_based_fusion::calcIoU; +using image_projection_based_fusion::calcIoUX; +using image_projection_based_fusion::calcIoUY; + +TEST(GeometryTest, CalcIoU) +{ + sensor_msgs::msg::RegionOfInterest roi1, roi2; + + // Overlapping ROIs + roi1.x_offset = 0; + roi1.y_offset = 0; + roi1.width = 4; + roi1.height = 4; + + roi2.x_offset = 2; + roi2.y_offset = 2; + roi2.width = 4; + roi2.height = 4; + + double iou = calcIoU(roi1, roi2); + EXPECT_NEAR(iou, 1.0 / 7.0, 1e-6); + + // Non-overlapping ROIs + roi2.x_offset = 5; + roi2.y_offset = 5; + + iou = calcIoU(roi1, roi2); + EXPECT_EQ(iou, 0.0); + + // Zero area ROI + roi1.width = 0; + roi1.height = 0; + + iou = calcIoU(roi1, roi2); + EXPECT_EQ(iou, 0.0); +} + +TEST(GeometryTest, CalcIoUX) +{ + sensor_msgs::msg::RegionOfInterest roi1, roi2; + + // Overlapping ROIs on x-axis + roi1.x_offset = 0; + roi1.y_offset = 0; + roi1.width = 4; + roi1.height = 4; + + roi2.x_offset = 2; + roi2.y_offset = 0; + roi2.width = 4; + roi2.height = 4; + + double iou_x = calcIoUX(roi1, roi2); + EXPECT_NEAR(iou_x, 2.0 / 6.0, 1e-6); + + // Non-overlapping ROIs on x-axis + roi2.x_offset = 5; + + iou_x = calcIoUX(roi1, roi2); + EXPECT_EQ(iou_x, 0.0); + + // Zero width ROI + roi1.width = 0; + + iou_x = calcIoUX(roi1, roi2); + EXPECT_EQ(iou_x, 0.0); +} + +TEST(GeometryTest, CalcIoUY) +{ + sensor_msgs::msg::RegionOfInterest roi1, roi2; + + // Overlapping ROIs on y-axis + roi1.x_offset = 0; + roi1.y_offset = 0; + roi1.width = 4; + roi1.height = 4; + + roi2.x_offset = 0; + roi2.y_offset = 2; + roi2.width = 4; + roi2.height = 4; + + double iou_y = calcIoUY(roi1, roi2); + EXPECT_NEAR(iou_y, 2.0 / 6.0, 1e-6); + + // Non-overlapping ROIs on y-axis + roi2.y_offset = 5; + + iou_y = calcIoUY(roi1, roi2); + EXPECT_EQ(iou_y, 0.0); + + // Zero height ROI + roi1.height = 0; + + iou_y = calcIoUY(roi1, roi2); + EXPECT_EQ(iou_y, 0.0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 57d6df8c03c9350f7ef8b380917cb04f957bca33 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 29 May 2024 13:53:25 +0900 Subject: [PATCH 047/120] feat(behavior_velocity_blind_spot): use TTC for object in detection_area (#7146) Signed-off-by: Mamoru Sobue --- .../config/blind_spot.param.yaml | 8 +- .../src/debug.cpp | 15 +- .../src/manager.cpp | 13 +- .../src/scene.cpp | 287 ++++++++---------- .../src/scene.hpp | 61 ++-- 5 files changed, 170 insertions(+), 214 deletions(-) diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3c22d1fe65db5..d7131d8c19245 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -3,10 +3,12 @@ blind_spot: use_pass_judge_line: true stop_line_margin: 1.0 # [m] - backward_length: 50.0 # [m] + backward_detection_length: 100.0 # [m] ignore_width_from_center_line: 0.7 # [m] - max_future_movement_time: 10.0 # [second] - threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] opposite_adjacent_extend_width: 1.5 # [m] + max_future_movement_time: 10.0 # [second] + ttc_min: -5.0 # [s] + ttc_max: 5.0 # [s] + ttc_ego_minimal_velocity: 5.0 # [m/s] enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index b733a1ea1e79c..a30836ab9f6a9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -93,15 +93,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() const auto now = this->clock_->now(); - appendMarkerArray( - createLaneletPolygonsMarkerArray( - debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), - &debug_marker_array, now); - - appendMarkerArray( - createLaneletPolygonsMarkerArray( - debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0), - &debug_marker_array, now); + if (debug_data_.detection_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0), + &debug_marker_array, now); + } appendMarkerArray( debug::createObjectsMarkerArray( diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 0843199cac089..07742a1217ab3 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -36,17 +36,20 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) planner_param_.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - planner_param_.backward_length = getOrDeclareParameter(node, ns + ".backward_length"); + planner_param_.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); planner_param_.ignore_width_from_center_line = getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - planner_param_.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - planner_param_.threshold_yaw_diff = - getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); planner_param_.opposite_adjacent_extend_width = getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + planner_param_.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + planner_param_.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index c591a240029ff..6c7c12b556581 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -67,6 +67,9 @@ void BlindSpotModule::initializeRTCStatus() BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { + if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) { + return OverPassJudge{"already over the pass judge line. no plan needed."}; + } const auto & input_path = *path; /* set stop-line and stop-judgement-line for base_link */ @@ -98,16 +101,36 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose); if (is_over_pass_judge) { + is_over_pass_judge_line_ = true; return is_over_pass_judge.value(); } - const auto areas_opt = generateBlindSpotPolygons(input_path, closest_idx, stop_line_pose); - if (!areas_opt) { + if (!blind_spot_lanelets_) { + const auto blind_spot_lanelets = generateBlindSpotLanelets(input_path); + if (!blind_spot_lanelets.empty()) { + blind_spot_lanelets_ = blind_spot_lanelets; + } + } + if (!blind_spot_lanelets_) { + return InternalError{"There are not blind_spot_lanelets"}; + } + const auto & blind_spot_lanelets = blind_spot_lanelets_.value(); + + const auto detection_area_opt = generateBlindSpotPolygons( + input_path, closest_idx, blind_spot_lanelets, + path->points.at(critical_stopline_idx).point.pose); + if (!detection_area_opt) { return InternalError{"Failed to generate BlindSpotPolygons"}; } + const auto & detection_area = detection_area_opt.value(); + debug_data_.detection_area = detection_area; + const auto ego_time_to_reach_stop_line = computeTimeToPassStopLine( + blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose); /* calculate dynamic collision around detection area */ - const auto collision_obstacle = isCollisionDetected(areas_opt.value()); + const auto collision_obstacle = isCollisionDetected( + blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose, detection_area, + ego_time_to_reach_stop_line); state_machine_.setStateWithMarginTime( collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); @@ -365,27 +388,28 @@ std::optional> BlindSpotModule::generateStopLine( return std::make_pair(stopline_idx_default_opt.value(), stopline_idx_critical_opt.value()); } -void BlindSpotModule::cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const +autoware_auto_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::PredictedObject & object_original, + const double time_thr) const { + auto object = object_original; const rclcpp::Time current_time = clock_->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } + for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = rclcpp::Time(header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); } } } + return object; } std::optional BlindSpotModule::isOverPassJudge( @@ -419,86 +443,61 @@ std::optional BlindSpotModule::isOverPassJudge( return std::nullopt; } -std::optional -BlindSpotModule::isCollisionDetected(const BlindSpotPolygons & areas) +double BlindSpotModule::computeTimeToPassStopLine( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const { - // TODO(Mamoru Sobue): only do this for target object - autoware_auto_perception_msgs::msg::PredictedObjects objects = - *(planner_data_->predicted_objects); - cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); + // egoが停止している時にそのまま速度を使うと衝突しなくなってしまうのでegoについては最低速度を使う + const auto & current_pose = planner_data_->current_odometry->pose; + const auto current_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, current_pose).length; + const auto stopline_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + const auto remaining_distance = stopline_arc_ego - current_arc_ego; + return remaining_distance / std::max( + planner_param_.ttc_ego_minimal_velocity, + planner_data_->current_velocity->twist.linear.x); +} +std::optional +BlindSpotModule::isCollisionDetected( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, + const double ego_time_to_reach_stop_line) +{ // check objects in blind spot areas - for (const auto & object : objects.objects) { - if (!isTargetObjectType(object)) { + const auto stop_line_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + for (const auto & original_object : planner_data_->predicted_objects->objects) { + if (!isTargetObjectType(original_object)) { continue; } - - const auto & detection_areas = areas.detection_areas; - const auto & conflict_areas = areas.conflict_areas; - const auto & opposite_detection_areas = areas.opposite_detection_areas; - const auto & opposite_conflict_areas = areas.opposite_conflict_areas; - + const auto object = cutPredictPathWithDuration( + planner_data_->predicted_objects->header, original_object, + planner_param_.max_future_movement_time); // right direction - const bool exist_in_right_detection_area = - std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - // opposite direction - const bool exist_in_opposite_detection_area = std::any_of( - opposite_detection_areas.begin(), opposite_detection_areas.end(), - [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - const bool exist_in_detection_area = - exist_in_right_detection_area || exist_in_opposite_detection_area; + const bool exist_in_detection_area = bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); if (!exist_in_detection_area) { continue; } - const bool exist_in_right_conflict_area = - isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_opposite_conflict_area = - isPredictedPathInArea(object, opposite_conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_conflict_area = - exist_in_right_conflict_area || exist_in_opposite_conflict_area; - if (exist_in_detection_area && exist_in_conflict_area) { - debug_data_.conflicting_targets.objects.push_back(object); - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); + const auto object_arc_length = + lanelet::utils::getArcCoordinates( + blind_spot_lanelets, object.kinematics.initial_pose_with_covariance.pose) + .length; + const auto object_time_to_reach_stop_line = + (object_arc_length - stop_line_arc_ego) / + (object.kinematics.initial_twist_with_covariance.twist.linear.x); + const auto ttc = ego_time_to_reach_stop_line - object_time_to_reach_stop_line; + RCLCPP_INFO(logger_, "object ttc is %f", ttc); + if (planner_param_.ttc_min < ttc && ttc < planner_param_.ttc_max) { return object; } } return std::nullopt; } -bool BlindSpotModule::isPredictedPathInArea( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const std::vector & areas, geometry_msgs::msg::Pose ego_pose) const -{ - const auto ego_yaw = tf2::getYaw(ego_pose.orientation); - const auto threshold_yaw_diff = planner_param_.threshold_yaw_diff; - // NOTE: iterating all paths including those of low confidence - return std::any_of( - areas.begin(), areas.end(), [&object, &ego_yaw, &threshold_yaw_diff](const auto & area) { - const auto area_2d = lanelet::utils::to2D(area); - return std::any_of( - object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(), - [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & path) { - return std::any_of( - path.path.begin(), path.path.end(), - [&area_2d, &ego_yaw, &threshold_yaw_diff](const auto & point) { - const auto is_in_area = bg::within(to_bg2d(point.position), area_2d); - const auto match_yaw = - std::fabs(ego_yaw - tf2::getYaw(point.orientation)) < threshold_yaw_diff; - return is_in_area && match_yaw; - }); - }); - }); -} - lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( const lanelet::ConstLanelet lanelet) const { @@ -523,8 +522,6 @@ lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); auto half_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); - const auto centerline = lanelet::utils::generateFineCenterline(half_lanelet, 5.0); - half_lanelet.setCenterline(centerline); return half_lanelet; } @@ -589,17 +586,23 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( return new_lanelet; } -std::optional BlindSpotModule::generateBlindSpotPolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const geometry_msgs::msg::Pose & stop_line_pose) const +static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) +{ + lanelet::Points3d pts; + for (const auto & pt : line) { + pts.push_back(lanelet::Point3d(pt)); + } + return lanelet::LineString3d(lanelet::InvalId, pts); +} + +lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const { /* get lanelet map */ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); std::vector lane_ids; - lanelet::ConstLanelets blind_spot_lanelets; - lanelet::ConstLanelets blind_spot_opposite_lanelets; /* get lane ids until intersection */ for (const auto & point : path.points) { bool found_intersection_lane = false; @@ -617,17 +620,10 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( if (found_intersection_lane) break; } - for (size_t i = 0; i < lane_ids.size(); ++i) { - const auto half_lanelet = - generateHalfLanelet(lanelet_map_ptr->laneletLayer.get(lane_ids.at(i))); - blind_spot_lanelets.push_back(half_lanelet); - } - - // additional detection area on left/right side - lanelet::ConstLanelets adjacent_lanelets; - lanelet::ConstLanelets opposite_adjacent_lanelets; + lanelet::ConstLanelets blind_spot_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); + const auto ego_half_lanelet = generateHalfLanelet(lane); const auto adj = turn_direction_ == TurnDirection::LEFT ? (routing_graph_ptr->adjacentLeft(lane)) @@ -659,75 +655,42 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( return std::nullopt; } }(); - if (adj) { - const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); - adjacent_lanelets.push_back(half_lanelet); - } - if (opposite_adj) { - const auto half_lanelet = + + if (!adj && !opposite_adj) { + blind_spot_lanelets.push_back(ego_half_lanelet); + } else if (!!adj) { + const auto adj_half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); + const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::RIGHT) ? adj_half_lanelet.rightBound() + : ego_half_lanelet.rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + } else if (opposite_adj) { + const auto adj_half_lanelet = generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); - opposite_adjacent_lanelets.push_back(half_lanelet); + const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::LEFT) ? ego_half_lanelet.rightBound() + : adj_half_lanelet.rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); } } + return blind_spot_lanelets; +} - const auto current_arc_ego = - lanelet::utils::getArcCoordinates(blind_spot_lanelets, path.points[closest_idx].point.pose) - .length; +std::optional BlindSpotModule::generateBlindSpotPolygons( + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const +{ const auto stop_line_arc_ego = lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; - const auto detection_area_start_length_ego = stop_line_arc_ego - planner_param_.backward_length; - if (detection_area_start_length_ego < current_arc_ego && current_arc_ego < stop_line_arc_ego) { - BlindSpotPolygons blind_spot_polygons; - auto conflict_area_ego = lanelet::utils::getPolygonFromArcLength( - blind_spot_lanelets, current_arc_ego, stop_line_arc_ego); - auto detection_area_ego = lanelet::utils::getPolygonFromArcLength( - blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego); - blind_spot_polygons.conflict_areas.emplace_back(std::move(conflict_area_ego)); - blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_ego)); - // additional detection area on left/right side - if (!adjacent_lanelets.empty()) { - const auto stop_line_arc_adj = lanelet::utils::getLaneletLength3d(adjacent_lanelets); - const auto current_arc_adj = stop_line_arc_adj - (stop_line_arc_ego - current_arc_ego); - const auto detection_area_start_length_adj = - stop_line_arc_adj - planner_param_.backward_length; - auto conflicting_area_adj = lanelet::utils::getPolygonFromArcLength( - adjacent_lanelets, current_arc_adj, stop_line_arc_adj); - auto detection_area_adj = lanelet::utils::getPolygonFromArcLength( - adjacent_lanelets, detection_area_start_length_adj, stop_line_arc_adj); - blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj)); - blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj)); - } - // additional detection area on left/right opposite lane side - if (!opposite_adjacent_lanelets.empty()) { - const auto stop_line_arc_opposite_adj = - lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets); - const auto current_arc_opposite_adj = - stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego); - const auto detection_area_start_length_opposite_adj = - stop_line_arc_opposite_adj - planner_param_.backward_length; - auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( - opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj); - auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( - opposite_adjacent_lanelets, detection_area_start_length_opposite_adj, - stop_line_arc_opposite_adj); - blind_spot_polygons.opposite_conflict_areas.emplace_back( - std::move(conflicting_area_opposite_adj)); - blind_spot_polygons.opposite_detection_areas.emplace_back( - std::move(detection_area_opposite_adj)); - } - debug_data_.detection_areas = blind_spot_polygons.detection_areas; - debug_data_.conflict_areas = blind_spot_polygons.conflict_areas; - debug_data_.detection_areas.insert( - debug_data_.detection_areas.end(), blind_spot_polygons.opposite_detection_areas.begin(), - blind_spot_polygons.opposite_detection_areas.end()); - debug_data_.conflict_areas.insert( - debug_data_.conflict_areas.end(), blind_spot_polygons.opposite_conflict_areas.begin(), - blind_spot_polygons.opposite_conflict_areas.end()); - - return blind_spot_polygons; - } else { - return std::nullopt; - } + const auto detection_area_start_length_ego = + std::max(stop_line_arc_ego - planner_param_.backward_detection_length, 0.0); + return lanelet::utils::getPolygonFromArcLength( + blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego); } bool BlindSpotModule::isTargetObjectType( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 2f4ccc37ec51e..709942794ec1e 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -36,14 +36,6 @@ namespace behavior_velocity_planner { -struct BlindSpotPolygons -{ - std::vector conflict_areas; - std::vector detection_areas; - std::vector opposite_conflict_areas; - std::vector opposite_detection_areas; -}; - /** * @brief wrapper class of interpolated path with lane id */ @@ -93,26 +85,23 @@ class BlindSpotModule : public SceneModuleInterface struct DebugData { std::optional virtual_wall_pose{std::nullopt}; - std::vector conflict_areas; - std::vector detection_areas; - std::vector opposite_conflict_areas; - std::vector opposite_detection_areas; + std::optional detection_area; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; public: struct PlannerParam { - bool use_pass_judge_line; //! distance which ego can stop with max brake - double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary - double backward_length; //! distance[m] from closest path point to the edge of beginning point - double ignore_width_from_center_line; //! ignore width from center line from detection_area - double - max_future_movement_time; //! maximum time[second] for considering future movement of object - double threshold_yaw_diff; //! threshold of yaw difference between ego and target object - double - adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane + bool use_pass_judge_line; + double stop_line_margin; + double backward_detection_length; + double ignore_width_from_center_line; + double adjacent_extend_width; double opposite_adjacent_extend_width; + double max_future_movement_time; + double ttc_min; + double ttc_max; + double ttc_ego_minimal_velocity; }; BlindSpotModule( @@ -135,6 +124,7 @@ class BlindSpotModule : public SceneModuleInterface const PlannerParam planner_param_; const TurnDirection turn_direction_; std::optional sibling_straight_lanelet_{std::nullopt}; + std::optional blind_spot_lanelets_{std::nullopt}; // state variables bool is_over_pass_judge_line_{false}; @@ -181,6 +171,10 @@ class BlindSpotModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & stop_point_pose) const; + double computeTimeToPassStopLine( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const; + /** * @brief Check obstacle is in blind spot areas. * Condition1: Object's position is in broad blind spot area. @@ -192,7 +186,9 @@ class BlindSpotModule : public SceneModuleInterface * @return true when an object is detected in blind spot */ std::optional isCollisionDetected( - const BlindSpotPolygons & area); + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, + const double ego_time_to_reach_stop_line); /** * @brief Create half lanelet @@ -206,6 +202,9 @@ class BlindSpotModule : public SceneModuleInterface lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelets generateBlindSpotLanelets( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const; + /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. * Broad area is made from backward expanded point to stop line point @@ -213,8 +212,9 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return Blind spot polygons */ - std::optional generateBlindSpotPolygons( + std::optional generateBlindSpotPolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & pose) const; /** @@ -224,23 +224,14 @@ class BlindSpotModule : public SceneModuleInterface */ bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - /** - * @brief Check if at least one of object's predicted position is in area - * @param object Dynamic object - * @param area Area defined by polygon - * @return True when at least one of object's predicted position is in area - */ - bool isPredictedPathInArea( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const std::vector & areas, geometry_msgs::msg::Pose ego_pose) const; - /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. * @param objects_ptr target objects * @param time_thr time threshold to cut path */ - void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, + autoware_auto_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( + const std_msgs::msg::Header & header, + const autoware_auto_perception_msgs::msg::PredictedObject & object, const double time_thr) const; StateMachine state_machine_; //! for state From 8237b7d15f90164c9351db8fe8651ff4ecb75f23 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 29 May 2024 16:18:51 +0900 Subject: [PATCH 048/120] fix(multi_object_tracker): input stream timimg functions (#7127) * fix: rename to latest_exported_object_time_ Signed-off-by: Taekjin LEE * fix: add time range checks Signed-off-by: Taekjin LEE * chore: add comments, refactoring Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 16 ++-- .../src/processor/input_manager.cpp | 74 ++++++++++++------- 2 files changed, 58 insertions(+), 32 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index ad8c4ae6e5e24..f46c088c6d508 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -117,17 +117,13 @@ class InputManager { public: explicit InputManager(rclcpp::Node & node); - void init(const std::vector & input_channels); - void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } void onTrigger(const uint & index) const; - void getObjectTimeInterval( - const rclcpp::Time & now, rclcpp::Time & object_latest_time, - rclcpp::Time & object_oldest_time) const; - void optimizeTimings(); bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); + bool isChannelSpawnEnabled(const uint & index) const { return input_streams_[index]->isSpawnEnabled(); @@ -138,7 +134,7 @@ class InputManager std::vector::SharedPtr> sub_objects_array_{}; bool is_initialized_{false}; - rclcpp::Time latest_object_time_; + rclcpp::Time latest_exported_object_time_; size_t input_size_; std::vector> input_streams_; @@ -151,6 +147,12 @@ class InputManager double target_stream_interval_std_{0.02}; // [s] double target_latency_{0.2}; // [s] double target_latency_band_{1.0}; // [s] + +private: + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const; + void optimizeTimings(); }; } // namespace multi_object_tracker diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index e29b54e971491..44bdf09046ccd 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Update time latest_message_time_ = now; constexpr double delay_threshold = 3.0; // [s] - if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) { - // Reset the latest measurement time if the time difference is too large + if (objects_time < latest_measurement_time_ - rclcpp::Duration::from_seconds(delay_threshold)) { + // If the given object time is older than the latest measurement time by more than the + // threshold, the system time may have been reset. Reset the latest measurement time latest_measurement_time_ = objects_time; RCLCPP_WARN( node_.get_logger(), "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f", long_name_.c_str(), objects_time.seconds()); } else { - // Aware reversed message arrival + // Update only if the object time is newer than the latest measurement time latest_measurement_time_ = latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; } @@ -150,7 +151,14 @@ void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, ObjectsList & objects_list) { - assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); + if (object_latest_time < object_oldest_time) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::getObjectsOlderThan %s: Invalid object time interval, object_latest_time: %f, " + "object_oldest_time: %f", + long_name_.c_str(), object_latest_time.seconds(), object_oldest_time.seconds()); + return; + } for (const auto & object : objects_que_) { const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); @@ -176,7 +184,7 @@ void InputStream::getObjectsOlderThan( //////////////////////////// InputManager::InputManager(rclcpp::Node & node) : node_(node) { - latest_object_time_ = node_.now(); + latest_exported_object_time_ = node_.now() - rclcpp::Duration::from_seconds(3.0); } void InputManager::init(const std::vector & input_channels) @@ -230,26 +238,37 @@ void InputManager::getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) const { + // Set the object time interval + + // 1. object_latest_time + // The object_latest_time is the current time minus the target stream latency object_latest_time = - now - rclcpp::Duration::from_seconds( - target_stream_latency_ - - 0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin + now - rclcpp::Duration::from_seconds(target_stream_latency_ - 0.1 * target_stream_latency_std_); + // check the target stream can be included in the object time interval if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) { - rclcpp::Time latest_measurement_time = + const rclcpp::Time latest_measurement_time = input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); - - // if the object_latest_time is older than the latest measurement time, set it as the latest + // if the object_latest_time is older than the latest measurement time, set it to the latest // object time object_latest_time = object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; } - object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); - // if the object_oldest_time is older than the latest object time, set it to the latest object - // time - object_oldest_time = - object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; + // 2. object_oldest_time + // The default object_oldest_time is to have a 1-second time interval + const rclcpp::Time object_oldest_time_default = + object_latest_time - rclcpp::Duration::from_seconds(1.0); + if (latest_exported_object_time_ < object_oldest_time_default) { + // if the latest exported object time is too old, set to the default + object_oldest_time = object_oldest_time_default; + } else if (latest_exported_object_time_ > object_latest_time) { + // if the latest exported object time is newer than the object_latest_time, set to the default + object_oldest_time = object_oldest_time_default; + } else { + // The object_oldest_time is the latest exported object time + object_oldest_time = latest_exported_object_time_; + } } void InputManager::optimizeTimings() @@ -298,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li objects_list.clear(); // Get the time interval for the objects - rclcpp::Time object_latest_time, object_oldest_time; + rclcpp::Time object_latest_time; + rclcpp::Time object_oldest_time; getObjectTimeInterval(now, object_latest_time, object_oldest_time); // Optimize the target stream, latency, and its band @@ -320,20 +340,24 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li 0; }); - // Update the latest object time + // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } else { - // check time jump - if (now < latest_object_time_) { + // check time jump back + if (now < latest_exported_object_time_) { RCLCPP_WARN( node_.get_logger(), - "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f", - now.seconds(), latest_object_time_.seconds()); - latest_object_time_ = - now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago + "InputManager::getObjects Detected jump back in time, now: %f, " + "latest_exported_object_time_: %f", + now.seconds(), latest_exported_object_time_.seconds()); + // reset the latest exported object time to 3 seconds ago, + const rclcpp::Time latest_exported_object_time_default = + now - rclcpp::Duration::from_seconds(3.0); + latest_exported_object_time_ = latest_exported_object_time_default; } else { + // No objects in the object list, no update for the latest exported object time RCLCPP_DEBUG( node_.get_logger(), "InputManager::getObjects No objects in the object list, object time band from %f to %f", From b7fb01c2d85d7928a9a882e8a3fce27d9939c306 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Wed, 29 May 2024 18:03:52 +0900 Subject: [PATCH 049/120] feat(default_ad_api): release adapi v1.3.0 (#7139) * feat(default_ad_api): release adapi v1.3.0 Signed-off-by: Takagi, Isamu * fix qos for diagnostics Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/diagnostics.cpp | 2 +- system/default_ad_api/src/interface.cpp | 2 +- system/default_ad_api/test/node/interface_version.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp index f4e58b5a4d441..6eaaa5bf614a6 100644 --- a/system/default_ad_api/src/diagnostics.cpp +++ b/system/default_ad_api/src/diagnostics.cpp @@ -27,7 +27,7 @@ DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("di pub_struct_ = create_publisher( "/api/system/diagnostics/struct", rclcpp::QoS(1).transient_local()); pub_status_ = create_publisher( - "/api/system/diagnostics/status", rclcpp::QoS(1)); + "/api/system/diagnostics/status", rclcpp::QoS(1).best_effort()); sub_graph_.register_create_callback(std::bind(&DiagnosticsNode::on_create, this, _1)); sub_graph_.register_update_callback(std::bind(&DiagnosticsNode::on_update, this, _1)); diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index 506a4aa8bf5eb..d53d12afcc499 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 2; + res->minor = 3; res->patch = 0; }; diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index ccfa3e1101185..63a79e8722be3 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 2: +if response.minor != 3: exit(1) if response.patch != 0: exit(1) From c95e1f4ab9a9ff4e4ecce62838f1a5f25cd009af Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 30 May 2024 00:14:21 +0900 Subject: [PATCH 050/120] chore(pointcloud_preprocessor): add maintainers to pointcloud preprocessor (#7163) add maintainer to pointcloud preprocessor Signed-off-by: vividf --- sensing/pointcloud_preprocessor/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 47fa39e7fcda2..619bf3180591d 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -12,6 +12,8 @@ Kenzo Lobos-Tsunekawa Yihsiang Fang Yoshi Ri + David Wong + Melike Tanrikulu Apache License 2.0 From b93f173b2c758fb7fee6e91eee04d0db27f376f2 Mon Sep 17 00:00:00 2001 From: Yukihiro Saito Date: Thu, 30 May 2024 10:01:00 +0900 Subject: [PATCH 051/120] fix(tier4_planning_rviz_plugin): memory leak (#7164) fix memory leak Signed-off-by: Yukihito Saito --- .../tier4_planning_rviz_plugin/path/display_base.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 4a59006e3bb96..c915e38977012 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -316,6 +316,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = velocity_texts_.at(i); + delete text; } velocity_texts_.resize(msg_ptr->points.size()); velocity_text_nodes_.resize(msg_ptr->points.size()); @@ -339,6 +342,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = slope_texts_.at(i); + delete text; } slope_texts_.resize(msg_ptr->points.size()); slope_text_nodes_.resize(msg_ptr->points.size()); From 1b5a6b21907230f4ddcea5f8985f81883f742ea1 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Thu, 30 May 2024 14:28:49 +0900 Subject: [PATCH 052/120] feat(object_lanelet_filter): add velocity direction based object lanelet filter (#7107) * chore: refactor lanelet filter function Signed-off-by: yoshiri * feat: add lanelet direction filter Signed-off-by: yoshiri * fix: do not change default settings Signed-off-by: yoshiri * feat: skip orientation unavailable objects Signed-off-by: yoshiri * fix: feature variable name Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../config/object_lanelet_filter.param.yaml | 10 ++ .../object_lanelet_filter.hpp | 19 ++- .../src/object_lanelet_filter.cpp | 122 +++++++++++++++--- 3 files changed, 129 insertions(+), 22 deletions(-) diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml index dfdee95642fed..99050d9738ae6 100644 --- a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -9,3 +9,13 @@ MOTORCYCLE : false BICYCLE : false PEDESTRIAN : false + + filter_settings: + # polygon overlap based filter + polygon_overlap_filter: + enabled: true + # velocity direction based filter + lanelet_direction_filter: + enabled: false + velocity_yaw_threshold: 0.785398 # [rad] (45 deg) + object_speed_threshold: 3.0 # [m/s] diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index f3871aaf98117..00826de68157f 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,6 +17,7 @@ #include "detected_object_validation/utils/utils.hpp" +#include #include #include #include @@ -62,11 +63,27 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; - + struct FilterSettings + { + bool polygon_overlap_filter; + bool lanelet_direction_filter; + double lanelet_direction_filter_velocity_yaw_threshold; + double lanelet_direction_filter_object_speed_threshold; + } filter_settings_; + + bool filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + bool isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 50a81e95d5a9b..fe304a3ea22bb 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + // Set filter settings + filter_settings_.polygon_overlap_filter = + declare_parameter("filter_settings.polygon_overlap_filter.enabled"); + filter_settings_.lanelet_direction_filter = + declare_parameter("filter_settings.lanelet_direction_filter.enabled"); + filter_settings_.lanelet_direction_filter_velocity_yaw_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.velocity_yaw_threshold"); + filter_settings_.lanelet_direction_filter_object_speed_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.object_speed_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -97,27 +106,13 @@ void ObjectLaneletFilterNode::objectCallback( lanelet::ConstLanelets intersected_shoulder_lanelets = getIntersectedLanelets(convex_hull, shoulder_lanelets_); - int index = 0; - for (const auto & object : transformed_objects.objects) { - const auto footprint = setFootprint(object); - const auto & label = object.classification.front().label; - if (filter_target_.isTarget(label)) { - Polygon2d polygon; - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - } else { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - ++index; + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject( + transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, + output_object_msg); } object_pub_->publish(output_object_msg); published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); @@ -132,6 +127,55 @@ void ObjectLaneletFilterNode::objectCallback( "debug/pipeline_latency_ms", pipeline_latency); } +bool ObjectLaneletFilterNode::filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg) +{ + const auto & label = transformed_object.classification.front().label; + if (filter_target_.isTarget(label)) { + bool filter_pass = true; + // 1. is polygon overlap with road lanelets or shoulder lanelets + if (filter_settings_.polygon_overlap_filter) { + Polygon2d polygon; + const auto footprint = setFootprint(transformed_object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint( + point, transformed_object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + const bool is_polygon_overlap = + isPolygonOverlapLanelets(polygon, intersected_road_lanelets) || + isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets); + filter_pass = filter_pass && is_polygon_overlap; + } + + // 2. check if objects velocity is the same with the lanelet direction + const bool orientation_not_available = + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { + const bool is_same_direction = + isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || + isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + filter_pass = filter_pass && is_same_direction; + } + + // push back to output object + if (filter_pass) { + output_object_msg.objects.emplace_back(input_object); + return true; + } + } else { + output_object_msg.objects.emplace_back(input_object); + return true; + } + return false; +} + geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) { @@ -201,6 +245,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( return false; } +bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_velocity_norm = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double object_velocity_yaw = std::atan2( + object.kinematics.twist_with_covariance.twist.linear.y, + object.kinematics.twist_with_covariance.twist.linear.x) + + object_yaw; + + if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { + return true; + } + for (const auto & lanelet : lanelets) { + const bool is_in_lanelet = + lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + if (!is_in_lanelet) { + continue; + } + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_velocity_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { + return true; + } + } + + return false; +} + } // namespace object_lanelet_filter #include From 6e5d597e7b7b47284819d7c8278abe82bcd760d7 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 30 May 2024 14:45:42 +0900 Subject: [PATCH 053/120] feat(behavior_path_planner_common,turn_signal_decider): add turn_signal_remaining_shift_length_threshold (#7160) add turn_signal_remaining_shift_length_threshold Signed-off-by: Daniel Sanchez --- .../config/behavior_path_planner.param.yaml | 1 + .../src/behavior_path_planner_node.cpp | 2 ++ .../behavior_path_planner_turn_signal_design.md | 17 +++++++++-------- .../behavior_path_planner_common/parameters.hpp | 1 + .../src/turn_signal_decider.cpp | 5 +++-- 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 2c181489ba4c2..e5c3a76fd1c45 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,6 +17,7 @@ turn_signal_minimum_search_distance: 10.0 turn_signal_search_time: 3.0 turn_signal_shift_length_threshold: 0.3 + turn_signal_remaining_shift_length_threshold: 0.1 turn_signal_on_swerving: true enable_akima_spline_first: false diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index b56d1a207f76d..f7d204d2bff87 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -260,6 +260,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_search_time = declare_parameter("turn_signal_search_time"); p.turn_signal_shift_length_threshold = declare_parameter("turn_signal_shift_length_threshold"); + p.turn_signal_remaining_shift_length_threshold = + declare_parameter("turn_signal_remaining_shift_length_threshold"); p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving"); p.enable_akima_spline_first = declare_parameter("enable_akima_spline_first"); diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md index bfc22bacc8ede..4302b74dcf2c6 100644 --- a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md @@ -20,14 +20,15 @@ Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in c ## Parameters for turn signal decider -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :--------------------------------------------------------------------------- | :------------ | -| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | -| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | -| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | -| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | -| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | -| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | +| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | +| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | +| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | +| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | +| turn_signal_remaining_shift_length_threshold | [m] | double | When the ego's current shift length minus its end shift length is less than this threshold, the turn signal will be turned off. | 0.1 | +| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | Note that the default values for `turn_signal_intersection_search_distance` and `turn_signal_search_time` is strictly followed by [Japanese Road Traffic Laws](https://www.japaneselawtranslation.go.jp/ja/laws/view/2962). So if your country does not allow to use these default values, you should change these values in configuration files. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 1241f98daa747..dbe27d856bc40 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -54,6 +54,7 @@ struct BehaviorPathPlannerParameters double turn_signal_search_time; double turn_signal_minimum_search_distance; double turn_signal_shift_length_threshold; + double turn_signal_remaining_shift_length_threshold; bool turn_signal_on_swerving; bool enable_akima_spline_first; diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 21937aa76da29..fb15391550980 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -615,7 +615,6 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - constexpr double THRESHOLD = 0.1; const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -674,7 +673,9 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( } // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) { + if ( + std::fabs(end_shift_length - current_shift_length) < + p.turn_signal_remaining_shift_length_threshold) { return std::make_pair(TurnSignalInfo{}, true); } From 3323c21b243d4f63baa83882745d8877de9a9b19 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 30 May 2024 15:58:27 +0900 Subject: [PATCH 054/120] feat(map_based_prediction): use different time horizon (#7157) Signed-off-by: Mamoru Sobue --- .../config/map_based_prediction.param.yaml | 5 +- .../map_based_prediction_node.hpp | 12 +++- .../map_based_prediction/path_generator.hpp | 33 ++++++----- .../schema/map_based_prediction.schema.json | 20 ++++++- .../src/map_based_prediction_node.cpp | 57 ++++++++++-------- .../src/path_generator.cpp | 59 ++++++++++--------- .../test_path_generator.cpp | 39 +++++------- 7 files changed, 125 insertions(+), 100 deletions(-) diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index a1b776bdb6393..a5b7b0ad6be01 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -1,7 +1,10 @@ /**: ros__parameters: enable_delay_compensation: true - prediction_time_horizon: 10.0 #[s] + prediction_time_horizon: + vehicle: 15.0 #[s] + pedestrian: 10.0 #[s] + unknown: 10.0 #[s] lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 213f18d63ef3a..93cdb24f91321 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -106,6 +106,14 @@ struct PredictedRefPath Maneuver maneuver; }; +struct PredictionTimeHorizon +{ + // NOTE(Mamoru Sobue): motorcycle belongs to "vehicle" and bicycle to "pedestrian" + double vehicle; + double pedestrian; + double unknown; +}; + using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; using autoware_auto_mapping_msgs::msg::HADMapBin; @@ -170,7 +178,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; - double prediction_time_horizon_; + PredictionTimeHorizon prediction_time_horizon_; double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; @@ -253,7 +261,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::string & object_id, std::unordered_map & current_users); std::vector getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time); + const double object_detected_time, const double time_horizon); Maneuver predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 5023051da5e71..3c1a5432d3b08 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -80,21 +80,24 @@ using PosePath = std::vector; class PathGenerator { public: - PathGenerator( - const double time_horizon, const double lateral_time_horizon, - const double sampling_time_interval, const double min_crosswalk_user_velocity); + PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); - PredictedPath generatePathForNonVehicleObject(const TrackedObject & object) const; + PredictedPath generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const; - PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; + PredictedPath generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const; - PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object) const; + PredictedPath generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const; PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; PredictedPath generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const; PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; @@ -111,22 +114,21 @@ class PathGenerator private: // Parameters - double time_horizon_; - double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; bool use_vehicle_acceleration_; double acceleration_exponential_half_life_; // Member functions - PredictedPath generateStraightPath(const TrackedObject & object) const; + PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; PredictedPath generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, - const double max_length) const; + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const; Eigen::Vector3d calcLatCoefficients( const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; Eigen::Vector2d calcLonCoefficients( @@ -140,7 +142,8 @@ class PathGenerator const PosePath & ref_path) const; FrenetPoint getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit = 0.0) const; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/perception/map_based_prediction/schema/map_based_prediction.schema.json index 8ddb122ebb56e..8e5ef9e542f54 100644 --- a/perception/map_based_prediction/schema/map_based_prediction.schema.json +++ b/perception/map_based_prediction/schema/map_based_prediction.schema.json @@ -12,9 +12,23 @@ "description": "flag to enable the time delay compensation for the position of the object" }, "prediction_time_horizon": { - "type": "number", - "default": "10.0", - "description": "predict time duration for predicted path" + "properties": { + "vehicle": { + "type": "number", + "default": "15.0", + "description": "predict time duration for predicted path of vehicle" + }, + "pedestrian": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of pedestrian" + }, + "unknown": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of unknown" + } + } }, "lateral_control_time_horizon": { "type": "number", diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index b55ea72402d74..9b527fafb30e9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -766,7 +766,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ google::InstallFailureSignalHandler(); } enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_horizon_.vehicle = declare_parameter("prediction_time_horizon.vehicle"); + prediction_time_horizon_.pedestrian = + declare_parameter("prediction_time_horizon.pedestrian"); + prediction_time_horizon_.unknown = declare_parameter("prediction_time_horizon.unknown"); lateral_control_time_horizon_ = declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); @@ -840,8 +843,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); path_generator_ = std::make_shared( - prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, - min_crosswalk_user_velocity_); + prediction_sampling_time_interval_, min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); @@ -1050,8 +1052,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // For off lane obstacles if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1066,8 +1068,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt transformed_object.kinematics.twist_with_covariance.twist.linear.x, transformed_object.kinematics.twist_with_covariance.twist.linear.y); if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1079,13 +1081,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Get Predicted Reference Path for Each Maneuver and current lanelets // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + const auto ref_paths = getPredictedReferencePath( + transformed_object, current_lanelets, objects_detected_time, + prediction_time_horizon_.vehicle); // If predicted reference path is empty, assume this object is out of the lane if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1120,7 +1123,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); + yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle, + lateral_control_time_horizon_, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1183,8 +1187,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } default: { auto predicted_unknown_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject( + transformed_object, prediction_time_horizon_.unknown); predicted_path.confidence = 1.0; predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1337,7 +1341,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( { auto predicted_object = convertToPredictedObject(object); { - PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1388,7 +1393,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_ * 2.0, + edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); @@ -1398,7 +1403,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_ * 2.0, + edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); @@ -1422,27 +1427,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto reachable_first = hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.front_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); const auto reachable_second = hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.back_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); if (!reachable_first && !reachable_second) { continue; } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { continue; } - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( + object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) { @@ -1740,7 +1745,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time) + const double object_detected_time, const double time_horizon) { const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1752,7 +1757,7 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.acceleration_with_covariance.accel.linear.x, object.kinematics.acceleration_with_covariance.accel.linear.y) : 0.0; - const double t_h = prediction_time_horizon_; + const double t_h = time_horizon; const double λ = std::log(2) / acceleration_exponential_half_life_; auto get_search_distance_with_decaying_acc = [&]() -> double { diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4238859535c8e..838d7b1c8e316 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,18 +23,16 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity) -: time_horizon_(time_horizon), - lateral_time_horizon_(lateral_time_horizon), - sampling_time_interval_(sampling_time_interval), + const double sampling_time_interval, const double min_crosswalk_user_velocity) +: sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { } -PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathToTargetPoint( @@ -75,7 +73,8 @@ PredictedPath PathGenerator::generatePathToTargetPoint( } PredictedPath PathGenerator::generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const { PredictedPath predicted_path{}; const double ep = 0.001; @@ -99,7 +98,7 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; if (dt < arrival_time) { world_frame_pose.position.x = @@ -131,39 +130,43 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( return predicted_path; } -PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const { PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { path.path.push_back(object.kinematics.pose_with_covariance.pose); } return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit) const { if (ref_paths.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit); + return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); } -PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const +PredictedPath PathGenerator::generateStraightPath( + const TrackedObject & object, const double longitudinal_duration) const { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; constexpr double ep = 0.001; - const double duration = time_horizon_ + ep; + const double duration = longitudinal_duration + ep; PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -178,11 +181,12 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit) const { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path, speed_limit); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -196,13 +200,13 @@ PredictedPath PathGenerator::generatePolynomialPath( // Step2. Generate Predicted Path on a Frenet coordinate const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len); + generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); // Step3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate @@ -210,12 +214,10 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, - const double max_length) const + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const { FrenetPath path; - const double duration = time_horizon_; - const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory const Eigen::Vector3d lat_coeff = @@ -387,7 +389,8 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit) const { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -413,7 +416,7 @@ FrenetPoint PathGenerator::getFrenetPoint( : 0.0; // Using a decaying acceleration model. Consult the README for more information about the model. - const double t_h = time_horizon_; + const double t_h = duration; const float λ = std::log(2) / acceleration_exponential_half_life_; auto have_same_sign = [](double a, double b) -> bool { diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp index 555dc217fb5ed..6134f0c6a25d7 100644 --- a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp +++ b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -52,19 +52,17 @@ TEST(PathGenerator, test_generatePathForNonVehicleObject) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate pedestrian object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); // Generate predicted path const PredictedPath predicted_path = - path_generator.generatePathForNonVehicleObject(tracked_object); + path_generator.generatePathForNonVehicleObject(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -77,19 +75,17 @@ TEST(PathGenerator, test_generatePathForLowSpeedVehicle) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); // Generate predicted path const PredictedPath predicted_path = - path_generator.generatePathForLowSpeedVehicle(tracked_object); + path_generator.generatePathForLowSpeedVehicle(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -102,17 +98,16 @@ TEST(PathGenerator, test_generatePathForOffLaneVehicle) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); - const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object); + const PredictedPath predicted_path = + path_generator.generatePathForOffLaneVehicle(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -129,8 +124,7 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -144,8 +138,8 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) ref_paths.push_back(pose); // Generate predicted path - const PredictedPath predicted_path = - path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths); + const PredictedPath predicted_path = path_generator.generatePathForOnLaneVehicle( + tracked_object, ref_paths, prediction_time_horizon, lateral_control_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -158,12 +152,10 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); @@ -178,8 +170,8 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) reachable_crosswalk.back_left_point << -1.0, 1.0; // Generate predicted path - const PredictedPath predicted_path = - path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk); + const PredictedPath predicted_path = path_generator.generatePathForCrosswalkUser( + tracked_object, reachable_crosswalk, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -191,13 +183,10 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) TEST(PathGenerator, test_generatePathToTargetPoint) { // Generate Path generator - const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); From 566ae09e6ac9974c56683047551ee3c9299e33f0 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 30 May 2024 16:00:40 +0900 Subject: [PATCH 055/120] test(lidar_centerpoint): added gtest for the cuda related parts of centerpoint (#7125) * test: added gtest for the cuda related parts of centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed ci/cd errors Signed-off-by: Kenzo Lobos-Tsunekawa * style(pre-commit): autofix --------- Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/lidar_centerpoint/CMakeLists.txt | 44 ++ .../lib/preprocess/preprocess_kernel.cu | 5 + .../test/test_postprocess_kernel.cpp | 367 ++++++++++++++++ .../test/test_postprocess_kernel.hpp | 49 +++ .../test/test_preprocess_kernel.cpp | 403 ++++++++++++++++++ .../test/test_preprocess_kernel.hpp | 79 ++++ .../test/test_voxel_generator.cpp | 230 ++++++++++ .../test/test_voxel_generator.hpp | 66 +++ 8 files changed, 1243 insertions(+) create mode 100644 perception/lidar_centerpoint/test/test_postprocess_kernel.cpp create mode 100644 perception/lidar_centerpoint/test/test_postprocess_kernel.hpp create mode 100644 perception/lidar_centerpoint/test/test_preprocess_kernel.cpp create mode 100644 perception/lidar_centerpoint/test/test_preprocess_kernel.hpp create mode 100644 perception/lidar_centerpoint/test/test_voxel_generator.cpp create mode 100644 perception/lidar_centerpoint/test/test_voxel_generator.hpp diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 353f797b0bee1..8a7f4c6107c67 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -159,6 +159,50 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(test_nms test/test_nms.cpp ) + ament_auto_add_gtest(test_voxel_generator + test/test_voxel_generator.cpp + ) + + add_executable(test_preprocess_kernel + test/test_preprocess_kernel.cpp + lib/utils.cpp + ) + + target_include_directories(test_preprocess_kernel PUBLIC + ${test_preprocess_kernel_SOURCE_DIR} + ) + + target_link_libraries(test_preprocess_kernel + centerpoint_cuda_lib + gtest + gtest_main + ) + + ament_add_test(test_preprocess_kernel + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + ) + + add_executable(test_postprocess_kernel + test/test_postprocess_kernel.cpp + lib/utils.cpp + ) + + target_include_directories(test_postprocess_kernel PUBLIC + ${test_postprocess_kernel_SOURCE_DIR} + ) + + target_link_libraries(test_postprocess_kernel + centerpoint_cuda_lib + gtest + gtest_main + ) + + ament_add_test(test_postprocess_kernel + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + ) + endif() else() diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 118e31f892d72..218aaee125c02 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -78,6 +78,11 @@ cudaError_t generateVoxels_random_launch( { dim3 blocks((points_size + 256 - 1) / 256); dim3 threads(256); + + if (blocks.x == 0) { + return cudaGetLastError(); + } + generateVoxels_random_kernel<<>>( points, points_size, min_x_range, max_x_range, min_y_range, max_y_range, min_z_range, max_z_range, pillar_x_size, pillar_y_size, pillar_z_size, grid_y_size, grid_x_size, mask, diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp new file mode 100644 index 0000000000000..fde4fcbee3d6c --- /dev/null +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -0,0 +1,367 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_postprocess_kernel.hpp" + +#include + +#include +#include +#include + +namespace centerpoint +{ + +void PostprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + constexpr std::size_t class_size{5}; + constexpr std::size_t point_feature_size{4}; + constexpr std::size_t max_voxel_size{100000000}; + const std::vector point_cloud_range{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; + const std::vector voxel_size{0.32, 0.32, 10.0}; + constexpr std::size_t downsample_factor{1}; + constexpr std::size_t encoder_in_feature_size{9}; + constexpr float score_threshold{0.35f}; + constexpr float circle_nms_dist_threshold{0.5f}; + const std::vector yaw_norm_thresholds{0.5, 0.5, 0.5}; + constexpr bool has_variance{false}; + + config_ptr_ = std::make_unique( + class_size, point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_thresholds, has_variance); + + postprocess_cuda_ptr_ = std::make_unique(*config_ptr_); + + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + + head_out_heatmap_d_ = cuda::make_unique(grid_xy_size * config_ptr_->class_size_); + head_out_offset_d_ = + cuda::make_unique(grid_xy_size * config_ptr_->head_out_offset_size_); + head_out_z_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_z_size_); + head_out_dim_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_dim_size_); + head_out_rot_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_rot_size_); + head_out_vel_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_vel_size_); + + std::vector heatmap_host_vector(grid_xy_size * config_ptr_->class_size_, 0.f); + std::fill(heatmap_host_vector.begin(), heatmap_host_vector.end(), -1e6); + cudaMemcpy( + head_out_heatmap_d_.get(), heatmap_host_vector.data(), + grid_xy_size * config_ptr_->head_out_offset_size_ * sizeof(float), cudaMemcpyHostToDevice); + cudaMemset( + head_out_offset_d_.get(), 0, grid_xy_size * config_ptr_->head_out_offset_size_ * sizeof(float)); + cudaMemset(head_out_z_d_.get(), 0, grid_xy_size * config_ptr_->head_out_z_size_ * sizeof(float)); + cudaMemset( + head_out_dim_d_.get(), 0, grid_xy_size * config_ptr_->head_out_dim_size_ * sizeof(float)); + cudaMemset( + head_out_rot_d_.get(), 0, grid_xy_size * config_ptr_->head_out_rot_size_ * sizeof(float)); + cudaMemset( + head_out_vel_d_.get(), 0, grid_xy_size * config_ptr_->head_out_vel_size_ * sizeof(float)); +} + +void PostprocessKernelTest::TearDown() +{ +} + +TEST_F(PostprocessKernelTest, EmptyTensorTest) +{ + std::vector det_boxes3d; + + postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, SingleDetectionTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_log_w = std::log(7.0); + constexpr float detection_log_l = std::log(1.0); + constexpr float detection_log_h = std::log(2.0); + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::sin(detection_yaw); + constexpr float detection_vel_x = 5.0; + constexpr float detection_vel_y = -5.0; + + const float idx = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index = config_ptr_->down_grid_size_x_ * std::floor(idy) + std::floor(idx); + const float detection_x_offset = idx - std::floor(idx); + const float detection_y_offset = idy - std::floor(idy); + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index], &detection_x_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index], &detection_y_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index], &detection_yaw_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(1, det_boxes3d.size()); + + const auto det_box3d = det_boxes3d[0]; + EXPECT_EQ(detection_x, det_box3d.x); + EXPECT_EQ(detection_y, det_box3d.y); + EXPECT_EQ(detection_z, det_box3d.z); + EXPECT_NEAR(std::exp(detection_log_w), det_box3d.width, 1e-6); + EXPECT_NEAR(std::exp(detection_log_l), det_box3d.length, 1e-6); + EXPECT_NEAR(std::exp(detection_log_h), det_box3d.height, 1e-6); + EXPECT_EQ(detection_yaw, det_box3d.yaw); + EXPECT_EQ(detection_vel_x, det_box3d.vel_x); + EXPECT_EQ(detection_vel_y, det_box3d.vel_y); +} + +TEST_F(PostprocessKernelTest, InvalidYawTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_yaw_sin = 0.0; + constexpr float detection_yaw_cos = 0.2; + + const float idx = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index = config_ptr_->down_grid_size_x_ * std::floor(idy) + std::floor(idx); + const float detection_x_offset = idx - std::floor(idx); + const float detection_y_offset = idy - std::floor(idy); + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index], &detection_x_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index], &detection_y_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index], &detection_yaw_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, CircleNMSTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_log_w = std::log(7.0); + constexpr float detection_log_l = std::log(1.0); + constexpr float detection_log_h = std::log(2.0); + constexpr float detection_yaw1_sin = 0.0; + constexpr float detection_yaw1_cos = 1.0; + constexpr float detection_yaw2_sin = 1.0; + constexpr float detection_yaw2_cos = 0.0; + constexpr float detection_vel_x = 5.0; + constexpr float detection_vel_y = -5.0; + + const float idx1 = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy1 = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index1 = config_ptr_->down_grid_size_x_ * std::floor(idy1) + std::floor(idx1); + const float detection_x_offset1 = idx1 - std::floor(idx1); + const float detection_y_offset1 = idy1 - std::floor(idy1); + + const float idx2 = idx1 + 1.0; + const float idy2 = idy1 + 1.0; + const std::size_t index2 = config_ptr_->down_grid_size_x_ * std::floor(idy2) + std::floor(idx2); + const float detection_x_offset2 = detection_x_offset1 - 1.0; + const float detection_y_offset2 = detection_y_offset1 - 1.0; + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index1], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index1], &detection_x_offset1, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index1], &detection_y_offset1, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index1], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index1], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index1], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index1], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index1], &detection_yaw1_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index1], &detection_yaw1_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index1], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index1], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index2], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index2], &detection_x_offset2, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index2], &detection_y_offset2, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index2], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index2], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index2], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index2], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index2], &detection_yaw2_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index2], &detection_yaw2_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index2], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index2], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(1, det_boxes3d.size()); +} + +} // namespace centerpoint + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp new file mode 100644 index 0000000000000..d0c9123da9e77 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_POSTPROCESS_KERNEL_HPP_ +#define TEST_POSTPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace centerpoint +{ + +class PostprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + cudaStream_t stream_{nullptr}; + + std::unique_ptr postprocess_cuda_ptr_; + std::unique_ptr config_ptr_; + + cuda::unique_ptr head_out_heatmap_d_; + cuda::unique_ptr head_out_offset_d_; + cuda::unique_ptr head_out_z_d_; + cuda::unique_ptr head_out_dim_d_; + cuda::unique_ptr head_out_rot_d_; + cuda::unique_ptr head_out_vel_d_; +}; + +} // namespace centerpoint + +#endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp new file mode 100644 index 0000000000000..db75328f9d5c5 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp @@ -0,0 +1,403 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_preprocess_kernel.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +namespace centerpoint +{ + +void PreprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + max_voxel_size_ = 40000; + max_point_in_voxel_size_ = + 32; // this value is actually hardcoded in the kernel so it can not be changed + point_feature_size_ = 4; + point_dim_size_ = 3; + config_encoder_in_feature_size_ = 9; + encoder_out_feature_size_ = 32; + capacity_ = 1000000; + + range_min_x_ = -76.8f; + range_min_y_ = -76.8f; + range_min_z_ = -4.0f; + range_max_x_ = 76.8f; + range_max_y_ = 76.8f; + range_max_z_ = 6.0f; + voxel_size_x_ = 0.32f; + voxel_size_y_ = 0.32f; + voxel_size_z_ = 10.0f; + + grid_size_x_ = static_cast((range_max_x_ - range_min_x_) / voxel_size_x_); + grid_size_y_ = static_cast((range_max_y_ - range_min_y_) / voxel_size_y_); + grid_size_z_ = static_cast((range_max_z_ - range_min_z_) / voxel_size_z_); + + voxels_size_ = max_voxel_size_ * max_point_in_voxel_size_ * point_feature_size_; + coordinates_size_ = max_voxel_size_ * point_dim_size_; + encoder_in_feature_size_ = + max_voxel_size_ * max_point_in_voxel_size_ * config_encoder_in_feature_size_; + pillar_features_size_ = max_voxel_size_ * encoder_out_feature_size_; + spatial_features_size_ = grid_size_x_ * grid_size_y_ * encoder_out_feature_size_; + grid_xy_size_ = grid_size_x_ * grid_size_y_; + + voxels_buffer_size_ = + grid_size_x_ * grid_size_y_ * max_point_in_voxel_size_ * point_feature_size_; + mask_size_ = grid_size_x_ * grid_size_y_; + + voxels_d_ = cuda::make_unique(voxels_size_); + coordinates_d_ = cuda::make_unique(coordinates_size_); + num_points_per_voxel_d_ = cuda::make_unique(max_voxel_size_); + encoder_in_features_d_ = cuda::make_unique(encoder_in_feature_size_); + pillar_features_d_ = cuda::make_unique(pillar_features_size_); + spatial_features_d_ = cuda::make_unique(spatial_features_size_); + points_d_ = cuda::make_unique(capacity_ * point_feature_size_); + voxels_buffer_d_ = cuda::make_unique(voxels_buffer_size_); + mask_d_ = cuda::make_unique(mask_size_); + num_voxels_d_ = cuda::make_unique(1); + + CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(mask_d_.get(), 0, mask_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(voxels_d_.get(), 0, voxels_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(coordinates_d_.get(), 0, coordinates_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(num_points_per_voxel_d_.get(), 0, max_voxel_size_ * sizeof(float), stream_)); +} + +void PreprocessKernelTest::TearDown() +{ +} + +TEST_F(PreprocessKernelTest, EmptyVoxelTest) +{ + std::vector points{}; + std::size_t points_num = 0; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(0, sum); +} + +TEST_F(PreprocessKernelTest, BasicTest) +{ + std::vector points{25.f, -61.1f, 1.8f, 0.1f}; + std::size_t points_num = 1; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(1, sum); + + // Check that the voxel was filled + int voxel_idx = std::floor((points[0] - range_min_x_) / voxel_size_x_); + int voxel_idy = std::floor((points[1] - range_min_y_) / voxel_size_y_); + unsigned int voxel_index = voxel_idy * grid_size_x_ + voxel_idx; + + unsigned voxel_count; + std::array voxel_data{0.f, 0.f, 0.f, 0.f}; + + CHECK_CUDA_ERROR(cudaMemcpy( + &voxel_count, mask_d_.get() + voxel_index, 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, voxel_count); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_data.data(), + voxels_buffer_d_.get() + voxel_index * max_point_in_voxel_size_ * point_feature_size_, + point_feature_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(points[0], voxel_data[0]); + EXPECT_EQ(points[1], voxel_data[1]); + EXPECT_EQ(points[2], voxel_data[2]); + EXPECT_EQ(points[3], voxel_data[3]); + + code = generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), grid_size_y_, grid_size_x_, max_voxel_size_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_); + + ASSERT_EQ(cudaSuccess, code); + + unsigned int num_pillars{}; + std::vector voxel_features(point_feature_size_, 0.f); + float num_voxels{}; + std::vector voxel_coordinates(3, 0); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_pillars, num_voxels_d_.get(), 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxels_d_.get(), point_feature_size_ * sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_voxels, num_points_per_voxel_d_.get(), 1 * sizeof(float), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_coordinates.data(), coordinates_d_.get(), 3 * sizeof(int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, num_pillars); + EXPECT_EQ(1.0, num_voxels); + EXPECT_EQ(0, voxel_coordinates[0]); + EXPECT_EQ(voxel_idy, voxel_coordinates[1]); + EXPECT_EQ(voxel_idx, voxel_coordinates[2]); + EXPECT_EQ(points[0], voxel_features[0]); + EXPECT_EQ(points[1], voxel_features[1]); + EXPECT_EQ(points[2], voxel_features[2]); + EXPECT_EQ(points[3], voxel_features[3]); + + code = generateFeatures_launch( + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), + max_voxel_size_, voxel_size_x_, voxel_size_y_, voxel_size_z_, range_min_x_, range_min_y_, + range_min_z_, encoder_in_features_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + std::vector encoder_features(config_encoder_in_feature_size_, 0.f); + + CHECK_CUDA_ERROR(cudaMemcpy( + encoder_features.data(), encoder_in_features_d_.get(), + config_encoder_in_feature_size_ * sizeof(float), cudaMemcpyDeviceToHost)); + + // The first four values are just the point features + EXPECT_EQ(points[0], encoder_features[0]); + EXPECT_EQ(points[1], encoder_features[1]); + EXPECT_EQ(points[2], encoder_features[2]); + EXPECT_EQ(points[3], encoder_features[3]); + + // The next three values are the relative coordinates with respect to the voxel average + EXPECT_EQ(0.0, encoder_features[4]); + EXPECT_EQ(0.0, encoder_features[5]); + EXPECT_EQ(0.0, encoder_features[6]); + + // The last two values are the relative coordinates with respect to the voxel center + float voxel_x_offset = voxel_size_x_ / 2 + voxel_idx * voxel_size_x_ + range_min_x_; + float voxel_y_offset = voxel_size_y_ / 2 + voxel_idy * voxel_size_y_ + range_min_y_; + + EXPECT_EQ(points[0] - voxel_x_offset, encoder_features[7]); + EXPECT_EQ(points[1] - voxel_y_offset, encoder_features[8]); +} + +TEST_F(PreprocessKernelTest, OutOfRangeTest) +{ + std::vector points{25.f, -61.1f, 100.f, 0.1f}; // 100.f is out of range + std::size_t points_num = 1; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(0, sum); +} + +TEST_F(PreprocessKernelTest, VoxelOverflowTest) +{ + std::array point{25.f, -61.1f, 1.8f, 0.1f}; + std::size_t points_num = 64; + std::vector points{}; + + for (std::size_t i = 0; i < points_num; ++i) { + points.insert(points.end(), point.begin(), point.end()); + } + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + // Note: due to atomic operations in the kernel, generateVoxels does not handle overflows in the + // counter, and instead is done in the following kernel + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Check that the voxel was filled + int voxel_idx = std::floor((points[0] - range_min_x_) / voxel_size_x_); + int voxel_idy = std::floor((points[1] - range_min_y_) / voxel_size_y_); + unsigned int voxel_index = voxel_idy * grid_size_x_ + voxel_idx; + + std::vector voxel_data{}; + voxel_data.resize((max_point_in_voxel_size_ + 1) * point_feature_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_data.data(), + voxels_buffer_d_.get() + voxel_index * max_point_in_voxel_size_ * point_feature_size_, + (max_point_in_voxel_size_ + 1) * point_feature_size_ * sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < max_point_in_voxel_size_; ++i) { + EXPECT_EQ(points[0], voxel_data[i * point_feature_size_ + 0]); + EXPECT_EQ(points[1], voxel_data[i * point_feature_size_ + 1]); + EXPECT_EQ(points[2], voxel_data[i * point_feature_size_ + 2]); + EXPECT_EQ(points[3], voxel_data[i * point_feature_size_ + 3]); + } + + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 0]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 1]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 2]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 3]); + + code = generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), grid_size_y_, grid_size_x_, max_voxel_size_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_); + + ASSERT_EQ(cudaSuccess, code); + + unsigned int num_pillars{}; + std::vector voxel_features(max_point_in_voxel_size_ * point_feature_size_, 0.f); + float num_voxels{}; + std::vector voxel_coordinates(3, 0); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_pillars, num_voxels_d_.get(), 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxels_d_.get(), + max_point_in_voxel_size_ * point_feature_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_voxels, num_points_per_voxel_d_.get(), 1 * sizeof(float), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_coordinates.data(), coordinates_d_.get(), 3 * sizeof(int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, num_pillars); + EXPECT_EQ(static_cast(max_point_in_voxel_size_), num_voxels); + EXPECT_EQ(0, voxel_coordinates[0]); + EXPECT_EQ(voxel_idy, voxel_coordinates[1]); + EXPECT_EQ(voxel_idx, voxel_coordinates[2]); + + for (std::size_t point_index = 0; point_index < max_point_in_voxel_size_; ++point_index) { + EXPECT_EQ(points[0], voxel_features[point_index * point_feature_size_ + 0]); + EXPECT_EQ(points[1], voxel_features[point_index * point_feature_size_ + 1]); + EXPECT_EQ(points[2], voxel_features[point_index * point_feature_size_ + 2]); + EXPECT_EQ(points[3], voxel_features[point_index * point_feature_size_ + 3]); + } + + code = generateFeatures_launch( + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), + max_voxel_size_, voxel_size_x_, voxel_size_y_, voxel_size_z_, range_min_x_, range_min_y_, + range_min_z_, encoder_in_features_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + std::vector encoder_features( + max_point_in_voxel_size_ * config_encoder_in_feature_size_, 0.f); + + CHECK_CUDA_ERROR(cudaMemcpy( + encoder_features.data(), encoder_in_features_d_.get(), + max_point_in_voxel_size_ * config_encoder_in_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost)); + + float voxel_x_offset = voxel_size_x_ / 2 + voxel_idx * voxel_size_x_ + range_min_x_; + float voxel_y_offset = voxel_size_y_ / 2 + voxel_idy * voxel_size_y_ + range_min_y_; + + for (std::size_t point_index = 0; point_index < max_point_in_voxel_size_; ++point_index) { + // The first four values are just the point features + EXPECT_EQ( + points[point_index * point_feature_size_ + 0], + encoder_features[point_index * config_encoder_in_feature_size_ + 0]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 1], + encoder_features[point_index * config_encoder_in_feature_size_ + 1]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 2], + encoder_features[point_index * config_encoder_in_feature_size_ + 2]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 3], + encoder_features[point_index * config_encoder_in_feature_size_ + 3]); + + // The next three values are the relative coordinates with respect to the voxel average + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 4], 1e-4); + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 5], 1e-4); + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 6], 1e-4); + + // The last two values are the relative coordinates with respect to the voxel center + EXPECT_EQ( + points[point_index * point_feature_size_ + 0] - voxel_x_offset, + encoder_features[point_index * config_encoder_in_feature_size_ + 7]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 1] - voxel_y_offset, + encoder_features[point_index * config_encoder_in_feature_size_ + 8]); + } +} + +} // namespace centerpoint + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp new file mode 100644 index 0000000000000..57ff51966a417 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp @@ -0,0 +1,79 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PREPROCESS_KERNEL_HPP_ +#define TEST_PREPROCESS_KERNEL_HPP_ + +#include + +#include + +namespace centerpoint +{ + +class PreprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + cudaStream_t stream_{nullptr}; + + std::size_t max_voxel_size_{}; + std::size_t max_point_in_voxel_size_{}; + std::size_t point_feature_size_{}; + std::size_t point_dim_size_{}; + std::size_t config_encoder_in_feature_size_{}; + std::size_t encoder_out_feature_size_{}; + std::size_t capacity_{}; + + float range_min_x_{}; + float range_min_y_{}; + float range_min_z_{}; + float range_max_x_{}; + float range_max_y_{}; + float range_max_z_{}; + float voxel_size_x_{}; + float voxel_size_y_{}; + float voxel_size_z_{}; + + std::size_t grid_size_x_{}; + std::size_t grid_size_y_{}; + std::size_t grid_size_z_{}; + + std::size_t voxels_size_{}; + std::size_t coordinates_size_{}; + std::size_t encoder_in_feature_size_{}; + std::size_t pillar_features_size_{}; + std::size_t spatial_features_size_{}; + std::size_t grid_xy_size_{}; + + std::size_t voxels_buffer_size_{}; + std::size_t mask_size_{}; + + cuda::unique_ptr voxels_d_{}; + cuda::unique_ptr coordinates_d_{}; + cuda::unique_ptr num_points_per_voxel_d_{}; + cuda::unique_ptr encoder_in_features_d_{}; + cuda::unique_ptr pillar_features_d_{}; + cuda::unique_ptr spatial_features_d_{}; + cuda::unique_ptr points_d_{}; + cuda::unique_ptr voxels_buffer_d_{}; + cuda::unique_ptr mask_d_{}; + cuda::unique_ptr num_voxels_d_{}; +}; + +} // namespace centerpoint + +#endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.cpp b/perception/lidar_centerpoint/test/test_voxel_generator.cpp new file mode 100644 index 0000000000000..5b0b3cd112e6c --- /dev/null +++ b/perception/lidar_centerpoint/test/test_voxel_generator.cpp @@ -0,0 +1,230 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_voxel_generator.hpp" + +#include + +#include + +void VoxelGeneratorTest::SetUp() +{ + // Setup things that should occur before every test instance should go here + node_ = std::make_shared("voxel_generator_test_node"); + + world_frame_ = "map"; + lidar_frame_ = "lidar"; + points_per_pointcloud_ = 10; + capacity_ = 100; + delta_pointcloud_x_ = 1.0; + + class_size_ = 5; + point_feature_size_ = 4; + max_voxel_size_ = 100000000; + point_cloud_range_ = std::vector{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; + voxel_size_ = std::vector{0.32, 0.32, 10.0}; + downsample_factor_ = 1; + encoder_in_feature_size_ = 9; + score_threshold_ = 0.35f; + circle_nms_dist_threshold_ = 0.5f; + yaw_norm_thresholds_ = std::vector{0.5, 0.5, 0.5}; + has_variance_ = false; + + cloud1_ = std::make_unique(); + cloud2_ = std::make_unique(); + + cloud1_->header.frame_id = lidar_frame_; + + // Set up the fields for x, y, and z coordinates + cloud1_->fields.resize(3); + sensor_msgs::PointCloud2Modifier modifier(*cloud1_); + modifier.setPointCloud2FieldsByString(1, "xyz"); + + // Resize the cloud to hold points_per_pointcloud_ points + modifier.resize(points_per_pointcloud_); + + // Create an iterator for the x, y, z fields + sensor_msgs::PointCloud2Iterator iter_x(*cloud1_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud1_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud1_, "z"); + + // Populate the point cloud + for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(i); + *iter_y = static_cast(i); + *iter_z = static_cast(i); + } + + *cloud2_ = *cloud1_; + + // Set the stamps for the point clouds. They usually come every 100ms + cloud1_->header.stamp.sec = 1; + cloud1_->header.stamp.nanosec = 100'000'000; + cloud2_->header.stamp.sec = 1; + cloud2_->header.stamp.nanosec = 200'000'000; + + tf2_buffer_ = std::make_unique(node_->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + + // The vehicle moves 1m/s in the x direction + const double world_origin_x = 6'371'000.0; + const double world_origin_y = 1'371'000.0; + + transform1_.header.stamp = cloud1_->header.stamp; + transform1_.header.frame_id = world_frame_; + transform1_.child_frame_id = lidar_frame_; + transform1_.transform.translation.x = world_origin_x; + transform1_.transform.translation.y = world_origin_y; + transform1_.transform.translation.z = 1.8; + transform1_.transform.rotation.x = 0.0; + transform1_.transform.rotation.y = 0.0; + transform1_.transform.rotation.z = 0.0; + transform1_.transform.rotation.w = 1.0; + + transform2_ = transform1_; + transform2_.header.stamp = cloud2_->header.stamp; + transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; +} + +void VoxelGeneratorTest::TearDown() +{ +} + +TEST_F(VoxelGeneratorTest, SingleFrame) +{ + const unsigned int num_past_frames = 0; // only current frame + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_TRUE(status1); + EXPECT_EQ(points_per_pointcloud_, generated_points_num); + + // Check valid points + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 2]); + EXPECT_EQ(static_cast(0), points[i * config.point_feature_size_ + 3]); + } + + // Check invalid points + for (std::size_t i = points_per_pointcloud_ * config.point_feature_size_; + i < capacity_ * config.point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +TEST_F(VoxelGeneratorTest, TwoFramesNoTf) +{ + const unsigned int num_past_frames = 1; + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_FALSE(status1); + EXPECT_FALSE(status2); + EXPECT_EQ(0, generated_points_num); +} + +TEST_F(VoxelGeneratorTest, TwoFrames) +{ + const unsigned int num_past_frames = 1; + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + tf2_buffer_->setTransform(transform1_, "authority1"); + tf2_buffer_->setTransform(transform2_, "authority1"); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_TRUE(status1); + EXPECT_TRUE(status2); + EXPECT_EQ(2 * points_per_pointcloud_, generated_points_num); + + // Check valid points for the latest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 2]); + EXPECT_EQ(static_cast(0), points[i * config.point_feature_size_ + 3]); + } + + // Check valid points for the oldest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are tf conversions, so results are not numerically the same + EXPECT_NEAR( + static_cast(i) - delta_pointcloud_x_, + points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 0], 1e-6); + EXPECT_NEAR( + static_cast(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 1], + 1e-6); + EXPECT_NEAR( + static_cast(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 2], + 1e-6); + EXPECT_NEAR(0.1, points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 3], 1e-6); + } + + // Check invalid points + for (std::size_t i = 2 * points_per_pointcloud_ * config.point_feature_size_; + i < capacity_ * config.point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.hpp b/perception/lidar_centerpoint/test/test_voxel_generator.hpp new file mode 100644 index 0000000000000..8fb7d8dffa44d --- /dev/null +++ b/perception/lidar_centerpoint/test/test_voxel_generator.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_VOXEL_GENERATOR_HPP_ +#define TEST_VOXEL_GENERATOR_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include + +class VoxelGeneratorTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + // These need to be public so that they can be accessed in the test cases + rclcpp::Node::SharedPtr node_; + + std::unique_ptr cloud1_, cloud2_; + geometry_msgs::msg::TransformStamped transform1_, transform2_; + + std::unique_ptr densification_param_ptr_; + std::unique_ptr config_ptr_; + + std::unique_ptr tf2_buffer_; + + std::string world_frame_; + std::string lidar_frame_; + std::size_t points_per_pointcloud_; + std::size_t capacity_; + double delta_pointcloud_x_; + + std::size_t class_size_; + float point_feature_size_; + std::size_t max_voxel_size_; + + std::vector point_cloud_range_; + std::vector voxel_size_; + std::size_t downsample_factor_; + std::size_t encoder_in_feature_size_; + float score_threshold_; + float circle_nms_dist_threshold_; + std::vector yaw_norm_thresholds_; + bool has_variance_; +}; + +#endif // TEST_VOXEL_GENERATOR_HPP_ From 9dcf81ec79c7f465c547196148f8c9c40e6d3411 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 30 May 2024 16:03:19 +0900 Subject: [PATCH 056/120] fix(autonomous_emergency_braking): fix invalid access to imu_ptr (#7171) Signed-off-by: Mamoru Sobue --- control/autonomous_emergency_braking/src/node.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 905b66df288b4..71eb92a4e95fb 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -301,10 +301,13 @@ bool AEB::fetchLatestData() } const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_ && !imu_ptr) { - return missing("imu message"); + if (use_imu_path_) { + if (!imu_ptr) { + return missing("imu message"); + } + // imu_ptr is valid + onImu(imu_ptr); } - onImu(imu_ptr); if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } From 7bebb1a69f6e703bdf04bad08e525487c0573a0a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 30 May 2024 17:01:07 +0900 Subject: [PATCH 057/120] feat(motion_velocity_planner): add new motion velocity planning (#7064) Signed-off-by: Maxime CLEMENT --- .../factor/velocity_factor_interface.hpp | 7 +- .../src/factor/velocity_factor_interface.cpp | 20 +- .../motion_planning.launch.xml | 54 +- planning/.pages | 4 + .../CMakeLists.txt | 23 + .../README.md | 165 ++++++ .../config/out_of_lane.param.yaml | 43 ++ ...footprints_other_lanes_overlaps_ranges.png | Bin 0 -> 127593 bytes .../docs/out_of_lane_slow.png | Bin 0 -> 110077 bytes .../docs/out_of_lane_stop.png | Bin 0 -> 183400 bytes .../package.xml | 41 ++ .../plugins.xml | 3 + .../src/calculate_slowdown_points.cpp | 85 +++ .../src/calculate_slowdown_points.hpp | 58 ++ .../src/debug.cpp | 242 ++++++++ .../src/debug.hpp | 31 + .../src/decisions.cpp | 388 +++++++++++++ .../src/decisions.hpp | 116 ++++ .../src/filter_predicted_objects.cpp | 150 +++++ .../src/filter_predicted_objects.hpp | 60 ++ .../src/footprint.cpp | 93 +++ .../src/footprint.hpp | 59 ++ .../src/lanelets_selection.cpp | 126 ++++ .../src/lanelets_selection.hpp | 80 +++ .../src/out_of_lane_module.cpp | 305 ++++++++++ .../src/out_of_lane_module.hpp | 63 ++ .../src/overlapping_range.cpp | 127 +++++ .../src/overlapping_range.hpp | 63 ++ .../src/types.hpp | 236 ++++++++ .../test/test_filter_predicted_objects.cpp | 58 ++ .../CMakeLists.txt | 14 + .../planner_data.hpp | 106 ++++ .../plugin_module_interface.hpp | 52 ++ .../velocity_planning_result.hpp | 50 ++ .../package.xml | 40 ++ .../CMakeLists.txt | 48 ++ .../README.md | 58 ++ .../config/motion_velocity_planner.param.yaml | 3 + ...locityPlanner-InternalInterface.drawio.svg | 538 ++++++++++++++++++ .../docs/set_stop_velocity.drawio.svg | 197 +++++++ .../launch/motion_velocity_planner.launch.xml | 42 ++ .../package.xml | 57 ++ .../motion_velocity_planner.schema.json | 33 ++ .../src/node.cpp | 458 +++++++++++++++ .../src/node.hpp | 140 +++++ .../src/planner_manager.cpp | 83 +++ .../src/planner_manager.hpp | 57 ++ .../srv/LoadPlugin.srv | 3 + .../srv/UnloadPlugin.srv | 3 + .../test/src/test_node_interface.cpp | 139 +++++ system/default_ad_api/src/planning.cpp | 3 +- 51 files changed, 4814 insertions(+), 10 deletions(-) create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_slow.png create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp index fdfd3a25eb3ad..077c66f9dd6bc 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -40,10 +39,10 @@ class VelocityFactorInterface void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } + template void set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail = ""); + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail = ""); private: VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index eabd00fae85d6..20742af0b6c0c 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -15,12 +15,16 @@ #include #include +#include +#include +#include + namespace motion_utils { +template void VelocityFactorInterface::set( - const std::vector & points, - const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string & detail) + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail) { const auto & curr_point = curr_pose.position; const auto & stop_point = stop_pose.position; @@ -32,4 +36,14 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); + } // namespace motion_utils diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 18de04fd9e317..f8deccb188b71 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,6 +2,24 @@ + + + + + + + + + + @@ -91,12 +109,44 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/.pages b/planning/.pages index 28a791801b805..4514894605bbc 100644 --- a/planning/.pages +++ b/planning/.pages @@ -63,6 +63,10 @@ nav: - 'Surround Obstacle Checker': - 'About Surround Obstacle Checker': planning/surround_obstacle_checker - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'Motion Velocity Planner': + - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ + - 'Available Modules': + - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - 'Motion Velocity Smoother': - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..b96ca3017a031 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_out_of_lane_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_filter_predicted_objects.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md new file mode 100644 index 0000000000000..99396eb0edf22 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -0,0 +1,165 @@ +## Out of Lane + +### Role + +`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects. + +### Activation Timing + +This module is activated if `launch_out_of_lane` is set to true. + +### Inner-workings / Algorithms + +The algorithm is made of the following steps. + +1. Calculate the ego path footprints (red). +2. Calculate the other lanes (magenta). +3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green). +4. For each overlapping range, decide if a stop or slow down action must be taken. +5. For each action, insert the corresponding stop or slow down point in the path. + +![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png) + +#### 1. Ego Path Footprints + +In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters. + +#### 2. Other lanes + +In the second step, the set of lanes to consider for overlaps is generated. +This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. +The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. + +A lanelet is deemed non-relevant if it meets one of the following conditions. + +- It is part of the lanelets followed by the ego path. +- It contains the rear point of the ego footprint. +- It follows one of the ego path lanelets. + +#### 3. Overlapping ranges + +In the third step, overlaps between the ego path footprints and the other lanes are calculated. +For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`. +For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored. +Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored. +Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information: + +- overlapped other lane $l$. +- start and end ego path indexes. +- start and end ego path arc lengths. +- start and end overlap points. + +#### 4. Decisions + +In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects. +The conditions for the decision depend on the value of the `mode` parameter. + +Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path). +If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. +If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. + + + + + + +
+ + + +
+ +##### Threshold + +With the `mode` set to `"threshold"`, +a decision to stop or slow down before a range is made if +an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`. + +##### TTC (time to collision) + +With the `mode` set to `"ttc"`, +estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated. +This is then used to calculate the time to collision over the period where ego crosses the overlap. +If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made. + +##### Intervals + +With the `mode` set to `"intervals"`, +the estimated times when ego and the dynamic objects reach the start and end points of +the overlapping range are used to create time intervals. +These intervals can be made shorter or longer using the +`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters. +If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made. + +##### Time estimates + +###### Ego + +To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path +at its current velocity or at half the velocity of the path points, whichever is higher. + +###### Dynamic objects + +Two methods are used to estimate the time when a dynamic objects with reach some point. +If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. +Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. + +#### 5. Path update + +Finally, for each decision to stop or slow down before an overlapping range, +a point is inserted in the path. +For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$, +a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$. +Such point with no overlap must exist since, by definition of the overlapping range, +we know that there is no overlap at $i-1$. + +If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter), +it is skipped. + +Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible. + +### Module Parameters + +| Parameter | Type | Description | +| ----------------------------- | ------ | --------------------------------------------------------------------------------- | +| `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc | +| `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane | + +| Parameter /threshold | Type | Description | +| -------------------- | ------ | ---------------------------------------------------------------- | +| `time_threshold` | double | [s] consider objects that will reach an overlap within this time | + +| Parameter /intervals | Type | Description | +| --------------------- | ------ | ------------------------------------------------------- | +| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | +| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | + +| Parameter /ttc | Type | Description | +| -------------- | ------ | ------------------------------------------------------------------------------------------------------ | +| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | + +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | + +| Parameter /overlap | Type | Description | +| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | +| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | +| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | + +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | + +| Parameter /ego | Type | Description | +| -------------------- | ------ | ---------------------------------------------------- | +| `extra_front_offset` | double | [m] extra front distance to add to the ego footprint | +| `extra_rear_offset` | double | [m] extra rear distance to add to the ego footprint | +| `extra_left_offset` | double | [m] extra left distance to add to the ego footprint | +| `extra_right_offset` | double | [m] extra right distance to add to the ego footprint | diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml new file mode 100644 index 0000000000000..e57b5a45d8be6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -0,0 +1,43 @@ +/**: + ros__parameters: + out_of_lane: # module to stop or slowdown before overlapping another lane with other objects + mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" + skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + + threshold: + time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time + intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego + ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer + objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer + ttc: + threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap + + objects: + minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored + use_predicted_paths: true # if true, use the predicted paths to estimate future positions. + # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. + distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights + + overlap: + minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered + extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) + + action: # action to insert in the trajectory if an object causes a conflict at an overlap + skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed + precision: 0.1 # [m] precision when inserting a stop pose in the trajectory + distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled + slowdown: + distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap + velocity: 2.0 # [m/s] slowdown velocity + stop: + distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap + + ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego + extra_front_offset: 0.0 # [m] extra front distance + extra_rear_offset: 0.0 # [m] extra rear distance + extra_right_offset: 0.0 # [m] extra right distance + extra_left_offset: 0.0 # [m] extra left distance diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png new file mode 100644 index 0000000000000000000000000000000000000000..f095467b3b0acd9a0d1f770422b2f845cd342246 GIT binary patch literal 127593 zcmYIw1yoes_x2DfP^S8Lr8ZI-Q6*W5aNybW8Z4KY?B6WJ>&QSBKRRA;QQwdqorB>&SSAW%d2x!ce8qrdh_N~ zhq_Ed;=vvZgL>4#OGN@_XJ?c5lhrw=5S*EpF~7WpFU6WKynARFkWTJapDrDE-DFY2 z@3G=P&&YG5U{I*>;28Lc@ z?gvY4bycz@$|Ey#K0(`=_>`0-K~C~;#-pJ1Gj)QYAyq@Y28V-@XI@xp$Zjg`1G(zH zzND>A=h715Hln%S96#5 zIQNqz-0X51BMk{7P?z)EA&5c56PC)7p*4OmMHIx66ag(4K1*G^F-fRBGIRIfYtnbn zY}sMJO&cG-2Nps{;J21DF-Rt1|DBHn+a-OlJ%m_5ux7VFN|~F3F%{Kc;@*=jj%eM* zb8&P$_+~6z+eW6VaQSwe7>15G^bZJd?GnDY{#kC4CphGxA#{;QgL3;bO?7Gwg7WcM z7O9{cI1zBA1Z<*YvVP0QOioMnGJ3Vtf2W%=red#)W{vi1#`5K34zUijnb-VczJuCc ziiKY&ruMT_OZLG4*#WkZ}EQzK?5aaCy6IXM_gy8$SO6 z#rr!YU1<+Tbm03>9Ni=~-`;fVj+;AB{p45Puh-~oHbKDM`I@l!=Z=Pb=oXg03kpzP zN*dj9mi@QbKksPSDH2%k&@13y$lJVmk$k;T;N4q7dggKkh_m7GmS`^RrqIj}TVse^ zd_(b6YHx4vB`J2Cw7SFQ@T+1(?)*Iz;lH6UdzjSNbN8O$+LMk`vVsQ+LQxNPg@(?* zPXfB$!5eB%bw3KHjzW=;m{?3!5ovJJZgID_lv#UgIHNh;3q1%S;KKMf+V}PV z7|}yOhe^RKcR5h`g!xEyIq)+Q#RhTXc%p&>K! zI}bw}2e=Q3iHQ#;K~9)Qga54~OP7RwT(J4hR@|?AUw=3=ZQ@7k)2h;*#gWF#-R`E2 zGhKdG;+QM{#@%fK?q38N({>^q9^VGal%OByP-#sz|90b}yX^-P2(gtA=B(sadJx$z z&{TtfdWoE#{}#0|m485>pb}YQvetS`fJYDB-BX_nO>46F@6!$7({?CZT!0^-OP|-U zF4r4(N4oEs+zmGB7211LqhfFjk$YlWwVBoQaFLMyicEZMtg=eCMg_eHu_606h+15o z65{*Qlp{|Rtc<}0N@ zbDguvbLNnEW+JHyTQ>rauX`(+H%W%?SPTjBzs!ICGVn!!`Y|$#JzU;k@S2ze8$ctC z61l(A&K2W=p9$8KW=DsIpPX%RAw(Z!ot#Rod}t+LctI{qK8Oi1qvQMK*chPu2vCh^ z&;;8K)A)AD7mK_HOaQo0`1oj*6t2Yn9jrwpyt;ariu>E`{}5sr8K&|7XQ@Bh<2t`` z_>HseJfiE?LpTi#%DzC`V*MvRJJLwB9C|U!FG%9MnS(9__GSO=9VQr7JcbwBScq*l zV1FAM>sti{fSGtZ@0O&{dM3>N`xXSEciP#sO|fz-@LNBX@(_5Vo}Pj(GIxKtl@U+Z zJxM4H)h}^%RvyhWSG;ncE;+OQ?>iD&Q9qd93t`8RY;iUxzn2=pf8_2=y;vC-{I&2Z zV2$U0CIe~}U!=lpVOGAC#&M67Ds_mhMiT$eqenY+!p@_IUjL2f<3pgAYZA>GB5Q0U z)}5GQNgwKoP4-#=nFaU|B;>!H{=(C^eW@&diXxI#*-xNZTcYwGoPw0xFv;1qA+YG* z`|WYBS#Hh-ZhE-YW0Lp#Q0q4Zs{JYRf~#~oxBeD*aD&pm!UPa15DM0C=Y`4LU7i{( zHBrZ$D(fKUOfJ+ELkgETpp2a_^SWfl4 z7`HVZAqCauw$42N`?+snPrAwx6AW4@T#;yas|!E5)gz${WvVj>i+`8~qI2q^WIy7# za?RP+P(<>FQS;1uzBJqT z5E9Y@Y)SKe#;3oN+9^{@LV!5|wV_w$jh11eee*K#|7M$O#EoEmr`;OsAyc?Fv3mDwNs-|30(ajWqH$dJ#j+-kQy`2Qz_ zm;1p0j*z{Y9Vj2w`R@4%_zV|?Eyc#Kv%ayhQ1c&7iCY3`OS%eb(6&b-caRKH zpc?{33+hzzAD)t+dc|icJO0lYKTQ!wpGRL~4dI*)!xW?7orfBa18L@CTZ|FVd9-V) z$-v-GW3tO-KP}xy9D0(vkHnYOG|$ieX}BGtJAF{xtc71U9VT`XZ&^j)?-&aXin*4* ztk8Vy{mfSJ5g4ErC$zI3e`fV~u2lciXAny(cT|fLQt25_)1UiU{>A1*G-L<%^Z z37(xsYx|~ngQ6$J4GoF2_x`=EbTow*E&nCj>g}_nhVmR~zXuPOuOu%B`X=YU5z-0^ zJ5pNP|9cbU&Q*z0w?;xKf7EHOsL23?Z8?^cU!eUA0&qyeO#*Ejra;nO&MAYt2q=RD z9>cmvH9-7Q;G2{4FC1uS>WC`a{yTgN7-4iFY1UKALdMbWB^s1NgA}RKY`}mv*7|KF z4}On~?8z;K6G|d8I|j3wn?>DYQgT+dG9V1^LS16G&dvhpq#Cl~DU(OW*@63yKFj6q!76K0m*5LPa{^ z`TeKuO1*)pc_s<@Bm;5$W-pX;jVASo)$WwmZ<*pzwRZqK6N67k$o~8gZL&YOxX6Th z@|G0wbLQvF4l}BI*=>cUJ~MTt_kVhmYayke`qZ9i3c1 z3a+a|MX#<1w%eokLG~skqn#I<6`XYqB2cGGn?2{_tJ$pH^dQhI1tmL0GOPOCp+qs} zkZ3_=`~-YfrE-T7ySx%x{hRt6>C|dO4E&9rd(+98d8*=-K0fhZNM`jNERla4XJ%N? zz-Q=`S8|j+{(?gf6U3Tp@yi!hWx$Xq ztK+2rL!u)>9Nh5YBz+-Z)8b+FhzMn~78?$b5Ah@QTSsM@3Rz67;4?qVvZ_CGQ#Cclw5Y)x2m=9pvAuaB*oz0E#95V|`7%R1RX4 zM;pA3&-<^lw`^xVo`wL1=EVmLs}`Uj>weJkJF#Zx!^6(4z{Q&>Dt2`hVtQ>+@~lFp=&=4!}|r z+*UFW(zk&_GE@#=oMS(4?OGlYvE*+Yf52#TRzmnRSO3+N7H@64tPkXx91=f6yUYAqF0nQ`clv8fN zqe$=#)QVyMUidE-MxA`M>{n5X650g{^Y>^ncYYyLX=^iz8b=bnBy8Lv#wT`|pj}^C zK%o$v+h6Bqu@)9$M+?V0OX{)jz>3G%b~3TcS|JO@J$E_mt2Ph@+hdH?XLF{a29 z9UY0v0!3zqpEwjga#8XZ(}j9h{)bZ~+eGSGygza+5yYi)> zUyJ}MjV-?6LnYTWF65G9JtYtrtIjz55ox z-5x)fMnL&+46AyJ-FBuNjUB5Jdrn>sNcLPE}KgAw{kg~ zH}zsm^~y4QZ$fmcOAJ!0$ZJsD4D!`Ub&+C7j&Rzh8p`a|)PYOMLBFQdrmd~9IJ^2E z<%tMtc^o~A2&(HB)gFnhxO$6z7gdDeBgT*|3tK~|)M^o~&_>~$K|+Dd)Psi$y}eH+ zt3{p(2}wYupfAzL7jsH!YScu5IBzx*SwRp5fI$^CBPfZO#q9b_V#dcJo_ZlxKV7*2 z{2Z>(p>QAv4R}vf(hmEx@a{lhKP8bLVv?`6a8!EYxYi0{;J70862BM&w`Owy z#CQc&3`TQ=LPKfQ;CgaV@Kt%PnuYi=0EInxk}lc~&&}Ui_+o(`i_+-PG&x>6>$)*NZYD*Swot-Kmx%FT@6#n}uJTsJMOlVQ(D{f6X1Tjw;f@jRvJL{3A8^PgKjx zIY_nHaqX3v7@ny9S_NpP-r*W?9^4OeqG|97WbrO6T5A;3h3auoLq zQY3QK-175(=IH^cnK_NujLU9AIDhQCO~T|0Xlu z@4~Ut?{Rg9mg$pg|4~`@?zd-b0zWzXpvhEz=y7FY0dWq+15d0beUxF2C ztwEU=EFeA24FollgH#JnA&_Tq$!9>Isz>> zh0KXSGf}ZijNLiobf)-{Y-9T!e~+U_V4|HHd?j=xgn~lP3$4zY+WIHN8cqlYdVcC{ z4O>k5&aG{)xKF>mO|O+)Squ#5jj0%-<|G(`)bmP)kuI;nrLu&^W~_7c;Kp92Zm2cBg%a5$HMArta2YSR~ zdG=uS9vjSqYxIn9?T*!_vzDS`L-`VK=4vU219`{q0Z_&g7>qw{b9rKR%KDSB@6@$f zBx{|QSX8Sl!9(qnpYhj@Hw01q0DqmVu^lZ*yCRGmIo7Dl&P*rW(BNkE^|*}eK;(Mq zH+hAR?{Yu15G|XB>LNKxI6F%Zk-*!=@!Q|clD3uD-5x$YzHXP z)(O5Vw3_x>5kM_Q2)fVA#bOr1T<`$I6KitYz>%Z85RlM*xZG>;Dpsqqa`Si7$t+(K z1Q}VJ5?vVVpfBWwHWrroz_QnM{^6ny7pzae^flHsyL@IpKVEI{HC*wJdC5m7DvFeBNh+%UIE6Ury{=_=y{Q6%gb(ie|p^|*mz# z%RyEfCuIq`Gvq0#TmVXbhn2rh^!h6GX3hBKAqsykOOe=fVo!hZIcN@nt3J+}MFN3A zqvQ>&J_PGN`)(h+5OYXl*WQVz_s89>sG+U0*n4Dgm_^R`QnIwMjT^vF9zU<*(n}BB zzx?4>VB`5|!66q)C$BY{^(iCcPNGYh_Mi4ARn^4|NFa~`a+gXAo_G^Dbd)I|@- zWo&57a!|Pavykbg`iPRnW~ApED2HOvV&8>7xokf_Z9;q;J#wI0x%%VDK?C&AV8df` zvLAK&X?qX~<#)H-ekZ_X$nO@10s7ikER(4k0bl*ZB%?Tl0DOBK;PH7f^)2zUfK^FWcUQVhwQ#cJ+QM=v$NefL+8 za3>MlKqH>_&$rKiMu%6WeRuzEP_aaNttE{9?-A>EKKJycSomSxmp(O`scLZHi+ zez|GnBoNmh_E`xJ#Qw!a?PgHksDZ=Oq(`YRsecLHnNeEU(9lp+6ar63OkCJ|jv6O) z;{*0L#UN{MZ_~*}cKJzZh%r#u#7ml>4oI6~rbl@11FO_HgA0@;!bLCko2tPc6`H)4 z5eml(+Fh{-3w(U(TCL2YBJ1h;JB(7VrKQObU%l?+!vOtwe?<}-M4U%#xlzX5Rw)BV z*dQ24yX0;d_#Dmr;i4M}9?_@bIz0NTqk|-19D)X&b1xNVDzBak1PD?OkfnUF``WPH3(#f^ij%7lmFTih*=AM@3sMz*QN-) z&lld*apJ&nn{d4{>#!Xgbvpnjs(=}XLZ9i?*e`qmDoHfD^=LHWzgoq#{{LW?PA&hg zj>GThjX)7nV@L0dPzI**9|pGCJka_N7UzaJVs+lzRo|)rFYYBl^q-)8G_uL-nI!w; zU|K?{-TKPpvo?9IPgM_o|6VsKr;n~rR+w$}h=tqCG`js+9}Y8ndSN2e*rov@(P}lk zY%`2&K8^9J^mbY1bVb&jzvyF|Et{jz`TAUOzp!yY z1|;k2>&Khp#^=7e^BAb7BYRKOc(1M6YdGfefV#-v#FIY2b^l0JepO=oeTW{=T;+gP zuK#3r8QEo*{Z{+&mfCJ@43!PlKXIyEF1^-ey(0+$;Wj9pG;6at#ksoKeoTKFsgagx z*hmvsxIQ`_#Vs9(`$oerA?)_q^-dW z=I7rw21q5de1`fwuHGCaLe=un;PP}k34nDOQMH4o`>0>xbEFU*aKSy>O?1+nglWa9 z?09mU=*;DBIk9W6A8b*g*yNt;C*0^@R8(Fj6tP!~_iX{7QeR=82TKlXyb7GBhn=Cn z^IXc?>>$$cfP9SG^;hVyr~R)Npem44l@}k#%u1HUSyC&H_Q-p@=(Qf!nSp_m;_%+s zf#!-m(n-NbM@RN7EAIk@_XU@Q4FZ5#(BE3T4gx`XHi(_G(seth>F<(5vVrX$8~nvw z#-Fd=-T|!QtBz^#s!MTND?}{7Z6yn}ET^NJn2-RM$uGEQEa=W(Dk5L%jSKc>=Kqc% zJ^%#Ti&n*pj=5^;2+{@JvWS_sAjaFG+yMPo*!9~pmrqeaF^odag0_OXe*iqhV7fZb_2%b0605cVdZ8NcGG=->%{KSH)v3CImNIo9egG%wDZmwD~u!sJiHq6N>FFeng zP&&9@VObn{zNy1V!)E413D}}xf8B+;jgNI+JscFX(Qa0wOT$BC) zT*Q(zAW}}DeLjKm042Oax?52WJ}$_}f*arCEv%*3vn#}p>RWW6y~_I383(s;a*Fa) z{xwszH?zBHgX04;rD9jMtcw^;Cnbf1jJ~mhd{Y^uQyUyg{ni_F3CDayPyIhh2Isp# zYaC6fm#x*)7H?9~0dZ&}w#YS4UnX);Qa2g9G9IwvL;I(I&Dv$-lR?@Wn4i~>Lh|R8 z&;g`O{#i28i;U!(xMqqf9YpM2mLS5w9mz_!;>k8GrlEOY6(i6R8Ga(L%L z+m>PNQ!9uWmJjb7R)WRi^CqB(m+yTYpDMpQ-kB}A8M&q>R}qz#NoR$25&So3`WNuD z6wmSljX+jC-Y4z!p}I4$MaQ1XqtqK}z&MnWa8YlW!)4-@VL(W~v9WO=M82wd9?nV) z#M3&unsvH`i|$CZaiD-R9X|~Z3;f%E!0u1^t@IQt>3Kr0iok|RoZ#d+l1WHj#8)rP zy6b+Q|%RVYZ*qD{f7jB9?E zx}EP7uDj~)Y(oH0C)&)~$YfPUA3L4rWy$q(8#V*G-I}bKlsConvC7=lDSGL0}A?~ zE1|V7+NBOizYwoVLKt-1&h|s3f|xhfEsPo-37-(XV+N&3zJjm|X|gK-;n-&hCfK*& zP(v>JcPERB(A)eGcJ``*;9<#sM=&d`(KNpzH5(*10?1r*&`cj6uMeU9^#hjhjH2L*k41Y7aY5|* z-_uFYQaGGlU2pWxgiwJ{v?Q-vUfELvC$gmeA9aa|iDy}9?5qls^rYll7*h1e5)%h# znf-g=j-}W>MZwPQRyLNrG?-=VvcJ zIAn=<9M`I!e{@+cv_w|2)dNCrVn~<+>I3u&n65&>VStkg=0Q%H1Lc-fPBC)CeV{y5 zpOK%+-sP-b^?|kse&|X#t57&+DIQmH3Q*(=c#fxMG|{c`l_Kyw9X-QKp4YKC z_DYzp4#_&q=Y++s63S}x)akBeaUMG=^9S=lT+CeKPIDdj`H zXp28QwotiafAXxWuIRo!`>S`0muCYdUeWhx95qIYbORzFK2}O9xzOr5l}p9>!xuH1 zYN)}RqXpY1^hmv9>0Q#Cu9l6^Krnzs=mH#I8Css#$0 z3Ed%6M!?@?jub$lMj}jX{z3ugq->ex=^`gHK46jT~n*4vz2ii@Zcp(I!5d zjStFSSK%%(kYIq4hG*tOO=p!Kr?Ur z$c};np){)tlKs{~ChAbm_wu%uXmW;wm%~)c{VzJq)SY1`habqX!(~wKq9B8_HQ)-5 z=!Rf$!-U{H)ao~*HfL(}riNXONj?MgeCg?acD}+3IgtdZvLEwBnYAA1p&G>eSfi#p z*m$Y8v{LasT!wk#8EQD=UO|HW9oVCyl3u5{n!H-p_5m_;BN^ohN1<&ZAWF*)HK4pd zE-lBZjhQR6{S{~_d3pOZn)lNtu6H!v*8Wl!L@ur901MAAC@8q?=)`LG`ad)-q`iW9 zYZiAKB>^=y|EsTGvRPi&DUz2g@Tv1{ww1M4;3Ae)Gqok?CVyNc(aGdL(Edn0G-~4_ za)h(4U`&?_%MN8&VenPTDPS|MON`M@?0P`k-oDYI-wxwIDK(WtU8(#F=%?y zrBtB6`$Vnj^67OyONLv-vAim&egmN-kc`~ka<=5E89v_c@$&kU!iH=hAf8aI=Wv`n z7A-@g_Wi$59q5Z2C$OEiFw?`>y5jZvKDLeV?C6Jg4*DhQy}rL}-8F_yMJ1I8#y@_- z`Bxn6({&M#L_du!JO7*+kFTKV5I!9SJ?;q&4J`rMQ4O7kZ;SnEX_-eqx2;5{6Au&w z@pl=22^EXW+J@5=4!@}_)Y;5Z+Ak+TputPnpFJfu1^c|emvW@BkVvVMc9wpZgi3u_ zjM}s3CPkQS%-wmZBtlvik{>j7z%iftyrZ~?!qaYz#aPXpkr`JfVDsL<-JstYJjItJ z9cG2Lh3bNbH0n*6tmlQ{2w*ClQ zj8|+$23fI>7)`dxEaJuzi?%tj{ozxg4}4EU`u6! z+xeS*BpgOPv|;kLF@oZU)6Q_Mg&uv7)HfNmA0?kvDK1+G@~G9H%=?U)p?-;8(FcvH zv@dsPfUbI~)-VnKU@=%-bv#REm4+%KgITK&y4@O|-}t?eineDTQIYs)(QG(#-&ntM z3E;XSNQ-EDi=>mEV1Kubdu;foyu!;XZ+V4H+&TpqplDL@quoXQlqUEGo0*;>ZjZO) zDLBW#foh>Fp2Z~kCbK?WbD9_;stpeZd*!j}k?Fk65iY3E)eDMUCz_Ki-N$#{-KsLFLDL^*QiL_J?Sdu( zaJtt3b@jTic4-C`lubdMSY238rH@ivTB2tzvSrs3=Kk|=vAGuML6Lew_C1eZGuTVi z=H(0a5@Ct>bkWOKMkwde(i&Xn>!GqQk zwGfguuPR$D@(6*;^@si*(QuP!8@$7t;VBMPlvNCdr^T@?)~Z8uPn(oTz7BWb3dK_H zpICuF2I>@H$OKC_baA_%GlMEad7(RKCTM_FFvH}w45pk|)@sn8V8VOM4V5rEXRx$= zV_CwY%K&D05!oBT#itLE?L~fMi>(kU9KLUubTIGRSFTlQr_e|i@{PNnx3~Y%l!u&@ zvY7JZbwzwuwQCh+>$+uCyM`t2xJ$8mL{UU$bdl7ZKQOM9m&#omzDPZ#s4CI_M>BUd6{2D z`MMzmInXBMRaL_oM9*v7XZbl5V9eZ4^or$;f+Jz0#`lf$O1f){^FQ!|9;)SaJXjhD zoTr@LOXN@sd7D`* z4P8m2w8raKK;!xYsc^V~VVvnE!#dN8@Lo!LPak}|Pe*cAai;kMmAp>3Xh4` z|4_jM98)h`o&8v>q$r|NhGa0`J%|u3^5g4`8XQZoL4}_2EKzD*= z|5EMP`yeKvPkHt#<(X9#AaVF^M~vuL(!^yaPUu$~&Ke3u;kl`3d4qWkcAyjSZBw~u zru`tPM3~e-EkWnjR9p4HMpJ?#l^uKru~o%d#YIQGWc&0ku+%x~m?-9vCSx%}G0d zUw4H|8p`nEF@j{kygr1hL!4GCK^6B%8Wa|~CQS6POOzu|AX|2XQVI=6{#gASi!Qa% zw$Uyi;+v)(x_{DjSeU|%(u^WNu9n*6d3sT*JR|2yMlX$-IEDORe*R`A;x@kebT}Xl zKr=3Z;j1j5*{VJ_$Pj`I3tkdfvc!4x{7>Uh2>kBScc3|Dg}53>lOZ{_o^Jn&O1@&T z>?5csDOjhFvZQJ&iZD_CwOu@UAT&xUVWFvptfND;iErL0E7SS4GwnJ45KaA2%&%eX z^oCShM0q!oQT1^k!$f%(mye=1h?Q}u%WxNN5941e4}NLyf2Cm#6!4}Drz*Q`5p;T; zOPkX6k9Px5;1o@CkeDC8ffdcJl5VeJ zpJn&i)73RgTz1D7SqhGp!~TBwIHCIH{z=kUfb*m3f&rkz0NHt*HJ0}Yv09(My!@i` zUX2u((Uw#}(6;Zw0fzX&fLGSo& zHn6fD`NBcl`(iPk;Ah%XJa4?C#;|k#8aAtAF8|)x&w|iq7}P#6z*Qh|!#H|yjUT1P zrjn=j(#Cs)Ksu>ImdmC7PGBS^WQdXQ;vP_&ISB~wv{oPP>rlh5-uMA)3KsU2kF2Qd z>>^Yy?8HTPVFjuX3Xe)l#gOG`>0{I9kCh=bm9*scl72DtZO=<7!Dhy`d8_?BPJT`6UJ z7Ya>|h*BAjvfzVk;Jnke_m;Rc0@|klh zj_tS~xN^2F5wB8rpTHz{j5CAW(R=U1$QYUVZ#LOtX}Q;4GoRY|7dYzeIBn4?hK7c# z-$*d>rNqjuh)L}7mqiVV!f8q{i9D8FJV7|J6F7swr~Ts?;} z8y`g`8ygesZzIph=-Y-LA?BChA0@x3=?0px@8HeRmR?7GCS#M@oByu27feH~^!npH z4?J6f`fYT|FLJWn@@h38a~>E>1$febYFT zU=ZhVyU*^t)MM^ImS}UZt>7MeTQB(Fk8fPOXoI828^0KvuS>mZ$Fo%I5-&V_X2W^F;L3<)2*5_mdsFDD{L2w4NU&|5%9+Fx#USixIFVT zel(C|JIh*kaasPLJ)b$Z3YJP_3(h)jh!;xeJ(e5Pkv-RyeSlsn^m0bn6s!X*k~LO1 z934EopW}q#^J{Ol3X@K$_P|B`H9rYgJ0>-|G+9!(Mr|hHLA8pkpIke}y0A$+EH_yi za8j#dO*0g|?8-nh{UL%BxU~h;!;U!abw>k{NWt9N#1c?Vm`$!v6jXi}TJUDzjgcbI zEhi0_vGB6)OmN?dLrK(bbb7tP$I@5l+I2!#tb0gnfN_j$RPgJ?lW=yj)h%l)> z8=zW#h2}C6U^~p`K(ipOM~LjlcmW`VHh|@+x6;VM#TB?0cJh<@eTSZ<%q{oVs&z8~ zq$7-nDX*P1u=nxP&Gi+KWV-a9vo|GCsfre+X0ZYfbQ7bJYQ)hu>r6wu3_G>DX+QD9 zE7>z?${SzB2t(Mo0yR*+aDemCsOvM~zAxhbQOcfqP?w|uKZG$Kz?dER6KN#rBRs|| zcUrtf#jXknw+(Q?!K*~yGR6z{qS!dQ=P0{3ECenFdxC$oW>IAf0Qn*b!Ot5VoK(hJ zS~*O0`};P|VV$=g4Hy4oZ~17+;o~^bmJ%*<52n{U`#}j%hJoqnCeI5OFRX#%t;xxW z3E$LLIYVm`6FOd1B&w>amnzTVA9i|9aVb2A!T~P2oekrcLrE%C8>xlZlm@97w67io zh^j|jK=Sm-f#^VN_R(aBu`tQ8W;RsLk@k8wtjh!QIjg=R-mBLbUER?c&w12;rHh`n zJclIsKGj2ay8Tg+32;js`BvwwAsWSrft@70^G)Mb6#lc9FXf@xbt2O!M2KgkwaSrjmEq~Q^j)*j&M8ZXZT&j01Wr?_ zBVpGz#V{U>^TdhF`}6k!TNfN+C=sj@dQbo<-bk}HTd;H_e zX3vXb(vk|-2($^?tMI>g!Ojy#{hY@WxFH3c(^XQk8s&wFTL7n^mQ!1W$f$2Sz0iuu zBhvSdd|srurhj&^?TDW@+!wD)lOt%oV+#9h`a%y~^jS0o=*YApi#@d_XagvA?N8f- zZ0y44uV#|)zI|bn14%GaUj3|~?(S6qc~yxElu$_-nm-N>4Ly*1YvgEkFf+UTj1WRC z5F$)R7FB2khy7*@viYi5 z&1RG-oaBDA;x4grS6harjE|3ayh;mo4m1)++uya3c6p1%xE)BKjJnd1A%2E>Um7RA zY6B1*=hyl22A}3en}%gk`B*`r~_+-KVDh6U#n2l%eRo zVT&5x*C0R{8MINc9>L#v^PhJ-d-i^XF|*_~L-!q{I=Vs6Jp=J!_hfySga6_QWm+wXveu`nL1(GOu#k6?E^xf_4b z3K=0YSFcrb`H-XBA4DTl{zM|}ttLGPxL>G=1WwTV=DKFqYZ6_{P(TkeNo1u62r=R7 z(|m{dJmN{be06m-r9Dxt%K37uDIi$J$1mTJv8eB2LyrCHr6L4igLgNGe-*QLNuM3F z$lEW3;RlB7zpLQ+^769W{Szeef@l4@54f-!C%L1$dn?09EYDAlm0?CRx{$SeAHcK=b(r(k!6E*#H~EP5Mf+{ct-kkIgHO zROH|fAJ9U7s!lWNh#t%$Z@Z?M4y2?Hw?Q70O4KTnQ)ve)CMXXlLN#xh?*n>jLbfhS zDZK05?yhzK>t}$pdUC{XuE51TGJH@gW|OUvQ4;B!6jN$ZJJB8wKi2KAC2_(7THc3g z&5AK0Qu4)}9+mei*|B}_e~ijch;0;-LQ3T1HeXo?Gzr^2T29>C+k5tYWVhgKs@^%s zFrU*g=cBOAATRpk6)QVfcrt;IWyMsNBTyg&&yW>1aa>bMjvyr3eId!)<}xaIUT4@= zOCLyN?hI5`)@regk0pq(!z{xj49uuS;|LJt{0u-;oPsvK z?d;~?#39=U+Lyt$=_ZI6#Jf;dho%oi<5a9ya$K(Z>Z*a|U!h}_|LX;iCof@z!PLED zdt+Q#TKuaUG|sQJXXd#XBxS!1Jd@QcWF=X;fkbfrtr1~Id8Irbc?^VU>{%#&l5MR!k#9|6wC+Vqn` z6n_BmV}YhVc@kJm;D%4ZOWT*1{=L{)hz@22ucbxDfYb#+Y@cS5wWr!(z`0%U25g&)!jo`&U{BTX(MqT|z;w0JZZw)b0S>BaK^Umzfe zJkj>=uWZdKGP{U(*ZHHO%|07aWnBD;C!Pu@1YDBSPnu639DW*gqPqR71SO)^>jXQ^ zEhXoWa2Y38SKHak)k)-BO5RuJ2Q0(G492IAMe-C#;&CE371|*}k!^&x@bk z6qi7d#_1%evPPJT0i{sbcZk#d0UAmmR=75y78OqDb%905JI&a62m!XnCcCw?ia0>w zeFFk@C%}JrVS&hPw#n`f>8-5|(NAhvu6) z@COxu_4kI%x(6AWYkeM+-U~e?^kU?Yx!Lesm*D(B@NV-8e7m zpw(^*xjY`e6gir79Uz+?;;Bz147;-=Vbe2aBIVvIS_IsulU#G6Mb* zUMB8;*~NS58ndN!w~D2Yf8n6WuIG?07SXX_S|}-Q#H|+LVcMj()era?Yq#!29%uOn z7sT`4_bI;&`db~qBuuvwoo6rvpFL~i_R|0^Sq^ieqq4*iSu$-QuN)-Bls&i#NZU5d zQq5m*Dp^qx2qz11Tku)rOlJ67G)MM7zngH~Mo zE!1@thTAu4DJ~~sWHR(CMR|w6&kx5o9X#j z$=rs93znNhvWw4CKqXc)v7{ZncxPIwjek+B(4MzCrWBicd#vSuwCyC-S zhc{dE$-udQET!jZQJ}$ zoaYz0^e?KFJ#d2H+?E9*at?{MIUJH>j;ovNBk%qvythiaTrYT`K~A#tP1aa|?MuUc z3}+p!iU=}^-0Q`lHMMJu=l6Elsrm#PLDeWzft!;RsfN_*4Tk6 zh5bL8zB($(?hAK7noqjB28IUdmWH9bySt@RrMo2s8M;FnR2l@NLAtxU@9Xd0d)E6G zYi126-W|_=_H&w&dz-5L0|(z4a}W$BcI(OfBNZsT^@f*aHoUlUb0+)RC7J$QISU+W z?(+&0J7!*K8D-miNnGqh>)P=<(62+|Qp?~2?<$9~R8RT`+tig!LZj5%w8F1gZJx@4 z|Ju>+VX9yJ=gIhj=l=wRPKr-7p&AC=)MHO179OQo-NaPhh#iTc)JGKxZ`d?)9S66pjur!vCuilq==m{5qQd^*+UN^? zS4lS+!gCATrinW3ygm0nb$aRNqp-K7DlA@XZc=(%Ha!=)=U~$Acl=j+U{}=~8KX$( z167PGeOt)#)##+oxxfr_P{CcSe9@a}-0$IIkn>c|^%u#H`Ac^@@!X zj7n~iWWNFr7F(qkc=7D9Q*y;%_@JV+?0-4yiTX=A=kM<#`-Qq$ZZ{%g2CeVJP&R+;WYGDW^&iS;#D2Y5BFH;XR*BAr72i)H?L z|A^zq`1Pjm#Z1!i>*L&uWWjAMW~U>gv>OriHcX3y_|}l`RYdQCyHsVs z7=-!*uhoDg(f(V_Dnh=zCf z;Gf8ZG~a~TCGq^W2)c#w%~X5TVE+E{4Y}*SXO;=t#|}j3|1SM$VD`~m>ETtarYR)t*mtE z^D&_^c9rF>L>F`Ke|(eBjV%$l96Jv~D+hzl2Ziht7vqkW@6}zr+SwzP?0Ju=-HC4) zrR)3zenmus@Ds=#56z^!5jm8j=vR>+5TFI06&?}Rv&!4*gB|pR`m=C|*f?k~PZHtcs&KVZ(eM(uHGTFdN&Rpo34=&s&d}(DUTM+Q#)bAE3K>i zXdiN{7C`*EVsFo6d1-Y)0C_7SI?aA%fd4O0_Gg;m5rj&;Eo+TVGx&aLiO5)*QN{23 zxE!YX)l$=ZNS#GnQc6iF#@>F5y{pCScUrguA|M~RwqWV!$sw#b+o183KZK4YLtijy7&%ljZLHE zl9g80qCbw~P_tagI;(!7=-L0mQP^(&FVz$J!_WaeikdR^%^@U)X{H(JPW>x#ZjZI? zN$pR?5B}WGBcnKm*DOJNT)JU6Fjx8hKY{_g>P1S15=7>uOTE{;PM3usPR7CB9?-Rk zDy^yll7x#4bQ}gm=xfHG{D4!D8vli-*u<-JlwW~`J{ovRViKs1p~+<(Z#wkSx9H?1 zJth$%LZQ_a0^?H^WknG(kgo1$F3F5kvejjtfu# ze8?}HVD3vN1fY55=wBk6Ee&k$hNuaS^Z6%3X~|&cqHw|p1(1~+yQaH)A={1RVuMWr zXRj4lOjNO(O@9TU?Meqt!?9AGg#h@^X$rHlT4~}J+dOoUzW9bFUkP9*rFJw~Z|Po4 zE=-C2ec$*~Rr9Dfd3N@2bMvWOVKdS2JOr}BE-&Rdb<~rxMhGGPlFovk0p8zs3R3{u zTA5WFnFQbrH8$17qn!5~D=K1W6v{bDU)|B^Us?uzsL0!zeQIrh3`5F*uB@XyOWZ=E z1&g%>eaAq^AuOhB6w+(4y)D13(g-^U&-Sc-x4uBIys>){6TRBpDO+7 z0v|$54pi!ydarFBye5utZ7;rBmI3lpfOHfLCOwiYVJRMpqX5`nTAE~0GeROg-~CgD zAmt|vUCyUdD~~WGReP=@^LN#R5W5K77cvLHb$7dM?!XibGT2SDq3K07QwV$l0tK%G zQ16~v14H60k_?=Qdf2hl^!z+g%nXaYp{61G;%`NWN;sCxT?;-_h?^9j-zw8MR@NFB z7poz(%)b3aLW1fS^PiG& z1LHHe5ci9s&*~f9S$Qd7!O8`BxICJR`?sbtX$%t`x(qeieR!Rw5RlpG#o!EExOv!S z!sux19m{JRN;7EG*(yr6A-z9>@?QV=B5Yd)Ovw(bCNVXkjtNz|?r-ZJiK-dWM~@(e zMmm=r#C<*;i$O3;qN@D`X4QXpQ|adIuEI9(Ruywl@4ga)XkmC~G&@FA6QNW%i3SJU zfAB?ydMIu1gP%7xScg|{ax|MSgVmtepOC21V zC$j+l12<_)#=t}=RxGp;cWLkq1So|kFR1CM*iypQfi72D{V$XHYKAN*-kSJUQ2a{s zthJ|rVH2I1!`3BztvGr0Z&a?QHQt(Ok9Y0uS`{h|^iqUz|*=}s1>M_zhZ7rNxfW%YI~Eod7XXQq0|;IJVf z>CRdnD8_`wZaN&(879%&VSgB-v+1HP$T215z910;)z%F4)RvP}o`rj=p@RUYqZ6x==!R|` zAX^K4FwiCEa9-`|{G$pm7Cx{T);-W6Q5izf(jcpQCwt%2{9+6+2(l5f980`~gjMH$ zkBuD+FcDkOpF*C|u$@lT^*(tzntk$&Wf6;fbRKwJyspsdT+JLwTQL-PYLD7PXP3a6 zc9W9^@cHT&?bGPOC(-P;BUKG$*$wO3L&>uUw=OQqFOnKuW?z#tx{`+iiFgvkKlTvO zVPlUZN6N)ZCxNxNf{O>?=KaAb3;0ous1lC>L+asseYUAWUV@tcStN?vhIxbo9hL5@ zo|$|l8p*nio_!BpJhSnL2QFq>a@c1L`ETxu0X|+G@iIA@HY)cwc0DF;g-KU(*A+ks z%b53jkmJc~I(BjygCFLU6N%tech3F3{D=7O(Eu7z`LLXsT3+lfP%&->4+E@OU0q#u zb#-J!DScWSuSjlupS?m*%?J3FACo&y4OCG(F~Ahk!SeBUiD0F1 zMWDe9L;zIK{)s|F|N6T+UwCPcj*_eM?hE0!fDF&~NDl~DbEIP*vXua+_aL=MS^jS! zWKMJDPV-$AIl89g29R>{78+L2 zR`%X$Y)_*I;4>V4Pc4_l+kb?v0Kl&ZC(-jM`*W@xyMa}0Frg)-+FwH|l$tB`tU+CrQqTpIBZ#-NdM1yw0gCOPwokRqxs?w~QlP=~HI0dd{VL5}A zv04Xk#Cy1Gj+xER$~@S8(wxpolOzrU@G}UqQc+UB=5s$rErYhqOQqJw({!2hN0m$m zj-_{@hR8Vv-{mGpkasXv&{(D1HxL+4HTQ%Z^5DVJX1mAg{Vn^q$Z872l#_q)NatoInfWr%+f)@izBq#7mGZ}%&5NS@0{j;UzOtYafT zeF@%SSHm!rPF9~Po2@L3+JdOJLwH#g^a_2Xu7+jYZV8&8 zh_Q==5FQTp2}VL1T6B!3r86FF*E2uXi``!9q*ZO1lS{pMFqCiqz0@4zzb$Rlzcz%- zWg=Q(q%wIjqtac_E~mQGKBY}=%3L=R5k}0xIP$t2p{+feL;UWKBow_4uAt ztWk{aLxpS9W2Z8O7qG+(gdf9zPz!-|WN;)OGq47N<}G_oq?}SNt&n<(t2dR(yQTyL zpcAr`Zc47_K1)Mb#>`f$KS8davkP!W6CjfkOF&z(LjmIB^XGJw2NxfomaeY6ZCM%M zy?FcfE#Ri}a&oqsx4!`bDe8FO*;2 z>OrG~VpbTPb=4WsNY<*r(BCEGO^{HSQMhPA@t>nm3HPsS%d4~)ZdCNMcFG^2@-^!N` zICM5>GW`!@{Kar_a&_|7`BD=`wPxJGYy+IeKPc^;yg&3Q?96SQ-0trrv|L}j>z+zK zjz>Ce$xWNy*Kx1IfZaG=Z7MxTqscIQULXfVvi=BcrX23Zv~9%*y}GDb+SJw;kmC^6 zHJd(l)&;aqE{RQu{0C1O^6p`pWpFO#H<~5HM^B~&ZyHy&Ep?WA6FmqQfMO4GDAzas zYfw!R7Qg^%akW|k<3Mfc@!?=VeElJ zCfY^S=G z=AWoM4>O93wMKC~R6A?E_jkrP zNpQ`6zOF@(mRFiX=r0PSsR|6+SH!cLaWhE@{&_FPU&_QBj*k8>qpV_|-bMZDn%x)15Or88$mR8&=Gsq@prD zHa6BG_~i2NKizy)0r!#W+tUEFNVjHG7tGbRaMp%aI^WqfMnwp7L{Ujf!EEg5>=&6%c|gEl338- zXs67{*0yc$er50ODZLn2#aeY<+~=rPUKTZ7A!T{~pGSf%ITNt4)kUq<8}By#EMwhc z+EV{sKa~RuUE}!IGNO6=VFl*5#D$?{2!i(k)<+_w-s893VJH)sM>8tM_LC}{ z@Dfac4rfYdblk1q9UL^n?&Rb|%J=em)al?#rjx$vdtnVmbHSrPIH!VyA>x>$<1p6A zQt%F%c{j*9x#(VzY_8i{`1J@Gdsu|8N8L{*_C)7?#U`KPRQKTrO%aQyu4YvAxWmN6vV`?!(PeqipR7L^(Pex~Wsx^ptJ?mdR~>U0;j2o<<+s+bi8;GL?5V#1e-h1d|R| zIi$lwg(<{>UxX>IannV58;kn;Wp>|OM@69)GizySoiBnwUtzGm*6OW-QUoDmI`RvF zD2k|LIP-EEVzmg(YsK^;f%}IOVsc~E)TwB5LC(CdSLNm5X7uHSZEdYhO%|^&{oi?a z=WNbmy&5zth2XMM2JKvpeZ&+daUvr6A0c7vAgme2Pe zrwqz8y(A?o>*UKI3JzsWamUB0jk)#8SW{swBXpx{NU)tM!_sgwlrV&ZICYCY6xdiq zuTgz~xSApi?7~Q2R$cxP6CaPQj@zi~Rpy!)2zwH}jLQI_Xn4@|3jWGPLR}px zI(otpJ{UCr_b)=ogvqlOiiUPaT4Ar;+$T9r_fl68$|Iho>!*o$S`((48WW9~*0k+7 zOLy8Ysf|^~c#b8eP71??Y}*m3#_N)7)CtJ-58m#F%*dF&!lq)v^x65B)WNk(Qd+T= z*TJ6&Dk6+PNAy+N>Xet%Zd~D~7i4qp}oISbk8~)zxi$Hfyv{p& z`KRwWrs+UMPkErAQBya3y5MkA+&jY`RiFyuBK2(?wXiF=Hh_=0-O=YiD2PKyNQjsB zJFjS0{rB(n1o7%@%B3N_4;_JUr&U#6pwiOHCp!mbyr>jI4H9ywfZ3YHewikDW+Qjd z;m+0FZj6Tq@PKO5m-H2!N5K@%&SNzt!*K64Gw4Qz&Q&6M1h$ZLgJ4n%FM)gZgPXn)> zC&ACNvr0kPJbmDhzwPufpT)|8v>MB!j4~!IjYswzb-fxqerUq1zS zz=Je3wd0$b-@K1aJR850A4<4WU8iFHSlpIe#3n_s+9{1v+q8lfp=O%ZdcCXQp^!jP z4J<26$;sJh#V~(mUeVEop}9Hb@Q}GYOA);xNr0*>q38 z8ZyvmQ^e)XtnU4g{Jc3$!<)y4A-Z4_M%jPb`Pkgc>3V-kEh+^(odxGG;+465268ggqYp3UEVKGzVr}`l6G)#0Ad0w zE2|d)H{i4b2Q^?_~jf8*k8UD7QSgsHZoJz2AOZR*K}VT6Ydw$jBuWcGE?z`4$S zBqs-*%^c31dV8L+x&=kTrzceucrsY^B~DKZK9iv3KA;wa)In&=1_3u#rWbhs} z0DHLe8n;esX69=ayL8~uos}@02>d;X^3PMPIJ%k#2;URn! zei{CO4mLK--&JMJs6tSFo++Z|ohD2XnH4AZk>MeRmc=F_4J&=FI%VJumMG&FVT#fK zVbi6VO5|46e?~}I#9ABui(?SQSf|!WF5+ORHZJeFq-?8qy7togZyuE!-mGuIH$$mu z$CLXPe;LXiY6kdLW8QRpB{PmZdL37-5e{0^RD&;Of+tdda#c!|!vA8^ocjIF$}Us* zw{r?6st(V5hO&oEqEZyT1>dBkHDntjMrB|N%G7s43$jft+I{zGD@2W|WH8Nbx(}=W zEmfMc8+&Qy>DgFPGBESwgYi^s{>`7W&s3jjY}KONfHzG|=U{)ouI*OX^25K8-U&1j zB8Bw#_s6QVd_tAcZo|1_XH9~G4^Zj(6hp}VvdQ(d`E*EY(v39N@^zON%;w(x&7!0A zOXmuo$TKq+S8oimIy<$j)k3+EvZ`O%SEg_VpP=iV_amWi@SBElmljJ= zw-&sUMg>vLTy<;ye%)^he9W@8HvgnM(^g*?XW#sL+=)$YD1q*JB+ESLijK#yivvlI zn2w;KL*WxbHz)@;op2|Ea0hPfXD2E>2i$u}4=#|V?8uC#ev%ak!W_c{jhE!7%c-qn zTj!GdC0BQ3&+kR7k?L=g<{!Y3lZpP-*pv*%9@E*kgvYV z$o%NObA9*|FY^ha`MJdN6G@4+D;^tS?SM%VLj z^Ng3VeNs*Kc-3~uzG7}QB#w_SfsfC{#W^qNs#K)gCS;$|oDS0YMR}K2KTg#zD*-<* zY(B$WNohOlyHz{CQTh5zpHqlaspdP;Vn%=4;|f-CQofR6W!yf{FVF%9dx{_QO~qs`sW!ZZS7QL}m&#ZfD*>7jca#~}nT-R1kah}Oq`{f~ zD4dCwH0fq?qW~J{Vkf0pZ<#>4i0Of0#y9ff?zqwP#e8q2EqnjUZauHs>qeapUMLNsi?d`@u={NlT6~)nT$}Wj6DOvx@;5Y;xC@Jp1C5?3E(uCH^mm&rZsjSQdAxe=^QqIq~`dd&?KK3F;E-qVFo6!*&MbgCM^ z_C29jY&N@elSS<{DEdF+B{(MPh(UP*xM=HxA}SF|9Q^y<(2#Ff6v*IZGF4On7II`O z5+^D(L%LHvqi}mWV34!a^A2s8Qck{rG@~~tDd|@mt8lMoc?>oVg-V>3dI6__$EV2; z4RkFYNpfoWxdB+adW)!`@0AZnJ+nkIPSyPq6F*4A%Da;3Y3Yv>lab*yxGLuf+c7aw z51xE8EyU4X3fdjd=VL9RBx-}BYd*j4?UkZQitIDjo=jd{a3xbz+ki553S3){o=yj3 z2P_g@k#U|o3Fqb^4W5<2y|=bG%*md84ZD7Dz9&tM#(q6g0KV}JUjK?%l~i{)x)dn?R(WFZ-#fXSA#d=4T_} zVfL|~K2hm#yieSaK0OXusLaL=loOV;E4#LY?~zwu#|LK2rgzeJSubhYP(_;>R$d>q z)t>s!E=nP66Gaz>-{4Ax@WucE6)9+*l4Il8W)uIv%$$MU2Yn;JD*3PiR*9zjhVISS zptPTwW-4gSXYRaVSPyj8B-K1`)|L#-U6Vk%8k;4Zu$`S<4QgXQOgQ!HXksVEhh5aa zd%WWFFWM>r{RuqM-Pje2$W@QoVNOI=P#ckNMiF@XC$$(ea$89aZx4RzF8)Vjvt0sx zQutpYkkq$$eNz=VReP{5ElSu{Ps&Ra*QR;~OEdD`cL+}o9ex)5Px7?w#7OAR$8NN{ z0J4Is()F4Ky&)v|heJ)K!z#QGw;^uy0Z&td_dq;(CuK&0m^(2sAw^?B><-%7-(RQs zYy}u%N5`WS6;ZdQmdD=S-oe4ao(RnQw@CW>`tjsKA2vS4O>V^3SmpiMd@~>|y+7K# z5>N}PYw*qfU}>S z{yc0piT2ESg{K=4(tfNczu0~xAoDN7kl zHxfe^sw`t;Y7O?o3h%s6kdTr4LaS^|Bodb+O!)9O+X9|r{lXKh&F!_(TFje0mgolr z7#r~t1}XnzySMNF`99!c?Pr->k$!TU;^aE^+_VOosjoL4PeV!y z@Emy^hQ2u;8++*A34hQ{Tmd7Xie*OV#?}TYu)~f&&0o1vL7EY1+`$O)u9Y{f%0Bk_ z$fx=^ofakdi1V@aAFn2fs5!+vHhM!@L6L7n#?J6{38=&FK6Mu+<0^KBo%zx#;)P4mOqD zZ=Xz!rwqN{idgklhO`B-yTS^wu*aaoe|mO~-JV1t zj2V%qS0}no%KIhYX)gHr1oL(E^~vV-MCJ81nDb@Cgl%lRWX3RygxU4;I|Om~elumV zT58UnUc^DIf(l|qoSae!T!?;f_$3=+f9%(9h_96$$xt_W*NxUcE?^7QPusn|{>=E- zfNOW}X?SE%k*YDxOj)ZOJa`!dk`oXMDF$dM==SyeIQp>$9Gu_PL*r2>-wOvG zuezPi(NL8vDwHrE$)C#vktjOgtaWdkoU+2+`2(L$GLZI*k1=x}4ZJx{&VH*au;bwd zb~1NhfkZFU5#p{t$Z>c=X4q+!9bx^=(x0l?;NAn0bQxqBTKeWDOHIW$vRj>VGsR*# zqCq>}aqG9?tB*(R`EL`c>T43vj=g3X;YrybXwAKnLe$5feycvXF?n(2j&)C9V*VVz z)SU=C%GB0Q*ELY6tV+vB10HE$c82zfZ9r=k!@sQ@ud0>-gF+gos`RCpV>4~Z@ErTZ2L9vfln$>k*N#j#V-?~)ZsXerEL z5YB0lnxDgzmlKmeL{Z~=(x8jr5n0CExyArza78F9{ZHz4{E=OavrIGy;81rZ`@*%{ zmwJ0YHUE3>x&*HI@_nBZVt?|LtbFQ5TzxrP?Ek9L>at6NeIlOiv0MBn5UUp7H|Zcb-z?rr{ch^?NUk;f$h zwd46a#jueGu!OyW+xN+2lb4GRW6{ZO8-y{Gv?%JjK#*BU@_sf-P$X%f$B?3TP^m$~ z_2+hPwoal_SAabza`{w31e{Cv(~Vz|Jq|lLSUD#U$B?KV1-n-=tSbQ1MCfBsmX?-&ep#=jaJ!R}-ODdE17fEfGAlWLm;an9R0b^e z>b@#waGE?6+|r5)^8J1-4ZTqC<~7#0c}+HH`}P6jJb&7BGu?VpH#?W0o78V zHP7U3QS6A{BM2aKu$}>&I98D-%A`|hWF!)?Yq3=eOcmDB@GI~cet3a4H5zfcPoGSi zKO^0}Jq_q!>pLg^=M+)Yk9?M%wq+a+LkUfUXXabH&DPb*?ArXUVL(ct?ZD{qr~!x- z45ctAVfG~oddi5r0X@F%um1UX+IgK9Rk)R)G{h~vptV8D1&QkZ^fTROGpOK&ZZFxY zxvImUcSZ8d(W}E*Gu2agA`K0&Veh>>Jd7-++OfZYe?IdbKn2vY(0@>y_1J5J#IamZ ze>+icwEC%g{+q!!EB;Ty6sqar_obmV)h4@=umKd9bIKY&S$Siv2XkxNi&91V)zwwx z<%#W@bad7)ldk3E0YRa5W+o;$Q^zA2Mu)bKA3rMUZ}1UeHLQC|N1g9XuOfu7uRLrV zfodS3F1%p1|Azmvk=F4+jI^|tzta6;QBX=w?Lm;hHe)BLC)LhB@fe+STh=jtH6Y+_%Lc$-m;7l7C(c)#` zV{6Ey1g{XLagv+XT-;&`fIq_`DMOp4YrWzXdh1l!fCm|FuSLu?FkJY!A z0CkeO^0YU{pF*<9(YX*cQ%O>Wwr+Sql`O2%->jN52=$)Cq|<^Mzf_i7Wi!qtI9Y0? z>rWfGyAh-lL;KR-ieu4j3GYAg8wiqv*yPmn6iPDQ@lEYdh2@zyl?RGS1q1xBqLPY) z5r;XCJm^wYtm@x9WskWhL}_d#MU1+(H1~ErHj}QU)zxuCAyN|hR;(~Q)&LD6P=5d) z`EqeD$Bd`ImomKC@$g6g;EUTktRDjxHo@{irddtvvM1U6quWxyT$bkfMLFxn%@^9; zB*mSZYaLs{NIPP}*6?AYadE)oZ((uU-+!{&Apmgl`FNU|+On4BIf9PtHQ@k9gXW;z z4%2?~Lw^}X{he!GB*z_MV&d0!Rw=$XY?hr|%wny=XE{Ld^1>(N8Q4bFmO?r|WszH( z?T|h4%PqrNgQ#a~1`9IgUb>dQwv7$#2Ej3J`PHWp0`FzIG>doJi8=qK5&?z#9U71; zfU=1;F4hf)KX$&}_!}aC!iTE5>BK+{9fo4=tBSin>!Ei84D@;39wjnZZ+{k2H5**T z;O_pxJ^3{UP6QQWZ6#vYb0(j{J}~dx|)(lyltj#b(x)m{W}?~BA@dk z-Vx-n;v!o6wtw}3boiXctApMO|JP_8gzDhKhbF#iVjdUTp!rlO>7lMUz%jT8`ND>*RZ&sd*VNu1A|n%a+vpA9 zb$uEbXjMw+y4lOU69TDD&CH;>x;XxBnPJGrAtY|DwwzWNRMY$Z?!%6%_z)^>Dm-j= z{wbSf@yJ;1>yi*UN71Z}=0tLHabY*Vd8}ohhqRrC#TyWXvdWtfgY&^?gHMf&Nf{dZ z1W+Bi1f< zsEUouFAbNg|A9=gv>53K8RJ+XN>@AGAJ+Hw0D1zm4lD4(g#|FH^L0tk9vH!euSjm) zD|Agy*N1MWYd=%_=md}x^{P}z48B->k+1nXA`AC&UGwbl>}8;#VW2T}`sR19`O$k{ zXASqXJL{*jjh=|ag-p>u+$NN5euRLS_PE}u<-dtC*BB}4h$Z29luqwO74~Wt#m&s5 zN&RBRm)}~Bd|FjdPb+THV5R;h74`}7=#m;meOh=Q1rPK&2OjD49sG9m+s_^SKMagKvEV!|MY5;?Sqf2Z;H>SPscG6fiJW~K!!!v_ zNNE41ITbgs32ws48A}xGWxoaVB7@J7gA2NitIN#pt&U~mQ+2d&Zf{KK>l$TB%QR z_8tln$rLz~5_agYsdWg5Fi0C*2Awj`3K#fzPv!TB?SeXkf*=n4Lgv0P|16{d&4n-k zZhug6#5Jtveh^1$`p#WB?D7v_W_c8A>G(6ha3WzBMV@tW9-N5IE-W~eL!xxk_IB3L zvc=`KkAjo}<1pd=WA3R5xEt>6MQHvSl5|S>DoQxSb*psb;YuE3EWrkezT@nFj*rhL z6QigtECROU!TB93s03e{1-;3@tq>B0_&v9BdWTO#NQ20&DphnM%)n3}n~D{hUb9*6R-!CW`~X+p#2Eq90#Sd?Ud%fWGxC{==Szq}iN@h3Z86A4|dO&DMV zT6$aE$|TcQsSsj(NwMF)OY|1ccQrpEib$_0_tPKBlohLj&<+)GszZ9qfAZktN8|j#V8NOqcm!f|H}G#+<|Ynx@H=)LJn% z{uxN|1c8ti^PQ6hPn493N&;sXRbSY*?BQ@Ac;9APRT@ZBic$;1Z&w21R(>3jUlIRt zC>#JDnJ?HYYO&W5N^|Zb3{2wciiL$mmpG%BKJ!b0;<19Da`Fh;2ny_dS%qVZx!F;; z&@!)_;Zw9JcXdiKt!@y z`AK|o5$62X-aD~q^YCaX4=x`ZXoDaW$bLT}S2!bfe~D!$k8M>MEE^g1%drpuiogvd zG1PLl>sIi)z@F?K9qoLJcXQA%k3=nPzgub0KRK?tXueaW=SuxBbg?Tozp+uM6h%uA z(`U{(mD6I(^?LaqjF4qXzE(q=(II7dets|lEfko6u|$0kI~`@$7wGHocl+|#Mz!Pg zd;ADtaT8GdY6x)dsah|J98#grp@qT>SEzBd6q{_>r5=5NYxf_dp#M3ltUOC0H0!!G zm?u@}v6{k?IOZH=*dI-?=U7=yvI_qLx+5qyW>uE7p+`Hzjmmb|dbU{H+YHw?;Ko5OFx>7rpGqVqg9sHN59QrRbM`lp4Msh)8!OYcYT}{+ln(Vo`2vIT9rKt|PkyfN-#=Wb4Z~Ez=ahiq5=cr1 zeeLeH`bstCg@+rhlkl5Zo? z;y51__?KbHP5eAIO_qU4~u5D{Ng| z&-_;(R{#h`_r-8a2hPvmU#PtucrbCyw|%VxCfqY*OLZ+f=2vQ_Eq~iyj zG#`(P>|qcQI!iPKMgbWUcFduCS8WPuECf_k+V~jXX52|}IeC0Qd|WdM$sjw}Id-|F z?hG=3mly!36whURbC(a{*gwvvs-N0mLV5g=Bh;X)B-JGFYo-zv1I1E!5MW7w zaoYXb71ub;!#7~udw38(auM|gS^fC9P{|H}Bs6mvU3h_59l#{m^n#?-FgE5V`SRqa zOcu0TQ=9eQf=rE)MN@|G#M64o50fqRlhB6X9=9;)_|8gc2H7Mt(t8VpWaq{N-C$qY zmbryU2!$upJar!D<4A#==N(@-3R_&YY9vYi4Ad1ab#LeHYBS*;3sj8|frq`|?`2G! z>Ujb97O4gt(AD+zd4YM4rdn2ViShjzKAY`F!CwE*{Mk4I04c!PaD&%tbo4ko4=Lo~ z`nvP-a-xbAMD=*H-w&SWgdi?lAfX=8DQWywj;v<>qMa$>{#88LIMwRAebk`MyZctf zKi(gyP?FOo6|-c1^`~hL7r2!vq_u zf!>OIzBAAu=3wu)(=Dz(1(^^boEf9ONjPam0eMgE=a)=*C3`aAh>s-ebU;SN zhuoVM-rGhcu9t?ntB+MeaX?3sH8>k$nbeU~k&%*ij53kH$~7Mo01+f`!$BUFEfZPU zTww(DTNIZ?`*XLO)rOe#h?s}B%5jwuIO4Xf6TFnus7 zB2>bMH4m@4U&k%|O_jQ;cYS^O{bd#g-cdGZnVV(P(yE)QsGGNRIFzum0!s7-Ox2|Y z{%p=(!baXIRbp?*YkHoW2{K-NICDki)<)-5Vg1^=FadR5VC zKZ>Tfm-{Wqp5u0uueOLgj4D#{s!s*<1|C@NYb<)0t&MsF?b#Y$O=o`c*y%xX|e+UOEfP`wG@#u}5R84tQ!|28ga zpl4vPz(~^EbXnT6I^iZlVZtF?z46cE0-VQyB#L0963G72>LrQ4TgIeA*)%uIsQ&1< zQa{R)?Fz?TT&=QPGpJhX1()r7CwZ4KFmG~Ixx719(?l7DS+c**ZH1WD_Q9xISZ+rx zQxXf|96lX)piSlJyP~f0(g_z^oGKHE<&#{P#cyThAot^}V0(i7gwUihQ7nbO2%G%=scS}Hz^haZ8WTjcHZsLpca8WuCY5c_YA?pV(Ot(OS{kHC0% zh(2Kfg_IQ{fb3d@$sd)Tk1e2jaHq?2r{`%NNrQIMoUDJ^SyC$0wwL8XGuBbW6eS+4 znMO5>03=5^S5t*y#t+&G-38n<3J%jr(UfzH?_(0JHkJ(Jnif_0DOXSw_RDU9a8ir? zObrD3V{csC)G?1&zQjjf7a}M`_<1;Rs+EuTdyLLEi4UAAr0>Sj-I03$T+QbcU#BTx zN*yY|1eG{hT&1jbwF))sdC35-yCS&WBQy!PWe{tH=cwx(b;1E^VgRv@g>4guIo}eo zO1M%>IBn?49Io=WWnN&I5vB%{KttUa3$>UUjjbw9U2-Bu86yLex{=@ra$;p9D|Of> zT#Zf&aS0SIHUL{>n4GkeUf}8LU@t2#g}ZMne+Eoop3doP;dm|K=Jxd$gRfQ|)JE4~ z1c6KfzKq2Xizql0S^P|BSa*{>Ov)at-Zso0ctqFe(PW z#en}2h0CCCU;x}0bjukJ>|RWtZXfI{!vLy7xhuKwCk8P`JN3r1Erg5IY{!t!9L!K< z_m?o0-e@qdJ!hBE>6^Yr7W{kd(a4Wl8XG=&>Y9gG2;zygoDz&c^s1(viy#x5NCc{A z(L5q&t!u$i=(xD{k6Ug5iY!3=feOhFLy1ntL> zN-*@ZxHfj2Q_>{etGpsfRcV1A7UiduWvIA!SNYoqOQk zE`qP;QDark-;3=|D-QB4i^C2FzSq>SR{(d)voI+C)>@3^Iy5tR=WI zm10TjPFblUcZp(r7b_I}WYN*GzNY?zcAEgKo@%*09{#AdRY+bvdF8TCO_TXNayO06s4gXC=w>BYHog6!4-Eu}|a!A@Jw zwe?6l{VF67R^Ig*#TMP{(G&=j1&LEb%iAdi)1iXWt?HGVwB+@Z@({!lj&_P9nBU5A z>t_{%V)Plpj^Ih?%Jh@V!FEbhmHMU?{F~gAgYOkrChBUOT)tW22Rwy;3S}T?K<)>B zZ}KTQ2V4gTI4Rh-NMDGkd7J450A-z{Ez6g5uzo+ginX+@$Es&63FRTc=$|gpy00WE zNOka9M1_O))v+4HF>L`R)^Y)mKmu_9-q6~jE_%6O##4j^hG5MLbNNs9dalypz5IddlNxrbktd4`xTq`$yEcTrG|@hE@j`kWZ4EI4ntX6j#Z zh5y=xh?<^8_Fd>SwbLJq#Qg2@Au_KFD8f5wYH)l$RJH_(Zt`PinkUaWcqtvu^r9?E zWu`UK25%A}%nuoa18Tt5EhxMlJ(6|xUG+1-`T&X%TnNgPOU)Ukmj2>gri5n3b)a>00qOBor=!*MhjaACfj7M{j^K zS}>e@=&S{vYH2`%1|&h#CkyKYn^H;C%3g|r;cqQo>*K!Jyu^qIp0|M2|w;&KY zm+6ug4iy?u6-8Yp&};1*b~26tQWl=Eqy?^dPC@f-_=`6>MAWh?zH!_Nx73-@ zIJv}Ydyrxc*oL!bCHzp%^iHiDKK(Y%n9G%X&>98nc?dV$n)aU0NnQpj2U7+-*|dxJ z9xy`D?B9JHfZjk4`;(rbC;nf9svsSSw%SCCIMj_(<}=f6o&;A##^)#Y2Vw{fLGUK6 zkFP1_;ub=GW8X9OphVt{nVP=x1E~Q{jSL9DV!lIoK>DaV30iG*6d+Geqj_j1ndN9R z7+U|fkn;*uQ)&4-3!}$C9cM!3hf`oi>?TA35o9ARmIyK!^a?H8ICUxevHhaLA`Q2A z%0S~pe832+23Zz-s~D@$@2{806exbI@a510$q&E@!8WgQ2g5jtOq`7Zk>cI^R3;4G zx|HN{BvTM-d!T@Ts_=OtbN@x-O;9#4w5k)s<0yR;qJH0)>LjWEKub*|``&W?C*IO5 zKD{X=fYbMCF* zn7NHb*dQGl(k-K=Cf*<=l$rMs<-#@np@+DJhzcWheJ$ZyL1xtSOdG&i63T#a6K84k zON>QyX=Fjz?8F09Jk+KhR)sE6AhpCR1m_7Ioo*8a5M6wJURs0gZ&|%5OHu@1CQBQbqM^d#DCQ-7pgZ zQBcqjwgh51BmK>EU}amJWouhnZwbZ4CUid5kSWl%(XG z3p0Vt%fjW*mi#Q66idwUneIZkc3*o zIRXSy73jTzD(*S8it&<#G3TL9X8l(h2e=l$eR~!tYwS3bs8*6Z;VP!c2yI>O5c@OG zos)ogj^DZRM%9!hZEl+hQK69>;hl-J7?_)U`+aSuY2zm%YF9q4+Oa18Vtox6{xBM! zJTmo3@Ac^PQ_dSV?t9UEnej3BQbFWK;2%HPxds9cqdb94Dzh(#c2eQ5OXn1Vy^nwL zofu4UrSmgqs>VHai)0r}8sjhNd?tM1#R4-;L)tFMQJT4T6BAB?sD{&BN6oFw7D-=3 zKaY>)69>Fjhv|AYG)y!RC9*kVV_u-B>+I@E+^{t?tA)T&2jE<>il#6Y2Fooy^5(gb*3U6>k= ztvZuLbbi%1!#ocTsJdD<`u@zZAHPSd9h9ZIENXHlyUP(^=F~V{sf$<5tX<6IRg@Pt zAhysOedF#bxdXj}yvwYb#vMixur0!NEt7(2K@P}^W2xgQp{ZrWgBH?|FcZrrn%9J2 z&*5wn(0e}i7iW{TvvyZ5+c-*1a=ZFaEbueI{cc?ctt6}=j`)zKT^3+Ft!obz&%e9` z#N}cTJx{xWRh@_D`bpaTr2X>0a8sal2?G(hs}3&pSGIP53(uGYULv6~dp;E)FH`r; z**ZejUZ&`WJKfl*;xj+Z78Oxz=u73F&;9&gwD?eW%*_IsfT5bZjK12RDd7-d*7A#J zkPJ2WN*M0{NuDZU-i?$b_x1Cg8kSZ46KPEiPqe?mxl^e6t~}(~@C!u&4A_)ywKFCv zZlbyc5j(MVgpg;RwP7!fg9zLL^uAPR99HsDh`-%hHH!_;B3R~Qd&qM`*`2~?U0!hT zgGPZcDmZUUayby5{VLYZylb&TWrrr?K+FN@U762}(H2oSdAs zl^}%yfAL}UsfokVVj$dM>C#n9fQjSf+ui6i4ncl?LH^W2(_;42#kz+zionFepMo2? zZcI`?nq_+HzQ3Ucs@3=}+E$_VGVZJ9o@VB>=H|2?yiV05C0T~#iReQW!f?Kom;v2R zV|Wl^Pc(jpA3sM8M{oxPq2KTmyUP{7NUhQ z(R7rIi?JiA-7x58jF+IsnaUxP5>dxw2o z?dg0I(ylY8j?Of2tN=mD<^RogSjzVHxSB1F?yCj6 zY|RRLLt?Sq^W>x>6&+gHjFW4i0U?NQm&xO>PqmPN!O4N(mGTiWutG#Kd4KU(61LZg zU5yhg6f4}h$Kh6h<>yCzRt2h* zam}7}SUaI{=bn{d-*_oxSB?5CJoiKt>{}iEuX@~6ReXM-6T&laulKaM~WtVCe%}&|dH77fa^%$&q4_-i->7OSh14j87uY%WkWpmv(C?|)dFRfz6CmWj}>`(G7y4&OwxzyVk0W(Aj zvsLseT@{RvFQITSOc`zBL#5dp`7MSuHf}=pknzIsmvnd(Kte?3N7ZDlHP3Dl!@-4n zrt?BduRSEO2B)?wHLEYgwTi<$hygA7H#MFu1<~fjO89eCpyC2i_oi~nT0W-cqm4|% za}Nu$NOa0K`EB|cQE%e=)rGfHBAxK3T{))@LD!pniF!H~fB3Kk#ZI z(E_Z;23&6TzT%?ZCwqGZN&04{YLtTB;-h(h50q95wai_H|>-lM!kNgfsiJwrVMailZgBWucc|&+0 z!NqP&FSm2a0THXeWfmdLLEShqqsK@^EU5b1)SMCrGMUTY~ZYZ z82_hwY0K^Es1GJlJYHGi@$s<|H5gP?RdoVz-C0kbopFGeguMuY1MF5jT1rZ&60X2t zc^2X%aj~a8%lF&qhomp#{7aI=rkGjAqKShuFwM1%n(g|YCCkR*v7?PHII&YEFgm~_ z395hx)%piuG+yY_*8!Vq(PumMTp!3Ns-CF{hr>?0ul`~+aPazbWSA2Ar)F~LYf!d` z!klX(E1j77LY0lrES34MYCkM514S7loGYWtKMve)j!}N9)80#)nVXBUse42#bP%`7 zfS>v#Y;%WkwJ?f36pXPh!#E`A&mStEv=wOE@hg3*`wK&hxBJ>816~GFokBx)q1a)s z{*dhExio+Uct>Zvqw6-?A-Q|-i0~6pC9RabP z|1x?@E|7V*W3)S`Fc0l{5W%&E4r)v1IZT4bmxb?K2{rms9Dk;?Y;6J6C8zGqG@>j2 zd!F@U6BgQgir*o7CTa}CDUUL+y{9DL4A734QfLB%q>lNN08u}?aIBpQyY z*$=^r@BoT#W;agmxLKubT_co|MCrjBz z5ul)fIFOKgmFHqX-_rL%B+|sgBe%s< zRoRjwkyT1Tvd@yKK~J?-O&VrNe1c z@ULYk*cgPHa4y9?ke*OJfM+kba3hwAI)1mV>EfUq!mmN$GLWV8W_MGT1WSR}N1tDoIeH^cQ$Jt^to5NRbh}$kMR>dIgyO!W2S)7cWuD&oAAWgc4QGsa#E%ShW z(z#VM30BFj@x{Tcw)<@o-23MV&+py{5?V58yJCW<4mcTlFuue~z-lA``auvU-*O?q~k{*nj4^SJQc2 zB|hiDeuv*-wZDm4BMf;3{pLXU?Ev5~Tt?&ZP$-kk5^Wda^x8o+)rkJ9^bEh^r|u35 z2ZKn$-uxCfo84|@4*-8wD>DBhHTV^X<;pB{9;`lZ4t)FOzhgM(9g0F$BQ%^6IHefs z?PdZXQ}2CMiF9tw}1Lx|1y~?$Dtp#r+jtILneyO z!A<`v|KD}3_P@L!+3Niww+DBT|u+kJOb0?;e#k|Mkv)LaxNqAG_*Dm9d?+ zOTNm_hj;ID4ir=}$e*Xwhdil;3=IIq>iVcRSRxHTV_L-C|D@e;w^E$|@cU={um76k z7->WD{r<2p1QAY#FI8&|f7P~-A)U`&9($gdpfYIr{;c=y0Ke%BmyPzMvkUzgm;MK! zIKNjpu#V7^l>1emuNe($PG<`LbfPH60n53_Vfs$J`vQ82YqMBFypo}}Oj$M?kGWm4 zxg`$WmJQba{O@H;$=&V+vYW8@48C!4#1tg+2(l;NrGNW(^N*F2O?#OL^1rGe<>D%?Xd^*%?s(~z~5g; z@181m=BNmMma;8L(6FKIEpdMpP|B~m)XYgwwWQY=EnjhHCqHNpc&g5Re3joza-5J< zZI{bct6R-0yGbR3V0QQ~q~f!;)icsPjFQV0g;#}Xg4il8`oAWSsTT70Je>IaRxqyHWDS211|ZI=aS>qt2`a;g`&(v`hE zuoso^x|Wv7((5<>Yeg?GD&@C=LOujZ$T>CNZTOP-9yZxC0w7!nHLXY*Iq&hA%}xg{ zPh{aFX9AJc?dEaO1-`0e*w>ltxL+*63|OOZk_k~UK1uq3#DB)Hva){GkONyzOiC3Kwsb$e9u3d z;5h=7_#j)5f|+YvFB0UhL9!-}+#q03$y8L-8@;Tojl3Z47K?qlTW_5d)8D;W!|#Qu z|Nd2JX0;}R9FpRweSz)l3l%AD^kIqAB?V!c|G(Ia8@*q&+3m*3n=i*2DiwJDWm%MnM(_F%NovhO6zbrUk`*m6)~4hIaJt-TkN!r5`i&#pHp| zco7)R%;6X+e2E>V;1gX=TNC$LdoKpY!7CtyN~WvWf?;t6VomfoR98^Q8d4<^eVw$! z2D!|k&7!V`Mn9#szF0qCwjR>h1qH3V_N7_TpWKx;NnRFiv@I%37Fw3KKzk=$k4W!E zFwm_ak4tpNIL>@FklcB<&&gRIY!Z}=@JV+e@;9f(4yByXyX`+s4wJ{c+#|D43T?@x ztf$`_VRR5(4SALdNO||9zUL!92@654F*c|vgAc@)B|It#`j^R_3%=J>=eCN-4p7+D zqoxFc{YushK8H10LWth8KAPkaJd_ciSb8X?(_F?!J1?hPHGK4R{wNcHfsk)s=+HJ} z#?z^8oS}M-gW>JP=~mV444M{dHP6HohW)?bZuui@iU9BJjQ%kse~D?kE0mp9k?GLu z$@_VJq`_TR_jY9_$;o_bbO8LS{+Up5IZA-~ReQ}uY#+@=PBqg_`np`$Qb+rHD`BmBzi;Z0zaFL0{{(K*csV(|Ihx$#A zEmUb4^sfA;-Frp}Y|?ZOZcKDG3^ok=S20(#yNURp(EZ7bu| zb^>`ypS_IMTCTq%h>A()SF%=Cd(O<1n0|fUk&P2}uW{g&Cq6LyT92EhTC1$HGrL`w zvx^b>)SWV4slOjWO9t|*Xy&ir9@mcQqtVr!9vm#pZZKVadA#xVGEep6()AJ0#+6LZ zwRpT!WMMnwyC#Q=%N=Z6Co|5TbrGhOv9KR7!6SX zjza}mAI(l4xj9Gj^0qcjSpz6qJ~lz!+GaF=U>apZIOK4Z|p8w7B)A>4< zqp`!U4I#K4kRz_a)U`%^c<@I0$<2!bvcac%AtkB5wEXw4xrs+jlIZkIQQQ}8OTBH@0 z?Xgna$}V)Pb41uysIqj`B3oS*aJnKm~x*qTCG>eU--YV>ql)c{ZpO+*W{uGQ615`03C0?HeTk4;KTo@VUbv$>M+sTBkNgl^@s`grM z1}0CRIk|gG6T1Zg2`&K6_}rX<&HPV^u6~fw{$AdcN?K0Q2C8a0ST*5};A;CL&~IN_ zof6-<&{S;*NqB{M9~aC1yX({9Eb2S4wMdyOz{`7^Mo_&c@U8+1@qfko_d+?xUgO!n zFbiE@=b51tr?P&#o0XjJP<}yYYnA=}oVn0$f*C~ZFLqE3J$wHMY&Gp@g^c%=PQ4K% zHE?94(2kJ-92Tfs98}8=%;*MqafqoD-xWW1cpeU&^Ke;H2ZZ~>b1=37@6F&QSsM)$ zRMq5U4cQ>&myscfA|_tz{zWV0_y+kGVw>|V59q`y00Gn$5F9KR6;;)av9JJe*V*5e z4Pt2g0DZsuYHI8qfTc~&qC;ELcHk`f{d-I~;{_t9Ywd3BW>0{O8C;`*e43FV7quZP zoCtM9Ucd#~WUwbQ_=M0#{|vMwCW^ZqAEjq?tc?G5L&i_WLKuZt=({FDLNE60@RPI) z&PY{6LZ)IstIZN?-FJ6ts*}A)BC=zsUDHM_sDD^V%I?eQ!X3&BkqVNRDvzCkw!igr zcKitbMH{4nRDDW@W@{LU;fn6P;kxhJiwKnZhzNKdW2s?h{P=Ca%BH6M?ahy071`$LBxI(dJ%xgMR!M> z=i`O2B7q)ppxO6b196FjJ>4RovQ1aDeY|?T=pACKLo0X+-TOHiKI)pi-np{&ob>6R zI20^Uj*WvuV(fhllj#)(HIz&u&%~z#$M0aS7CXYb;?x5a`{6NLXx9@>dmur%;$ed} z%T0TFw6lCB`*$8_aH7ib_bpio^wiPiHG6Iw;Whf>9eHC-gw9(oX z&`DvBu?t}7s>J>OKsWBMSGJ8)H$2b`@?SU0tc~HWsCImRE{>||0g$1PX+Dz0J)V%! zG}D3H*$*@yX$@jK8e9|zH>Pd-0eCv|(BecI&&a12g}y7zoI3`OqwxTrtQM0QQ;6E> zxb&E?XRo()+~?yrG!!FN^DhhjX5cUa_){a@?3i3<7Vs|2RJcX_!BIVyjhX&W^%wDJ z**!X zwGXv)8yj!9Za7`1hxUlz$xN$!@!BW1R`m4b}``p5lH?5 zY{V>x6@`Vgs{j)#H)Tad)yxbcko48FZ*;u9I*g}PItz!ry>_@Hdw~2Em9h^h#k7vn zRvtTHh;f|JQ+@_g4ydw|5*qJx%1g!u@i7dA2tVH0Z>@J~z}gaHyQrvOLbg~P80~>T znzeK?u2Y5ZWtntVdQx3=hl6kbRKgXKn$4378sHPKw~t+U z4~=|}o&Yr0cc^MyZORA8|6?ZOB1mX4`HP`#)~->>75%U2aUKm}p<};UCVwDS(L8vh3g{uV8=4cNRwmSR5Vzx%S`(vuC}CMwVSO z>Gq|O<;niVjvkbkm$xPs^{uFn^X_&;ATWjOOF1*y7{?mA^sXIHr_j*>@ZVYaeWRm_ zL_B7OelOcFE6>M`J~}%)zaw_PF~|z2$@agS6Wf1wQb7ce<_x?TiV^621e?yPC<`Rd zDSin^4IzZ`)8{8+i%g-@P}Jj2P|NBKSq*m%rLID#F;7=%>FP!PXv`8Pyj5oKhHNA8 z^3=Nn$8n2wxIz77FGZmkG+>}(bQCihx$Ng@{6H+gjq%ApkYDjUKFq3x0;xZ;XIH9m}Lz@z@i{n#Nv2!olC-!UT$*zHO=)6$dm4?I@thF`?NKMFL5Poj&!b4HOOyq>TwAVFTi3 zsZO`(?akP;?u*r0ZzoBO02nQ_fNaAUI`R7J&%z)YjR6Y__Z_l>6hg z=!2fz20yz$VO{qUhfAc%jg+2B_ zmx>zD=%}Hsy!)RAa$gC(l05&qLL^Y}?MkzN1C)X!fL5nU#SNQ6+sI0d?dGGF#6+kQ zM+ZHSjns}u(Lcr}AaqfX+rVCeuDM%KUC-jhQ(TSDNzZ%Lkkek9@F^nRpmh<_ts`4x zThW&WEM^zK>y8fJ2%nonLBTRxq$w}A;k6||L|ucl6(vlksp%3A<$cm0XUoUTR36(w zhV)-JDa}w>Sy`qjQU@QeT)WO555lRT{BH(zNj&+fXB{XKE6We(T3>^%re`Y3%0OLb zi+cTIO@@+_;Dlfeu{CVmm9EY1MqWCPQZ-MpQT4`4B;G05KM!VrtkJ^Z)cx+M3sb{+ z*PO^EWQt-}Hu>_+`=2=o`H&xMxM5XfyUCP1CDK49QDRnj3RKMA=8=24O9G?pTpNBpLml^+-ToOPdcEZg$@feH&w$$RnP@oDI~DRjvGmFE zbJngeo42RPRclsg}li9E{OCUiS`-L*bMj_7Q;+;T00NvZSS%jD;c2!%joPOx;Wa|Ig#| zZ&I1hWZ6!!&sTj%ne%@0ynQ?S>pG!XB@yHPWL2v}h-FUEGspT@n!)WweUF`Y$KSC& zglh>2%^Bv+L|dErgyojY17bS@CG}BNd0B;~rZ9Rm!cFRDlkpNqW^;SQ#8JO=b#!_k zTaH;qsN*G^+w%fKY0}b`)6qV#aZl5Y;61~R())|yMQ{2jgivV*&)Rt-w0>>Dn=0B!;KoO9L9Ahe{~nM3fOTL zSZB0qVlLKwk=aF3usF34x{?#xKWoQ`pIXOtwK%2l9@oQx@;vClW*B5A?LiOWZqCk) zb<|5rE!^DPLj`!WpE+*r9|1X29){_{Qzqm&JlgY&bBpO^rxq4;6F-}A)2qrhm`1_F zR(^gB+EA%Jf_WqzGHth)Y8*0O#uxkmmUGSCFy{FtB=vTM_Pr%5ef&VeU1M4fi`+9{ zO{md@)lDWGsq#Etw+kyBo|ghRnF5!_f0;Fs&OH^;f7Ic8R*cC0>6E{nHMyo1N`r^{ z$$ppU)IvX-`kvH#T*2)794Aq$n8(VBY-f>B$7YPAmd3IUBsBU@5_8OtdOsV1+OWM% zBZ-qdujAi$yCaUDb~LM%q%=RIF4wJ}iDMD=0TgOC?&k-NS0k=CF@BF} z1y0HE`E@5wBu!UK3zH2hi)WbRe2|WSx9_vwriv_e+Uv03;@s!2Z;O1bXop9`PRi}P z_c26*R~N@RiWM-FYBeRe2AlsVaI`E-N=RUF^6Z}SvdeSxfX5}g@7?uDarDb;y(;rV z-dNIlp6!3gCLp8D)j4KKbUU@tRSqseNeI7$WqeIW0+yh>Zp6eQRd*NDn3Z(|d>tLM z6$b4clmT*xR=>fS`deE3R{6L=myh&SC{?le9Y67%l#10XrfU1_oblZj*s zpq&CztYRp)mQ{f}UTDC47ru`Gj^y`X_0F{NHq%3+#Kn}8AlW1%|ACU zn~NKylChnVkRLdq`QR1Cox(WrT`ZIG0%wLn^c~czwrYBOJX7&Qa(I0aFimJ<^Rr`O zXlRGq`sa`p0r`k@6fok}4bXuNloLi0O*lMGR=c6m^Z;)v#}yeNOLDpsf0q6YZUNAe zRUi9|!c)b?rLIZqxcWPeaCP6}Fa?zcM~;c!VUpbPZ8l#LAy zFBRbZ)KNj?FyVDK-QsR?mN4Ff=G7COTUe+lbH7sMXv<@T&arRDNX}Jhpoy(t2pK(J zm-e`_Z_s+TJNe7U!Q%R0OOq$Y*X6E@6)Pp1h=fGVZWNeKs&j>%F_Z$cIVjLl?P+gq zQOw95+a54}QCvLNDGu7-O9P17+uU~FzpZc$4cGTth)KYHXiMLLC>NV9fG=Rco_>_P zx$P7ojMTM0>Z%mpGt+zOk)3KKN zRrx$yvBbjCwJd5DN8_5N@tCn0&8zufrM#*hicPAxLaI@6Fi=K>34YpR*6X|TFOO$& z?tEv+yvy$E1<3}m@I9rRK_16o$>P?Hvo%4K1QM8nC>>Q= zptmd_<%u6QX|8$pJu;r7EGDq0z-Q^rq=@n36|sN(%U!R~rlHg(#V8kWYq=9;8pAYt zv7XIH2jSF#7BPFZpll{(9k*oLMz?)WPk;=AC?C}8L)G*IOulf_NC6w6!0Xg+VoTFA z?33@KkNzZ}3n<{E6xi>x`9o zWknlA@*aGc56l%vCWZXl($#L0I)J?3iumx*3V3<|cx+G4_C{kU8#lSQ#Mmbn@td~h zwXS=4y~6&`qWHZdOTE26P1b2m$`Q48$Sz-QICV(Ja{<$qDptcSlo{vhVq6YJ>fT-G zMnB&exZCx)t^Y}xBk?yRbf;eFgP@RoT4=aSLeY=@BfutmwllkN-kJ$27VvOuq=fb+ zShIGd;B#HDH?UIpPQ9dPe2k=0gt{hhq`t&49L4~)_6^D3xAY<`)HSZjP~_y$qQb&f z;Hx|io7ZW{M>YxlVR$l_)WdO1sBhPBM8Tcp5z`VX;h~<#4(V?APMgHoEKAWG2pl?Q zt6gaU7x&*rCE8bablJWh4|RGpl=Hq+r^~w4$satl-;(eNoeXYiUWB+ zZe|(*UBD>b=~9rFwXvKXHJGjg`Zj^a=hKtA=+*aVpieM`J7^bmQpfBI*@P zgPS$80u>Pby?$2zS`l*cKQ$dnv?lB>s1K5w=Gx6X)5XED`L(tX=4x?Q!Ge-Js4>Y7 z5j{K&iwW9QxN9_;VJL4RUYo~1FfX^Kay54Hsnx>>I&WKe_;T}sB=lZnE`IHI*Y215 z`&+sGgg}6>I66AoNZh`EIH8B#lEoOR5~SfNZ(-#1*89FiuN-+h!j9O8Bar01W34FK zej@@P+v14;zNWZbvNa+HOETk}z}Ib=50-M<#OWI<)ci~v(MzF_W$4`+fnci8T=rq1m~>d^&GmJ1 zBt2S#xV@2awvTo*fW*|btz66eU;E)y&ue4GKKnqb6v+tK`QI=V8 zR5^;o?J3J7I&Tu2n=W1%uSK})GP&-CoI%^}5ig01x6A_5643Y`!5yK_386)DyM$p8 z$0Y9s;zxq#6e+()Pb4yG0*?yWfI6_B0B#_%x`~ceAF!gPSly`@yLW_9A143e8%azR zSI?~7$9?0l!>6K*@wL@_{yVd%`tVfNki~&5W7h+VhK43z3he3+HHWY8bomK2b!W0=w5oki2{ZJ-59 z!7WIQ(+Q|@BP*2lL1Hqgr5H(7JxoU{BS0KFCA1M zr%W7Ci}6E^1Bn&~BhE@7`BBJbJ3{9GGP7!t>ZVY+qtQO_`hyY17PPP*?$WL$G8*rE zAZ%wa{mPM&=F>Q# z7gcL<4|kfzL1^R2_R^;sVQA0ekV*$qRxsp@@MCiU|KBpg-n_x3R8 zayMZmPsWqE2U0ia&eG<-CN(LJ4=; zv5>yCi6Ao?#^ur^HQ7NB(XLBPrIGkWA|x98?B5oDr8j1ZrmS_A#rW}u1F4W>G>ocM za>{INF1hYWc@LJf3;{cpL5hn|U^dZ_-zPkEE-@u(T~xNI`Yl|q^i`BlPgU=3X^RHm zaBH0YyRgvcv%LE`A7g%AJR|5#qoW)Kqs_qq7jGi<-e)TxtXl#(4%g}VH-L)5EXK!l z6gW8)`rU%&54o6^TWOO^df)zOkeYmBChvSvW+$mu;0Kmke$s?ocw2Q7;25gyjRWs0 z19I||Uu2B9J*neG%-rQI6!N_y8QgIc40d>L2Xqt}~N28NQ8snh2G;R)j<3kHD) zA|N&${f=|Muj;0mnIzz2i0+$X(e#umC7#&i4AN^UJ?ByFks+$Yt%x&CZM>@sOJALU z8Zo`krf5&)qMR@88{KQJCo0Xa&96}3+u4a6PCzX~GUZ83OH12S*%?HoL!+&wZG0SC zAUpb?Gpdl-mk=mVW4s}q#!1K#lo+WIozpS4b$t%_E*1W+J0CJO-%@76Iha`{zWM8w zf!Y6<`vQN2b3G(#LN^32px8-g^Az`M=lQYBxMvez~VpaCPN+F7Ah(IHy)p0YfOb$-H!&&KFsDoc~?0xhR)afEb3D z6fV7ol)+zSLPw~oqABs=>VYcJD3gYRROEG{qN)ing1L?vpYs{ipX-lC`Tw7}Wi)@2 z#B!_L*ZQPLzsRo#o6x-8MqS3wTr?PysmUHnFyT$pRNnHRbG!jNr#Q?jV4nB}{ar%Y zuqv3w##1N;{?LN6Q5BmWN@QSFmFdQrF`UK4x=oWiiCFl!hxHiF(8|s`t#Q4gNXDRV* zZr}8>UJ&4>dTm_b?dMdF7naKy0CD2OX6|`A1Q9By80V1)#u=}@$dfQ-hEOsDCeF(X z7N!#u52U#|qPG((N?lNpj88cOGKyxgMO82YKCIxduEZ$Io)Tg8!O z=B6|WM8e|aaBP3ihmitH&PS(-$-HLnCo)Z|z9yP4n3la8rB z94H#8KXWj;Bq(BKRM?-4kbi@!IE(~in2X%2wH>{^g}WvU5_p|QI2BspZJ+k{H#9dl zH1kt}A6uh}VnP!XadGCj($D?^kJt^t0$xNhX@T-6t;^E){1gBdYzK^)fAgQQ;vO8) zTD-W}9us*S0q)c8?wI%|hBzg0dvO6@Gk!zyC(9JNu@DXeR8_~SbZAmgwH5iIkJXaB zx>R=-%3ewK!n}COxISG?oWl z3&w?}Y>4A!sXieg;oovn!QsTH%uExmL=t1@j5%G8hx$ArxZ~)P&!z3@P3!e&wcz*A zow$*7R{Vz6OD2{IR{83BR;D6A&AHPh3N0E63cM|$Z3g;)v6k)kOc>dbxG2@DV0h7t zMr8+_1qcf{&^J(eeW>53MoHk!UDAAz*jCAH11@Lr_$UzXQ7SjPja; za-V5iU*gt|TG7tHa-<214o&27bgW?TwD$XbI6WG`7UMfHZb zbiE#Pj7d?sF@&ArfyIkcK24zRs%t#ldw5!t=J_0$0^(+wlrHgfDq5vTKEQ@|!{xead-xxe*$x!eaW&(fmoQ<2iAOk<>_cL$~8_{21rE3@zzRyF%IF z)-XI}iHoYsY`kmd`*!Oq*3xn1Z$F;G`mTWCP%3XAFVr9`UObgY!Ic=gqtNjOY{jNI zNZ@}iv1iCn$C|Gi(fjH*^=_lfaxpulCBz1H0Tspv7Se5qi@WYPl9_~m+q&Y{J3649 zbzq3)_5e(q<4iXjPCUl&8e2($+3#Q36vO0cvj040#fD4C7wg2vX?rG*TW*L}m(6ZKLYG93< zO_piX`S9ppb6)f|WbM>DED(5`42QiI7wERhHqNK_=nj`!e>LPK44C``iZ~#UyM2bx zZHF3;xV;KH`7xb#%AwAks2rMPFZUW}K5m=?K}HN&gxVf^Gtzr}*V8roId4d^?NKXz za+~q@6c2;Q(6jXOJ$Y^RmA`-84Cd~_o6jnDKcti0Ap3vSEm5K3}$ZSy!&iZ)?%q5Uum-n`|UzF3gNnqcE?O* zeW^O_X`wz?cC)YVR)^l@Sjm>Uqpb4Es;iAxrf0&}Oo`0L6$4;3tKFvoWOp`rA*)lHP@CT9 zsutd65+nbdFbY1ZV(Po-q?C(}5=&13s>8jC@p@>6ZVW-7rLLtVFqfef7%S$oud1Bj z&KT!A2ZIb`4JTP+T>?+Ei%Tlw_pvv=aRywPUR?YPi=_=f%GCY+m}pHa^jn)uhp6C%xuK-SVM}P|6Z_V_Jg!_@ZPmAeB9EBf z-&N2|Bpbu{%hZ_Qbu;sJG`U0S0zJ#6a5&-FAJy>c^fyxE)9bT?`}K1Thk-2p!lT=? z3J66foJM^DP`=H>?%y@3n`a|l+wkp8yPk-j=fDL38i<9Hp+fWX^F}PeuHePk(N?;~ zC4tMUNDbcnAx=8%z^7dHp&~4J6)w9{+y{4A`oWm^t68q#O=({~SYqU?G0geKQ%=x?7H>XCiJL&{{7LXRrO=i6krnT$7l-Yob(c#ZOufSs4pZ5*YJ$7F|a z3*$Pf_K2>{BP_d9tW;_H_=koXalYPw<%OM|UR>@wPQR#ZZxHV9b#Ux!E7jYJWfc~H zVtd=BY1=LXyr!T*PL4Ax`Z5W1Qh%ykn7IAg1_z2tE7D>LwRd>aMfGe^wd_()e&5Ln zISh)8G_-AsD%a{A=PtKAI}#(nO?u-qPpckXZLHGwdP{hGcyZC$$*DR8mX?F&do`%N zHmd6w78(jxMYP^((qpcGsY5qCDB|d5ln3p7`j6}z|49Q#0eV02e@tCvR8?)T21O|; zK@boSq!EyoMw&x6lG4%*(uk5$0*CHCq;wxd0SN)=IFxiF-Ed#f@7~M${(Q@|&fa_W zyfe={^UUl&f9i`g%VgU@PPXTE#UF4tHLiU*qR+^;@vYQ$M-nR96470>Uq;Uhj+HN8jOTP0|U(QqM2zZ@q*Sv47dtrir6XSUt-g$1u!~G z5Vb$BdxXVa7(rA7FD8nj$5V|%7&cs@ClQEVoZQy0VH*j90Gf=P zd2rpK)$Ln}G_PE@)(hN!abvxk`yEknV)9W;Ju3$vGb1s;2;P#h*H^4I*jNg0eV>H4 zF+5JP_lH4{jg3;B+_y=y5b2%i{-di_)SZSMCg_Tlc?)g40OW~uo9*);ZxH?_VkFKX zW}s;I-5aKJ9Kjj%SOcLiq$w9)PF#^oz_f9d?q^=0XNVOli7lK~AJ`L%$6$X_Iq5Mg z-HApq(;^^)i5{XKe#4;3IB;b#_uTCddaNoc1(ak+NCE(ORO?cCT%Fo==e|i%y2CDB z)x%bt0~b_NK!MK;EWrmQSp^%N&uY}xR##&gk@P2gF(Sgx ztgq|0dO|Wmna&EQ=&ZN1L|LBoRo~VZJdDX7&Xeq{)J*%r|D#P7xT5P1#Lo~W1r>Y&ex+uFfL{7<99q&G; zmlhi7)Q)T>2BB2EgqbmucYt25Pp+7m_iWJ1 zyk|9MxeJ-k4G$+|VL6@33!!mcZel@>hhu?$qwgw>;s#8bO;T@scuX zwmdY&)%vw>nRcT29SwR0CdKM^W*=QHf_szlwEz-<9Lkpr#Z?Q_WIS-U@4D!{2FLv} z(AlegGE%#lsc{RL$8ll5hYP@>0!^0s?E7%ZPt+k(sIkGFu78-T!XE$iFC}zfazM2P z>S6?vL}kv4*}Hwpu?}^ZNZG@*^K>vAqF5p2h8$WWhMJh8LgMyb%HioN3lSO&Z1yv# zfF8j~-xcY5``!6J&msk8X5jihSs6FUDWKHH{BrYh2}^0>`^1@XkFmq$#MS%ho`D?j zT8ol2h;c4W^;vC5RV^FcB7>7iNyn_mUw~-yd8&Ww)1C&yqwI+ML$(?wLZ(p#aD;Vv z=jFAp#L<32cWPW}YCNs+>E&v#lvWb|ls2UB?AiN;b!XxkqL z=N{9*r?n@t*<8}Buq;Y)@CF{VH*%g%%q@=jK$nH$ioQ*jRh+)Exr%peCaF=8)1@$E z{>9fJ!B z>cfAuzhCso%0DSD=XZGe%IIh-zsHX7U=wUk%It`RU$yYqHPu(aERD_c-!cXa4WlI* z6vc+RXTIYL3GPc6lnE5utvi$bR)4Zee^a0Nu_~mV?e6#cR#sQH>&oSlW97rd#a9le zIJ8siEA&3?P?mCsO}(WlQ8gCllrLYNzsq?wNb1AnJ8O~UKjKyq@bI|vHwdX_Jzch& zw!gU?=5+*>o7b>H4^OB8Demp9n8;LJr-4qDd!T8D7SyUPP2$hN?Yi^M4Ha)|XO?yS z^87r!7Cjbxig1-)nX;H*a#OvNag$!=b5Mp7*CV1Do?qH@(WC)4oI9tll= zL9ZM~0?0@k>p@0!gwpe&=YgsmlZbFHIsS@?GH_1CQ}v)Sm*?dT^^Hx9P2dyIpE4%9 z({PPGAhz6nFj`{gdj_koVApNb@@Q$~K%^;mx!3Nu<2V$J`q=*Um#+qRH?)?;A4}<# zek~qFXFO7N8?=Y7k|pJ>zEC@8kRWoU*T2D(8+k|vPszgt#o_{|-394H4Sh4((|^tH z=lCiN8#My9=Y`oo_9P;PaKmisRlcIT%T>d+6y+JG`Npm zsm5|?DPnt^`(>GA#bq^RTkOAR@l@O%t#4@#`sMw&0<)xeNfIpy6=|^i`upqisayuV zu~?SKVRXjP`_Zzk$)krJz5BRRot;vHD7rT2S7z8v4kKz*S+}`gMQ?KYUvDhhsod#u z;;U5uI9uF!YUdHF&D*Op9=NSfjj!*uh>_nEOFL^i-}zDheGW^c@2$&aK*o-7YHBMPAfc6huU{7=Aa=UtbWhmz4CG9y|V6PZc-^ ze7d~;aTH~$o3d@=;VO?s{UCvZQ<6nVN5LP~GxWv?xquuwr7R~^4>fX(*Xt;~m;OAy z@^;r{61&!3C8svQ1-nNr@r-!R(+9_wa$`Ie+Mn`>HS5A8G(nvV8a)$J+|bT;Lwoi3 zmr-*>b&vf$P4}4^6?cScFQ42KJ~zwjh^@JPbr)~C!Q0b3bHM&IGborI3Mt9SQ&}RT zskjgKc)Ve>xj+^EmZv$h@ButC`=;HC^geVMH)X7~=w2!zXKL{FhOTqKR9ImB8nk$5 zf4F|PZ`I`#-SEQq=ZK3rA}y}F{8Yy!eG?k0uI3o}%UW_0KH5zb0ThG^ z8`MLs5JgI`S^t5)j07t1=e&*dwX@(fUobk|MWndOyJjlAGWhADkznAmlW5e%rDw$d zq*pk!ca~iw96W_`shGT3H)~=z`@5k{M?*P)opZ9>e z4xLB+4jIjj!O`Sh6ah$di+vvEaF!6u8_h2N3ZIx}1+Fqp=SeEONyvK0b1lZn4;j>& zo&45iYw5HVniexNeo9>jIG@FbBK4s@X^&zQ3|d@mPCu6AM_N~N6=(3sl-Zmjaw6adLJH15vvw+u8>*J|TGzGCd`>OSujmcyLCHoQn^9bGS`jfUwrs;Ifv@Y6fa8Q zA9DsV`U9B`Sd2*CI7xjjnys-Yi*(te@Tm( zxmg=ixV*gj4%60vmOHCM>S&ai4_lh$)830gITE~7rNFr-aZK`xIzB&VCzFapsv-K@ zn3U8<>4vtt|E~td!vZO)<#_G*Ed3MSfYx*u{K@`s!k@y$O@y8vT^8u!sWC#U0=>a5 z<-1ra_j2~sAzPJ|(`(L7w9nv6W#v~Zi)p34u|4iK_b0I7_iED{Yio)Ax?e%u@83>< zNhv8|UN%agYsFe?`s9cxNyS{tEV)0Z%}JG2LLmHYQGMu<;*Ep0EHiOrW3#HR&iI}9*1rNB^lDfkZd z^d9yH#O^kB{bKr+b<4Y@r8Hj_{rU{7Z50x%E5`_iL{0G`!(0-*F+D>m3xs*PpzCkZ zN(*cji?hC$-KVIh7s4KP_hG`TXtu(~tshX2x2KCD_Wn6*ChQD1g(QkHi`63ToE5RD zB4}}#!}YKlZ~r(~eJI+=siWsi_v*I`>7uaEULMqVdNj(b@>w~Vzf=7x28dJw+y;m= zX#fwVyEeO)P)~#2I$W0-rQ<3`vms#g22*DP{%W6%Aj^4yB#^>(mudWkB^==+wi>YL zI$R=-ZF=Z1a{sg_1^@Gy{Y2(FU9xPZQJao*Sjt?4eg!Ra5h^raXBw za4t!va?4Lwb5ik@R$VRUmDyWcTZ`A`yqvK0yx=0%_=A+)Jy=@H8}HNHG;b+`3!(UY zlb^%n2FcU7PevKv8!YxSmkq9MsL*H=x+za<246-6*R&gXr+99u);Tm7*fPqn(F}2j zizmuSu8ZfWFjP+vyoNr2hwN6xb;(#(9ZY{z?vvn$2^eC-$pqdhojrZ{4<}>gD_owO zuCy>8?9Zp=HtRY`S7aCI&%1H&8JWOM(t*g`0 zG$%%}vbCkm>fiA)WAVtkUlF`o%KkcUn}LqX1*s%Qki@DcPegL-{)8G!&|F?VM@dPK z8|U&E@>Ee3jDV|SGx@jTTDx~FZrM&~yO$lKsbY72bfPh&rc(3SvuB>i+gzIBS}@u( zNV+-u67$Nov9;#&Jlc?t4_0_We#Wye>^?J17FT738w{&M{^f=0u&|4UvK@5T$;JL` z64Qnmst@1k4*5JI@SynweC`Y-XK|M-@P75ReLQSsCS8W>ysXW;WN4fMZJ0zmzQ_L z8M}$9?dXfIt5=20QFdGPAgOmL8)M&#Fh%59x^_a zahH-Zug7Pg{H=SHEILwBkf&09Is2pXb++N}b6njT0|my*zD!BHJ?FQU1%@gSSbAVvay*t-7sD-2m@%10YbvfD+nfH6wJ~g|$OZ^i3AM1S$MM5fW zZ26l`cXf=4(?&xo$a11o%Ej57F>*dtPAm1IZh2+?*EK-`CwXSwFHUZ_8YJZxfUKz( zKLG9#EBl^!upW&#QIQ2vFmbIaEEQX6`xQ<8_?dy$*a)+xRZ!2f@sWG$gEIC*Lvfxe zfFWZa_jMo3%DyO2BbI*fxU40$hnU~PO8{LVoZ)g*SdV1F1teeaNl+CUC`1N2=|WFF z8!t_O$T+ZYI|k;XOG;|{4zNyc(^5er=W7b%8X$U`kN<#*Q^-d1*Abp#07p>2q^{#R z&EjmL4V*iA8wR$gz_NAV^u*$Y?eJ(k{v>sl>=MdUgNh^QWH6aw&$G~V!8L$h3Nr3~ z6oJQ-={4@tz?IW@xJW>NfQlQVAQA=zy)FKfn5al@i3ht(tCTMX`$bQ#;<2ZscbT!4 z=`#yw+Vf@=xMol3UR1!yXdb|CYBdq(6u!d+(B8>4VmvHRLyskiS{v4KTRl#5s-(Fi zIIzP6!)$K`ijW*~1D(uPDe<)fn|)RB;ei1Lb)=Ml-2m?}vKU!p=`EfT+Onw54B^3F*{^s&e&xR>)TDCVeyxN1l zs6Jm9(=lYUwJ2Dt#iCL-lslVZF>P{zBXDlGat8Ebn)WV;*FFX2TWf|u%W7-gF8iNb zjoY@_CQx7qVYg0n#I`YQo0mySO6G(*Q|n?m;43+qW&%-E#v&t4)n~S)yMY6M}za|AGCTG>-daDqsS5eI7&jLc5m)m@+h*BCbVD-CtCpC-CymF4 zhxglAdq$+!_ugUpejEWpBsDa8*r!1$Nug*)q_lVEVPVC+?mhIyTbs$PPc#_q&`QTQ z5e4xH(HVhI%eQ*Rm?0mO-b+DN$iO zOi)9%e>00+zzpDCE~I>d=hfu69w5WY69(0Zun3?Y5yl5&F2`&~mfqgwt}221pvO^X zL5FU0ye;!+#S~HF{dJBHvOY&@D10Lr5wV#U$cd07034P=imq$XFG35JgJ>hlBPfI% z$(*nk9&J*xkdyqImzhP>u>B6iY*3p9Nm18Zax?`LosM1IP36e~*sZ*6vAU)4aRpl; z^g+nTl1Rihb&DnKvr(TxH8&NJ6ihH2J1!rqfiJ#eFBh#RklyuRTsrwhe`iTDIaR6D>y|c+S6O#BZpibs0Cg{UWad4cV zUNb_o#R8%gX+FG>pobccPDm-#{@ygl;vzC(P2TDKK*aa}kQvY~JJ`b@65Bjwd#Y|N zL)xf^-TLSg1C-~)Oli!8E@ptQo9$P?RLSVhA@=(+`pp%MACGR15)#!0s_=W_hb7@)vAi<&rT?tg2n*-ws7V_$MEsCU{x$)QQS=vB%J|Km{w(?XafJV~7xON{` z*QfSL&H1PIBg@VzG!|rsUT1}68`yl{z%=rI&WvQMu6;E1pLZe^KLsiFJUlICRN)8a zr#SzGo9l_6LE76wG(Kx6#xl9l)xOdz`lvTO7wz3L_ugn`Ts}g~-ck;>fV1HZW)Up= zv$QuFzCWO^o^GU5x6Vf!iImOwU{_Na^f&GeH$3&FCwBRb(<;N9Q%%BO#wnB}jlc)V zn0h+W>lKT{2Q>Qj5W?W?m~B~+uICk2(5IGJ=OY_|eCMuD9YhLao#UK%#O!OB8hXiS zf({2+a)&HnD$p;3{~HS2MY`O_s?U-sLVt`L|EM`$#6(<9S@wX9Ux57#ywSljz9$(x z4RiTmnX*b;(x)4eG>osQweo zD}0yg%~40}Um4DjNWoVe@7m?6=ayiYE4F)bL+hPOcG_P}2i&V(z`w)A;li7avn{CH z=sDhhqrpI8`r6r9w)AiSaFePm_beh}s~!&E5vmzjTr6YNFDe_Ws=9y)sQ)r+;n&KD*l}n4c-;N#^=R8mOgoTdn}3?v!)j|BKBMyH+tie z-Feo&2((o7{Y;=Je9wxQ(*mXkI2RiF*QLH@u|pK=j|E z(H)X3-m8(eFGOgv=b-_;B_EfhxLe4qW~pJoutju1iMq-*0p&x{vj(#3^*Y_c1lCHs zz{-?l1Tk=a@c6#jPh9I2>z(0p#uI_i?qol3PknYC?m&T1X;3eG4m}55d-~Bg80M62 zY`vo`WdtXD1d&B0ESoF0u!+7OrYfuvGys8X8r&5L>RAI-r6u<03TA9+^UQfJU5H_2I&#A_wBfD@a?-G zz_Sr}o9Mm&nM8xp_>O{e>`y4@z@N&q%6ilAAm`4pzp^dZAw6=s(Q_fGQB_gaDQ{%i zQcY%;yHint8@kS$&FvEIiHtou+$b8Uy)yq|O7* zBR}~c;=Q?K=D$JaO#p~5r3vTVj=9`M{r7%pCOI{&{h?lj50ZzI8M^egnMt=~tdudx z^Z9~qLToEM6W1GY>Izfo+DryHD%E7uD&!-HD&)x*3&vAv8$-9LVo;_vA0p=hKHwIa@SkN}5Qf?c+isO>pm)!GH`hPW_nmivIfm#( zGnxWY{`kdc`*_QGd^RUl9yOOKXcj%H&>PzlDsCfPdK~{}WXfDB|0>JoD1HxLoWNED zmh

>GnC3_8UZ@GL}>^C;JKLVg)_fub)v5#as+fj@wMk_fG8X$iVA8sEPzB1^ZOP=P= zeOj>55F=ANOTUK|_m~LJo9lQYed_!?-~39WkhIUFQXRaYzMsLGpe-hn_ch)|is4tF zrj%5W+AF!+j-f^kF6dRqa*3>WnZ_?pLxgZzPhF5wasZKbG6Rf6L**f5;1xrJMPH5L z`ix=;$Oll_4^SUrtUIwObqU7`S4M)4aIWYv8G=bR|65`L5Ax|Z?&3wqJ1lIbyl6Y8 zr~8Wi7dGtYA@2%IcM{TjZvtDuAB2o(<>3zf9Rqb@FR{OOLN20C>>oqDc?N;wwBBp| zapRuNZ6x9%Yv*1wPn3X+HQ>G#^lReO6Z9Wv=_T>9`kPnpX`i+<^oxQ^l7%K6^&dMS~>u z%A4iBl>Ej?ostZ$VD_rOXBbN=^ZPj;u4<>xsQDZwNl?%^A zZK*74&-++xMDpiQXFFMLy#owdpgE&$mwt*3PY~lR`v|=t^T+HvlixeTJI+a+qKp0X zv_SPEvho-sm{A!dt!*@bR&&9exTMRlwm?3Qu z{IiCY>0c%kt2PM|`5*(YJEvV0vHeR%I1jb>W~DG`KtcEOws!jZ;kzqZU;LNBfN$#>IZiHU3X5)xJUZh`Bp*BS@@|rvI#F2a6jg@WG#k%{Dh>3niI-zevG6RX2>4xnc2s^B33Q7LF}QK8G+XDR}FOi>za zRZ=1Y+OVj5f?fw)0wYh=y`~0jii0(bA&BdY_rcL&pb4Ej6QspThJtXJZvO_M%E)1% zewc-?#F!_wMz8$mynZA>g*N8z0eEu{^F%7HdVLs1kxWrjQ*&@A_*+)13*RL>s-*|& z|AL3#^K#y<)aT;;g*cuk=Q39Y=qY?Dk-jRrL7HhNM~Os9@bsfZinF?0ph99IRDB3) zS*zG);a24vthx8wP5eNV$`B=1Z2(}IB=&-2t`hb7!dE*hw2cXaVD3Wg7J9zJEg)@V z%6FA}+;zPf-;~|so!cGHG)>&h5`Xb@Qs&vt_0;7KC~ZO#;p_m+8`exvRcJ7rG>Xwn zNf;#lR;6nJX3%@S@a%QA1&)p<&wG~3axA9AGBFC{ve7XjD>@0@jA83D&yq%8-${}a z3`5~s5>+ayf6=nA)3tuEH1s>E~eCK&&a!Q#CvBOm(Nu%G#N1l4hnEl6y@$vBm7etopm1~u|KEz86 z(dX;ZKwO^h$#}Nnz(4EB$xMfVeT3i-nm{$P%+_lUMnne^PBlX)>mXTzK8eYxWG(LB zy;)`6DSWaUn-_PM>6iFz_gluQsJHlm-`ti}cwY;22jtVfzoxt+h>hWTN;tUR8E=q+khk~=ksNt>E- zQ)PkDWH3)l%7tudIMZQM2d_2_E=L8<-7!t`vr12$_Fn3?b8|^dGoW3bjxb*u)f%n> zfL7l^OP3_EkDz0gJ>}WIK=5?m8#EY^?N7O87mLW3ipYCwy#;5F4yVq1WtM7Uf>(K& zw_WIJOQKtTSrPs$3I;R}8paVv|BLI=rZes7h($?UordtsDU;ePezqvwvXmv=kI*ph zK_GF1ytvnsB2nj>^-#RA{v}WC`d;&OW*B&-r{qfPQ&cQlclAxZwK9Jjx@+9e zNQeS-g3`GX+b0l}mX5e|>;yD=o2L5M)5-A(3m@)o%zB%U>{(Fa z7w*-H{7nKGj89a!0SN-##8^UrKIqN$)_a45>l)UME}qrP`x^Tc0=m{Sgi8%vJ?6~n zRg9~PlwnCUJ3q>2r9MsLYLxO^guux%t}1(qa!0qvR4TD%ro5OK69d3S;CY+ioR~!2 zKHE1`R^>maf`sx)u>Pm_Xs0XMHgSnUVcnuVQi{8G^ne1%d6TkQg^l8V4N*9cJ+0(S zaObg~O2W-}3kIONOSCN+PvN)Mn-R3q<*?e4J1vbY*WB2sZ8zd@iX%{WGG0i+BDG}6 zpw1au>&?zyP8bX;Ri!_#x1qc>_n)~rfv;FUl znER_?m3H59N+XlbUK%wYu6UEk)JCMYNISDWPT)iy@;2jSPN7~g8VkZ>JpdZ4cEI82Elu7e|y z!cVh6?=?D|JEV6uTK1lbh|E^T2n$F%JcXlbd)>3l(<*m6dqZto6B6*n?3YKQS;g%C zDrk8#Ot>egcJxR2=Cp*fdbQ`H%Ait-*Ah!Wra6qHHxB)UZ(I_c z@F6*^Pt8Hojoz492^+^|dfoC};d>uTSS)}5mQ!_+3^GDX0v0wA!IX9EHS$H?EF8uk zxG0Dx8prm?O0u)RU%WUQZdrDg?Flbsf#l~l?1;-*fY=O}o{vpnGScYpOE65L>A_hR zBp!4pTB^|*lDQRoIKUnwT%##{_9MreiB8v9LxMJEf@;p|(}(8gz28aUWT&cyd6_?Y zTrQ8!5B*BW-yPXqEj^iM$07XOQ?_<%6Dm9CjL0dyhOAKlQ#;`+u{WrF_)y*aezxwU zNWLAWUIZlK@&R9ojX$hMy|{Th#qUm=bl+#l(8bHtnxw_aX$qmNgvmFPx137NOKj06 zx;gr$_XkZWvtPF_kt+K!?*19kkpOD#4d0p>T?RW@Hwt5wbK3oU_v1=CJxJ}Umr%O0 z@zbBEk_EMYS9uV{ZwNNZm)z}ATua~&gA~E zO~$_OCvxt3L=&-@49|4{sa)CJj|_0$me-{ck*qomHphhZK+o%R(>;>i$^?OiVnwA> zdiAE+rBR$Jo`(ggk&LZ|aQ#*iu)TnWOQ`L;xXeRgBSjzdu}wX{2?xuluhlb1rPst@iEC_A>8ie!UXRTKI9b1$K}i{pu!MM{5nDCP1ToHT z@=`dxzo@1AH6t|CXI}DBN3pi*FqkLpnVsaWcuRaDUBk6I*tgVH)3%fmXb8)ggMn}QJ9gG(z zZl$X1u5~Wje@oog+!34eZa~lbv~eV{SU{2vtCg2^kv5BCqPXFe%%jVz&ya?bk|arS z(B}L+ElQJee#>7dF*|76KI^KrEY{qDxNMw#T|K3eq^v=9@0A|d<$v>MwsxSUs939e*hS#*Y&qs> zC_ZyEt?k!tTSob4ADxvB@q|A#%tx7n)_af#Nx&+t68tiJ;R;dMP59PCagDRTz$k04 z;gTqlZYaL8`Y0op@Pyn>kAzN{@NNU=`7)HVMp8U}|MYrjfildyn=nV&st_EJ7De~u z%aD(g*6gu7I~qDxP*5LKm6|(y=kI>0pSZBb{i1YRux30UBO?tAS~6N{`=!zJ;-#@~ zlP{FAQF`-&fAiX8kMl%nK7rk4Nt`FXOS?Zel6G`(J*rb zWH)KRzA;ZPKN<+Ey|QLQum*idF`pfdS)exO-RWNG6^teCXn0JzbWx~>=ya#7rTeT` zl%qiQP`QJX`(_XtsF=fSaHKJidsizO zV`udfl+z(CyRK^8v!p}8-cI>m>%TZPEk7aLff?>5B^!(5-DjKoraN| zuufF>aE5v}_@CK=qLkU8L(hgQFs-_M6Qrh}FS^Ecnf%1bu74rjd}15) zdN*##tv_$rw*y|{(RFJIXt;{@%OvIjeyLta**3C6-w4a5jb*Jq*ey1!W!V}guZZ+p z%aT#uv?N+~FmAhN(sW-8wT3AR40Ke`K_j9+B}7*xFsSA*0akVamTp=}k-!Hb?&~+H zgoicB%gISYXJ+(aDta|U*A>%>0bm+rF}mzOk68c7U$`Qj->ty_>)Y7#9(84g|7iG* z7V%4KU(+Xx|!i%$kS5IZo0V%8TqNl zeJlG}zRm*4T$A*0l-LJr&f0}wX_qu~y`uTVZpCqZ(D~K{{4j1cJr5_B9&3Q4`K;Ks zk&p;E7h_0;5`V{PEo}l~7_aCjt#w5$kRoVZh^nC1Vq*mcN7YUjVTw7S%!rAi3S&rG z%F@&-MRRHoF!?10J@Ih$)cRxwBctms;CSW!e6Y}|wS_Wj{XLwxWIoJ^N1P~hcOc?w zI>4wU%XZ%yB4+ zgw0CT;*(O6eBII@F7JmUqS-vv_2s&lDBR3ytEv(x^qbxdTqs3Da)#B;~r$ zyyPRo2S!W8IjcRYcjI1t6}_b*{DO=28(VHx77b`6Gl&jIg2{0a@%RH>=KHZvzI+?B z^(*{hpiPK(q|-1PZ%Mcw`qucUF#6H-ptNSkSvFV}6Tz%glfd*UO2O&LlP7YyD$D8+ zb%UZDeJ{p}bi|8+Ve0r>|CxJb=kYm&wKrA zMOpq>UAWJCUaD}8F(&fbt<{iuahR1chA_xH)9ct_$QiQO|L*5?b-DtEgW9VhHADv5 z5v8R%7zF>llOSiu*BM1@Ih1utGhJKHhjpH3MMNmnqu|ttv_vs2xyaVqsAlgaM3Xu> zfySVnNsiNNUU9Q_^!(U9&&>q6VW< z)M@BRXm%X^F%AYPlmxCssmdjXMiW%Hv%Ae&%FEec^Q;y}B?)TXgMpagTkV=<(IyS` zuUSqpFrl}gl#F7w!eL=VJY;sr0L=Z}v@MD< zH<)zkD|=3xoJI6UB7J&H*d1@P36f}Sw5I?PPQpp}I2P{?#mtBEH=U4oZEJsdU6DiQ+draFC zXNJz((ad=8+7Zhk`f8IYG(H{JhWWar_*k$!ce1rqgJ(64OzL&pD~QWh>Mf&}!C-W5 z+nnTwCY9n{7Wq$Ecpm?!g9mc7<)Gh6+aD-J=hthp{YDwxX_`sf zf%QMwElSCS$Vov?l1M$Bi`Z-8y) zGx5^6146sbndK7VZb5-HTjQ?+h7~hxNYuMN+0oLphz17ytv1woB$}9$oY<<($QSHz zu^gxhN405CS!$h$bntAN^xfsUV#>?Q!BOrUp$hXEa$M+|bjcE3u-KjC%BIrhko?Xh zGkn%X*X!m|G4h`lAUvEjy!NTKra{Y;3uD#Sw%68{qcH1j)LE1TZ$h(TRnIH#rt-f) z=4r*Sx5#u`;EQid5o{7~aTn|ql^%@Yej}pGlHH6RR=Y)Y@FaPfc%25f2`;Ws8q@B| zyr{A+twxUDbxG24a&o%QuXsEvR&r`=tU1mDlr2(E1>ip3j$XO~oP0c4$ke8(&r!$S zH~D~y;lau_>y@)prS5If?T3oaE(EeL&y zCt+U_-SAvjPpE{}G-smd!q%OjD0-r{OY}z@Zx07Q|Aw9F+I;(`n-Q}L9R)MQrq268 z`ovxZ2c|=6e&fzkt+6%OyNjEhT+Y8MrWUEdZ6H zJ#EO|j6mR|_mtv4B2nZwW*bmuXB2maIw5T4so+%{gow6r^&tvM?oX(h+1X-+FLfnv z51?O?8UCgA>z7hd%nflumotzi(~_azn})E5Bli~Gm9BDo(3Fhs@)#q# zvaLCL*xD+T25xsrT~28MRJ2Pq>H0bgXU~0i&z9vF{O9}pc}kZVKuGcX?h65c_qO@e z4yfm3AfPY)K@f;>n!$H_b+O$cQzXl;dd+P%u*kf8wj*SP#U@39*r}Vs2}p?shoJ^+ zxg($wk$myP&_)S2v=_eX5(h^Fjir0=Ri~(7#BQ}4^aZ~b#wZ(Q-Rh0@aC-mkU!r`= z=;}zUMql+s#j0qDz|JqXzZnE-5K8y)LBcaJn;9$t)s2fVkNOkU!e8U#3Az~5#{5E` zWqs_Oc5OmtQXKsCyS{2p{6lh2=Lr|@yyJjeIMHlJRU!PaaXIr9`CT3`Kj{PYCqOp2 zjz+47Nuux$Eh%ON5vMqX{3VswPq>l6C_NaI>-e4?UdAIx94pizpyw5YKkJX_W%xG$ ziNwm|rROgp-v6N`L3)bRQ+pXdPNUkKWL@J-R3Jm#v-t=QZ#R3YY@*hrUQCCwSRJCv z54x$Q+uj2Q)HE?AmS*{)jj<6A>pw9NH}fu_G`~0;S5y2% z{N{!D6I}(z^!w|LlzBVI1rOq`I^Gc3uSC<_3J!)buOwBc+Q%a>$rb%}G zIX4Glg2~qyVrFWc5FyGhMuy7tVG@j>mg;*!ca>v2oT&ahYI{TSHzN3v>idXd7`~0h zoC{agMtLetBXm4mJwe%6zmvBW`j$sN9D~?Aj=0J84x{HC1)KYgZ0zh*>cb8%$^dCC zKvVNzD8N8{hLshNgZBn%ZzC(x?8v>Cir1~|<0DuDZ!AW2!7K(&kA5_g&d&_ADf7TL#yh ziSg~PmaT>}U&J%c_A@_z&+6b$?w(HN3?B2p<3uxw80(}Fo~q>*7bUUz^VZ>qr;|Yg zi$0-SQ#|5;XHQ2&t7$e+^c{o~T3EbYr3S{-5FG&*dIQYda-`;QA*Lcm%~hI|ieF(? z&~7t?vLl5myTUKk2_F=uoTI*f;oxfwXeu}^y|1oYX!iOpxnKx4G_KiNlRkv{6!g@Z z^|n4Im&v#5p+Um$NxcRhd+|bPaGSbykfeaW<-+4td7dOL8_n9!id&l`do$5OAzd^? zUQfI@{$B{wW@94Cl31rF{!8;OS(+4MGwp%pg$(g?$h1FXZgrWqH$sF5iklpWglILq zi*N>I!|zqBK%K&r%zD=V4#exGD<nS+K6hbnJNlZmGrCG_# z{&o*t_M(z#&x-R}Ya2SWyqmUFsNjq&I40;E28o{;Q-wFI^*6F|)pCfA9;wpyDf z?@+6ytDinPa+*EiQBA%E-tKaGP=vW{`BZC-q%ANV(?N+(PDpQFB9Ex*1lX=#*)J;r1H$=aFg zkEI)FRh$~k79N~X%AbDswM@f*b&+LfluQ1@fI5=~YHqg?!+ez-X`;_~TLTFmHdcB_ zIsweKYu|vPiGZ)=R#x=wIz$iaRqXAEc(78rXm__*{YB(a*IE zrc^&;_}zA_C+lQN@0%T6Sq~M`?w(zIYa$$)a&CjC_1GzP+AT-nfk92z!D0)_#?bxy zrDF~X;-A8*OD)0Ahab+1hRu?tCsul$aNCsYW^HCQ`8FqL32;00@9F3@U0heXt*|*R zucVHr9~WeAly38#(I455f8QPm3*=yzk>%if?r1WkZ^|8k$J(@8x{3CgvXfmyi zOcWOGUACvx3e+W7R^_jmj*Re3?jbJ}bJPS@nb??EnQ=Z*ar#q0DWTz3*-+THeJ;57 z1EF_0y5qg!GrL~6y@Se}F_T}I+-p=RdAPJM(4w25dTm<$v!i4+Q`up-LJS}ntW}*0%^jlKAk(V<&*R3B6sI!<+ zd46-}no7`6*_ju_KI*`LVzhr_Sf^mLv8ikUUFB>rIrd$HKh>t3!n}xzqKD^5N_Ez}R!&uV7q;t8$`h&v%W2ikjAbw4bo4`)pG7?yj|Y-AR5G+UfDR9StT zT81x%M-F{o9V2iOqoOif%sx@@Xs`1l*T!!MklH;+lr!`8)-%{W_u#S@%%<<)&gI8; zSmdV3rjjy|fS8(Glsv+?m-jbUz90}fRu}p~n2*sh`-bGf0qynPR*!X;rFQ;LQPFwy% zm{c-F7rS&{d}=wb+<~9*9@%n)bW$O)0mwhjq)R)p(E zF0FnTGIvVaC51m$6xc;kmC5H1}~7dR3-smTKuVE##WPBE6O9y5V781E|_h1mC3GeXLS=dYgUV8NS@d%Jm9%;D$jE94i)mn#}u`UAZw zugyk0*3cEdvOQ!%uNgKKK28Wx!HMJaTKf;5`OylDlX@XxCSLsX9-5f3G-*5vUA5Eg z!yQ-`cB+hgVQi2bE$M6a`g5gYvvV!_%O3ODFwcOn#-L47ub~$cDP&LdRjTtgl6;Q^_SaSkSapG&z`_&rfdn#wt0w z#&A+dkaq6*kIi-3@7lS+)_cj-U*ut_z3ML`imgs8zyB*Sh}GFHFb^Q{Jo&%mzdu*~ zMnEZ-aEhb5P!hp4zH8-hnXB%zuqA6KF>rr2c(tqFJ!G}-RO=jBu7->FXQHWlM5R|V z=l8t;NYacIv{bsn!82AJd|bc*8i4h;|Aj=S8=@9-tcMiCwR(Qh&xOhtYeVm|WOoez zHTWH;K)ajv+ALGF{Flug+dBtVP&w4u=hmu&=1w%8 zHO{0&jML5hYacLgoZ*dR7j2J(Vb;NW>EIJIZR-w>Ac<*vf}htjGl!Gc%TuR>c+E#e z=T^{aZW_(Auq#SQ;CO5?bX~S1#6i2`>=ho(RStLhZtkBP5wJPaPo_a&%x&)eJ>KDO zVT1IO3RTB1(_%+Ea+ZK{Zh5vgxqRNd-ab3D{N*gwiks@SX!#BIS>*jLFs4COsP#0mY+gBUB3Zjc@Q{mhcoj2|XWasPY; z$q${AX={6%9lDa{nFjkBVwy%vy+fB6y78Jwl{j_3vCL;h;~WA$ojC1H?{f9$qN1yc zK|au-Bw8LALiVsD@gM>&s zP5B?P-ZCu8E@}hCRzd|SX@~BV4yB|7ff;J(6c`!?BvqtKVhAaT85mN!K|s25=nhGx z8_pxV-}jw!ow@kKkD2G$d#}Cjd);fTz3u8wlcA@;ZE?fLo$Hzt6^wQ%TaBgOb$s|l z!Do2VwmWXQ2nnBvZZ>_rmIJBb5yka8f7UW4iUDWvv3&Awa{rOTjZf{1U{|M-!t`6y!*;dPYPM150~m$ zo{UTAe8H#xxklXR|SFM zCiAC%SOOK4xBZnmCj1Kxxkw}07I&H-4pxd`U%Ld9Ml9iC!)l?kj}Nx&+vcROmH4;# zojlM_=)r@mt+z*sN-Z@jW-L;Vn0;~$ko(h`5=RE!YteW7$)0Hqh#C1A8{ICkvsv9e zIkbth_PK5QlhH?ssJBy4-ehX92NrP+AHVVbfQ?2iLzF|uDSpzq;2}bLJ66XAe-_B? zCi?7NaoZ?F{0gr$+S;G+mH4f-l$kIVylp|~CseYTVra6dh6ayC;qfGNM?aw|pNt8V zgXWqJ&5`5Fm`2CFOeCwn9)=;D`ws=EEw^Q5Y^>a;>AtxprlC|5Q^sA`U)}+U=M7hJ9*D_Z18T2aK=~cHtibyQ@JFYqE(iu2$^lKzm}-?xwpvc z(+8NKJSLu6=x&CH>zUxX*th$681d{g_ngp|4)WjA-jfq+FUyBj)~xBx|Jv9us~{4_ zEBbxEdX?DiwH4gi3#NOfS~to~38y6-G|=I2-b}d>!+!WWIOE9Lho;I#^WHY<+{Z?! ztsbdl*wuKR@u;tD1=EoWfLu(@7uEyD+?t{{wNNJcALk#yQ5Z#o$R&89TE_UWE%2&= z?TZJbXI^YOq3L&pulj>2v?u=(;ocv=mYcCCa7ebOr-cszM-x4~k+tP-?~8WB)G8qb zP7GY8WxQ6;lMgB|2Ske`S9M(tZQxC7b)I6A=6$-g2eV{h4ERKz`Zbd=;nHdS2W`0j z#{ZqXxq%tD#LM2@PXR&av2r1FLZQCaD0qDM;EA`Y&-i4P{yCv~k)2G1zhx2gBVGy? zOJ6&iF!$s@+rGXE)*!?{-|j|Z<)&z#1Ml?Tn{)s_cdNT#70mvF%hid=tZ&*p4nsC3 zVq)YVSBZY7^}GBp_0)Xw2eaN=vpy)VGr-yY-d=;iN^0IR&piJ{Xu*&DK~^>XV(LKW zz?-dj3%(Y@zcX2@-tou82bU)SKMsCFM!JOoyvX0c__Tw8i)iApleFp_qn9q|pk(oX z3zH3?A{UdIBE(1MX9gbG4HUuCHVU#HydszIf|1B@%gf&RVFUMCxJ}kB++U$8p=agV z7=AJwJ&AhC@HFUq%Y8%C{t5XtL#9==1UFrT0uT$=cP_XaLTuX6Ykk$SE0$ z^Jn^GFyQmU^%?#Ek^EzFZ%$VVkopTyBHU#IBFMq2bRM3@v(6+#mGu z#-Ds!A1dKD{n|fKwXPlMH)t(v!6+B5vDRJ8`tbcCzXaB?2C1C(;6;taY)bSv4Z};b zFa@=xz2c)gr%#4-gMb5<(?uTd5UI-96kD8(JA_VD!v^VYRsd_%-T8-7kQb{*C{t%l z9s2`c@iet{(L`z8daneu^U(R`CHLXt9^KZrk@fl4OvJd-DS~^O&n!~#?t?a`pJi5a z$VK+$K8C|a;#s-Rs$Lcb-iCreCCk!_I4-^Q2tnJBie1RhF+B)NT>On+3`MkBN`?KKc zVQ*Dk7J-EdeQ)(NRmIsu5WCp8Ph&ij<$l#5kvzYR`k<{T&vSFN?(i}N@9`P->Fa64 zdR1KqcFs#g70rfsFuwuDd!dVS?@r`0 z@@1c7)5fDpJ*tYX<~>RNZul=H_Lk>EwxrMK*Jn_f-29hX+bCo%dXsegnqz;=Fjo4< zQ12VU<2UU_YpT5ux?Jn?3X`Jy);J$5T~S`$xA9!`Ico*B3ApCiBhJU}#JX#XBeTML zIsZ`4J+^|`vII8Jj*VYd^L3?!vTacFOSlEp+K#MrPj4d+_Rrs9JmexaI+`0~eV9 zfS*O4!9U)$Kk$7~j@>T+R+v$_cH}z7R=(~#>YO>51OvIVUJ%8&at@n|!z(JH`lp+t z?@Sj%xt z(ty3#)JHC!TdU00yh)eJ<+FY%Jk~eu9gqH{0E%9d8)^j{mz#(bGQb#vDV9uo8qqe+ za=l?7 zegJDfxA1|~?BaxM`nH-(gb(*vl`MW00lEJ!s}!^Hv^_G?&uHj*nJiJWh|Nw>K+yNc zJhAK>w`!*;58Rc7dpEYXK62dA0Y{4a`VHc_OzZ}+cM20?j08!9E?+u(f4VpgPzb9K zz8!a5CMcHA8M-}wwKE=;A2x_l@NuvVnW;Ie8oufjrP-(98{V9J_c}^HPt`*4kZIFA zql;C^SIxk4i_&bHx!iqjL4RGT+~i$>b}l#IAV(A+4XKlh2htg;xsC~|riU-B14?#( z|LboBK3r;nV=13aUh|Ld-X3ZZb?;Y9R>{9S$f+-eyY~nZ3-H)XtF2ih?ewdgr9`O|B#fl{0*8DoqTs`i3j8PtVVr(y%Z2Ew z;N(T@ocOd|{cic=c{I|;EpuTeu*j&e=Rfwo&CHu{`j`ir2(!fU+~dGZ4Qc{eCyckJ zL)_aVmXj&GfDh@?=GOVoSReft1!M8rn0*vD!&=<9a?+63h!1w{=>q4ac`vo7Tm-FG z7sx1HP0xy2x~C@$Ods_`gpq4+87>}x=M!E1c8?~hJ-6COcJG=0%^!iyI0OpR*zpuw z4ca^<3g6gC4y7Rr<3XrZ{Q_T4sNw0zK&UD6K3k*e^JTFs0K=eU@(Xw+R0%63ocbrJ zV9Y_E=gw+hc_!y5nXj~>M^#a50AnXGIvHnCdn%6J!qLuZkv7O!&uA{pWcdAx^(MxW zKXLm0>_mJdH%`_(fq(Liw4si(p%dQkqKDh`OL0DO26-F*!0gD(`X<)N1qB9XEGtG|{gy~yrslJGY1If%MtRo_tW&bv*f2(T?hSm=YbTNP50k{O zW;kz?ihPt^+`vVYOS2lZ1TvB07Smr3E#BccYb_Gu5;XNCwe=tUXKxG|zOwPz_2%(n z&M&M?5!+lSQbBl}Z#Z#m5tjT&PqLA(b3!V?WAtRCODAA%^zsJiz>A}}2$Rw>1zdB0 z+|Cqeo|QA>t6K54fb+1eYm-B&Lx8Q4z*7)-lSI{a9T?zhfbFAndJJ z`t|4RVN-d(n6NL-yjrs)_JU)X$cO)@% z1PxZ2_#FO~TNRYvU;H$>!au+^!eNGF{eo?{m@#C4(GpHfN8KFnCj;8;rZ)r z{o04#M2tIzTODS!(aK)A@a_7;0o|)l&g;-kT2v6v91X!oq#Gz$#spTSPOojmvbyA? z)Z$o?THg~J82Fn=^*E&~Qbg;vO=#?)qsZ2z@52${;-KK){RYa25!vQ3)V?>}t~-<9 z@n$0WS7#;TY4Wyin#O!(;vwhyHG+IJF~lZelh67m6sCJI*F4Mr%ZL)yEbcozB!=Fm z0kMOz1Dat~LjYTYt;tOTb&?x!jZ(4rNqb-tkHw z=C`R*dt3447issnuuNrXX?N_L^g>YfA06bO`q4Q&CiAn3QIR$-gh4VFLYvE)eRGzF*XE(!9FT9xNXA#qS zvwzE*Qr|T-bZKM_#wg*3Z!$e54M^L%`g)+3JM5NA>**k7{rXJy52tZb6|b4@+(nDd zBCW^Ttwe+|Ui3_A2Pr&13cwW)vBH4Q(tJ&uiDfq8eWaJwad$(ckVXO=H2PCctT z`2Zq=AHG#*`e9(7RHo1sy#7b@g5Az1D{80W+P1MuB(L~GF9fWTbj?>lZPwHc-9fWdP#%Hg)TTf+oy$fyQhEImq4GPy;!YS3A@hBB52lZ(l(0|7lD^>YjEti73oOnl{?I>+Y>~bC_)MxAAD^-9h4SY|lTQ$*+w1(cg1fDo zcnDdPop#P}=Zw&7f-byXf7;0-bU-b8bRZCnPkB;RRE$V7E$3GebBN7{^H6$P15dU& z%ujsJC#-X(BE9XloHNFC#&5oIGb`#{q+jx$UVW4QKyW;zNm=iN%4)!m+?dCVM|~0( zBID-=ff$6=)0zqdE4uzu#CtVv?I$Z(XJOoxe(LEpoGyzPjB1XS=TD(c^P6Llj=gL4 z!qOJm8Y8>3CIx)Up*9MC-hj6Fk@ym_=T18E12jfHXBWvJHF|`ag_A zaf3V7J8=3P+0U`g5%J=Mflb7HdU*(yv(>WB1YHdYyow3f(@0ILG6^K8c<%(h;A4kT6XIc;N)Arj z)E;^kyfz!P+U>Z3k1zA>$w_|UQ`LCak|$!{TV#h5D8|Xi5#fhIxBCMW=IymK|MVT? zg|w2_1sjA64Z(`?-!#3fOceEr&NOhX;wKA(7g4gWQMtYEn|`^F@B?rUlNGtk4tF>e zM_I&7d=l08$N+@?_puLE^0sunI*?6lDDyF)me@_2Fm*$fAK-)+Horpy4yY9xP=$-% z0>otE)7HkeOdEF4As&-;PMxv}jXr%QqTZ#)yx(}M@)n%esAQzU#AC|zH3wR`rb;n# z7?kv4PrVColMawr!E;_&lnJ-hIpq{@oD0QczlOh`?=}O027OuceY`hA#N-DnrOVoP zetS+b#0wS7RH;lJ7vaA(XdY-sZQHOXVM(12 z1AMRc73c38$57+{Zc)xaAwme+)@~{Q1Y-5uPk=ujc10@ma-9M7mt9 zNT8NWFSm5!J?Ui3{#jK=@VJ2b|Ls2tN_HVK7XEaT3Q}@IVV5ro0Cf-({-5w{%bK%T zBQ5@5<@4S=DJ2D#D??oJQ2y^jIre~|;BJBk zk-quk$p7LZM*TP0!yfsA0I(U<@0ocmCwVIS2rZYFN2ds-PHw#b<(sX!ZV$)h23&|Y znoGqjmyC4YX)d6La$6%fmT=OrMJ7zeOuQ$ziu7muMek`KmQ<7BVW0n2q z>GhPOh~NBr<{<6etnmGC&0NPDELaJ9BPZ)65+DAoZy;Ge5YTcrkD(D|3a#!7VRzYq zUJevqqh*C=VpwSx`%Y7|U!Ig|V0O!nbm_b9iwxuK3wvqn_uoVp=b$qiMiVh?|HuSt z5bM7RQ!G+w4v!qnIh5N*!Zi6*I+RO<00G&Ay%~`hKj@vEsT5)C_4?S+=s?8PfKi7GQ9;|iE|)QW$Q%p-ZDw= z_B~!0aFsLHQ3r|IyNC)A{|n=EkZEkJn<3r<{vRfD$9Gac$cy0~pwJnEk zyZGSI!GDt-nuk1AHu_+ zU^>=k^T0ems%_&rKn(POGRy0=Z)NaPX`cjh|Goe_DoH+ z-3|8r3$qxLef<^Gn@Y9O48nm-%Q)u{=QT;RJp(54j}nazu(wSPovZ;ZoE4!+3q)~2 z?F#KbkRBlw0-7ucMc7S+uepMUb1vn{dELaI+KXSWXhq(hTbp-~eSE6~pUne85XA>K zs>p%RvI90z@Oa&na<&FN=uaaHGWa)5dYx~{{LGU#f!XvgpGbuLhACEq#*10=ek6*O zwUnC!a}R8AAR{zHhz(0YXsx}cAlGg2=SRNh>x{h~>|Eu$C*u6_AKpT~GU3^Fab4c$ zGCBtisE-GL#DvA>pO{Q!2RoFc2Te>U^L)Zzbn*FnrK49P(kD&?%qEF7URMVF5a)ZP z7#PoPCON(z5xt=sWz7fERh=6rnRnX0Bi0TxmV`C()YQ}{Emy6kSg?#Xt=aK5F7@~b zQkW}F=i9j2SbsC`V^Tn>(WO^4E7QXh3GnMyQfd~t&`@%Ay~w2!cC;GHdI-wb&a`s1=)9SvXhu>3Yo3)XyXd39p< zi!Bcs!unOmC0upA#R|4Ei3;u9vTtWEG%NU#3IhdP^koUgEWY~>*jYE1ppmqTCD;3a zE1ApS4sZU)3w8TpH&ExYBdn@s_#}M)g*UscJRRY`ux-ZswES|=5nuZSNF^hO$@kJ? ziJ|YRs%s$M%(C>oJh`VyFbdH6{HDWs*I*+yq&@A0A{Xout;dsdt=hN$6Be;z=Yrld zHa~{gU9|PBAF`Kv`^9oxBe7fq4NW)Q z*n~1fKL-wC*}IrD{p)rUQ(<8-_%nhQb&ymnB1EX&sB;4K1aBjm7wI1rirLC^dOBZc z^L^KpPJ~+hug0s7v|+9GAT`Y=!2j?B02qol(%QHF+lKvZR7G8U zhTm-L?270=9mXmz^xI@_na!J_7Q#Lxbiypl*B#@4hJ574mNxWrW{Gx+WrA?P#CMHM9c+?@O|Sy%?+$ebL^U&9Z(p~Ua)9mkK?{cSn# zI&m4P@7y5?X^rpvr^UBC-ZJUhrO-qZ9!JLkY3gpSU7~H>&Y;~l2T*(359R4y z8=xU}2gB7I&YSz6+J9_EpCs~p3=--Vx1Fwc*=drQ)*}pQ4W(3)29Fz&P}R{j><8=z zC=IlazLobCp=S;OKoB274~mma=kG}zcdynz>OCn9SY8$FTfhB1uRjvOmFOE!Cxa6d zYGQi}K}=+(nl^cRc6-B!0NFjdf@bV3=Vasu3O}wZOOo>+ql~2zZ_+$T6tBN1n3!l* zceBYfK52$=*<}xWX*MdCmX_wk;q*xnFD!I3(Ra`0#O?EY)~=%QdAb|;HC03MKxw^Z5yqT za|(2|L}T`w{H6B&Th4V*pBrh^xy#jWEC>r<2!^v#U~o16M&i`L%go5=HBbsMWRx9K zn`$bIaL3sWQtD6VpNC{iBrC_^LJ^$2p*Yn2@Cl-g;!1aT5y|~4>J*}<6 z3~t-}QB?ExquXt)7SJ=BGQ991@96{I@gmW-3tg$T_Dr?JC1;T*_kRz%ZsU&7nN_{# zn^-igJS=u%7v$B0+-R8nnuW+>rA{Qm_pq=`Eu zWx?DRmP3OHlHBBgY=+0!$Q{qLisDurcv4oi21K59%B~c~N)T#mdFg%i05`q41UqIB z>lvl-g5%58heAxi-0LpJYEWg$=Y&1V%r;;EE~DdqX5VPoPs+lbFhTdc;rWf}V#U|$ zFWUJ!iEX5F^Irk$bYXU?=AKkI8aJvtL?vZS8hgvEI8mo0%5MT1dnmvp^y_e#oLnwQ z@`tv;hlM!s>aPA*l`09iTi*=ps+EM50offM^AB(>)ley`I$rdtEk}9Tbba@Sm2Y7y z6OBPGTd&rIv>&0ZQWk3;BrC{-x3e{qMbEBU+(bHn+I&w~k{;NjVF!3asI<)Z&>^EN z0+%&QvVOX()#_wzRMtyYh_%^a_)aYd`!snZ-N=HvR62=~ju|7OCz+xd$^ zYnj3{U0f7rKh9PcAhUbvxE^i3C&;3Nwkm}I6QR#p19Im9WBkO{*5rU&3dV_9Mv7=; z#5v8c;?Ym!s7($YW{MLAah;XeMHj~_S8&D=y=0RQxfl}KfUrW!Z_w|)OxpI<1P;Ff z{@I@?EH0<9xIW{e?1s(q+#28zUKT+9FlQMGdj7!rM^#U~BsUaI(Zg(#V-2ZMEr|DY z$}xHEqGy;Jt6+l~BN%v_V&?7}bB``GA-4Rl$-8kWH<9B#?%+}x8&)SXt|E90# zeK#q~{|W=OMXHiWAUoJmOVUX^p_X=NYYDQVm=V&erdN3OwZk<2&RlgFPFY;?YM$-8 zbrhJR2vCwq5lmD8Rd*sUu6VGxTyPoNlt{YkDG`|Awabo=-VDTCjeHwd*kq6YP}>*A z)ut*L2G9`XhLGB5nu^mwAkY>T%G^)>#&ByG1qT!-LDKS2o) zow{{wWu-OFa}@l|R;zZZ1Huv$|9Qt7*Z8r#+SZKg=TZV&#ZyYx<3$%75o05li;FE z@!0+V(2&p%0O8t4mR>sAB;uuzwo)-2McbkzrT^t|JFr`MyCzz7?>n=(Y&_yIce>f- zv_GLZAGIy0P*c@jA!r!D3yDl5OBsNytkQFj?=T_&+VNnw*U>uYbqwzlA_dA20i|RMrrfaU$)m7L0KE=0J}x$esSm{xjTXd+ z1}wtR-aFOcicigg-}PB88ZdF}@}*l?KL>KC^M>Pf6pmf3SGTRHdk(1Ovg%vJ7qZKQ zIc@M($}1|yqQw6CsG7@R2!8@{7;zXkqEp!qT}#*S3vB**y6dBPj03>wBZvp<_GuGu z4BkXu_#nR(%~{TA=9*4)DeZLepgI66$yrZ|KEqzH8>~;PZ_Y9kABR?Yw3uKvn&&IL z9X~y$c;N5RYucwe<1aYn?DOn;Rg?w?2SL!{x9{VRvf%#`4MdlBq4ZJHL1>_yYilZJ zsqq_0d|rW%WmrH>0>C&86gD_9<>UBO^`Ut0TJx+tClbu0moSu2S!p8j!LN&%ZDh0; z#ZI73%As+Atf7t@0{HX(ZK$9EDkus?boqtgQmU)mTF|xf{(gBRx=Y~pLV^kBYp@s4T**w8`s~!=SSMK&78E;Fnv*IDw}uGg#TM4^YH>z zj^B|je9+Z9@3uj7cheLe5A4eO>hi&`lBk`pl{4b!znftl-lx1>-vusokoILOj>5F& z);CSNhNCui62^+phBv0yV&=O9#s;0^(^h{xkZB}8Oc>aB(mwxA z^dO|VE@#xh$qnP6ci)uAOG5R>l*3GFY|mR5CnTbr`DT~o{{5$qV{r{Jsu%$?;_9La z7goCua+miGLMxRFX5Nk*H=8nD+Ho&6TRnx{V9nf^z1X=hRCuo4>&g_Rj48bO3S|E< zj9?yP)MO}Q*#{GS6c6Ewppc@dQ$H%x0ZqAqe4};WIlc7EH+H?Q+|h1FF8=!Qh7apz z3@Kg$RmpF*&tQmlpho)mFE8vMQ%6h7P|?0=ze61$y)9rM=3x_PADey`OO(e!9^wsz zS3$L0pUhVOSK|B0J@_5%oy8!29!h)Um6l@VQ%U9RH9cL9(@yLUkU}NEnlzd7 zZJtzQdvy1r@CD^U6bWUl%J#2**kA7_;Qb6q&7`uOBNq+SMee^1 zA1#%UlVxvqddMa3B?)ALmRfK~fQlv^`qpp5tYCMpD?=AMME(RD=uZQSpZ&ywoLV9p zNKbgl*b8QlSlLs&wc;ciV*f8iQIL}(7&qxSMBM#}{_dxI-1ns_ODC_Jqs7w*@QFfX z802(B>QJfg6oWeiT_{dlMZZ75loS|=B(`P&#E98Z>pow$y-S07P@vnHFI@zt`M<#D zmBof2oTbAA_aH1bE%Z>^myy%G4c4CJlO5Bw?C#1)|NrGXgbz1}U24EY7O{A&(H(h( zg%7#{7%zk%IUB`QQ`~3PHy-@WywHLHt@C*3^8Thoz~g{*dy0y`%a!X_S*6kAMl9V0 z+nv2eqg?gF3c>8-xLqIbr=^M~T9*Z8zZ_TJ`+vE!mN19PBEPZE2y8!5Xz{H+TGemQ zFRs3a`ofeJ2fLcOP8Dy=jv~#%*+$_OLU+i^c6oTWb}#&La9}(lzKqE*7^nxoQJJu+ znMg+&U&Q;$lQH2i5r-(5Z8;ap$VpNDUI})kP??p3CY`Xc)}bi&4*%R2E}&4(f@i#M zRu6Q(`MP1}J>T*}nMPbvCh$5dkg;w^I^W((dv6hI`2+X;t9llXW1SFcOT^9Qe?@W; zcPS|WF#4$b!Qg9)1gelj_Z+MT3H=oC6SnSu8Q}Z&cFP}-NaY-arSS_jZ*~_JXrvJ(O24QwQ1IlM7p6OdPW_fw#*t!ta_ z&4cf2eAmaZzh!ueN63;ecm#sLS*ksNIy*c$pM7%t69}Y%sP;4abaOW5F@~kgxh8v; z;s!?keVw7#^y@EWK-+>nQZ9d8f(7UxT70AR7sY>zx2ZtsaZ7RWogZaw-N~K`BV_w~ zR{-ea`^;6PH=Us5f0OX~$<~#jZh0tnG_?o(M%3#YoQQR<<>rKv;STNdq0EqdAu-Ec zAH&5|a%r*K3T&_6oAm8#GZhH7V@89FSi<_ll{q=QQ-M*pU*shk%-4&g%%Mh3`hi?u zB$1UkSbFJQ%xL3hr`Lo|{N42@UfwsX8~CLm(?G$G<0EgY0Xe&Ujd__FyRmlB<5mrdSshlP*YuuJJ*yv&{I4_Ydm)Sat)O~9hQx7u((G21Mk3j zM3l@-yfVBdD-EGOqEj(|4)Q)8RxnbPI=PWDWbnA)9n%lw9V*UC>ubEXUl2rnS6_=x zJ#U%4y0}`NoW1NS^x87G+Vi=bEp}B&z~FkI|7u_}SO&i95L_IUs<_OknW=%Hhh{l1 z#h_%6v+iPBg{FZ9!=5Cos;LyO>leZjRQ6sJ@ZN~?UL)yCZJ66}>r2tQ8fz0j>`=J` zoqb<3x8|y-t80m!GbQJsG#q8Qcl#4uWDs(hbII{9-LJIV4(Uhpi83T6#tAF?(5C+# z1OX)TW5Rc)<312@vRZ(f==KqHiv049o6qH{&udo+EN4;<@Oq;lg{yW|H<%&M%RGT0 zGe^HHLruD^2P-~Hb#1dCPl&=VerwGLt!w;WJXk*eSeO9YPru7AtN#nenUkHpgkDhI zUlH;@+ifszu-Ze`6gP|Qmov*&nuDdAErQHHi=8a27|E@5HiE_Zk&pL(Y61RPlc9!^ z;8Si03lVR^{>+Rm8ti^`QL|Qh?~l{1@Z5PxBvmMRT#8zXG0U@2@shl-HSp&FunUkB z9VnkfHryBosdNHr8xQER-zUSjv9S?%FFn|JdNdW=iL@;o}@)tvnl zUs=@PwA4R?aoh#U^LKwMl0Ay<`2dFS1KrJe6g#OeuT8u=NfavR?z1Ow^+&UQ7wJ0l zA!{Tms$&WGhyDZ7ywRpMeu;-p04Ork&IZa=0U9Sl&B3iX+PR25CFSk7Y!yiy>OX}p(W znu{fPxACqn^BgTG0yE@rpmwWUAwzVCxC`cQeAW^U3QVJm9 zll^H?s)7`wwqQ6*@{w~-Vz~5cL!T0X0rYSLccKaSu4>Jvt%>VVVnt&%b>F_6H?%b} z`o!KS&*PjkXp;ZW%O@Z8n=J#C$6dzY;O8bOl%nGJ&!GjYi2#gud@-2E+%FJKe2N_B5#BUc$GNd>X# z|FUjP0)ZgLP0w|800(X5oBvjlA=>E1#OyZ($C?KIo_v-j43aJxSE>@-!qMNz38r`y z-pQ1vCeU#vzfR!_ofI#YnFd-}WDy;QcMp+3&FP+|u)@rfhX_dh@mBbPso=9Fn*|XQVz?f zSFcoK-0S!SR_8y|gIPL?oAz`w2L8HSoK4e!`giT{)jns9PJbfBH|=yl!vJXf>{fxp z{KGRuJj_4(jtp^Xi4?}pjcJ(MbPg=*XSy=DYl)xae4B)~iL1ck-iM8Y(>jkMo&1%VnVH^R;ys%?-KDC) z_65{O4K~WEdW+Z|;*m0Ua2f&a&w{OO!=W*qGPVCn_fv#%DHA+?Gm1$KZCh+KY+v0Y z36Y8G5|faij^N~Ala7sxtJAm3*O;B`5=wsjdT!%@tZm1`qoz3T*t5Q_PE6hk!eRzu zA;d-~2vMZi@smf zh(lf`hPAXH5eRkLBi$<9iRo#tzjK!WG(3`z!toQ6kJ_HSTC4eUnW@3L;X^`9HZMe> zkoNV-eJL1`at^)}J#9WMx>$XH1Q6HvExqR3VLM5Mv$4pG^lFYDNabrmCU9mv=@3-2 zH5%?qTWp&j1u?j3u?uI-8cLQ+y8QV-oaSNER8rPB%+^jqM`xigWje5B!S)E)DdMSi zPP~wOjc+|DreLHRT9>1z=R;t&My}%nYxuNn1w2D1-`38~##UTQD=;vf>7jASc=pJ8 zm~4g)dtqMEU@;>o%B*U{3SYHkyw>$kIB)=5m2Qvu;^oEBg|`n!9EJ=<@wn~$ylH*i zB%!-k#oMt&!&**lBZ#p#MXV;JZ`FG@x>lGc!QdNl^AKGs<$h`p- z#y8fPl&^4bLM(doT8u+J(>z75?E`W2`S~98^q8BY1!J;;m{Jz?iKlqh5U_A`=w*1{ zqt7(K;clY64z)vT@A(rc+MWTU+IJJ32zy2I8(tGASTVWfEXd!{I)ZmLtBSSdw#MR^_7)?TfCf? zI9~?9`>5U(NK;`p4iuw+*W#=_vt>%2={2tq@%G@or>Dp`75E<%WO5=GF^^qcJWTY3 zKTn(HCHguL`L=Yr=JPD*3!F24L(~lb(u7QLDG_hetv%UR0&xTRy0rT1t}=ng3{X^DB2tz z2Sl&ldp#E<2Oa3yqrhipV3vwi*Qxujw5&g+OaDd*t8Z127e_NS^I zb`^B;%2_2NR_=8(vKcSrqtclu+JOBN?o@X;@dTPbx(M8E~3y-BKsh{a0T41Ep~ z=LWrx&$*1VrTz7$hsA%hqY4MqJJl&M&l;1oV|h!N@dtYOTv&p}C1OFT0xJrV%X;<4 zHYh>vR#^B8F?VU|(&Ih>fUMnMyCCWi+|u<3HUc<41L)-OcXqr`KNJl&w$w>#RYP&p5lFbqJ7p~RYD*uesYSoJ<%*C1(ExM!yCb) zzcSes31r^5tyw6zsN~B^FA>T^AmNLF(oyoU)aqTBWMjoBy4PbW8KsDsi8yvbAcQzN zlS=!MlY>wI3UcJ5mX00-2IJu1G#c95*>NT-ukM|cSO^5lB_>+)Z&ce^(Xf|eaS3Gv zC{*e8mWnVlGb}m_UdE0oZ3eu(BgaAC;eUs?ZqkM$TsgfSYyt@L_y)Z$~t%MLxYP zxBEk;M#$pHnBe*uAW!)$7g`Rz|weNItyP86xrU5G^X7H&o-`qv}h5|5F{7H8p$ z32UFDv?qF%o+s))CvjJOy}b;3T?eDz!)K#BzWF=OjggBH{vP!NkE4Hc9w^pCbQ1Sn zB6IAMiXOV7#*?m#(t+M?6iU#OGes+|WI&&MSxuL3lHAV`{K4ijtD zckgrkZ_pq3D%=&P#-oe1EA5?}YK-KIhO#nk4idm|&*<|gb4iz%LHBpyH`li_}+3gM;W^&Y3q=yx?8jl4H-iH{tw`0V#SA3qYZ} zUeWx3yKns=xzY#DThJQQr5qIpRMTw%8JTi_od|5l*YlzDx6HkMY)SQcRzhxhqn zD(-!$#Kgp|t`PCmxIpQONik~Gq-{0i`Fr4~GbVRkj!%owdr>3NIf_I9!>%R!C^frBgw{mg4={Xz?ugW(l(E0=>JR?W-TeAQ!@R$dT1`SCCR zsSUUrL$S%-%BN~=s)2EcuG21B{PoQm=AtT|{Wu~wyVcwgsy2L8pi-CQfHl@#ispt3 zi%DmMTrH)Zo3oYwhWD|vwH_?*1`)Hz}sTW5qF2PMiSkA=+J-epU8(M#dp(cmd4JLydp;4V*E# z_F&uAwvQ;xJ(eJ9&b}(ig33p9?x1b4tKo8^!QnUrufx(Qi{RsoR@&gdnx$)bI6)MZ z*1G>&;0L0NvGRq-wOAndNiyxXg~S;YfGpyOHZlDQ>t5R9=H7RiFFF#TXrM__hb1nk z@_S!H+_Xbgl1?CSG1O@}Er@63t?%511a}WYkhw4JDmI@=OP; z&oA}`KtTP9ZJym-K;V$6NhXYT5T5Ew9;2{!+KVq1OoP!W@moy%9kCg&`j>yYL%!;C zEq~6D2DZf3|Mm0Xmyno+$wy$f*hZ) zu6{Q*GovKt`K|yA20fFQ9cFj-+S#WNRduU9EJ$-Q{W(fc(?LmMCNR#=ly)7P+R&!O*Y;LI{fSUL$tA+ zOR^a?a5Or*qLLmpuLA`&wx3l??J$hJl~_jEK9EB#Fi_o8CKgdQwSD2yq2z7HL8z9T z($-gSwdix6@ZMSe>g6reFRev8RMR|gC)J3L!WGycRK}!LPZ6%AF?J6o@L`5Da*5p& zAru~}Vq%Bx89qG~hEhJNPP#@9<#5?mY?~AkIvIRDTY@h~)Ts3y|Gb>3#uOXnIhia) zOMVr4f?_%nSKda$zce!Ir^JPnJA7n#pasDk47m0O%Iw0sw{Mx84Vo?Q zK3M}wj@~ntG{>?n{0R+yD{%Mq6R4(Z`2)AsQbeW)KMwebla$j+tJ;8x^C}(5h7g;qJR{KY9)8bP`t>&r%Q~58f?5)# ziEY^hox-za(JL9RzWCj2X+WH)ns%Bf>sls|iCi43Q1hxzcAN_E|~T+r-d!M&Kvi3hns(u7mUi zP~@4Zp{BGWw=Yuy+Fl{Bu;;!nR8F^kz?dUtJhf-8ar+WFJf9;-bY5`eE`Bf?Q$ z^-f9T&&c|Pf@gEb=(;4EN)v{fV6kXaBKS%3E8vLAs-o80ZdI?YawXZus^K22*X@mr&X#6fQX8pyGv*Eij7%MH4D5URP42v-kU{V=em20?(oIaLvxIDA?yiBLl*$iyOr`Ys34 zdxvEWUVyAPf%CyUPUm&TZ!ykIXh-F&+wtybh-eC*Yn)%cO8Xj@X7W5Om_e5WN6r>HcQ zVG^J?zH)NK&BTb;ZJu~_$}{3`uMMTgyFbHi4lik^HVZv}7>1^Q>yFD=_-Ip-T{~bU zNTc;s%R;L`qW!)qLR%Z5btSy;>i4hb_aBOFcgA;Jz4Gy}-AhHP%zmkUzu^c-?Tg(~ zqc?jG$$a<5PVY&d$azr|=37v<`31}$pT9eUHPoDT?)7bKP*kdVo}YE9BJ;b*3zI78 zEoY-P9pxW^ZANUwj`>DVcPkiV2f9TbuX{1|8PI#sSVpwu9S%V590i$jlz)7egI|ja zxd{DC7i95qoW)pbc11C1%pbc+C$8G#6PO?!>3}h>RmFImwNrWVb!)Nw^Lq2OD)yov zGBDWo@3rCM%;M2bp<9cauH?{n2?ltW*d%v<_Olrhc}JY%6e>DC3 zE1=zDKaiJr#A?uIr9Q7D3*2;e`Sq>@>MsQh`(4j>G)fyd4Uc9Z>Qc*Zj`vYk?jzHU zs;2Y)(+}&XC=r(<1{WO**;u*?$2ParU z06$XGhjGn5fPcW;vSD;%tzK03_fL#acoaV^bTOx9fcSWAuxgW?4 z>(JZ)4jOQ@jJ7mf%<@Bsn5`J$^WZ>nu+Bc$CS_^X#P#bJbRt zIB45O==*G_IEsi#y?zG0)f@X-#2696j*@F0G1`zs9nC9ro3VjIkXh3N*SLmEWw6Kl z8}WnhhzC-URT_*O6%Ai>D@LC|l9B{or*~`LJcJJ$7L^0E9q^aj^fB{=Oe=Fb1$k8Q zp5*j?-D?q|kmoV?=nDlSrKXP^m5?Trrc+rhdAmUDme3)uH2F zDWrd~-oJ7ptf8@{iXc!KSrnu1f@Nc4b;;bZI9@np(qZGTMT2YOw6s$J4oBN77Za?Y zwGN}oKG4~ftyjuLeL?b;A%WhW53qaX0l~NKHo&8^f~Dd&#{(KVR!#v)@|gR`gY;YCJa#oy|+6lC`t07xllB z*U0~xCzhgOZJv9My=0|n!!b_;$d}%R>5jkDkCe|a1q6i8cwfW~zqBSRW0!i4ZuT9I z6rkJ9>(NIbruSp#42yJ>iVW8!Q5jCIenARK&*Bhyhj-eYjg~(v5hzv^!?BZ-lj5H1 zXn7tqM83waM)3Lv(J_OvzqX~~kMfHu?WV^>4|4-9;^ZWGdM&KF-xx$@6196aV^}se zA^m{P-C>7KPu)%ezpbOQdd#Dg+Um;b*7tEM`I#-;af2}1w{p*iXK%M`$(A;0S&ST{ z8<~2rN_*Nj>)fcy9Ou2l%jR>eUvHd7ueLlN>mR4Nd_|79wmkuHP8unYXR6v%kb>XSR%m z$fpy6(JIK_eND43*lDCf-!~mBDzk@X#LgR8FcfE>%{AWDBFB0OzLr!xZoFo$*~zlerSJ$paR> zQ3H9R`uZ?W?}gfiGLcQXBHH1E`|J%LL>~#XJCl)Io2K==E^Sjp=A1DZp`Xu_wif+y zqG34TM7F)ATKsqTT_Xu5WoWzkdAw$xmY7bCUQHhrHHX%kUwPGO=NFy*|DCHp8x*kZ zF$-&0#rd*D`zEe572;U2vZNH9vNmAC!{=$Ci)X7@GOsS8WQs$Zf>>Sd-P@mcs+0G) z=IQI3iN+>`4NYn5=u8$=??RFjs8fEo3S`E6OgJb*d{h^ zsLObd6$Ba%n(Fmz0D2EC%`7zeC`nJ?aigoXmAeLmaBO4d-^T1463KUGNd7JOc)2b* zZQ=;#^;;{sG)AyTr)%beP9nLdlha?ZVUWH7K>A+x1%O??g?>Ln>6yUldS$oyR_7lL z=S1$4g40DJDv%q)XzbPzi>Q6uSF}&Et_n#I@^|q7dw5+=Jby|I57+JYsy2ldSidM_ zj-ewRBO@rvUUecnr0LTrFyd~%TfSW$MmPCocVs4K)yvR1En2^5EAEP6JNP*^uf2-V zap;TsvY&Tjv2*p-t*J+L-Yi%i9mBsBQrvcldE=$0>yx!*9I<&8%Cf`qL>Ji3SFO1M z$Bu#fN?c&2R%pQw5v|TUN5|PTtj)S)L!%Ja435! zR*6NdQgjtKOnl*^3HNPOBZ^J1e(mOFwlsoEstSDg@|88ix^$ZKe9yC->q{06z4E+$P(1@2#j8}>cO242~VND^eZ z=;-tY@`6f8cXf0kXm+1BlJ4I=W&RzRc(#YbAO0-VAr3UQv~1^D7Zu_h!Yuy?kjowu zex7Y&_#V|!inh=_#OlSh_ni9kG-eC&m`gD>`c~Jw&xd8JDzsLv$(1DJo6^-?;Ti)X zsEGZ!nXPrO`yJGo@A&CgY-zPysJ&r#ad?gZ7T-9Tf%;ppp@3`hN**He%hTR zB`y2L`v&42zB?A+tc&EJJ$4ZZ_TDS5M?b9^M3=m+ID3$VG@n0J=7~PZrcz|hPM*sX_msjnT_E@yKofKDF+#K)kd&j3) z8>>)!<(i1Yp&P;dSp$LiHPuQ_|A1_Xn%k=7Q|6ENu6g&DPL&n7FYa@%BKBe=z%~Mo zkloMfu0-|^`Pew~IemQZD#neHFrqXPz zvaF^?{m)Y<)I`KzfldHL5tHF%-RH%z0AB=ei*r0V^gP|&t#GI!msP9sVHTT&NH;k< zK8=&4Bk`_JUH!3@@i+(&E=^gc%|>j9={ea3oV3L5{oK<-bDIoCV$<{*r@O0x;|a zri<`?;h<}`5YHEya~nzQ&KmkiZQflfml36&r_jzbNb)^~WUY1JGFdZZF3EZYKTxu=CJzcT`<)*p z-i};U<*EJizQqJ^NH*SZOhvUPmW1~-sx`N zT{;dRP=KPfewi=ELbpP%`pMwpqBiesyKYt$M_yUzJG3d7%w&2$Ay+#ofg~Y$W2b6z zGTZ2E>!6UaundUJa3x&UQPL~SKj?SN)Wqw3TxLYQR45DOR)I{yXDz7Vjnz8isZpc1av zE%lgOFr17qV{<;-`xqfW9^^dj@+#;`<*D&^NeE*4D=@9+M9`TUZL z90E$~_XiA3j(_qq?7Ex&-p*TWRdNdr%$lGD31Nsu%4{l;m6zS0UTE%jzw)$)rFkMg zG&|YXE0dpA7a>pVsiqTeKcVRjx__%@S?4_l)5dJuQy@#O)(a4TQwPPc3fFr8X5rws zr|U&p7=(QX$NGz1Rv+ublEVd=%ey!o|NT0Iwz_&a=C{^jZ)`HZomg6?^+=tr)@REV$mApDfuRsOs=J%+J-?0_V%RMf^kvxx zWS~ms5Lw_XyW6W0QP^$p?925yGLBwP)34Yj|L+SX7Z zDy9=?BsP-IP3xP-M^-Ig4a_`D$?hL(He)q=Yof7CBR0mg-I67m_uq}^5;6aBv1 z9r*wvo7S_YztZu;TnC2~$+~;W<_O}wuBFdpac3zP&VB7Lj7JtE5CD15Fi{Nwx0SN? zNnbI@0b(yWrn&@V^!I)Tb*|l?Uzkh(Gb+r6NFP;9v;vt76%abqQ_pL}j&oGt?aQ?; z5(R?ikZ$)$xVB8sN%^Ep{dms(UaxgL)_W(xtg}l7{0uvZqN{?6HBOdTe zf3Glej4-gQ9t^JIe3KhT$pL)et4|&a!oA7N3#Qyp5d4T><+}rTI7m%ClcK)#F{ zBQ{g(@R;b<_)p>m6+*VTtop>*A?Cyzo zVd3qRnlY~@`o7rzz2#29d}Yi1DT-1j-w(eD`}?_`E_4TN1e_Pi&E_n6vB}DASEYVZ zfAUtae!vZDj(+ zqE~C;EEaAhuk4W+69Pe@JYJT#C>v5I4EGN9=C{7D9=^h#_0um^s@wMD_do^74&U38 z4+O|VgHxY{@!K~1Tuq3bOxR?DqM4y?`jDZ+9GvE^wWsX%wT$#R#eQkFi*53in%6l! z1`5%)1=IK)<7atJWYk?9Cw@9qjIWK1OP?>D`|ycDx>$Sbn{i#sjh`P=uQ%JwP!;=uHQEZgY-WNHL3w}$LV^Xsvp|9#Q-Y< z{1x)xOHd+}T;v=qcsw*yM?ZCGbTu5%m!INE_6%x&tr`8h7CX^_Ts1*;HUK_38$?|nwf%G$q*isMFlACD{^(kZQ82@fn<4Yw!@0ofgJ3 z>9v?&b>Ev3Mn7kz=tPh4Ceo8>-mG5-g}5vhf8cXxIB6a)pCo3%uMhra7U zYbGOr0Mvhg=6YL_|Dc6Z9QYS4VxNymh0lIO9vKR$Kx1n%N7>SY9=<;M)rofu$X)m8 z2}PEV99m=!D?wq=6JMMKq>DXQsAdSlDq3IU{?uOnmyWni4Na?vuC1m+^&52MjGNay-cdy+oB5{qPyFasYYHF`aT z>J8GC6!W&dL#BrLpDXd)`kNT{fJ5ywZm=ahgz@{Wg$fI8Tu!N%_>_ZZ7F(3_+houF`b>e+~2uCuGsCX%nCaosQl2EZ6i{GyZr z<5f1oxL4oN{$fV;1`>L^w9J0zy8i6ry@t6IjZh49$Mbypcw@@T(cXU2U@7hVu<@n- zbf(6BOkN}We5-8PLmWr7{>QlU^WgHGv@Gd&-P?KFm5<||_-ZnEn^Edl(q>!dyUkrz z^-gVH0#4eg2KF1b%4+qKLfYkB&mO+|;P^p8i2vCKdps+1sXe~5fEkADkWeiAl1zKI zn~e)ly;X8+yowdb)_Y}-SvX1lCjm-kc4m;i6VkHsq<^!1!*73Kef?IFel4dmZs>Kv zYYp*iwW`{a7ldD5I^>}DhrBMb3WF(wzcKpKkbCv1Db$4PVZt1GHBMN>lBGbl#c{Io z#XfeLbb3(Qp&V^kv=4^WjqgmfGK-6gSB;L0Bmoo7@mb^S{U6|O)f%itND{P2uft0q zJTxRnHhMl|hjJOk0KJ1--4z23+Zf`sIU8~;0FfGop>ffhCLH1K)iPT=y|MOZVoxGK zJ3-5J(lf#*sj_Tr`Q#KaE(2X{J_bgwQ>gvkIFqA&|@2S ze$@qL?%!70jOvV>6NyOAoOBrFXwu-DSl;!HnE$gO?fD82Pmb6uwCkra0qqv`dY_oQ zc%Wj?xOAAM(&ao+=h9`M>aP`N@r33Mz7oGS6;8V>xn(7|vhP!Ae=nq^l|tCs@A)b- z46WX)Cc^JJO(bmYTIjoY^Np;&!@uI^n&{H3Bl)Rr`CjkVO|b1j$I7Xe*A7_8-s3D! z^?SZQ?I*EIsZ{xIW*BQKy*a-14B} z5HCp>WYFn>COI~k_HK~rLe%QW%5IIicG(!$pI8pV0GzhK^2>|?H@XnP!`-8^4EceT zP>V6ur0_c+rpISd=jhW|f;gLM;Br{ac9jyEe#giQEYkKo1nbRWdyunNWyBp}@I~Tq zBQ;kYmE}j<~kzsA% z2;6HZ`2sSzp%fq>XQ})=u6OL5d{`+hEc-M~t7)mPWjb}@Qm-?^LB<4@)Fm~xR?cFp zxSjIDNe?QQp;m?OTn$=OBC35_k%(WWBU8(E1&lo)HJr9@$DUimhP;}c43;?}U4#|P zIs%5#4YTUMzh;2^_tnxVA+-0TKM6`!KUFn!UKuIPJ(boxUT9LBn{3naS>VRIT3rfw zoqAb@*838W&~)}ZAw>?F@b{4nr*48oE4#v6`h3fS#-wzN`=g_yi$5Oo*VAZTs zYiPwtmI90o1)0D4<)z~JV+jkVzeT@|VnYo3^VFfgcpg)|ouE$rbm5W6j%^z~TLcx1 ztqx-`Z|cKAPe+FinbFhp&VlL%B;Q)QwQy(2Y72GMiG%h~^Vgp$t)=1lOoLqI!4nnz zgF6>dby|>Bn^>ifoO?;6`h*;WbEj*MSYw8f$*MJUw!#42IieKbmcnK)Cn_SkVILG6|SP? zjWE}r%^XWvxX!KY0sBH3svECjNGLw^L5WaV0r&`*!Ts|B4tFmHHam zqI3KW4#NgHta=4*u&+&3oi(NIXh?r@j?rb{lXa$}t9$LW z9k(&e)vey89+y|`K-V5*Oxot`>As-A)*k9Nss)x|X3mtg?N#ET_uR$d)6PRSFakn9 z;Z_p#s^q=4Cd9=eOCXMgOB|Fv@^Mp0tRVC2a4-E)gR|@PYI{$7T}Et*RI)Wz`s;zb z+Tdchyjguj(qdY*jP4NUG31ES;yycjhSSCeY^JO5)7)JJ%a~`y$g2H96?oP}*ptVa zCeOhu#&amHQI;|@b>+t+6LK^Z72mpTGotc9Skc(fT8V?OYokoy$7#Hh_|%vz2@T5{ z*`~inb+6C1adL*6tz2{=;c|e)lU`BR^q^)G0nVRG#kDZUB2*tpmKz{-T`DpI#?a>Ld>3-8sGI40`b!F>p z$;}^M8mrd0l;D;f;Z-0|$(P2Df&sdJ+|yCd*HoHkrxDNR?+l|0NVUVO?%3-?0YeJ> z9s*&y2$*x$80y_u*PyG+s1{zjo#Sx1GHq9sf-Plx$p~04BBJJSEUym!<3=&b5(NkK zz43ceS&`it(DL8QDpj1Z@V`e0bm&!rue3_G&F2W z%)akqy`LIpxys>Bib9zgMx^Bl-F?S#7DkS?WfzYhaPC$LRe z+fxCFKba)m-4-W7x?mHL(D~oLb7B5^uBAR(a=R^{t6R#u-AA9;Qd3KpO zHuQ3B26r#c%nW11K`#C+*^YC%?++g2DFvZ4X3Y@D!Vfj-y@z{zF1Ry z!i7ug1WGy4fbNTkeO(ZIu*0n=+0TEU?^*cAk0KE*a6+}@)~pUB1{!+hjzE$Rl~5AV zpcM)O{rRT}(WOUoow7UQ-5@fwN9HZHVp+EOd$i$@NG6!(7EB4P+6xR@!EDdKp~#nz zHYN0)Eq}a;N=K?u(Xatisl7g1jds4QT7fCHVo*;UoVsLGx$Nj985tFmwzg}#yL)|o%Af(g@)0%G z2*6Ro6^6kOJ$Xh=&Kz}UaNF}sv0j@lfnZkpsh!3wBYd8bQj#YHS8UT4SCpN<7})z7 zx45{`QHSeLvJC^bmn3llg00z_!qT7alX4mBhfn&8s-LvuMB%wjpVCs2-^VPdK!2!^ zS6LkXDT~?oGb1K4n%&szoLDfsGp-w}%zcbm*YfZEnomTv^W=}4*ne<#{ZxqsM3M%p z1&6kRWyDBAS_YgR@NNSppt~Xe3QHMgM0z5jMJXJ+_5FX7gfxO>OgT1_<{+fE9H0!9g_lFn?kl35zECgx#I}bnfP3=?7xF>t&?0h<%F+2JqQDaVwsk(79_0uKlP8`MT zOnRM1HjK1{Q2QlvJcFI2W!9;qAn+kFoi`$e85SBdg6wdfnBxFZHm5irX!Inw1S3Pr zaxJmT?vUYQow+Ge%OG}fYLNV`y;vv?o3ip7BL#ml`Gr_;HK&5quYo)17Gty)?oSzX1>Ea&`U zkr)MZyLU%WV@D-Fs6yU^MxRneLe@E-6z8}Y1YM3TRSzB3vsG^XS+X|2SEHwKr+-v0 zQUQf5{nO%L`F%A!U8F;XAiA{GaFS75Oo_*4YQ$ZLtcgPA;&gb)PEB&; zZBZf8+G->zVu12D*OxA_*C%$brN>oR@!dml_E6F?YCY(XW7Ez`P>Rm_1s9a^jlAij z(VuX4AxOz9Ry|vrI}<#VQ7h;&BL-#FG_3@9$*P8(yHUsy`XW{t98OslsPP2rvG4Dk zceaF>q~DBYHG=LO3vRmXdO6Y?n>>D~DaMGi83PZHD@}@3)bR{APc{ae1AOj(b;24f zW1x8Ttd$fN9y>2mIG;%$rGz3SCnhH3i+a(X)bWC-DgbMax@U8DAbE=^hPg8p=AqKM zb5ErqyYp>N7^YuF*U4PU!Y6BxJ2<~H9)r=g|2j+P zeGyBYpG{5O+P3L#R$gdRKtM1TWe);@V(d{JkyS!chlfQ!YzjxlD^s#n)7)w&MT8_{ zTa^O4*Y{WvRSSyqpCM)z!GzXr!#{peCm^l#RnaMKXsZn@v_3m3HY1OqPN(7bAJa^UjNmZz3)sO zwDUzV_Yju#sgrdVG1Ak^s?oh?0$FqGDw9&S#LvmFsF7`(INXwDm15mXsB)&90V16YbnEdYge3LT@nY;?E@x$j*b`?2#i4iDt6V3v)ut^;{_=O;^Ds;Jkg zs?7TONxD&eKcrtL@v@%g@$hBwF%kBwLPE=y8XpKYT!{_F2hV#@#$m!jZ$jhu?kmmx6_BQ!?6SJnKYrInWCL zy6!XKfj*74C0yOxbvPV%yE}KcgHRH9u4bs_&9!84i?gBX-=U^x~dAiW#0p(G}*0-2He&sq0S4C0)&{y}~o9FZwy`;Xn z8|%U$ls{IjdrWyjYW3?$PT%(Z08e02NJxr|y4lqPJ9}^X7Y)z(^uUOi>uOX4l>2n~ z5j_vV^BuM$1yv`MvQ{d|txBAnoQJQYnNw7R9XopAnC+jHI1YCLAQjMtD!D!F@77l_ zIgV+QXkA+i*cKY@3bGveraC(JOZ7W(>2#nfmF8MSZLQBzU&zdxGeoV@OQ^W#gtg+-w; zH#}vzeW$3nu6JE(B8-W$Vw>JzOw9AG$ca8C&~FCC>U z3K8BeY`Bi;PLQy7#=qt}mYBi}Dw@MNV+Rjxd2i2&(A<^RrIUkZzQp8fc2R^VJ?Jn! zJGu84Zny2{8RGxBd{(ee;?!P^MWraS>=Rki>Wpt;Qf|M(6VhtOB)9m4G-)Uhj39v!9HG&kr zRBN$nor!kry_V}1GPVU>-pMimCY&UDx%ZV8+ljl@uiv}*47W%YwOZ2xAnNa1-sET% z=#fP+!?uP2-za3L8Gu?oi3+oW2cb)G?4W_5y`Zh{E(u2+b=0KEhEwrf63>dP-l~50 z`8$)`aM=fK#$|BvEZt@3n^HxhJQQr|?Os$+kYE6b)K+oLXX8KP&DS~{(J?K|JWv~# z@*M71>IH%LG|I*Vo9=^HU}V?djsfo9+n9*SMfF(q#V99&`-*MQe0}|eS>X|T%}aR) zK^@cJN>YJl`-w*6zbyD#Ow#qHuYasrkr=;F7HK;oR079XILdA?a*nA;!AkC7Y`z_L z;nHYaJt0TdD}*Xy@gJj`xV4P;hkzkih2_fbU$#cIIt+pND!A17R^ivrDcQCj ze+Q&Qd}DY1J@*+gGsGy4ugug!oeZM64gk7zY^>&+66B>H^4*b{(O?ke7CreG5{Yu` zdD(#HOpNE$`qsS))yT(utL2NlmOaahoPW8R`GUU*YOY&pFTU#?boeVmWf;ar(EX_p znB-u0b7!uVQT$-7G- z$oj4~;(4VmP&ULZ9G@*N-s#`YTCgTqS_97qS+Gl07HA*dxe|Zpe_|Q~)rRSbF)v%o zC2dlY_fXadKG!BX7;W{QH&8{rPQ5rLi9TzHet~?rZlULEj|c zA%n*WIz#*BA6VJrp3vPL)HRx4moe+1YbR8j%CEJ4uN>ZQ zfJJ8euGhV=ia7ZQFGMTn(dK_sMZaQGl~Y+1xPO%&3`PL<;6ocaw)Xb+`CbTjx@3RR zuuYuB4u$n(vBk_LYh?r2ZXw78I&qmu4xPHU7&DM@Ukm@6ieh#1m6%f6C~s05oNPJr&)Qm>^PgjXQWC#u z;9lSeiyJgRtkWx{?kM?^qB^}**d`tK&SZk)dD(b;UpVJ0h$W42pa9)R!y8qWYi2nD<2(0;kV?r&!qfTMHn zU*nk2{H6JnUnyJP13yuJr~brq2~TrAho1MpqVd_f>w@lEp7e&WCh|&kBkB0_KP-gW+D&Z0%e~Pj(%{$py9BaA; zkB#Ivn-hXDy&b;}UXLlQZ~Lj*nk+VbiVwKK%{V(-l5_X(Yhu80p z=mUfyef!Sxp-Maa7#{Dl^=mWi{xQI;u!6hwx9h&Kf9SeJ3R?c8J;Oq8!G`Emk(j>K<1{n8t` zPzAd}97lgF8eZ$>lPXPjv%S53&Ja4WiWSux*FZsFZs`jfXX|gN&#TjpR~*ZQ(CQjS zRxqjKKW8C~{uvTPqdl?zT!}wPZmSUb&`SwUP0()k z=QpJM3{XU??$i6H|5BW6!3YR{ZaCqa9%z~K1I>DtcE(>hv~yq>l#f)ppUckGk8^xVp{Iy~=$-7|E?);JvVV zc)c<8GR32m^x2d3Vt>N<7ea=wOlRMd$Q@mU)|HlezkZ!?>oLFoOrmHCDGFw|rID7v zNGX$CXM<9SzZJd@3DzxxF@=H=hOb_ksGQd@wr>uo3FAdJACt7fp+7$G=s(EPP_|hJ z%-t@&5VGX2!-L^0@xD`Wm+Fb)_mN128)@WG+kdON#a~@**}7N}!B=qcmLCsENZZHY z1Ov&t8qnW=i_7Zkm0SO9khrTX!q;?%9g%;6PUft*s{2N5Np>?6_vQuiH;`_ zanupjzxy9c=p;`6p}Tx;Y83j=%!EfzsQb|{QWAi! zT^@c_G+H}(w~Wx4IbRbjAcIx~B?a_FZ(gtI9M~}e^INi}cV#SWQEsD!F-q|tR@xL& zIWc87n!Z-r<78J`KZGUFg~qB!*XLOghP<`M_l&e20`izxIO({f!oz4b_`+s|;|gvA zEq{467lJixH69;ezCxMP?eU$_^qYQ0?|`BZSAnzwaf5;Bw*lMl*o`{dT^Bw#OQ8c2 zVkmgnHAK{Vq!zz!zBCzoU}R8}dgpbj`^mVp*f#QcoV=)@5`{+blk zocy-kS9HmK8MmRNjQ%i-4{-8wVc90_`t-MUMPo>?W+DDyM{xNV=G<^ zAPmDVKwUu0Zdm7a_R5~sT%A0;IBLC_ z(r1egEKfD3zKI(xc3GS}og3^ZqKeRqcBs+-ylIH#Y4fGU=dIt!|LU0{|}CG@u&Qi~H~cfA5*BJNY}QAb)Zk_Rn{5ijv&!;Jx|=7D+x z{kUo+uIo$=6UM?Lxot`5?~h)-TQlk)zT9r-$wy$O+`bhsnEd+(xfT(JFNK^kyWaB) z-Osz`KFc_1eDK-&(FqknOiT!=oZ}CnP&DYX-T7u`Z``2E$RiY{SG~rQiGha4SzEwD z8_2AH4o4J}W!b)>0VYx9RADegJ@K1uH+LDVaPau~bKBK@vW=9j_aDa3TeiO6u;z~> z%EyL~UOtApyG-!8cbK20y)iS00X@3*UY%8Uz=y$=PXGq1&lf5pG9Pe$ zd1NR3e1(TFQZI0!ru4Hq-<@hZn}Flt0iF{lo5Ejyo6>P1eV`-tix7RS27$GepnsI= z1-MzC4sPuGO}o;yGeaxw$)k`$kccs9#V90_OA*q>$;k~i&63M_25m6PF1XquVZXL} zz5g6JJ! zLQ)U&7n{tM9^e3|2aacQpy41hN*`AV+2eZ~0dw-5w#<5XyR_-^#xd0ND#TT0TJ=#j zd{6MSkI!e>#+jTPc2=YL<-MCgd<$#YbJFWQbk~EH`qQSrI6s29MYleCQFDzpfMrWu zPy3I^W7ngK#Cf_z1mnLtfB%Um@cO0i+7$ajFg45-MhuPYfQ0Juol|5UceVqEa(IG! zoplM4KCuCEwyx)CK>Cjm|L*jFB;@VVVcU1MFc%R;^j4E8m4<(88Kf_hvo}v5|1cO+ z?UK5bv{_N{1fKK=px@fyIgl|uIb;HBn(-~CXI;`Yj)fizqoj3N-+tNLZ!Ej`&cd3> zNy~-LEM&4^=FEaEDsb0I5VB{d%J6}l7cd=tOQLboP(T^2I~|IC1@qOav%o5MVLVD{?P^xfGE}(A1(mqVG|H`fM@>q z47$PNmGs=12A6Z_BT zara$9r}KnH&o44u4DW|%W>A!b5nmBDG6|JxGnLeO~PGn=n^dj##PW4MrL>Bz3*D2-Vh=h+V-M^z(6aD6e~y~2_4%-Q6a)0h=Y0kX!$n!3*F*}8UXo!GsMlT}jcu|thOO(A+PW11sT#UUTV1!-#!dgAT$?K2TuHVR&4*YDXpiuF`EgDQ`ajGrVFOYdBi4IPaYNvoe;| zR0jHoN~WBHsWVzYy8ziCV|~Nms*Zr>3bLFiSV%JJeom<Znu2oc-wcCFUH~QQDOnmdljBtpf+)a+se8D*?VMy zfYZVV3CW#jXPpIbuUb#fE_r!twtzZ)ToUV6LG(FK>v!a((-v^)M@yF6FFj znsA^rL4E^8i~HLd7%phKjUTr*3znk<*^Df}xv5YQw0F{{&Q91i1@`l(=CgN|FRsS7m z4cl4VMK(w9X8XzQn}w?@)&cbnlsC64;jn0mbTC7Bvz^zKzVkvgTl=Y6>mMn>P#I`X zY+&w=5fdxkSs&+7@86u+xJAB85NB*iRVI;^-{1Y*>2Rm-e3auN;0Clx=6H{?YW_U* zuh+Vj>VPPVJ>1ybhQb};>fk4Gk}gqhyle}A;-XS;=54p6oXMvRBqWkLEo5`)sNHJiE2kDA?}m+m$0J+S=9XeJTrzp86Kc&J+OReEknuyy_afm&XSW?GqZ;v}OCr}geqG;*$uQJ6SK=lc`!04R zl){xoDfr@fzG)lO>xWcfYFN@9WpPy8pbdJfo8WESsmC*U2*u5tfdOT_`}}Axl06bx zFdrZ}m1~S0;=ZQ^kF>)(2qrp{O|R@KK%s%g2kV=gR)So?aMrIj%s=#cA1A=dUut_k zV55!sJc!KHP9Pth*^0kyJXEy%Adj;$GD*-sb6klnuCmDRu0-vnnb?U=j9V(DISD7Hj(v z74vn)Lp-vx;X^BMztfRkyEjlpPmA4ipS_)#1!(@35ruouH=|C1*ek`pI!u~kYPX~n ziwXsF|JTaB_$7Gpt;5#DK3pG}qosiEF{TLaQU_;IK_-7%3t5zsS&i5$d0Jp41ba=& z3>gw_pqvev3v3@SJ`UZI#f-c1sN4 zzueZPla4_-EL{7{6ls)e^!Ve|mvy!vjwV!|U+%cVrF6v@^S%Yp=pHq2g4*5^6EHc7PmRR~ zI7w*ZRqI%=niR5apf_Ra5o82eD=Npkmkk~M(zF5=Q7&*OH)o}$oou*Ksc_nQ8X zr|W=fYHPY!QBffj!3GjRdJ_G)=|ZH}P<=`XU3v!rLk~R^X(CO!5D7}} zRf_b^e~|b6EL@A_f}3;ioik@<&z?O__B2x1uC~;oZWH;xAU+96?28DdP-9Zb7x$l^~B(rlVH zn&W6njc!4^M+NyyR}b*WkZU(0I28c-)ucr1=@;M1YiVlLl$bMr{Drwy@xA0YTKUNX zxCvX3Rb6dgjh!)nXrx!Y&>i1S;BDFgZu58~ztMIYoic}RH2i?q`}|um%0#M|_g=gt z6}joTcxm{S+lZ51&j#4$kc|Qdc|`4~onXx8>qwk4Cs*1{vOC*PI^X@1?BAZT;55{=^Ge#e8W0x6q8U~Y|dnQ{X>oFxl^9f z?{d}p@3T2dPo9`rUofrs}3!S1}~o7cC2i zjdr$1`N;@#@Q-i&KIL|)%9(B3FnRT5*>ZK!GNE92MZF*%vGk1gy}tt%YdvKrsofQFH!S_T@9UMNbP>tCp6}WYn+!+( zWx^(l!M|5lo+zxbgSB6t`uFU@F{x_0EDtRxDoLe`f{SGyQk_QgJb}>gIV?+$>z*1y zk#KV`{MIx&X0F*4)>fjCO+NyZ9a6`4!NlGcEMHQcF_7DMeL-mPx_B6Eyn@s3cW0Vx zZ)1&0is{9@q~OHC63&u|ae|73=_6b%@AXOHN0Y39C7k_8sc;}gx{nHC7; zot+)?(kA3HI3gw%dv~y_({Xul%Wc2%!;&si`&^~TD0&VEf{dcH45e|%Y{wrI7H0|D zmvy^hSqG^PAFc#2LpZ(?Z%M0mn(>^)h|Jz z$;T9jx#Fo8_3(kf-JowKK?3HMR-ds${ zVunJU=ECuvot@=3)r0xAob+ivYxHoxZGO<1`~ia}25PXdP8vOgOt_^=w`d2MiWFV! zX3^LWKkju2TouSO_!cVbJz-&xs8a!@g%2*{Iyd=^0(Z!~|H+Tu&|zp`}^5zbe(fAvLSZ zK?&h%GW8d~hAq=!b}3nDJ`x_E6i$7yz`UPP@@a3{L+I}O4J_t$@>8a3VVkV|h&Cf? zX*jq2{)lgT@t??!l-K=JSsO}GqnJEM)1z?4Zy-qM)k+ScNUM<=DkEZA=)&DZ(4N-ZuX6b_FUaR)G&}s6?`;$sbuC5szHIR&kG@s)an1#Y z{e19z`d@r~k+bD8c}R|qeR*7JV&c8K5>>wgw8XqD9odz*H_6}pec;+^+8uF{=aT%RuSF}b$xf3J`|S2Ymu!%o ziSrKSi@KY;3-h{XVcuH;iz(vrzo|@J3K7=&FQQ9-%v!;upr5wpDou3wHpnVx{V&^Y zYe{I_ffaUS2HJ;`Io5fM{;=(xVga&{d@`9CAQVfwZ^k{>sAy@Osn!t}SvVc{>xSO7 z#s_klU0oeq*Wa5hT$weVb>R6xdppmOyFjnnYficJN}grE6f^CGT2AwLAwiGP-G!f3 zQ=3MA)Tf48qOAYks_+GF)obt8UVwHIvDKERuUnt~h%;W63+QNWwf-0(dL(QeaAiZX z8R`BT`SdM+xn){k{L|K&oQC^*;8JB268DGp;c)W&kyoRld>0r}D$nXW<7BT=%$b6k zTQ;UDX-yH&`@b!cwK*a0?~#N~pj&mTrr2-9H1K%~Oei2zYAkw-aVqdlP234c%mg5N zg)J1lp|Zvdf7kbx@L|q><^A)XH}yGvED$^LMb4I;)cUA57GgFbI@0hv3pxpzK`P1K z)Eg?lY_!fP6BhmDS$4*gikUkTpXf8uof)v}9#aqh4r{#7RONAUWR+*|)7h_!=k1cU zG^7>mg;hj187u8(oeHR>i}701d;HN}LH!5zpHusiMOQ&1>q*V2DS->qHeW?kK|FPG z2px&^+SM(=f@e?BcCI_D?r7OYFfR*Zk+v*5|GlIDee_+zr(SlkYzo}tvoAX^l zV{f$n2BNRF>{84N&Y2G{3si9Fh9U2ggwgy%6IUt_I-Z-BWWO_Av zw8TbG!JzwPR678^gvuVJ-!P-CObTh?{mnl$y0aSeRm|zn3(rYUtg`m+^5)+$Tit~1J1GaC-?Pbi#?Zk-fJ%edMZD7;q_dfQxon65V+lWCiVeFCkvGr;u%PMkfdUwRL>bMQ7;s3OFJ1qA#^CAGFTS^HD$v+yKJrbSk*5B8+TOe0-L=|TXoeZp**OzFc0~T+6qE+Sbu5SPR z?&+B&#VVT=Z_P_{i|L!BlCfQR*LWY%deL(0COW0|Rg`n$j|0cUdoe%Cthy)H|y?V`YR@a%2 z_`+VI6CF~_-48emG&mXt&ECMtlI35{#BJTGTP+;z4iH|B_qleUK%%)qI4ocZlkLSZN_Agm2cj?gXOGCdCo!LREP7 zMTPV|TFUNNd2!2_zxPj~6|N@V^rQsx)b7C>ii=%+(&ZF4#8?>ZZ|HXRUqB_M4y|kJ zxQ7p=H3t5Z?MQUoN1TmKCs|vxFtZh zz38v?O(CP>oyjWMo1R-oW(&*<3>JR6PmZ_iZDM8}_v2Fzvk_tr9TNZ5pR~>c8X)y0 zh>}>XaKXW=u&Dy&m~y$fOI+N)5_=RVcU-L}#BG|uC#9^v7BDgE*&vq(bDEFC*)$U8 z9NWkXLxbw}Hz!qx#&$&O5&7O(ssjt=6Fpv7Ro%N+#Tr%v2ZcF@CRSwC9@rG9#)Sr?-QEu*5*trf6%y3EqMN6#dh8xp7nA1{P%4#6_^T-xJ3EWpqoNGg8Nk@-5r7c@)o5< zoD__I|LzM1ch5%p>uy;?Fuw%U z&Su+{m$zrV64fm-HDq=-HV! z7$ijVA`H(kpZsfVf$92WTG64^&JK93ykyhc;gz0ucxt{iA*+S`tF5(iIp9sYdiDGc zDRWTmiW!x{M&T7$}oInf)gO9QMWJT>Da`o%3ShvKhOl#sA`Z^Om@E#0{CFhw-WuK2XgsaCoMpsKA9-b>;@0 zuD&^!-?nzf?Y_)!p``!bF-0ho1s&((r~Wi;48Psj*a5MM*;W19vJ~S8QK#Y+N+(V?|G=^ zME|s4{@@&Bqy}ATt(lNo{vGxbDUisFPGPV{ofl@%FD0?1^Q%8IA`8~QufClA_vpDa zRA#q9s&rVwdHy8sNvc~9!C183969C~);#w3IL1Byk+)M3VQw~^nGuQ;>)Eh3p0kpd zf-lOZPhXUD{k>S`M&A!Hr56q5^Wt1ZaX$c&|CP-(!Z3f!2rQZ0=>Tb zLi3Zn)J?&KwU(C5rLLzUuja3FK9UboTuL(@Cwdt%)Z zme#H*K>BTYb(Kci0*BsRGepGL4e!m27|<0z=L~0MR4S{(3&wXx8IA1#59+&qo$m(-a# zNH+Mp`rvD`UEaXb44P<(*IACy8r(Qys+unq5eCdw4xFGW^BHgzZHV>AN?uoJGWyF>IWc|XdD7Aicp3s=BpE%3W@fIU}q{^qmsl z*6QuJ+h%7Q%T_PrluK@#{pW>joS?edD+W;l@WCWDUe`2N$bS`x?R$nd?@as!>1& z3=ZUkG%LKQP*#XRCb{;K&XTWD&zIg8T^+B<*U0|SI)bVbxPC@ads}K49N$Ix$m?)E zd~UA=SqwD__@zyV@09Nu(q8p9{9t;!)EYm@sW6O^Zo)xC?!j2w`f8|?hCTDZErGAFGC4pPKJB&uiU9hmX<9LvTs^~dD#!j2FQQ=-m-}BDKgbt}mQH;F zz4E3OmN?B$zj!#@C%#$>wu~RTZ!EvVas2`cH)nw>n?rMlKJCza#r@I^L1vOK2<~+w zUvQ9MN3HUR%2?hEFj8BzE_|C3zJ+@KAXvwa&QFp8Zm7{>O84A9jMVm~KGDE`rmx{n zfqIbxz24afzM|ZBUj~p=a;j1C94?6!?+cNwcAQW|j3o}pOT-~~9!&nqN8WIeM2|Ks zP)pa9m(QX_l+>H=`G)7o82&~h*|g-@np^0wQN%kg-tSF|64EF=4FCU_d4{?Vni2K@ z&^PFy8lf|uS*8a11*vx>BbZX=uok)aEPPgU7GBHz=f};y+v-k?Y%}j3#&V%M^YJW1 z_GKrL`zlMkS4ugVjj*uhK*StkU`a?{eRS6h9A4Pg2eT z)0ghlaCOQ9OpUYfIhk~CabB&K2fNVIV&lCUSuDK^^k||s9lnb>v@Qa1sMlM8)bz9XpSHiU*r`FTpwcb zK$2o#I-Ke%zl%F+*_k77l1VOKTm23$>}oVg_fK|z`Kx9mXD1P79ys?NP=_mL5mtAO zgl8LGG*N`BeS-BwnZ}rVjYI76Xy(u@iW>`3EYj>j1MdtzB3J?^wepq^8XQI1Hhz=A z6?k0vkhmQO)Ul6{P=&LLOZ+doXH2-tOF{}o=~`Lp3oQYXf;Cq<{|oXa zY|F}tC@mGqWJUypBStniNg8gU1RnQeT0m(oNMY2{d<;6OTsaF3%(kC|RpInYL=X?AkMk;iG{s zO^RE;KQYOH!Hp<&BcmL(IKraFwZ`vsr_h~_yYp4$S`u3?BSpA;-h;KE^v)7T8=D^F zvwr{0L&9pxpv=Yb{5RQA$!3wzGh6qNlU8-Dn*&@;wBjd(#d3dxJWJPNFt$ICAixY{ znam8G>wylztfaxsCtz|0T3(@ZQIuE}sTmmU-ZZ@IoJ(K=t(6$OR?4==7+Hq!_4E?X z_<*>+%{Oi&`rm*`z^UakUyH6YuOHxS`1SQaP)L|xl{)-NP?6EtM!?N2`YQ2rw0Lb% zn{LBLW@?@JKZ5WRn0qHnKz;^8Qw*J4dA`FbLw->6o?(z^*}V0%I+8zAzEih1E&>xE z2l#DTNDd$~ltKhkKJHx^PkI19ySKL+s-&!F)ua!Qew>(p|G5x>uL(RobjCQrme)sp zHa5&~fkdT5Q&5VDcpopn!8joW`XMe!{=;Ml8d2yj{;Sq=Ttm{`4oECIJVhVRxW*(g z&+*+sdvR|+m408NlI{)PW{6kON@sOz6BDgl4Z>T$aB&#wm=!5nBz_VOrQvA)L=DjF zMJKz?z-arPfVlqR#uVpJ55>8eWS3E^m{&A7@PQ>YWe$x5ML)y&AR0&_L(NEsXHY)tdhhF>Ki$5PrKatL1OVVSd7S^)^yH(+7|z-(*QgS^ewJ3;xMFl>2;=t@ zv-l@1;SnOk%te)DT%kIM816UuR|u}Kr)e{D3Xe12#3)NTrIYf7H}#~5rkLN}@F?oz zpJaVoT2_+Q$0uPw?$E9;?CM>$f8_PQEV%WK&J0u$uAA95sReeXkut#tBdr^(hJ9hV z_t~kA!g=@t^wA_|&n!bwHHR*qTs>N>N}1V2zUX>4m&U%V=MT>R@&k^>KuviB3P+Rv z-uDel3$v7yPS}f-xz;CqT&1jmrJOo(CnK;SUdJ83sph*qd?1Mn@hUKN1^~U;Rjpp% zJ*unbXDvzd60Ca-c;j4#)=~gMFPk;xdOl5h^1tNum$R} zF*7MLNuj+q^Urk;27*HW9p#zs__Gg(Pm6>u8cKF<{Knp1(?C-~AOW&;GOE;ka-v{^ z1WuWU;=NL4?|Q1Xnmq5J@l(wMx-0iC8YW}{XQ%Ik9u3rKJURzKphEaS1C*+23|fC$ zx&W`1f9o0dTZgie8{%zqDq&}@XS{RQ>wzY1C@;s;8WL;@0{}_MtlvGzewH2Gpa^s? zj}JXey(C0$Hhy2uz*BM%kq{0e99}+0Krh`;&1Izqc)h%H$?`86D5d_cUD&t|1TiX7 z`IVNsYJt+jiplE_MTp|RTB$B8Xa?}XXg5WCpfNL3c^S9&{N5f&mBhne;{w0J zRXL&7D-lb7_9%0UX1^60-yd0?K1yvgHFA3OBXH;dw747;%pNd?@1_{6K9L|;%2#v@ z&XS6*wW>W)Aj!IlIGch)l0?>B+KDulj;pe+%gN#lz!Q-!4i&t?1OdL5X|&*xrVkKl z+DZt@g!td)YoV@N<+&BK{z59sJpKSPtPp9Evx=^$s`|y*!2Wf;!r>JEd*YSnl^YQT zKq9@Lf2*w|MeTh8WEtOZ)*Kz{1mWWwsDNz!akJ%y#}y@<03zG>8fUN7cX=HDL#DbC z&MIf-$oo83#1zueiR6GLC}vLm5$mObf}Kc8hG_zc^2IM?0RUiatU z0BbHSMMnjdx~%g0dTp3NtAKh+D3H4cnkD&XE=~S!m><+A{{yBtCaziOx_zV%JPN;M z)%}$h05hzC!q)t#begK8cr_%-8#k7!%u*no$Jx;r)p7?~LZr*UMW=rY#@T(?OD77WhRmFBSr+cx3(fHo4#X30q-XzxV5FO;`t^zX0tePSxzq#95(4EN%JLjNfr8 zw)v^NO!5bzv#pgBem?6BErO+=JZ-G91nPeXvTB6l z_V&&O%%K9!5&0tu>zWep!j8Lf86gVjbv^1Xk;1sG(NX;@LSomWp&^y_5}=}i(^6oI zo&#wI{>c>>iYvTk{HRx1j?Pb7b-zv`UJ+2QB0jmhUx09&X6P#`l{P8ypGQ_~D-B{U zl>(fdErbgny1^;b5|vr@Bu??svU3MrbLZD`*7ZHyWQuXnUcMi^A@52Wr3Ux+|D2Zg z4I~Pc4^G+$BWN*^GCVQ=lXd`xFO&5e#6Cp6K(3W8BZ@M7!SQio7ZSwp{YK^Tb}6)= zPz(JOWdxYI>oD_2(3|JSocJ$EelRtt!Lh=1}T0f-*Xg=bnob>bwcK{^H8&jT*Px#fn?=K698BNVA6AJ zCm=3f_p?TfU%Vc(oW0qjbWV-{SViNc5Vc>!q zR`W3A{?`zXlg-}Lb%`~DeHc8h8{6wLIuZqN%JH$mK2X0r^Is(S>98L2(%XD@)hUYa z%5%X3H>$h?v{PrJPHSB8|YqbO15J$Q(Yuc)V6Yv78S$hVL`&)vcn%`8lw< zEz<|1MqUEm9;_Nhv%feQb4g98dX1*1&-0|SZ;QOnwcje?)cp`OR~7U@H?05gHEAri zvmGgV?LMG{${$pLf43 zES-0o{YJi3OiTc%F~PkDzeUGzsSfHS(48FyT2c9hg{vzoukSxDePqXJ z6GW`M9yOzjNiHQb!UOq`617y*=BYmYm}kf2iNf&Bg7m1l#Un33XK@Bh1B(PF5nP)| zE`Hs!XU7wTQxmyasMWhY~0vAyuY*19I)vHNjsjPCk&TpP8oHX|dWUHz79AarbKh^Ckb;5k!^ z_L4GU;J58<=NK$7x19@6dmLc6DsId^uxsmpwHj8m#dR*r!9$AWol?qd#hQXt$WNxK zF&b5UMshy09b>n)lRHWTdV>7PSII9Q*wk)4;ioFb7-gojKF>(QUl8&K7Is@b3JVEm z#guTC5-Ez++#cU%NMyb5WTvM8aJRUO&1)hRFBW9>WyaV-5$CrTUkr`C9O{DpJ*B>I zJPS;ZQ*6oTl|^aBw16y`NQdHt#Fr$2vFm?;?q+~&UVnSZFx`|Cr~?KhUiHmK5X;Ri&L**-Ud3ASfe`8L;y*8N;sM4Q)t9W?DlN6~3PdChDn9YIbUrM0%HBjK zg<6&!R(js_bOK{@Lij)g*Wuz^?kOP~al2n!}u)UL6sYS|5h0(?5pQH;KU)L=>aJ9G5 zaN+IW3&&iPj59@K5xO=wuY7GYsVOZ*;p~!S;8CV>wME^v>{kwXufO}^@+U@3-e*$y z2a>t%jj{2=P-BQi#gN^-F#n7{jx}|4#nlHtHUP*y4fsdm+li#Y#$dDG-@4GGcy=ot z<^l=%g*Koxk(_KrslOoyL@|?^ z&}JEOxwaN?A_)SfS_OsX$Nl*8;DhD+6LX;uJ|PFJ@BME`9qOt)7<&ENj3mrp;K>-_ zzW-}_I=)Ds`j%g^pXBj(49N8lgM$O&OF2QGwv2N@N{d}yf8dDK0<4E8@G<|R9tV`~ zuUpc4RfAE)`QUeRb7+uJkj$#7kr1rJFz{7GwplwCYw(GoXYNHBM-S zb?xyl^Lp66-npb$n(NV*=~B;dJzGgo`ZNl5x*AN=5!j40qJ#)FB*g0`_8BET?$M$y zg*^RfE1+smeycR@Ik!5Cymchm`%p8X@`JG<;?PR*?%l~ks&LSRR&P5t4Rz^k*3G%r zFkO07jBZyIPm+!p-W5Fz^6XN|M8N+yh6 z@C;VBRd#mSNx%}CI;Z}yDn)k07HSvkHj6YhVT(`k!TV6>0JZzQ7}?~3PW7zCSkXtN zq7m-4A2|A>sqR&)RQ-{NCk{<#`cpw(hCDAnavD1FG~2n=HN3cHmpc#UkU`gLUZt>B zIY3Z2RK0y+AJQBV6NS}g>sa4j9!M4UJREPReVB{@+-s79n<(CH{S!`NoG|QZhs`X zQ8;a%TJ=nnHW{UfzZQ0DJJ?HaNvzweWX&^NG*cl6b8`!dKm}l`++LaiD1be%Z=(za zXAg`(fM6`d_?aTSCGHd#;p(tNk4IEHFYhOdc0u#$4(nyl9E-3J(jDmz2NO&**sSvZ zXj@=0ZOU1l!>c`ItJzMa_O`bw!JW%aX5x8Pf%n#CmK{dKf)|n~Tfb@lj9Iq;i&6V#pjR ztYvpCzTB?%dX`0i6gJj$t}^xF8hSZpLh+0a;2M1i$nbhqR6mE;ZC^9?(8J}nZNRQS z!#Sx&IW>l(B<(HaVob^6Rg*aa4xA{?YjX51Dn+}UH1fcD)2`&+DQ?<3@)fh(a?$j> zt066`Nioyy`z<+q9TtWm4h#ffFr}O{5gNbE-7lb;kwXicy3=JoE5qnQ#JiVYuKE4P z!m<4~xYgKL9{b~&x^hIkS|BD^P=MSWrgGr*G0MofK6eGEe2z4N7RZ#nAHTmEw=S>R zr!`;6fw0_%zf3v_4yqNgrOe4--|;vOToiqp{n;kp%Rq*$;&TWss~I+`gh-(pV+z-Z z$)tLI*YI95$XT^@hUh{rE8Jyo$h(?GNnv=E@vj*rv8c*#xrwPqNynfKzrhn|sBVBJ zx52?d%L-#B?b?L=n@)>K^Rfj@1;Q%Mx*{5;a3>?D_kZzO`S;h&S_;hY8fq$!q<(~d+c4)OpDM)N%dq1A1|nQm(Ra*P$v514PS`8 zZn}mTHM*2@E^oM!$FW&nlqZfR+|&%KF;D7=9dhx$?JU~3oSe=I?b%=u?~d9xpZYbR zy%iV{*r(#4*3X-1>_pzZpL9ltv;P~jQ1P9~Abn|WPy}5fOLt2o;*_)gT_p>^J3j&A zlDMIg6xMGxS4R?G4jP^HyX)C*rFVtgD3xFh z9A>y)-I#{iLKGKWJN*48l0)CNtxvd%rANo+hv(=`5}saQbL9C)nl_humN;+lV$Ujb z-QQmW)sgRH&x{pgOmUUXE%_l?^#}89Nc}sghY}RUC-yeH@ygP<5P3w*au(X$rCvn- z{tYe@c`{E?e-5X-KA18x*eBHaKtpGvUR<}}YSk-?LMJCwwnmwT}>bUFyLDi;w zj2!0k5#FEx$HyPu`G^p%jcU)kt0QvVd%bZ-;#IZthS~T*LdLhsg+s(^(HAR@T9 zkA&@{9D{)+qods{jU)RnNIFQ;DDSO$#8T*2JL3?4kNvmNTGY(~SIV?RrRgo4X8LIO z%nr7@{=D*b$gt^njMNzl(SZ{0JZ!%U^Gr}~sVYvkW|1R6JK6<~ng{~qAt|`Q!nqHENp(|y%*LBokr_d3|Zlg%+%zAuJ8k~>eQ3Nx#GEM^e^2ceXlf9 zp2ncIw!1#lBM?axvhnr>I3$u0igguQg2e^b^6CcZP`7e-EU2(Rqvp^tSeb)>Pj?2E z7j?bGVmKYeA-zTeeNy3=+}<%b+~3Q6rk}u9qY4OOXX#ei1jue@n1uuy{Z(_z;kW5oYTHFcZ=QA!1IKBfjqH4c7l|6 z>{WVJOCeFPg3*6#nXz`UdQ5*8a_gE7AD@l$X9h73(7)YlT@e>w6&O=qUjF3ClZpx# zkn@9KbcX7M6||rlHAn{ozaU z#xJLtslmd7pGMf9bW|55@A;=>1$;v;)v;qipt3Sn?``a$?!#kHonrD!tGcwdej|D8 zV1U8<&@O^$g_WdZYFLU{+e{QXkaGX$e_e}y9*;w9@sm=GU5m$WZQZD@*dT5V z&`Wr_9dH;xKy-qkGycj8fS}Oy`6UDIgGd!|wm>zMjd%}u9iW3Fz&J(bA2rhFpcW3P z4i04P3GyZT$j)P5&sTad>AZHd_FWlXaH$PwK1!-5LqUOyqgh&2x60ns)6?^`Mp{~3 z{j6za=)q%95|B&C91s!`aziQJ^P5Pnuiuyo6R)(NStn)iipvCz3@`2TSP?H!)GBy$g=RlBqCPSG4C*3o#lM zTL@-NnS1{+KbcdJIBWxe5E8d5a*N&Y zZkj@DIp$jk_h-)o4pr*asYs(Lt&p`MIKd;D&zi7R`PCamMT?0dtI*8yg;k zP94u;dyiN874ByAg1Le90tQ0uFL9NoCF{73!W`$d2_Eq(P%@lq4J*{I9Rz)4q#~TB zO3QI_P5N~nXuHTcP2&j5fdV}XjXUo?&e6Zd$p9#i@1-2kfmv=H%*WKJu%1Cf+#32v zGgsETSB^WUhcaqeROrory+NTA*yow%e;^ zLEK&Gzc1k}KW}1aDr^GuWQtxtKW0`}=sCu4bEjl!z(cwhY0jfS#DD%=F4#p`PqR$E z9o;u#-HLO3n2yKe(^)~jJ8>a3M>w>jj{6Oc#%NhpPf5Y|urF^&F}td|EQ#JeGmWIo1=r2C zR*93@!n%#FfdbFn35l{H@$lj5oet;f(&A#kV|&9pG@tJjXH)0Aq`qy22E?2CvF?&i zx+%0IUHRt`LFnU5MZ%ArBLOA-qWF0-jIq#S;^6X42pcw@COisjffrGwKCP|*Lgxw` zhr=ItrYP9@#}=$3CSdc6>1%M7;#^IYibFZ9|%)vcZ^j~&-Dc&tS(a-ECm2Q5nFH^+!} zq$2|V+ocD88dHz=Qgen1$148}R@80vNE~g_y1R>lGRtj#8fkjzf7OocoO+o>M0PKlKqziT|QYh}i+Km{Tn zr7Xh!iVf%*XT1hoC4vK)m(Dt@004)L;hkF6YjDiBCFkm?Ke9gHILnIiTA#KEH(zEA z1T!=am)pcO33baRkC~tl%5bebrKW+xdgrZ0JSZa8A1~GK+ng-7ZiG#ocb`jkSv}2e zd}-__o-qsGkSN+tHOf4rI8#7+@u-)lyxE?XZ51HB=Dl0ET6e%xpkM2}F^d~JUUmT> zkhs_3>&Lgd%m}r$Bj6h~P(THgb&1GJ#SBe#Y*vU{k!?Sa?yIAo;3Nh{`fJH4J~}8w zB-t61)ZgIlt!O@gUTYlxnFaks-D+ooW0F$?#Kz_2Wpe*#5K-`s`dm{6mbTVm3J>V6Xh3FB#vghcgejE|FlXt99-b62eh73 zvh&#uk<_e@y06+`=C=mMK07UK7ak3Y8tk<%1GvHjvE4u_0rXJ`0MsD5J6DUjuvB(+ z;W!hK9LvrcF)bU85sHx}S~1uqetU41A6i#DYXZbCP=4&p4UGp?UQnW$TYX@P2yHcn zQkHUg%+EG|K7sSDys3kCklzD8&Yp z`I9qHhR-dW66@I@fC(rZ`u-pPG!?+ujf;;DE+#DexCC#BI;9zsMQVb`5Yzza{Gd>% zDmA5?YGojEdin>>L`d(OCnKX?if}+A_4G`R2eWkXYB7)GGQlwU#r~yQ{rVAW0NP6& zPPmgZhyywIiSF*H{blX~@W|@^v?O-1=a-Q&ux~~GulMAY^zv`muV3i(jZ|;2hqr)- z1dkf@4OEOBx46PgGJ*IAeOSws>b;67VQvNfJ0ZimVA>CW(mN=jmiR&NeEVsN6+W!P zoSX*UY2;nVY%@{RxPeSi&H{1h#3>}1?FA`ahec!!1`a#>qA$VyaAr=0O4adi) z3U+Io3sV!eyVGrwVf`KJbw9lW%FhXN(28qV?&cCtLicXq3cJ0pnSeeMn>h*2TVh4! z2p~ho0LwV5?vN-R<$-LQOw4<JikZ((y%xXx}0ubfp{ zPE;Xm9v>Zm*J*RIZBxSeG~@x`5Z|N1>vDSQQ48G^;;^c^^*sH%Co?p?kY31n|0fi3 zWL;|eYAv{C!Kb=guX277F?W_|6(#9{-iDKz`osGW&!eUKZI1f&8!iXsi9+@ETZ{dt zt=fRgXEzK~lq?du&C~&_?3c-7>#|1>@Cxpz{z!MOrfTIHAX2yD^ld_9JtSVp<8Zj; z-@knfcY(uBJ)9PUx}6+>nC$q?$b&8M33hVjX5byYUUA*ODp|l+c;MClRhF9n#7UK+ zw|BQq%=M3;W6xme80Wp+n%@4>$H`Ro9Q*d*C*^B@J?S{7*n)rvLb&p&Pbr>qo-Da4ZRN_RllP4le#0|4p{}|eS3JC z9pQ&nUu7k>uF|D~R4!>_Aaprs)#K#kM4u&0jNcG9dkn+-o-EL9fJXgAWeSN_W+WPn3qteoPi^Bt{;u&#vt7e(4{gLt#gkgMT>`P*o ziYS!<{ie&Wk2=_8Y#z_>0lvOK>t#!~i0j&`B$C%*orl#7B!lzaEQTkOv^FiI#9g3R zZar4#DJE8p!Y@uV`gGVfl=9W zF{)*3!WgS2zwrJ>j}ORwS#x_dmyWjFi3z?ZQVu~l{=(HAwdJ_Vk4|-+{5C%Rok|6U-k7bX zpJB(9^m{IAOe2pC+w6AK&7xv8V;l#pAaHJNwD#!efZp4q2vE*DW?C5-JeF{%KXR}y zot;0LnqnvA)0`f&Lmb&9YF~o(hFg|`PXP<@TU2tD!>_z*`HJ?LD779`;n7U$iS*R7 z!rE=*6ytYUjdtw)Zz>!;AYW#!sZwLZ3hd;ShIucl`;(K4?az-!^+j`(^3-ld?rMzF ztLBW;LnXkkI^tsW_uCYBj;6t|3N85sbBTkUUEVHRgVnE8(Ul!|8?a)RZ*oO6%F5uk z-%eKcAFHIj6NR6mlPlF;N6W(D=Kk`dy9921oj1`-Q{caUemPj>X$p{93e4V=tfgmp zY-ZR{4@Mt+9IDmbF(_eF`3z-0M?GCgMnVEHmxD{55^laD^&rvWj(<<$e-xag|5hWM ze;yxQuuIo2p@~i@6k`Cr$0Q^rEIO*EluWIA>}bNZUo&&3xYp}`fu=`GfE3j!7*6s! z;cv?H4*||x^7_>mu~USDz9qzsOF5ZeM1-E@8vO6oKU$=p9+T9>b2f(-iOO~51yO}S z=9%Ozw=;+7d>_{<a zb0q)7-hoLt?gWAF=LSj!Rxl54u>sdOwa@$C`db<~s;5{|eK@1n)icHEj~CYe%_6{+(peXlg*Ow82?1U%I&L$Wr-qACu-$;G`-2|`GEL%VKq9i7{_WP zSJnLOxA!R`yWAufhh}7M=GSHXAh%SoaQbjfklkwedJZ1JE~t~~PIWi)1O+8e7M^3XRNmo1=mU&nLxn}1ooV_`ekX8$aia53Uv9A3oY8~(4XYmbLA>*LS1 zZ+SQAHZBz=X=}!9yhWN2ifE_IGz?opawm!?p)$3L#x==W8kuNN7*-9@QWV1yVi~vE zFzzaacE!t{b7pwk&->@hd7g8Azu))w`~JS?d7gR3NqXOCr?#QNh0A_zE+Cdd*s^*eH%#FK@jPM}*oY{*zo~3uoI5e(- zU^L4=Q;6s{yK)Ux=J6@&R1Is{h$|!Y^)6)uR>>gsMK!B$)D_KSH#Pvuz8Q}y1+zr_ zaMW#nY8tSX#rcWyg%&QCOt&}qvkF+RqM)8w9~=2hAx;lp+{PERXw}LfuJMenoczH* z!vv8Y$HCk}&~HY+Mscm09^-X_t9-TPorw?pGai|3YcF}ih=PptbT3-6lb;wwa+#IS zR_HhLpXr63N@xeOthZ(8+ZxD=tp=o`PnA7TjLs^wEU^MsuRtFZN76c!2^W|Ff~E*= zSe~(_Y(&oUpDrFL5PZYT$Z4baFT+-fj?{&QbyZ0NMrGz*D1BE3Eoa4S+O!E7)2XMU zBS(nJ)yQa>CB1@l=y{KKjS`l(Ck!&6zLTi$Ae$Hkz5<_2ET+$;OSU8_!ZY^Wk$I*O8 zh`UGMU1_AQh1|?Mzp>&!N`$EW3{_lKR#qPfh>3>M#Ow4=6IITaLJx7_au5H=7q#T? zA4waI#U0p!{#rpxC&-&bmA$z{>|idA+fO8|YSz{DaN>^V!^20}OkQ$+PT<23T`Kr0 za0jsZF+z^GWHn1`rqD4SB9yly^sJs{W@i7%I~&Zl?HmQyQ$Q#&Cuttnj?Ixl-e9TQ z#mhe+mv11fq`;hFUY&IYul2NR%3TR&@yygoPLIQdH}|5p56y3;KOih_ z>Khsu;Co~AjJ2*MLsdgwULHmbbG4xrQ_@ITjNZC`wv4>{ghq=r)!s~M<#ESTsR4`eF$#doDM8>WR4_}i|hD| z$F@(Dg}VaTnz@H<`^?sf9l2)ruiy{Yht?K1P4b8Oq4A>jiSS-B{og6kFyk=S^c*vD znN_P$_f;B&xQDBtz~}2C+!MaUT>?rWDWX{CWQ`Gcl=r6k^k<6?;6V`17$`K$f|_;1XMy%v@!LSb^i<&eMsT*t_j`io z#si*QGi z{{=#$7WyEj-1)*;4iO z{8!74V7UutKur|i{TYa5yzDJ2iq{X3#g+}g6;6)3dlrDwjierHBd}6nB#aC4Xvy_z z+~p;g;jM!9#L^tI{-FmHaY6ODpERip$k{-oU@{Lc=6MhYS6RZIa!?a!oWoT1H z=@81N+at7=0@mFB$v!Z~DGwop^qbA26f}u`k(L^b3*V}qASWz%?4|||5JjM* zdf0(l02y)ZtzpQuoH6l9p)$8O$KYlNC=3dc zrsA?w0rA|g$JvfCXh8>4Q&Tinw2$GejC0cl7`>^eoScu=?mjp8vDII7=pnl0BHg|W!dWWJO_fxGbGxX`F@XDPSXJwiFeW3ndO4F+5_oStf!PLP;i&m!1L`0{2&P_1 zf?u~l{xqN8s$kJ))`u{SSs*4A0}&tk>j(0FcnzPw^oeVsFJI zd}uJE<>ly0^2_|yctdD1#}xFu?NpPXdV5rcDzj)p&NO0mXW%S`U2~_+pI|$Mu4r0Q z!(t_nc+;WnMlVG$$F!5j?H_U{wI|aV&^^qSdOUuD73b!0=&%n6`Cnx5d z*tI%z)~Zqo=`jzZX1ln#O!8`}syzMd0&Qc`;Q!JI5<#lwd7j@cV?>2CS>ZUe-nK^gsfK6CVxLy7S z_`Y)H+p&vFvXQfPkk+&t|0!+wl;O8RkV4RuT4iV~^TjlEJPz0V3Wx;J=jkz+ASzAj ziAiL-EQ))fDPp0Xe}o2ZKEV`Dkw~G6$2xl6%-kVY$~QnRk_a0~-7L19$C?lTFqYtD zYD($`ND||qf>A9g+K?321}YL-u35Jj3#vmbPWKGxUq^u`eYT&93O&zd#A+KA`=^9? zTLdKx5MIE1?0jQNJx##6C^6X%Bu4ZAkf?;iH|sSP6v1no>FSp<7*RBEa9IaZR-|^E zRIzVHkNtgvqJg2!-bEi*`OFwdrIV81&+#r>=U04RAczEM!{iC&=p(20euyu1IR`Vb zth2P=BYk!tAokG0(XbG%ze<6#uNo8lwBG}~zh;jTFxC;6?Qa*4BzY>P_c4CPrM}J;zPF$LMB$eu?(y>W9{v5E zHQe}NMcIJntQBo;$l$w*Wt?rh@;uIc*RYIZeZTDy5SP!9q>4HnLrE*5H&A@Xk9D(- zfzvbq9l2+D=<}NIise(?R{Fia^yMCaG3K&>pP8B|UrcJTU<9~}1BQ4S?UDA1h3kcGUC<0QKZjo;3F6r*Pgmi;+!yEj+ zw;t=VV8P7nGw1C1_TJwPey=2rg-MJF005S(jD#uxJQ)VRr_hnXzo@VacY{AL9AtEy z0D!;<{Qia{(mJaQ0F;2N#9MVYgS`b$FA~dCw)?|@E!O@xmhQ6XxjKg6=q|;SK$yxN zt_KE7Oq_`o1Hy6k>7lr*uY-1v&L7+D^4WG)k~n=ITb;`cE{65_rS*GB|4oA0DgKvu#tXqL zMamaOLC2iz?5)`5grS`?8J&5VnVGq{LG$M~cl-PMPVqE>FT~MV94(X)>inb@L6|R| zqe8vxt*w(9M$kvCV#vml4W$2jy0f*lwYLYUX)-umZ1P0R&25wffcH^VS|tj@ZoEWX zdWys|BF{t4W@d;4E8U)ceyh8T3bZUm_>)k90^|c{$E&AzY56MoLPqR+O9~4^=l^;% zdBplxX%$s1r8YwCe{=ZN|HdM!eSDWnvjkz7TANWM+3W8IqA$3$iq7^!bQl8K{@mZ) zB=Xudg*!b84;<*7fF?_oQEON_+b;~|Cw8#sU|#H;&@~y@+t@G>*8O|dw?qNhxWPB- zbxCKg-g(;2$%TbMYyxQrZmiiH=V&_q1Cwh+h83u!W5|oWX*!t_aY9_~$$z_<6{r=Mq z$h^V*rrgOf_;`pq3H8x|YrkY5KjpD`70!36y0{pf%{H3i=QINF9OFYKEhdkL^*1yh zDg^iTaQ>|!#C^P)BdU<-1h8ny%i@T&XgUZ*0u^h*DHc%DdOY9^$&N;7a&;8`Hoojn zBIhM$_xijG$wBpKn9W=`?;L`#!kDnNpEtogwn(gnXl_Nzn^|? zdxC~-WQy@9ju0<82S)tsIl~~KDS4~HAmr=dU5!IFjJd5f>`~OyzAPx>@FJQ7HQwTE zhd>FTU}DzR5I7FqMIKI*03R^2V$@Yx91cpMLGm+Vw^HB=HghuDk|FIkV;#=H`flC3?tkzKDAW$Nba0D-Qj+92n#aItp@KS5Zkx38)&S z)AC8Oj(6vkU0q#U^OXD->ERP)f@c`-$R!dOJ;(pTW~6EV-5gUl<{~>M^Ua7|h8{9n z*hq%?1(cSmWVv$-k(8vl7vCEj!f=O2<|9jJ_lN`UQGq_1Q!c-`n4OjGpZZmvo`*0gpN; zqZqIDdmy%&ixy&v^0}?PUZ5Pyr^PVd2k!6h&to$cV|F0QFf^M^*FC?!XKw#qquDs; zBygUN(aX#W1ps#v$;iseY9sldI>^TF$&Bi}!puF?xZ-Tujt>bQbHPB!HP@A%;A_aQci(=CHSpejlaxi51zQGlJD zo!`ss?&cH6AsdbjTfWA0-|!&eTrvW=Rtj2b(k{rK1;yoGSV#vFGF4>3UgHMG6#P<* z9H;H=e`{l7vkXP_{H~s(PQF)mO(fSz`i*2{=GCFLfuzwmL~s%Y^$iy&$0M2_6$jUn zA*rzBm=^v~a;YPhk$$ZWAF8K6GxafdGc2wKNo;AH7VF2MXD@En{ceGwTB$BF7`vYM zg@j-pXXg$?^9sMjK)t~Iz4vWo#bW5vYHshFqj^YW@c|XbyGoX>ZFtxCMd>FDN6+~I z^x$OZJ9>;2YwnoBD7iY_xseaH9#1c}%qq)&j#*TVMu{()vh!1C(U7g0Qu$6l_g+dM zT+!pwO_EEm9&~Utd%d6>&;1^!w6|5+>6s!0jMP0eTDWO-9^NeSh#4M@EX4Fx? z_~jnZ0tlI$x}*of(W$Zoht@eOBjbpXpmPU%d+Kas0Z`uo!CFhX$Q|aqqvFwlqmSgQ zFFRH#W%n*sV`U($L2MP7R;;Dn^419A2Qlt3rhlSeh-Y_spI4-tcdIi0K?&wAN3>Zy z0c0SQeke{($gpXb$)bE;~eX=q^K0}`GN z>V_Wdu!xZ6HV%2~0VUz66)=cxvZjFXe{1b48aC;VRYCi@FN>&t+S~#o(=T`SHE@#f z6vs{lqCTlWO+U>!JWq%KO*4bG>vxBAd%!9ffP1mXEblTazI}3bHouyzQ>9mDAMapH z%o2w5{4A?WeD$&ZgAFlBf79Z@w1FvMY~XbW*AAZW(b3Vjp+_4jK7xl&66z-lQxi9{k5~+w_pf&&S}v&@egNDW zy9D`AxKh|?B$ezx%<(D{1uA&>L0dO9HTBaq3@@$*>Nt1?lFhK9}j0%9@!MLQUbM_DS%Q{$%c>s4Jr^L>Hn-MDT#PEHQDg-t^6 zw7$OnFPYC(tYthby*1%veRAhJaUXbA0Mb3}UhM+nMnirw!E=Ux5GFAw1FE;6m!f8j z*wi!7su957G4Bd<1!1B`y_ue#p3=on8OQ5dN}}flmRu<{VI7+%lZMU&Ky82D26K;B zZ?9x}@v5t^saKxlENu{5BynAMtRuL1nU`JJ`_%`v_&Ei|#W-I)65HTg+uIzQ zwtjP>td!+7QIRQwUv2v=p1k$5`ZQ{3BX!($$b67DD9!roosR4~`z|#B*)iN31M{Hr zzA6#@yDC|iN4?@iS{-vC7lv3X#Ht7t1E)`dV?_lA_|I# z@8QT|{G>ykCGamYixaHZ_GZ#tJv`Nz%M~DG^4bC+6M?VC`Ol2{Uik=_+AZJf-mEE?>t6hDw37TBgxk;-VPtfX*ycJc0OjzgGJEHkIqTEJbI zYgy*w(!?3vTz;TW&`aki+RjZ*O;xG%Ync4AByEqc5*SCBb}Ig> z{hBdrl8R+a`FA)mE79{YN5`@YW4fLDlsfqh8uUmR+Yb#>u!^_TNN|b!Q4^QpI;XuF zE3taHf0_uohXLeHp-HwDjOtBu_?7HLt~}faPQBlyaJ)5~h`j|dD3qX%Dk)(-JD)B# zP_$!@chO4M>;2^OuAb@7BZfm~G4D)`mgpwOtx`CN4s18xkN+_GR9GK3#C-&=sYF4H zf{f&$PhK?B6O{qlSi*&T^>#2{%bjwb$$Z zStRhRz_M>V6%>!;2magf^J(;eeqKxExb1k#AgSPJ${>D)vsXv_x1RKgh122& zZ+ZUbm=h!spPty6#1oeBo{h_@5rkccOj%J`l|z26L6^CWJ!VU>f3m1d|5lzCX>kQ3 z2}8ca5DFiu-nC!LPK?{;7F$}rw)p21sXia=h-tdn_}2%E-#|v5Itu8^7o-Q054$xj zPNX#XKKKmG{@ZT4w$IjSA;?VS{$f95@N9>foI_gS=mo6o+ZQ*X|J8EcDF|WUKCZJT z(CvL{&lbxb=Gx9LJF^Kog)!x^(NUGg)?$&gj9>OBRDcY2Q!n=8wREjd#Mp_L8ygp< zrljqT$FPAEu4O3X1~B3H25P~+NyX{G5I;6;-B6s*mm9#M4Qd(3B7=^n@pA99h^YxszGae}YN_#J*U zHKh(=%*5SDE{qTDN*noBBkf_Ukv3AT>eMXSTnboDLT0)u*}}aJsP<>=!)PUiqu;ktS8p zWBa6p1Aa~y&A4_7dn0$BvLJfmBSFDJ0{9C;G*PKffYTr7az}wwJzX6QILT`c|7GAN zDI%_7YbJ$pI~(E@nF*Z*^!K*JBW?aDC0EF*VUi8pxGYg*BYoIa+2--c~~UBnT|yYuhYd?EDYRGwXbkhv72*Njcl z*il9O)grzze5Rlgzmpl2?so2WjnSUzSrF&w7GHCyhv*>}ObZ_yGIX{HaV8j5>2AMt zX=uvGKmtgygMKpH-Q86%<(O(WxacpuK69~9)mUd_azbjIn3yOt?VjnRb=;dNo12>} zD0mIz6%@=hx;xFmo}nNg`dl>d+0OiN7)aZF6)a)kz8-6MyYf=dWsh=ISXek$IX&&J zdcLZ%GLqya>xYC3DucchyWBwwQ`Bg|`=t3_AJ84O#)QH*GG>H=U40F8ot}x#^+#MV z5fsAhZXeZYb@ra5r;b$JDK{{FyEw^L3bB z>6Wj-O@tub&Eqzzf%C2&ylzkLU`2>YjjxKR=$<4~*@t)h5ZwIoKwKKWuP8apo>I+T zoVl9{RN(V^qnGn7AE@@bAj86zXG>P#XP|3jo_Mg=*Q9TkMG6A%UtGl{3 z4i2Vgc;vD&Gd~}&mQEuU7fb9Kht!92ruDUkFH~#6Qo^dT9_DMWNr0WW`8O*&kCIon z=<75O&+gO@UNO&Q?-3nSXw1I1EtFk=*hh_igO>d-EqG51k(B_+pGyOV&vozl^=y{k zl|pozO}0G}l0!z?eh=#Dg^<-RB_L;jR@L!TcR@MA_B#@fIE$-0)NY_r4;d(XchPWg z8Gbo6YGVIbLWh#`h{17q}Q zvZZA*){W>_3GOB&=e8Ls+alG}tAwUCDC!hLkxTN{97AKy-=c;oz=JOphbj)sibGRU zI&Lkc>OYXX@{B;-)wpQ2OGOaw%JRhdR^ipl|OFIp0dl_&7~dK3x=Svx^Oe~ zm!Iv8c$GI&6n0`SC?}Z=Qo3y2SK6ZrP}_IuMk=?5hM4{5!cBHmoVgddSSjlHrO(4f z-JaO>nQYu%-eTn~=1~U?=(1RB2z%;0)4|>F)Rf0|*}TS#=A$J{2ISMe+)$9DML+P5gX2)C90zz2YP3%XMdM|mLUh6u@TkK#&5_?}m%dnxeAX(Ed{c# z@1+V;7lpMmL)54TE3D8?gW8)z!Jx21nsu>_7K6SjVU3@$-K_) zl;@k=2(m#jp(B~uw4kwO9b)C16Cu%|g`3L5M3a6-T!Ncf$^PiQ0Q}$~U&eDhshc#a z-mzb4>C8QG+M!ESIZtPaSRyR^H|_|=M@26d;B>IUh8nfPf(__&c<0a2t;-p02>uQ& z4L8iQX|cY3^3yzIqCtt}#x83K7dvQ^^)2eh+OgrFcXA6FJUs26KO2?L4AM#wETW2c z6&A|vE*A1PmPHJuT{6IKDJgF09TC?-zBJwl$H}jksiIZno-g3z6r@P4^mrH^B>0j; zktX^3a0$DVx%;DSDoOW7kGs_Ad9I4SN{cVA0D%9$0S5&qMxGvh-%} zRju9!+He}a`bI59Ho2osB{r|qk5q(6eh__%AKy1Avl9kqnR*&AcitBK1&s3z=+#9! z!Yz#SM^nb_2ddbIXPGK{X zaSCVh%5F{XbaE)Pv=M^V5PBVmG$?I#MfP8|EaJy&`zL<&jj*qIWTBF1sMi1`{!VIx zSvZdkv2nBW^Jqp(Jf9ZPag@!Si2vluYhH%c*Vn&i=m_8G2rNs+Nk{Tq%MqT^nsCB* zSaJ4bDfPxdDV%Wle0PN7*qh%Vtj9uNgGYcR3cVfS3!*kxJvMFi&MV3{U&y6svEFq* z49U2Z=xo5=d&6;ey6dVXGWoZ}s(+3BT7 zQszDpD)2BoRR_+RUxJP*O31{#g!_Q-iPf!OKx6o~vD|$lc5N*!X=LiK;A~+oRns0# zy^0(TFcC(@isQA6ZvL-62sMG{U3vXz&>N5XM6Q6} z5Up0d*&JDWl>EVD!10=%Qi?!J(GiiGt-+@q;jK4+j3MHc1UXLsel9NRXC%*kyHjy% zp0WAymB>wWei{YL9*-6EYBi#FQY*r?;V@ZaeJZ|%s<=hX_C9NU4aev;b#aU7RzcUa z!R5-|b)8ucQtyW3}?p65s|rs`WA6qe^g+#h$OcO(<}Z7yih8YE#%o87A8 zc}HZDp5;0yA^P3}!RIx6sz1gq{Ror03Y-=}H#ozSz9v1EtKI0ub)n61^X|`llk)40 zc?2cQC;~2Hzt8|-@s(~hPgh0UyQPnW7r>^X4uzx2q{IiAtd zu+|}?rsgFO&}KyW;)myCy5l|xv+eHC<_S{kP^w_q_l0QFOxi@@_giH5(UOCkK=1n zj^)aNlRU$Dz6Oh1;ZUI3em}3NF|n5|3272e6xS$t{M9`#50P1R4i*?#cQ-fU=gU#CG@gd_sVfb)9qkLM3_3hul#z+eV1TZo_ z{_dDMwJbb57v{-O+|^|fksI>rlYoAgawAxAh(4jLfgz;044DZ9-H+GeSoC@Xz{V`Y$xL(89bJQN15xo*T`iabSdvrjOjK==pGZE5x{*6cxJUkPR!>-|##v{g=HS zQS!pvlRU|7MZ|^svpe5cceq~p#MK0zRpf&db(90Y?T|p>a7_vs*p5xNuQT=+^c=G2 zLTcUq6dn;l@y0Q$u5NbUrF~$)Poa@_6Wfs6OC)`GlgTALSipq@Vf-_XO^=It4uX^$C z)S}uufA&F#f4yT&Xpttj80!M906hPxnHibfl+Q8l>usu&)^y)Y zWj-HEFbt+N*Inn3F_H1F`ho0)5>)Y3H?wVSg2j1WTjxDaL)b5d(+BeSr3F|#A zVs{(Q15)I97ERTlL_2Ep!7w@&TT*VrA!-b?$Ryc<#I1*Qf`~+v?po7 zu0_ynZR#mM_Tq_Q-0dMD_}X;WNI8~IW$l`!KVI(*&!YjzVvfg-D^rJWeB4+VFtzoM z7H|S4&2QGF->A;80`7tkj9^XVA}6+#$x7`1#|2onNtnsF(RcL<+VER5;(J3bW8E;8 z)!dV&lCCMah_SK|^wD%ehw9A?EFX&hdN*u|2*l5E3pX+@S;NNQF4E2tT@yuZvbIJmhQuP4ZUa2!rA-=^I2ULNpIa|Z|)aPG9FanN5Sdda%YO2+4nOO^=Aek zsNM8lXCdFvvT;{_O+Ie--Fdf3Du)mCs%d2)mpWU3m!{{CHr5vSlOp1n?_k06WTaJ5 zx~Ge%#c4wCH)#-2%8frcL(UB%0}VRh78S|T3uiug7IQ__otIdR0)V;idP=j&MWx@y z>I)fn;R$N*IgrMXFR2MwSz8m6lk;k9^yf=vRac8VB7>914vV?px1ph-TSs03>>xB$ z)U6@ch6NHnyDUYzwT?b3?t*?D@0~cRn50I6NWh{LVM2gXNLl+doLlP+isR0*&_Lp^ zN&YvotVE2HEi^~;=}$t{&Pz7lu?5>dz*Vco8SQ1}Pwih&tmk~~v=L3Wy=M4J*gxtk zJMYnYy>d5i#m4VlU3lo%IL2#eV7YYc-)ra<=M;BzU>>dPqus>)aRVyfj!0#>c$HW^ zsk-!^e7`!vG%bvyH!gB;WGq3?_9xZg%83bYW?+z|nY>J6PWmuWN(CdX(UphJ0#>%D9j7>Mp2QQv2~G!L!e5=h&Cslbpp^&0tq5`C-k;F88;k9*WP0eH6JP_uY>GE{2ypb z8yC-amHQ@>S8E>7i|iz$uC^lg|1QX_Uthv-oQ}#m4(yqDBit4YfcF9|ZAV_~FO`r0 z!{*=RUKOyD{U5Dc7j|*grteXif8)pJ{JHVNC#DTU2J#uSy5;IdphySwenzhpTLl~j@$yp&oe;ML*r zlUbrJpdosBrHByjv?^Yyb74qjj>3gdLetup%XRlPnE*eAuHxIhNo+~ka$V-=BgXf% zwt)5vs^s$K+tR7{2LhmV2lXRo-CgNr^Cr-GeN(FE`*4=;88PE~0`=K~01i*O#pGrv z9)6gxDL3v4hNm|^1L*pB1DGScR*3$JO6*lKZO4LQHh8j0eX7GmjqwAXOBu}F`UApN>nNW@5|PM^Gi`#SwFMRpmi$cA(Z zbyF$O7-B?>^c)`Poz=MS(s1X~B5=-;IQ%QX zRSE3ud>O#VYbM9SpUO^^YkJro(n31`3x8mne?kY$rs6(*C8{`y$8Mt@L1-4HE~cr9 zp%|OkRvz9a{~Ez{@mPTf{yqw6?^pbga<3Xy8!Ru^shc3@WJ!iNbmsPT+grwYZGvy6 z^S3yR{65HSdiD2lA=ATk9_cp8UE6+2r^?kAwJB%5Z2f$yxr}yJk9?wqdu@ zq5ocU@FsurGQ@+?%jLj+q)&CS^|8LK%P&V%HTg*hKT#2(FyXUb^~RW+6np&G1*F$l z|M{FqhZf(jp{M70d->6&tYqvwGO&Go(|NFe{rJ~AV)g|Zfw)SH`UlX9$V|{}Do#yL zM++-YXWWPqhGIQ`ZuEP~&iXa2nl%k0WnT!tH}qvfh%DZdOyGTs*WHBr1gtP2g_Mj; z9`D^t_C;h;9FQL5FcxF-aWL+}QoF<>d3Va&6vx+3%8Zud)u9I|kkvNcs%O%67(mTr zqE5U`@LDSvn9@LkGG{?@lO3}DzH#>gfVJSaf83OujXpoNZ>+|l$p-pQ$0T6^AYqasj_`g`?3Rnkq9BR}2~{z8On!dHS`QI`-uEWYlw_9-JHCC{@l8Kqr1I2`5vd$yiZdZ`Te(cb;V7z( z3F)xl*k?!Q55Ku}4EF2b#7tsIB*5I!qNazlt~%TNOzCL;f@$Ex+G9&Xmz0^E z&pH>TPB}4dUb|q&$wHQ298iY!-2F7N!(F{g7AQm3AdT*vplP`Ab!U6~ z6)EZI&E08KL1`&S?;7Vjq5(VcNJh3os2{9f#q;nGJoU?9dihdgD6LKL1H7W4Our}Ulc{=vk<4i!#(fZVruVLw~Dn!(mlknDHWuUaD)_k28G3g2@3@;DeLxuciMwi(#BOevEF!UV?z>Kx-hcqz7Jr=ag8V){SR!N zD-@hg*AvdMo{ z;#dW3WtO)w^p(2P!X+&&$9D5-GM?xSK10a+s;QeqqvJ6Q=$0a)o@C3qZz6AZ14AZg2MMP2PcI7hmLZ7u+XoN ztN;B&a&-`dFf>)v_kL?Mvpa$Uq&1ypeg;W9Akndqf)ON1fz72Bi8?t*M@B{z`00-i zVlyPCQ}Hyo8_loSRANrbE3UQ4J@ByJDapmxlPtd)S9296XxTUZ!{1MFuaU2f74Yt( zCX-V7{)i>y0T$wV{^}XfdXk3Y<+>Mz6L$aivzN}`g#4202R{8uO}bKrrepR2?gq8< z!lh)dU0N6X*7|dg$1>~I^ZpQv7q)oHUPGf^e1$cL7D0A z>0=LvkCcbL;ZI90ae1k8kxf2#9`LtdH}VXYm!M^8S~0$DB0XN`nEh~hbX&tx}9^CiXV z$|@Kj6y0`TKvsXqd=R46Di}LopQgk1`F#@Ay|(OhycWA$YX1jCZ~f&kwy$@l_bQ87 z?B8aB^TER!{SzNrV$`SehgZ2Y~3kmFr>dQv*QAz+ejIo0s%jMJsGAn8DlfV-A^3A$rbfdDQ+!Xp(f(MJg+J zn7}uVp_K%Eb8CetyeXby(}y#WvWsu9aadXdMacbz|7dQ@KtmzZMe~qW2^@78UBd^Ys61KKpC z>>vBB8^#&arBvK$XER`SFP^q^h=YRMrCwWc8acADmu&AN*ueZUpBA>D{4L9*Oj~XI z)Lg`fPIwbDbe=dYENt{$Hw;mdet`$qW)@(-jO*Y3aQjA3FdWU`y>jPZa5Uu%K?C2% zepePtA5>qjlnMbrOeM$iB@!U16z~P+u4VFH*#$ZP84?=QNjVY~K?07~2xy5ng!{AG zxXx?56@)HsPy3EkhV>uP;oqM)M7PvY04ayJ2oR13r;z4G`iBBqx1 zybw)mypKoqnh>{&hnM~RBkAI_Ehi_3dR?VTK^XEh4JF@V03!O&U#4Mh%a zOK08vV1{B@oA(#C?wzZ&=f12tt3$a0sS98830m;M(=UhXrkI{X`IG}y{4Pu zuo^Lw@;&b*uy){7w&d-7jX(m7RyKVV?FIEjSG~G7rCL0rFSwu{kEDolRcf47<ANRQu|c{H8cqMfWgKWEnL*707lI%KXf>rl#D1yVau2PB-jY>57!0RT+D3^(hOE4VRFKu-wgcuiJp`5JNPA*WPu!2OXl7H zq115lyRh)#JV`0H)y-a5y5`@yV8?XyaaTj|^}|N3+p>9^9-De8$=$u@Mv2JaU0SI> z#N#0CUg{r70aFevvCYoPUdx8K)@KdZ2$43=FWeC~{9yMN^Rs=UGJ!|^>NL}D<;%@P z1--G|TW`kkySmCA+FVl62K}Q6rhu@}h>y|ny}$PVT8gDW@EGLY*?+&)?;q2MU^dpGTEU!B49G+(yZQ^K$!BZ>F?wJ2nBc^Fb7~Vs0wPo(j_(Ke1aK zrQV;72E=ZBUyRAc)s7l$TIX95pD4voP~!%h6)cQN!OF#tJaV{Hg7nV_zC3?- zQ0k%kF@=F$3nX$~g3PWnI(uu&pmO+bb=kXDGR@9=?={P7{K=9+(ef<1G_2&Y85(O?i3h0Jr`n%vnL3*MeRXtTRDWo3JWPM_Lp0WR= z4M^8iVmXzv>dg7z@^DG)pP$cm0W`xsDO%FfXi(bjf#Uysul-I={V(;0V<6TiQKay~ zAI3^JAvk1~L7c0~!y-ZS^mIW%Up1aRl47`n(x7_Y^=7>{vLJW3l~hGfcs>1Kx|}g^ zc=O35MvPyg9v77tuUgz7WA*R(wGhymWOPaoS7HH=-Wno=>m*N9tB6|#4P@y>wF(4R z9cOuH8U$WO6*x9(+zdla^&wGzsmPrMmQCF!EJ{Y~Zc*0?z^RUZgB{Y|Pf6i*M*{!j zk-NVr(qt0UOt`%KLTF&q2+cp1kx z;c(g!1?(k8xD3IJ?4TXXomNSngkQ7od`LekD znDgz685QpMgcqixM1ucb?A3KH4B+>^t`u;Xo|;M%@lMju=eL^65fBuV9~&G4f#CQK zQ)#0~TA+aa9Gz0Si1+E%=)}On$jC4Ela0wjEt2zuILsUv3p?yFT+b5F!k)BlkLU8) zFDeAsYiPt8`zOmHXur?xki+3%TW>U>I=C_svFtFSdXb};{N3FwEC{OiOaTFI-g}o$ z(8oCJ&2TvX(ZtW5Ia?5;{AJvZdYvu%vwC)}%1I6(F*kq(XlQ6~yRbD6g#G^;CBgc? zQ4NBGI476Z&dQ3x`j1koe-?ybU5s*MeqO;Yxj;X;j|PUv?G*9-dsf#$r`8TQsPHrg z2CkVwFx3(;iyY&`YU^z1%BoNKpt}RrR&b^wX&O=W-VvcUgv&m8m+?M|+y=ZFoPx-z zg_3L-9Cz)WRoB$)roB}&HOwb+p-Ki@xFCI?uRoV>z=G+?3m~hm-$}~E73{}Eu&rN! znS`&6O(6ye^q;1D@2LC0>QaOrGTk>Iu`dG=KQ3u*k$gsWxLOUH7EJB*_;7nqP*pVv zya<-~By?&dm&-GuF;&-;{v%@L9GD!N4mPYJ^RSH46-Cu zNJeKnlaOVwu7*G$pp07a&t?@ABnk?(w$4T(;V$-vua3>j#(77VmeQfv(O*>m?|D_C zSN`=%|M@G^ipmD6V%MTyK3m%?60ZL>ayi5&4w6|>QSp~iZbC(GX=7E0-BN19H9{5Z z`DwAvcu)8S>;FQQp!JVEr-Vohg^5BihicOlVqJLp*HlLLa3r^@OuHASXJug}EMhK} zOm(x6^g$y{zn%1;fo=QjHSo*1@YCA2(Kl!r>h+T#p?HUmj&5cYAGEKKpOJF~?aMPD zpQ>bia!(~$i4LX!(_3ed}C>TRGMIgng>Efz;s3K!!y~fpiR}c3<14ySp@txpon1xLr!lb(!&@g*r4RBdnsB`#0Eeu6jQdMsAA zT2$+bC;I=_3it@-?8m33Hcqarn6L*1CKXw6@v(z?DGiv=sMMdUentVNmUIuHPOr&U z+W9&GzuODt^j}8y7z3XoBFUZtzn}X1M$`A|HR@IRPKsFWvRKztRgEG* zifaLFw)!#a{s;4V32b+h5&VOTo9!}12rkm|74(dNU}c4uHpQ;mIO4bme?;sUwFD#p zx67Oa0jH($4WR^uf)>}{_wkiqo&}t#Fl|r9Dfg>>hVaO+;Iu}S4qXv`uBv3vtC7o+ zPyQzHrvroEi zys4_{jF`=-f>LfS(TveK_cvwui?J__2B^TUy=CDvTC{BKXM|~)7im$}gd%6nnDKw{ z1{>k&;nDDF0ZTkSJD1nK-zn@~#5Q!sd7sl9+dh1aQk9<@78Rwpe1aZgGN$%s6mEhv zIq~Ak{Gh9A+b&3mRepAIlIIYgmM&uMYM(odn>U)d7Vbi~gQ3uabzCNUeyeEwLK+=P z&m#jKm71rdOqN}p+9kiJ9+=u?w;!)JkXq70M=kB9E!dpH@LA2R>A zos{K+we2>CJ<}Fv)`J1Xdg;Jt0jq|j`EQa1jx{d%V}wsd>8}sN53iQKYThNnU95M} z<0_+OwZVZ+D=)Ctq&V*26VOJvKMTgqoem0gl!>UkHIPl#GwUg#r4?^2I}}dg`*Cwf z&*h+zR^D77rYaHA&&R5%64h^@7L%xKwVSmHV$M3rT?U!zE>{p@91o zldO|U5}j{g7d+kmO^M7}5pPIuEA+`F7% z3{H+XfogS{9zwRj?fb)QuS|Oi0~v@^z3BGY{ootFS_ILLF@I8Sp;9FT^es>~_By%h zRxQQf6isEM)3TN*^V)y(mQLZ4{Bf|wV}s5NW&00I4#>s`!q7U!@Qy#teT`|wnnR|I zu15N^HNV;HvSX`Q!X{G_WRT!(&?%3omxh@js&^-|cO5P{fjm=J>$#zrejZg{lOIP! z7XRsqz5U=?5Al?R!%c9@efIn!XTK?bJM@}Nx-cX5WF`c(>Cv+$umFftyTb6r@3G$} zeXG<(@paNCv-F152;Yl2iIdDBriTV?*OqL_*(#eMW!~~E{k=*wsi)1{ooM$tO*y*=~KN}Qd#csNnC{6ps= zq=my_$oI`*{C7jijl7>(Z<)8|Sa z0CMqw9|hJrrmK^m7{#qo)txG}RA=URV|6W*u#)n@kgveh4-%Ng6R|T5{=%-|6pTtGH5KoL*xIA5SZ5#;y8Bldm z`Y^7{mo`_)m_nsxwRxggWZ>Pl`syc*R!OToTU&2*#jfpadHX(V^4*+*LhQ+LwCk*T z1MQ@5&%!6Qt|zzPfG1sgl#6wrMTJf=bci!CCB;l??CY^Yf??9RPAE7YI7SUyD&Mhv zeOB0lR66mcf3?+Zxo=z2Z8_+-=0OdBmLQS204*+dHHpfb_*%LKHl;RU8H#U=~Wf{ zz`Ta{x%WJ;MI(3fBYP)94~>X9&RNZ9jAOQPotdQ?oVc6M$C}@OD#{A0sCtJ$oW#xM zwiO+ea4Ua#&D!x9&4~y&2kXX-Gi-0qHD9sj6iV$H5{n|)Enn$_Z88b7t3U`e-Ay9% zrbv@*_=%WA+(~je#5q8;(E>}ClpSVcqF_BRp$zX`TVt?#7H++>m@v^rB(##7Dha~D zPa^}tuT;gI;yRwc5_ei;2N6$T;GfdY)xCg~A8PS=XccC5Rn>{R+t&79=qrfD>g7LG z{k_$EOQ}-phDO3Y_@wpZXM&oSOWXhB0@x&V+f4nC{C=}%DcT=y9_KM249?7dfAN!n z4!7>?)%bJ_h@sg?GsC~`94TyF&)rn3<$F#&yLwY6CnzfG;y!@-%?Umtq!=o@CVw_A zh#Q@u>UZ;2A`Fs&8d{|3bO~-Px>;3O5;nl<}JDci@N-R*4 zH*FN+sK3^TX%j2xg~^o-4Gpc!c&Gd4M^@c{t(_fJium!Rtuqf07MA;(5x&>g5f&XF z8*qNWr;a)6i-KNx^;Z~%Ibw|A=4{4}_o;(h!xi9SPVU?Ak`>Y68r=CH5zzLQQC* z@5(YybovsL&33^_V6sxhcf>(k&;4}z1`-nf(`A@~hF42+dOy8H=H!b_id8$g2u~wd zh9Cp02KM&)`ZMdtLyh9sclT3MQ_QLKU-+qH8{Ei&DGYJ>DS6wt^CTYhSB$IA{bqT1 zc^_Fd;MdMuIwzaPfv~RGBAyaWnPds;iK(NfGJt#u=Pv8-)q`tC+Mj)zyJkH*fDP>tZg|@!QmG zGR;i)8(CF7X%O#(n6#yY5r(DMAk>wWl_9wKrKKRbz$`G(p179BU_)wwDs`Prs0Fje zMd6uT{!fr*=i1WsVMxD0o0%B%jPvs|;^1jTMa4+d>M3~cf8z2e7{2vpI{G65e0jR{ z&{R*4FY}`nR3K|%NWcD*I6OQ>2t!%;>6}u-sYlbAdEAFH>cbV+b06FdTNxR{u_4hE zN;7eHkqlXL4sceZ-58u;51oJ9m8vSsp%mHuHKkDKwC4Yz=_-Tb=-TDr!4ouiAh^3r zLU6amCAhmgAxLnC;1b;3CAdRycXxNY^L|yA+JDr}&hDJ^NI%_u7$pC={m{}m4U84D zChPFQ0RA~t4Sr`XY_G|W)vKrMn4a3u8aVqRydlT-l^_e2yBnh;{|82(0FWOk8VZwTXMTCdPbaJYT!`#e_ zF^#RM?VMZ~;xbr(1!ne|)fb|=r!yh~d*iM(-*yxAw0`kw!ORwa5WZs3(obb+(|6Qr zZsR|L`!_#Xma#6s)$@6T&XjnApc&Gd=t95!LSbK8(+GwJXwu)3&7^FB;crWcD3t z_wV}3blRvX>KPt_$%I-CDR1-9&%SIMPkcG|#w|p^Ed08b*7op@IRYR z4@>NfcKx&5g-(Apn&khg6OH#V%52wU1)TthAh={HO<$e$R*mb*w;#f)QGotfNpI=J zMPnwPv-ykWel}PjCcxC)%YqrD!`JxGEwiB6tD46>P- zEKGjpa#E`V)6gg7?&;aWdx6`C*NcgClbGbmCfi{>R!H;?HYFuRNyvhnXK7$=7AJ#g zFjavi+Nw=hwU`1Apxs-ourzv@mPtU)7G5^3n~Xd8Evgp3P((g0acuZ|Lp~}%MP0#~ z+9&~j;9F$S{Adq``4?WEPSaab1^G)qaWGvvv1uryOtz5X+pJzPtL|vt+w}bK6}%UI>F6$?(U?ZU?z9|GMl^p6HBXh1|gxV(9 zXJg==tA^ugjQa2u`?ij4RqO9FJ_S*FGyt_)DRs-!OYS%n8gKzxpn>l{uQz^ng8n>+ z#Na@zQEPiU7XX-xhN@ls@MB)g{d007TA*9V&e=Z8#8(eFs$#nbRwVmu!TQl?4WUoU zY%{zWIyBqtDRO_6Ysfy3XvM(&vgzmx>V`AgP?>LJvvo(^NbQ1`nonp{D13GnprSj1NTL zq;R+{vj|{7xWic6(zW0=_=t#@?_`tfQ{{YNgu|rAn4Zm^wGW&x0Omsp z$9|~qZo5fJKkFAbDq>B}Y~yw8Q&w2@cls74mLYwC#)_{ILkH(o`aQh6@t<$qi9Naq z_TbjLZrN`nfb-pHO(+1cq1m%73cZYLW!;TGL7HPW!YdpT+B8za8Fnu*+aHnJ)_0;D zTr5|gRCSRa5J*0W?0^iOTF8fM5n}`Q_AUIJ4RV<-!Q6Ydw^)&`&G%B7>lb#aeCPZF zvHTpycq9p8U7^&m=vmyUvX>9_vkdZL{f02OxxWu1{I+I2ONfRKu^P5dNwo1YYb$Qm zY;_^-GqjOgXBytlMqa{%Q6f$4znFQRbY`SZ8o}W&xe(zlux=5P8QRUkj}8LqoL#Fq(rqWRP0ZcdU(4%vcH0c z#=^gU%sf2q2a^<{iUU?uoT;*k@n-BEJ42uoORNO>bVrv>gPN);$T>|K7Ci-3c%Sd& z46Tu~4F(vj_kwwNV)JVaU~S-eRP*(4Xg=VOJ@rFdfM~}{g)a1uFLIvUm1(kL&02$3 zGlZ`8Cd?dkbN0)eVZCLc!+$~@=#yaq=Wf@yPj1|dbC;YvH+mA(a(q-8e>s&?sx`A@ zlz_h!H09)e(B&VW;@D=O|KOkjIXJ(N2uD!+qopZ>nyTEUlUvs}2PHbJJ$6bkhWc0z z?3`bHw_a%a;ft32*Rkds@avObsgSr-I8Q!3^OkLAE_fC7(8=-NzCwhs^_yU_&;hO17>q-gJR0@$Cmi5p z&~4mPRnOPEw-}-t{24Dw9tB1F9ej)L!|*@L3JOXpDxLEB3NPcU(-pL&Fw6kH)u&b0 zHIJkD1Ue?6muvZsbGS=GF7GcGFEvk`R~(kRtwDq_I|;GV`wk2pyD9Q(E~$HqmTKkN z;Dkp+5gZ{kIUP(MRdo353}IjPt<25!Up>#Y-_~JL$?@N>{{++NPm<^Ln>oD>uLlPJ zbS8N?koSF@R+hiHz8+vhz~D@^23he9D=teBT&s;PCz=gQL{a@9Z#BdB^?@qW>vFnC zd2v|l-2vU3tHFucG-c-dq9=i8AK&sTqdf+U5UeL$5XQfpAZYnik(!*@N zw8^n+#fq-Bh{2kh`si1@$<5TCr$Z`zBysp?J9ugGRl>8`$i4R(HA))Fk2M$?VB`b7 zsDNvZ59)hd5>z7w0OUH!3bToJgpa*=;=5jALUUQ{b-54&L6GzTcFZG#&*P|zZo8(l z?EEb&c!C)Rsi}}_{kx!jNlUzmV?4lMKNP@mZ>1ET zwB|)HaG95c9^#ElIhKjjAAHcj+~q=JmE>igP1~2mgAGtS3KQ887~(wWG)%wjm?qk; z^1JU0?I;&e!owf$?9i><*6&7n8I6|f_};JH$vxlsCQ-$~j0;c&3KQ6Ntkf9v{!3Qm^nWj=-{nE*q@7t4FnP#0qZz$KQ+n<(}mZBna zP!*x~IGN-nxb|Oc>g@b_*{!9i*?!;4WV6=p?c`M3(&G93dscOIbyL#=)7vGJ>&fEH z`cE+HxkZaP2863z|L2ZHYHT%m_O*qt^?$yGVRI{A54e#-btgGHQ)Nkc2mnnt!4p{h zK!tH$;pplJkAP2Xk)l8=p<1u~9{FG!e$dzUVljANT!!l%pj|u6tlU_fwmJNVABdNf z@((WjEBlVHP*qNJXtEnEl7^YUaK3YfeA_2=>|xKi?x-H&)WhCjyDUR!ddYZ_gz1vmD((q_ z0PhbN%Smo0G-kDGrrWvoS?GFn@qMsp-@uWS5`0=5Z*Iwu4ZWZ_Yt$A@onV7(T4HvE zR>A>pod#N34X4VP4z`O<551FjaBOc$zC-)?Q^z@PGU<&3z+?c#W& zdqrMg#_`_nB$44XC1sdOp7?D8{c~H}pqtl{9{CWhG4NXfs7|Pz+H!9%Ewy?v6;y81 z?>t7i3+S*!nE)czJf0^ret^4#2G(CG--V>Z#UVc8V8GpU+7ZUvM!l5`} z#=W7~|GnAuc;5c$q(QIM?V=Y~&xi{-dz0?8B$l@@W&tPy%?JyV@R^~ap*o;1Emk-S zYGpr1OOMSmI^*be4P9-8=43D?jc^kv#t)3FH|7v#*eabc`0PX!-;4L1beCY}vdw#7ZvlPFLrycFwbs6n6~ zswttkEb9O1%kdX3{FR8+;=_`s{{OtD+OoL(K0!LVKRc^IYy(e=_dE|DR_{)@USEHR zzkUDyv|*VP++WdQ>1AgwmNGdSrcZ<6U(8CoIUE*q9&Y~!BmXH}wCNop!#puIw4~PD z&&=BETdG}3oYfcHr3bF5#P`5KMuMb9wR*{ofA{ z|BK=08{L4e*F`T#*RWpVgg~I!IXK$hUY~@X7REu@9n~-^NI+P1+lm00D&QD4BZC-F zExE}KCSGiIK8hgV@_C%fY` zByp*RKYUxJ6z*wcWK^doEDzC-O)*U`r_FP6nGiCd2tedX#qQrRuKHofzc$3|BAN%u)Vf0<;AJkiJJ{y9*;)utgYt-9SL)SOfkkGRaXIlrH>)_y zv$n*%hMqFGBkF8~V~G)|XT~2*uDj!|*yakS;lWV_Z=Kic$nf3kP>01GxB+f1TRGmc zcYaqg%b;x}D^ce#6x9pTR5q>he^y)q8R=VQ|x>GR(@^HzUnnOL*)zzi0=>wLMMlDBN&z*S`;Ir0q=s}kH3$K4g{+OPx7qa35KXL=G03u%zEDzlEu z{T5FlsVl|9*^^<#t5|DkMjyt_kLryhYfN@Auqh0v0Gu*%@>n~~-<8bbxZA@vHtAu< zh{%r28gss2a>f4wFO>orpdu%Um~b$hZT zs(Ah2g)VUYFGIldx?c`&P{$yJI5rGteC8t%I#+mq1{b@gwRA!)o98m-235o>f`_DjgIR{`F znI5OsIwGJnlxwh;2rs#OZkD4R)y440tmNs1f1_P#I(Fggm42oHz-`4s}e3(8)qp$DC9#;%^* zy`SoB*UI-VgYiKg&xF~uMcHPTe~9ECb%r_}!EXkB#rm>Yp2cTd7Ws- zC4JHyRsBc!Uc^tOM`k;bHSO$b?1eh;LHgXQ(99w+<~osKj0ZW=lk5#rzofAEHcm5Z z7;FriPGVKX>U#gQ-#XsBu+jPn3T;6rnl7-`kV=z=Zq{2by>2qS=5%xjeME`WZ6=S5 zq!cFQiQ!I{JM-O+0a^U`BrrW&7bGwH;{4-BOh51L?v5JtKlT|^F{3!%9xn_hF^T`_ zO$EXuBN>^FLgQ0^mtvRX)ZgI64$&>oq|wr-gy#M0fGrDD8vLFGPis!A@sObb3DsOI zVFxi6Lw;Ysy(RV8KIXeco-jX+3L{*oN&p$|y)>6LjisiZlVnu*0ftle>iZ7_sR94k>Bx) zi}R-tI}Ntnko$LZb*7g2xFa{*f|8aHwupd4SY&;Go@uM&*;Gc(sD5oeWCg9<5!o>` z0rxYCUuK7)S4kddLfvua+0kG=m49(@0a`Q)c^~8@5yu%bP~3l2n*(M1mn1RWS??d8 zp3J!eRv{2)clU)2yH+rryq%-JF$)T2c;`JRMJBiXW}?K&*F++t1w;Nf4#nq;{zqHw z&3>(U9Y&LuRC|}`!c3_XGLd6)11dw^hj1b9zD%Dct_ufSgGL69<;3RwSQC1w>|)}p zp<1-T^d4g_Q=ECuqByIt#3l66`S!qp1FSNf$?cQvz*ZQXZf2Yb{Ti;SwCG>rxlSm4 zQtyDoop3qMZg!3*?skbvsX@)%Q*NyjaY_+11X~PIm!>7r)Zxlo3uK|WjrCI1avI8c zHgkRoG_s98xW3deFP7$|g?736xE-L8&>|}H`kpO)o37KQfrQsd0;`wg?Y`f1JVQ25 ziD{r(HO`{41*Bze3z0=-XJ^~`T%|s;HG)w~E!J40-Tf+`Tv9MIYAltVMMI7Kb-vhq z&5m39r!_x2yVGV5;)%_t_y6R**uO_FQpu$iZet_sZ6=#$@Hp*9+r)WjP_^HLexyJ7 z@1l^1NZR1rpy{=|#m8a)bK|dWs#xgAUAWqbYX|R%@9ecw)ho2>&cD;YTfp)bKNYst z(N)J4wM;eDwol!X#c6W0Hc-k~YC}U59VS^T{)p%&;${B03m2sC`E*vba7Fo0O}{qP zF>c|Qr#vjZao|K-OdA?22VKb{0Ixr4FD(BiSW+h;Dxc8l;L>bl`>W#c6Xo#a{moQt z{I#pC?d3g+Wdj(xR_4m72Pxb=plV3zcs%jqlocTA*nQ~+=!IQ-DLtz4F?68->+EO1UxmodZW>HGZw z^+d;JE%fC#xZo*1`>mISLAn6y(AZEoDkwKCPB$&jd|TZtV;QU4eCrm$iN%Qu*M5kw z5e+J22hM(#j@RvL0)@xnsO!QbQQ1Us|Pa^B}k!@D2y92^Ln zRWW(z$-^-{OK0mZY(r~alvnfCF69g~WFbFTvZv)vFXGm`!o%fjj6pm&XJmXlq5bpv z|8oKO(#OCWvSCSpj}LiW=j%MtJO0O|j?oaZ7;;4{wX^|lc6KZj__LjV%T zYiepzpk`-gKEH_aeSW+-T9ll%X%Kq7-}O!pV+C_c3A?-xkNgRW zqq77tL&hyZIRne@L@}FZkW<0Pazs5IxIaI0-&$I-5&Bt$#?i3{9)&;ZG-Uv1xhAHX`$Qy_Qzq+5-j#-R^g1?Cd62 zj+Y$IDtL4lHvVA^UyL%Wq}F?TnOs&+trQE?2ygG~DA3j&+j+Fy5`{R*%|#^d?_=L} zIqe{@Xh^LzKHl!2zwVTKXSQ3eRa~|jI~>EL8oD3ps1|ryitm|f1ZAc^^@8H&i@UW! zeh~vm*WUvZ59>5l;S#Q%hTKM;B&BO-&*$y^*I3EZot+&J@UN?>xdNMj7>VuWCeSi8 zbDYa=PDk7FXu9R>GN-Yq#-Xj;XDSs^qpFVBg0tCu>IEOZmB{?p|4sDvIQ zGxpZp_tHUok~&S0yKuiS?o;2^_HunNHCLfG?u&*Q$B`3bp=FEBCGr;!mTOcUoai6j z7^E9aoMV*mIxB6d&b&_Pp=M2AN!HoD<0wh>Az&w_q zyox(}pmMKFj-NH3vB}!;A?Z`@z4^<^(z|aOu}h5V7*%mYrWGlr@Tc6;rXSbDN-@D& zG)JQIfuN%OnyWhP*44J-PR+-V2S0wNc4pVX(`zDcfo{*&{&hq;tTPqB;=xXDdOc<8 z=rA1bPOV>Ao2j8|W-l4Q_(;j_yLFhy1%4D>VqOhH2w6WFvKmt>@w~^H&XW&E^TC(r zWzS0?Z>Bot6@txgWJTL*e~OKGaQGh%T~!jSmc91>zVr{Q|NL%1XxgyQY|O*UdpQ}U z_dFpVr8i4#(7;OnGZ*6BMz@d=U8jiZcT0UIvBkNfWNt+DX8MmzBe@bmMtvVt8T zNc9OO@!7ZI*3{A>@wwmV?Ck9A75)0YxuwOhYQChR-Q)TGVmQe$bF9aR!Tk5)tf_=* zTCqA0m~TD!IN!h9=kQjn`iK^cno`YwO>u3)lW-F_u8nP+B315v(jhFJs^s3@T4=6= zs4r|#?4hG*#y6-h9C=D5;4?{r&HIf%`iR4^FVnn4m$QS75QB;ZiP-@}@>E+**nk`|Z5TGAw&A-IMqL_1Mt)% zf$Qr1(^)GoADi4-M*rZob9ET<9993y#=ijiJd_=1nl{pQuAV6O9pT=!EfBX+T++m; z&~5_vM^_iLp9tB-td{M|jNZ#kf#6LMI1mWU`7M=argqz~>T%@gzkYbEVx(aPnzvYf z4gfunk#QM8V+e0s2`PG<77%hJi}{_NKBHZeoSY28w%0c|bqx*3{_A$!n-}+kaa6&< z!QjYlrmBiX^YLGg(TV=??IEm`A_`tCud{_BlJ`OvIhW{C^)!4Q^GK7(N@nlb9WH9z z^JP-q4RlFh4%};WI*En_?#!PuDEh)fa?*kNXfIW)3YwuE-J+F}+xrlvQMg36HZmHE z=ufHSG^G&?^LLl^Xx&|l+}s~pR}WJfOiXA7Lo`O0bS3Vf@9RRY3xoMiT2URAJPnb6 zlQLqRXsGAbI{nXwIoyrL1ZvHh3LGe7Vb?=JOrs~s_3@9YSew$0t5{kaeI)_2c1Cu( zPW=l>p0*mArJ99YGIy(=4eNisxD<=MEg*VIDJnjGthSjryIuT;yf!4%UZI6*1zC}8 zLe}g2wCaO!X8MHhmL?;PY={{)5;T3kU2C086{JYfRQ;rzWf) zQSC9dPDzQH%VN42WGaDDI?8YGtr!-SwF$M<_@qN=3&A@%i8qh~7_A-CD(hYeu!ae( z5xvN3`rBG4Bv$c}h{gg_>SF4PA?)3as@j{dQ#+rbEhf!WhPrAP7OeW-h{k-`2e@Xg zRVP!5CRrJjTG2+tU=%;q**6LM2?`D>4Ud3QX8CIGip3TdTk*E%nzS+ae=B1+WxNL8 zyJ&(%HtS4uQy6o(QsRzt681SOPC{Irc`o8je9!`H6wh8U^`1uteQdTA{(^1z5 zO_`;urpvN9`1}Ko)nw4ofSOKBd`RUjVY4954MyhFy(EuhzWQU!Vy+_JB7i{Fjyiw0!V|vRX&kyh}R@Q1hnAZBsez zRxUep0>hts%IccC425WHH7|uaLlOgASLxgqVf*1ZVtz%w zxo%m+-H1baQ|DlUCZ5Dm<%e&BeO}g@rb?BE_UDLHV~6B^PHfpglYX8Pdd6?F{1usH zAWiVs_W5)T%1pLe@heAb;%hoEPm$;OcD;6pegg))E%6ptxVR^+SdlAo;zWVID~@eR z1@J3V6n!^P;<+I%%P%OXV{$U=7=oA9(!L_;|ouXW$7dOEqY$Nc`Cf;2u7ANp&WsT6hJnKQ!7RrDbsOBj~+ zSENc9Sp=>0pFA0xAWSGb|2Ta(p1MNXUYMe-_WYliog2$mgD5KpWCi6n;o097^AVZ!2&gJG+WFJNYFy9uuHY=>awUi z$bwnnPL?n@WttO7ZWFZDE*!MpZ=LO}2F%KpwzMY2FOW?p)en`H;0^hFjZd&}{xLrb zqny}6I|m31DRT=lJ~}dVUteE?RE{#$k{OGtsLrcx+s0kNG!q~L>yqQJlj zFFLTiqp9FpnL}95#0ZHqG<-xo{8~{HBNP%`ZQzt^g3^i6Ho$448BRAk_H(!>@q8hf zY&2GOs1xagaIf0U1xc1bJ$o504qKxZqo%K3a)%nn4O6=(M^)GHQI4$MX{#>tMF?2N zAb4jQSv!n#sjn=SVY6Dqo0j~?p7uj=|Kw~|MWcI2W>wrfq9d$7bYnb9JPhULHb%i> zJHrkgpN92JJ|4dJ)_8C6z9l6n!~3H5M?cA#D=!qg=$|LQr?cjx!=2&4+c-{4^RJMi z7ZJ>DFjQqT)|C}FKxqJ=&%@6ST>E3YZa98Y^CX%9` zCMb8VN?m=UZR-qY6OSyoiW5*elg?-Pt+E!%uIcGxhAT&dBVo54MHG3l=4HHE=)&f#dt|za)OtFsEy-y)QA4Wei-Doc2Ut<$3Tz>7F(O5+Q ztnQGge~U?iAnQ=-vMKfs^Dy*z>`*5FU)3zFQ)f{@_l|0TJ?>ZT7QV6 z{uG$H%8!q~J!Bpvf5@xYw?K5jjC-VUk1P%7!(R&8e}_bWB8=3wN9ClLPz^441YXBD~XXUlbzAG3}!|d$f3&;wFk@`Yo}l7FFR)W9^mF*SnjU+H)IMG#l?C z2U~(YkSM|=<%0^^1OCHPL1Kh7lua7UUtQz8&!Td)U4{_8mKiV$Ol~eb1_tf_!kO?P zw^^n_{mN90NYA<)rS~}CnNYb~dFqJGIGp-)EXKt%BlQh$!Vq`N=-O@>47-P{~ca*zv*?VFn6qS=(qH#Rm$9_xH`R z2WI2s3K$acF*`M#MBGHzSGRyl-!vX=ss6Au=>RoVH46ga&+%RRzcz|(CaM=W{vxIW zQXy6X#ntq5!#lDzeVgm4d^^1~9BneGi{aoGk{vu52nbxT&{pM0>Dq%RzJH-buTRljL?$&m^FEk@hKmKVFP zEX=Rm4msO>A)P<%hXaDXMLMR!;Lcbdl1ZUA&N^%WjnSM-ew4*VP(|YbtK@umloc6@ zC5IX{ivc1Ibvlb239k9)l;%_OPZ9eRvC8LPmzCZ;ASmnzL+)M|d66}Mz-S_WcO-xLN!g&)D z6G36K8D@X+_qX4@Bv1g?7#^YJL8D4RRhzlZm_-f5r;;YG2KmqDAPK!5n`2&8hUq@| z%A5htEQza7c0CnGzg?2s+E!%9p+$ z)ztjHn`6?;`mg6A5%_(ta=6Bn0hegzne!@V4*zOp0$*twiKYyYTHyFenM1uCd>ci3pt}yMg5Dn4o9t;pm90 z!PM{PY{6{<+IB?+?#Zaou6y3bPr{ctE|*N@RgFd|TPu5C^m{gs7Be%bUJHYGQ%JBW zSeMqAKwtii1=jC7*4yryQX|7T%k(*o+%5Pr&XVirvHK`l)C82ILu)|dcKrGbto$(& z+m_Pmk*p}ph(W+a*K)r~!YmcbG-6Tm)!z72*amS_!82AV93-zGf-$jIs;?Bi3{|f+SUV)QFFbcT8t9x%I0fr1dRk zHzb~GdLow-5UVV1E@J*H>+sz?kVpvzap$ksp(EmspS|dcLJ{isuQqtH)~QBOvkHw0 zmF$TAGY2*ei)pF_{TnCmQ(Id{Piw4Xh@UoBc9ZA&eRp$=2{`n0Y9^an9m|R96M*}`> z4}PzJ*C}>y7W4lqyA2(6<9vo*_q373Re0%> zl#qGJH0279T(;Nit5m~r>nJo^3Gf9kN-AiZY+zcgUotNtodp$D43AX7s)yP-g55+|!O7PvK;;4^d&Q02A z!7zT~d*B&%c`~Q?#WC@lAygS|udW#d4J3p<=d!IuWyM4DV=KFjhF^-poPyk=S7)=67O}=wf=bQ#3>Bf*y=gd?nKzO!dnBxv zsBddSgxGBq@#_7#V`u!qV}d@)>szl{&e$-#WuARF(Y>N--0jFtx&e6RaRqPiLqgNxTH1vRXmx@Pnmr# zLN`+>02BhIRw2LVFv+OptFx=qxlUoMwb2(p;7ctR>M0rE5lahr5PYFmtoxU&^N0zO zkq=Ji8>(g+?~2KaQ(vl>V;W4w4DVJ~N+`9aZ?g_#qRk+240Roe8-r27QO`5GiF)>g~&uKOP1&&L~^1tV+5O9V#M6!dH6pTjHMPg**d(p;(=ItP%$!Oq^ zWV$r1;!36*6&17*dGTD6WA?6ZhLETTPj0`q@d*`qDfY6pE|JNCROy-hzAeYKz#a~B z$BH1Khq{exIniT3$c;xndTtEdqI0>kc2y+Cn}okT^wNadp!5wW(oeSb?1XwbY>{2K+IF zm|?RSR(w`Q^G{sgp2EW}S;F^Yqs#B4)0Kt$Wls5)$?f^5njRup{X%w^+cOx>zZp!n zm#xtHS$o6j?R%*bDvKsXY`y0lNdDj`tx#3UWYzuJ8Q87o`MP!O68dc0PcO81kaBZw zWfY?E29Yh=1$9cODNhZZB*zp&yF2IHYVv${!nEUYzkjm_6pPA?_T7dGSmMiZ6Xp*I zzJJFLPT3*7D$6!pewRwA7~q+r|5}9|< zr~Qxy{b{}d3ZQ)v!R}h%yj(>SO+m<_XoAGT{`K_0N%DzfwAX6)=q@>qpm9trzDYI_ zQU;&$w8*=GOlR|&ticpM<^0JeW#??}eE&vv+lI@#{h8-jhB}^d%HwG4?>k`J!OBe9 zyt-KD=v<|oPk~L1!##nnazLr@myki|FD67zjN!@8$vGg5t8}cfr^!9&h+y?)eiK0J#uTgmzmi98u&;QW? z&m$bOe0Rt)PWSk6MAcs3=0{V8!LvQJ{Rra7P5gzlIuFro345SCj~P*iAmphAPa<~> z#^H?c3x6O;N1``sH0oxT3V;P-lfJ5+U`J*-ee1#2HRxqY=r6({+Wr`X5LmsMwe@Fs z-Y2@ziHVCX7_*}XU2iRqp=smcS@zbqDM|?CFOlh-JY-7L(ga$*h*A|-C-$o&%>0SI zYxkuOm7=No^xT-vI0&_YmeO@-<{3s19U;uP;u#mzJ{Nr_BM1X%o-d&%J=Iz0^Xx&f zMvZAZ&4%(G~e^?ea&9n_VvUXd9WippNzCOZni#l)ObN>dja}G`}_y5?M zBEZ|761EA*3^0@SyS-L^^0NAZbi0@CrOLsp^b4y4_AjUDXYfcYlkW_~zbbH6<9Id> zOy!HEF#baOyv$5jrAng#2xanv44$XSs%?X01p&eWvS&(|nl+h4LAs$VoIZqEKZ8-S zsWC3F$JP?2Y~K>;Zkh@w+dnl}VKFuRCc*FV-ceYPwY3~tGSP$mF~si3uc(osPwVid zXdV9h2PXHlHP-U6>JI1c()E`?pGQfPpCUmB%jj@=?$_8awe~g6&E@_7w8(g8$r7&OpNe>N@CthEJgaK99X&C67_g4j0pC^pd`-jcZ<`6!^YKCevt&9T_}nO8udj6-v++8EhSNtskp#*ypaw$FcA_bpb= z0v&bcyA{cnJH7}EbCo^}l9V6V!i(SKlDIe^^jWFMS6Db1iDJvnYScC^5-}M0?|*TJX0IK;KfQs)`E%;Ge$tvjC?Emdp)hhT=ktcTiM!7@zDeB8qH(_- z2B{;*LBo@-WL+LtL}&~*Ox6ZO_VcY~Br|EuHFTlXr!sS!vfmzQ$}na7D3^0_BJTq> zRA2;SlBrlU*ou{xAb3ln3@62#gs5Mic(Qx$>m1m-TUw9v`NUHNLYg_aB5Yh=mPGf# z0uMjj+PYqRdaG%6RkyGntWz5a4@KVtZKOU|vC)%(c5_OmxWcHmG`eBOyXgd2oY}>F zFzndNw60ujEj5-_i*)e_-%Yh^_bi_nr08 z(ECYjofV6BxL09C!KG43sZjIf>`zYtGNy6S9EQ7+GPB{d(=LI1F4+TpwBq7Jf8Kc9 z@uvkXdeaagGl88LNyFGkfn#YC&J$&R881aAkZv!XrQP=6gBtv|`Bm|u%^ivX5vZC4 z1~HDYY5d$=Ta}-LCVi{t~sYIS5X}8 z^lfOL3r#u^3jA>46-Ub3-Gb!j3g-q8{A;>ly(BP$3yyt96@j7yeaD^j*dO2Jb`4`t7jNXB=_L$Rf; zb4TMxZ}|MVLK!0(^#}PBFm@Z_&85@=+%FaIDsPI-RaH(ye;VtiS}{Rbh)ytt0C>~F!fU&nkOY%`zMe3@$6s+k<;F?2uBve%!DNm&DPX>u=m(oA*8C6Q?t1*Y^v zw}s~toP*$*E@~f9&Js;LrRqXt2|mhg^FUNTofpgL5}4z@4GymT8uXUyM9FMZt zny7rvpmpRYKvLwZ%3|O|4AN5NxOzNGMX5K^;jFT??3;AL-8v(NH1otxY*53jUXjAT z3IW@?0;SMJgRL|!9;WU973?npDNmrvDkX^Jikz}UB+o+Yo2^ySpmO#92Ujg4imw2pAVboPjQSD*!x9B|y<}uYzp8smA4$9@spPG?sP* zemRcJ{$icbyouWREbVYB4#d_}HlCJbhZ@50Nu~T&34C2QscoJUaU`I({^kVROMh~y z?IgK~fJlE4bh(srdE2~UUrfL|ZS7s$c|LoG;5||^Zu;PKJJA5o8}Q}b1uBxwc7(0v z>@rBtTV@;Q|7Lr{SJKc{L#wXoF$+M z=EvVDc_AHktE>NUKr8wU(#jS2^BfGdhNJ&=|HE7$3rW}QF|O+>{+x2>SR!-kio?1S zCr{xSee90HbaLu7y``dq#%)c@AVZ+jH>yF2r7!ZPuUZY`gl`tF-_u1CoA+Im&+;fB zenGI0#q6I`cb$G$7onocW4$fI!9L;^S)6mo!O&4Q0+&almcS?x#?`bI41+a{>GOfW z6iz%t2T`b*=mMJ}riEJv-(z*V-3Hjj6Un|o-D6ryLPU;}W0#!etQz-YQ{qYqth6#O zZ|g>gr{o+{cEt{e2Lsc%?G5U{AHyrYNW$6@JX7@>n}qd4Gxh>jN5}Zy()k!)!QI(f znAU~kq|qHH)4IN{^#MQ!X{5Dr5N63f*^7yZS-{ofr+@d8{%J9+NSh{hD7U`-Z~ex< zDa!1YPrOQnH8sFI&kSbz29(ehJoKVfwAx zG@bu0gHfd1(v_*DX=!*>x4KHZo<1ZbMAF&}r~A;{W@uee$%K=)_Z*TNlR zON{qA$W;*s)VS{C#W=P6lD9T!f5~Q*8DIe(QV0MY^^jdoZJ+>oboYcZ!eEE4{&Amq zCh)Ot!|uK(1oOW&;gPtgRe!wNXfMrA?0r(F1|F!cQ|MjD5fmg76chyNXtSes{(VOr z?LVq(B(+eoo)Pl?Va+=`H_y)HdKavAqvSb4hu)AkMDhy?yerMNn-~^?2%f|O(%{{$ ztyi;mrWLGn1-})@D_TUdr^}&qose!hPfh~@5jB$As<3ZIn!7aSg;siuC{Th*5dAI!n<6!R7%}WvtdIzo*gf zK#9VmSG#l)BiJc-_+zpnpupbbZM{ zLq%EMap8iIAW!_+iKmF=>A{L~5J|)bnj@C^9&56)u-Jo&yUcO%HHO}!8#FXD3u*Qu zZ)M2Yp1ZjO?@}hurq&tjt77OS9;qYczZ@%Y-0#xVe5xr?J39J-M=lo*_`vsnG<{`I zoK3Lq65I*F3GVLh!96VQgy8OONgxDwcemiOxJz(%cX#*ue&?LKwY5K}qUzn*o}TIM zN17JsBbGF|L7aP%PNNi%f~L`rVwf1)a#k%3ocpp>bYg9X#Y*L9jEJCKx2a~?Lo3yw zU4K{M)Zz4V=k{fj$KaJY5CK`eEPVxiY~5$YLfHYQzkDSh8h7>znNA`RWUm!Wh$uNV zdRfX8Ww}P#g?bW1C#Y{3$;{?>EFBS3iv?R5H;$%-p4ewzLKgp zJb0u2g7`Rg`+yYK}9d@Ls*O4k*K-@$4OR%0F)@wU7tu3IK2I zAGVfbbIRQ7WjC-_jHmPKoyV1cqWjcv16S>b^?)pgk%}piYFaj&$dZfI(3LWUCm!nE zkstTjt}l~kG)dbD!Yct`Rhclzq_!&7-f%+K;YEE7S&&`vO3%2J0c>EInqE&rq>5zibo#W?#{L*&Srysz~xEmS)=EHBi9^2Fr zZc>EA2ttU3d!?QyNtvo&3-N*7eA=QHigr?mKx_$~ep7Y%s-IY*JM0X+&qzgMfvJzd zzqujm$A=~{!Tf^Y$=uxP5U))Xk2+!}gP`)6volDr1z!mP1(g z>zvZ!ex671j>w~&-AW{A8U`0 za8vo&rG47)wqIv9O*Jg8?)r_|Km^I_6rK;hI<~igOdOc*_>}~YrW4nMy0psru7mZn zn=qIF00+w)#mcS6Dv-#yC1(pv&1`-nnnJIerhd_+0-_I16fFvR0?UnT%_MV>=i9f2 zHQ{NhQ+6YbN^Ul7*y8*OrYS{paW@_9@?{OyKfvCGh>g~gX-Z5N(rRSoV*$m+DvpId z>po6$6D*wW#JYCv$#X1F(> z+bI|^Y9KG+gq9Kyr2<}E2N6d;<_|1a1J_*ki<|gFDa;3tw}00z--7xZ46Irp^d0A0 zt2rx|or?TCDD1_$&VQrYg&e)0fSyKeG6uO*q}|)l73Mp) zKjmkP7RJV4=vk$_5$uQI=Fbdwe=SL$u9g9$uJX6&V^szJA~^w7jj|@fVF%qKATfg6 z(|s3)zjYX6tV~$i;T5Xjs4o9T-Z02wQTvK9(wncd0HAd6lAu7{n^oL77_c$FZwC{Z zT14)ugJHxt8RrR?G(D&Y^j#5T!iM zc+?3FPR{&{pj83)DZ*ccZMI`RTXNaH1eX%aW zI5LM$;a}{ZWdK^(L6^s~8+AZuozz`-&NVY%NO4Q>dm_E zd0aE4^Ff<11*zB9p&ndQ{<_X>*ONv43m>POMFU*CINoL^Uuj($>3RYKqRsg#EW#zk zTR>+xeZaUm+|2qRHF-wrEF18YSC`#F}RNxAqrI-^fMV*kZJLPKK)m$>;4O z#@CF}4z5oGbW6-0k}&56pu@%CVipoGw)b~A)mR(YIseQZ?t<8fn2^CI34tf51JujA z(cTu-YA)zSv=aMp+JSdO+mew(_gnB%L9vLkG3_T$-_V>??>74?JdsPZt_sWB)B5n{#Gch94j^NAtX? zsz5h4Z0?#Xtfd1V)HzOCO=W1HkD0DK6;f*NPg89Ro)kiC!T6)vOcqlZ2c~VdQMBCX z)u#95f_?3`#LR8O4W_yne+vrA_sLKDic`8wi{zaeB3g@r4Zcv8c%)&5cu7t&$6}q% zq-~mE3Mq!cyM3qsa~M=#D?7(smJMkU5KyeQ1)~Fy z)_p+dwJvor2s>Ll5nIx+4SN=@%6$CQ*4B?qS6QdW4p&!K2t+B{?63)D)w1*JDQD{N z1;m@Lo2TeJK+?b<{2H*Dr1aR&iJUN$X`c>hqb7Tq%5{Q;gIvEG){)@-6x_^#7c7vL>0UPDiS0IpE#yR=0Wk&Xm?3dpW48*E z@Vgcd13$jOL1cb6v3a+ZsbtZnAy&EbpvL6t(M1FO9aLA`^T1-WhbGDS(Q&{qz2n~v zJ-a)aG%wz?tlCFeFq5w{{datdi3K(ZaaoRyAa z(;qv9N*4R}B!H|5%!@vwlRTcSkmzq`wJnm>d(32zyE`Z5ppr&JX%EyD4aOY0sIt3r z6Wlt8*zeBQR+mp^GJ9fj0oeu>xY1_*YFgK4{j+bV{Si?A%uViJ=N6JW~p!%NT)y)P_?7s>%Tb2K{D0;_JGLqzBXl~k5T3hXEai*Ji z!nYjq$yLaNIk=1i$5AtE0`=`DZTS}Op5I&=2-NPFk~7?)ee@RapL@FN2hPuzYxn(p z$5Yv5z|9Ik`Vx8eFn7DVB*?Z$5nyKryQCJn2-4J}7RUfKK=Gbg_ogoeg}Z+93| zmk&Mb+0ME^USp;ORoXBsKi2IGeBiRxKYVl*>nI7eiPvwA5Ay2L4~2vKa4U{*`soms zhl%WKPPO7=pLM0=Cq$6qFd7Vv^TZtzk<9;yCe!I^OiY#Qks(22bAPTC;_z3wOF+&x z(^^u@99^XCml1m2GCCWTm2m~TANKV;HDv7ntaI2d5@XLjEEMznV(_u#^Apz|D^t-8 z&83DwEg|OounD6(i7SgqYCcuHp);eqn`dyNUDn|I_Ktv@f>jbHI7C25OMo{V@)znB`JCXVS`Y6Q1 zcYt{QZv>7k@np-8>Z65<0{f;B!o|>`pEJYE*}%1=bvOJo+n%ze0Q=I+^oQ!l4#8>< z;pzuokgoIDxs3gCYDcWr=aBKXY$ik0R=bzE&O+5nvn5o}!l7WW=_*b$xi+fJ-5N%n zPSa9_VHLYGj@ua6PR~pJK&cG0G#%KGG(Ru|)i~04$JCc*JUB9$^BE1~$&swNAuR6{VnS0B-%CGN~AgoUX){P>o zULCK|XM8TSb7}X1m?%5mSn2H@2;P4Pc=!y2@J%XVKsle0^;>BA-vfRl)CtU`B)h7P zWv0jhgAW_kt@A(L19r(3hnXv8PJEgE12BUtwy6WC7z8Sct7jPxRLIn{pce>KzLNeO z#@NT#ELi3a2j6L_pt{R{d896=l2vg36Xq=WM%&g8yE}0L77APB`w16%qT6tu22_Ap&nNvUWU@6YF+K6_J!#gD2+-`K;}zug;nET)76Wz=(q z9&ZkJQR6r*<-n%9_4PdjKxOnrQMu`F2;-IP7W-M6z&zh=YVaJeHYu`|+P2}07{+~E z*ZFnHg}2tUn#mQn^bcn~KkKah`Aj!LS<{O3J8+A(275=Fp6~GzEZwmy$f)-O`ZNmM zR{ffWJN}(#;&TRD+7x5&dOat{!L2^IUef$|LeAozmt^!WEm_Cc3HYK5+V!@P@1C>b z1*^lYibDc#YYwc6fj5kwesoflxCdifx~_i!^?Jeu1c3rf%o*uHyvBX?GGvxGped9% zvrRR%FeJlpn?FV9O8EEm1sBkLM2sLMu03TWNdClqKf7W-wyP*!6}=W~cqkOzxmWWD z-nv#K)*WwH_`wm4+3?A`%i^axk$)|zatff^9`M@)=wQ3M%AR%MuOL<^Ivs>hA+2jg zb1)7Ol^1C%P&-7r43j@~1g6!w0_Zd=T06MeFi}sZp*)@ri^6a^EAYjCQ~H|{Kb89b zJF+1l3Mna#`<7e!)J%!`=_*M{Nq{-&Xh|lILbBX$?Z|TRDxr7l4^~#JF%7^B-#vrE z?p!R}Mf)}?i^IJA#RML4RMt)4$|ixWlF$#*uEN|GuWg3a4O?4l;DWaq{n2Yo6F^`BnUApb zHSx-+pkKn($JCohTj9Vl?a$?;(T0E>a_wN;f-b4~+6po-W}?+Yj+Jtqapp>jD{P|k z1Pgkh>x*MG$G!W=m^-%3I z!4D2UihVYxT$O)nYT6q$EXn+(w=|qX!U%o+F?8V6bEgp^xp>MTX7;U-ULE`Q00G<@ zDkT}#V)ZV?6yd9T;DEvOFq8*OuU#nTlA&ZbK3w33{@k>6c;Jzld_ zV$hT1hlIGW;vbW(U?Ke>pJ?Vr95|ce{;4jfeooKKoMYhuWI5(Z+gx^sp8fm{g8=Tc z1UYiQ1lN?pcHS>e>bVwyZTs5))Uy8N@PN^s*Dz^6WMy%gbQR>q#nw} zlz6BOKR*0KOffam_WEo$A=Cvf=4MK(&57mg+F^NK(Rwju`{g~@v=M5Lgj8L`ObOeg znzNBC4XuzWMOn=4WXIskaIFw($A9#oG!)HADfotT6`!&H|C=85*VgOiT_|)xjAz7nEMCy zDw_%&inKUf=-qygwwjQu_)KDT$~ht?6m@$Dg&UK$#+Qyh{)#;;Is&U6a;&n6T6Bf0^ zYb42mUp{e~nWP2fYMRj}K{gb!hMO34snT}RhW_7qgGAy=6_#wV;0z7Un{{q+s5o_F zuc!W-n+PHAGefq1B9t+&wv@wJt~}*oP7|OfeGq?yFFQ%N+;Nq!-y%Umz4JF0<&#tR zzEEc*b799%l)=!i>b6zQP!}5L`d|Z}=^L56d~O%l#Ohxj3%H%eWACDm!TI-(u7@NYKNX_*uV`QQS6`d`B z)sA@*vx_I*3D6AJV2&04aK0oiH}~d5foXSd7qAN%Qu)g3@*8u(?SgW~YO>*pvI)R^ z$@V8qr`W{MTgvc{Hzn6wnEP5_79&M9`k? zEg8&^Q?k`8NSj|+D@^CRHmfg*U1CL{Icn80`D2~rPd~cw7NP0%{kL^!ZURCE<&Fc- z@>>4VKCG8*bu3B<6rFGZY-9b;fS@=!uFz2XC}>A^SJ(XX>+*pLR;IANT|yzA17*s% zP`$lC6~Y7wY5JV-Dk&u)LS(*`N$?zng8T^Pa6TpVKlWNi!Z-(|QNIfIaOix zFFOhfR9IaFgeMKcQ=QiNUJcSgrkUw?=H!P#h$(VclyQL*i>h_?pEZ~qt(@TF+RSFA zzSzg7kXUYL=qu2%G!n{)57`59bJI(w?BFyFqQ_y16yk=C%TUFz$Q}DXWAOQjvSB=3 zzis|F%Z=j4UddE{ils)XZE*LzjF_s9!wZci~BM~98Hn1kr~GL6EAR3 zc%rcJ0*hu1S{emsnZPhDv(;Hls#iY1BwdQ+Tgyv#@G1M}{27%B4DpDU>Uah729vWn zuSB{ydX<9o8Ux#|HVdjJnWv3b-jzh;j;5OZAS+9ct(l#%-|!c`pOh!a2!`mtKtnE! z<>04#Nb_VG=`d2ioI<={v9h}!-%Tfu& zzzAKCOYY7pwAEuFs(OxVo!Ft<8OZA;|K(PCsg)?8T>@`J5Mah!6tj4&MmI{@D(vIZ z=k&XMFEPK+F|ax&oC8e|EfyU@V zNqJ9mjjXHzp_S#J5fd+M?p}HR{>Mnix76Gh9&O>~EYds{O;LNe%7w8uOz?Ci@><*9 z1D$Aln*dnY*Rcv^w$}_RXjq7ujcOg4#3oJY=vx2M-7`m1`DR%Lusai@PRG(coyRku zw=ZW?^)!~@89Ov1WLtc`>vrhQtyC;u`8|p7;lqg%Ifi0oebI&(@YpM; zC5Vke^jOqmsHZFS%5}MI`bce5n!3P-qxf+DupKq6OF==A#H>GLsQWRO_U`5eRHaj1 zTB@X=fCtesH8bl6rn0ZGfZm|jc2dVnfuE=Cfk^iFzOEw+K0D>pDZKWbf4x>KKfb%7 z%G>!nKFupk01%}!3w~FB$aSiO=shqQLB9v<{-!z7Ri|3HJ0ucMt|ABg8=RSH0k~(a zb~a}!)0oUrY$2&~^l>+hbv|M`?a>ICGBC>j=rAM`7D~6K}6F(7h>si~6 zdpqDHLTXyot2|-T=8JSzTgwa`?IXgV+y>5MG^3{06RzBgmY#CC4M)s=WNe-oEyTBZ z>eU$rf2Tjwk!v1{lHSk5lZ|#yN?`l)7R=aj{a$;2CuWy^pRo^Vne<5O8U|heCNt;V ziO%IDZvV$=Iu&p7lyAc*;O2Q4F~6TTUczwPN=TE`RvNdxEI)UAb=DtpUs~fT%S=~b z#B-s2R)CV6sRqR70049tya-^dx!qpqR7%f@1YJhCFXu3R-Xyh~`0|816k{ zSsisvqoS~~tggRpJ-_&sdP%9TtITRErp=CsNEs$YmA}{iDlJZ_+x!sLWwb)nnpbL1 zD;K^UZk~=XF0$v;$7|>M<9SeUOn(zQB@UZeLOz)=`4oKj_qZsZIU()TO5GNEV#fs1 zJ;44U;AgSA$i7Z0A-+Tsu64xHm)U*ju*^ZfYflCW3%ef_=HO`ou!)^z`JVgAz_mdn zbhbtY3$tOn0@VOfjA*#`&nxreS)9L$)Tpzd)0U1jab4j>w)U>yp0PSaONx^p;|bxD8Nw+rHt&?gx`Ux6Imu7&pj)aCVc zSkM7Im-_=oG^gM!Ib)$=ZdhMM|c=R(da4+CKp^g{?`N-lC@KX-^VG@kCfziNId+#}-%mX~!* z%E&Mab=JiBjx_ae!*-wR?`~5RW7+fQR&0I@=*uZR{b)0>@$zRPQpucSnC3s@d{ya@7VF3WHlO>tts4 zQz59EeDaA`-r?@=J^_UUs5)9w%XVqC-L1kryZ<*-O?>ZOagW6su}!xusgJ(tQ320u6?e zC3+#gM|jIH+f;$o!m>o(%QLO3bqP};)2Q2Tok4QAb8Po*ZL3u;4RjTwfItnke2Yd8 z!3=A|VOOM?jKvQ@r~KkuwiW##Q*G<-``?ya`eAHMIEZ3ZEQ7;~`q~?!|0HCSGEe zHbwkg))J`H_r#=kI~E~4^qRIYyeBHFc(NkwJZ$e)h4>WzZQNQ|%j9%)37V>tnSo#AeFyH zJ4R#-^UKl#+lB!hi%Cb?d>UQaA7`Oj%Jt!P$hWq)8u9$eTIQ?`nDykV&3nQW=Oo*A zk)=PF%iDG#r`y@1lxz)U;|a49GR=si;qu&`gMGXB)oU&%fpcv2g~38O zHqc>xc{#3r*nS%Aw8efM$IuHgL0y{BcD`n&qV!2xL+S%7&Vrps^%>~9?8!E|;BHpS z9R!&IBz%y;x1#%zvAQ?1p`xGWnokAVp-j3RgDDqq>DXGtXf|I--n9Ib$Df7mf{^!Q zqY<+9Dn}IA+gw^q^0W~Q>fQF*3wC&aedfTsr@H05XM%;WUOPBaY8bGX=;$q2TBON7 z0|fg-Ab;I8cs)3ne1i&*pN_O!Hd{g>jgk2ZnQSeGy0K=sh7d5Bj<(9`^agk36kG+}4b|MX|X-2WR~wV_)<9_G#EnPN^);D^@XC{}$_i33=2S zXL|M@wSyt?4}7BBG4>(r{S_!B{m+b3f`gIj^IkW@gZQdVAmRVJsJNkJ$oescLkN8O;s*cz1Zk}xH4uf7gp?lC{c5geHaXYM55J4ED zW+YdSp!}Xqz180Vn+^+alTnUQTN`zKuYe2}n)dj9_|XDGTcTCLUW!pnh$Bwnrkzk=J>`54sZDG-?9_@{RiIvmA}x~6;f>;t+;A2 zpl&Wp78#23eqICg7I`nEHvTd*Km$^tK3XJ$0f)-*mE!&FB4$llml&)(f_hEW;l4zA z!DE)zpLV_?kP@Ns2$Jd?*`&?B;-T~tss_z{)oh^z#3n-hDhUv9UXs>wm@D^A$@t->FdbXYl&v1nP&*`peNqafkeJh6=#W!HaVSVf9I) zzj1HQ+^_3-li;I2z9)~JD^gYlN9&e7<}}CavnoS@);1kk+d!fUJ;N-~fg>;Mm?=(P z-cC;78;^evx|MBG`wfWM!Q*fG$c_cJhr@m`Y0X^Uv8bA$pOfe|7qek{w(Rkiy*z6euxI zb%JC=`lR6DgU2(xds=f~+Ij(vY2#%PoX7Ct+l%)fxWZ}AlYee*Rx)YYMFe>SU!-_n zUarUU=npg$?~?s&Fb%N}t-Y_Rj#SjAW_<*i>~vjY{C9OfY7gnrJSWRE3rYwvr0B-# zidm9Tobqc=&2hzhgqSSJ=|a+;Tv4F%c^ZRA!lIDAXffh)m(-5bNk$S2$!SE+^PlcI zHQldyj|+O!>@fbCG+6SJF{v8^+Rfo*nc*&5pN~LS(&BPSdK--tp%bUmQkWHSi)V)Y zNX3pu_5yXtX`S*Q1396$G$;_BatO!ACMnnUem%6r<)#i6CIck7F1yoCyVEVySA1Bg zbkGlIkXWs7d8SaA<|;sQup1US>AId1q9uqO7w~-#ADb?rL_BS^D2<;dmoR_!Q4(&p z%J?qWaeB{ozqn!;Ap>f^ot6eEz`Nr-KgZaee9e`j!~FgcbEZPvc<>HY^QJKRBKek$ z298ThO`g1)f3_T(*aJbd`&?PCx*=s*ricGGhoi95*z33IZkghWuWRJJQ+cM&rDWWg zOhukL%#AQv$BRa z>2YXay-X&FAxkzrTC~kvw$9l$6~4 zBkq3{E1GFAEojv!o?EP5un_c%TRWwt%Frj}e;&lY3?X?b-Cq*#c-X(JE?fTgoSWO+ zfA_{VN?T9p)rkd$VcoBJ%W`)`1wmU2OR6H3T`|R3u8K4&I$||AOT8WkC&+J6)Y{>V zeLjtdFNgp1znL02YHN=-?1Q{AD|lQRAyFzFrN#8(ADxEw&;&N~*tctdS@i zEAxfYHSUTvciV^rm*j#%W~tJ`oPIzpU_ z1!K3r%*8kSjxwv}SK77z{Hw^TnoP<52(VY{{!AWmQz`s8c{^QyGpsPo^4m>FI(XT; z&Z>WL9P_2fJDd0D2p&%Sn-vqd5Wh!Eiy*H~GCpUVf9%}x^ZbSNmN&qVFqv>83WO*7 zM`YoUQrK<_{1s^*CYA3%*uXKC^+(^FsB&NG*z# z^z5C9^{Cw$T!t?|Z^F&*@cQ!3+dJF?tyiOQMbV^i9ryCZ82CAGi09P;XC#&RZO zND>%N*B|oE9_=?;O64NG3`K5k$9UXjT%{M9-xBgihSUa% zBI@kXy_lBi?<{m5o54IIN^e;x{Zh})7vXdUJv+240ThY$eGW&sopgx#;jtE$te;&6V!nv z2z#g2_Bn>S4wV(M1idl+|B^su`QB;k{OY^1Z3l{f>IVN0x8|`h8+hX)Q$?uxeL%G{ z8{NF$knCKdE!Kh^bBb&s53{axjRhiIK?Mz>bee9s^_>3B{S$LfC{Asg&o9odg$0+` zR}l6=zhlwMC11%nANVgjQ&~P>AGm<5T>YP>Ny4Nka=u2}0rIa{pqSOmhpn0TwR*_3 z-mHo7O@*GDr+1*d*dh2UfpQ3Ctk_*nDxoORqEI3hylVH?{`v|1aUZs$?X_Ht^Q0|i zu0xJ2&QBVJ2^s%}#^J7{ihO*lF1kH&K@M6$a7L&{_=>Pqs9p`dv0643Rzre=3pFkp zwml7PuNOU0TxGSdZvmdArZbf#Q|1LEk@4?j@`(*^fznNQ2@;#UR0P`tMi>cM1M<|0HIyb8r``b^*7AodESc^uEAK`vEW?fCmv&6u%0CF*oxwUjHZV_io zTwW&&F~{@vc0Xrde^UF$Y=`s_CSUH54E>Zj%I3_(g!)>RhSJSPcBsk>Tad3z<|L@I zQGx*Mi6Fbj)gR=uL$|lp8<=g(FN}t&T>{%HUw^>pNMRax=Lx%s`fmFus@wjQf}DG3 zyZ7r$4*VA(cL2A0yyH$uiS6Ko#_-l53$sI)4&L%NjK3b)-qDOvj6r;HweQPck7iEg zPs`huY{|Q_*d5N*Te-UUj7TvbNa!x|J?>I9ad!)Xxg+KyjFQ~E%lDS7G##>&otGf} zNO>~`x5dhgCG_u#kCR5SYPK?pkgN69*dgvAm>CoE z5oA!QIbGM(#CJ{HxwWNWfBsdKMJlnNuMhv45wtOpBMy*VIsx6D-QlFsw6bV}_zK~) z`7ci*?{OSk3x*#4cPtN!L#e5N*t6Di_fk*}^7Hm{)_bTY7o~67qmQ49OT3@qg7OWrJK1lPLSi)5Y zj>vk3Dj2-Xsx4AUo}98q3nBr z{DM?rZdjU`4YpB|2gWd6^yzKgMRbq5r;rm;eF+{*=cx2>TWhn$OYV)J$<3nmT5zE7*9WJ#5C3! zVv8@0V7mXU> zJnAK03R3j;c64;xA8XrNT;fauEPM)gV{gLQFbJw&J(h%gA*V74zO1vTX*)Jo`m(BH^-F$EL!$60fC)%; zf2xU?5I|Rs(Kj+-lONTzmjM3&e4wTG_RDJ_5%H4?MWCb{app$PuVhfzYFiez)1kKd#&-eT^}v#=PkH)GyCcACV;V;sxb{rf$yxGjJ0 zAXh$V$SFO>1rLK;G=8-5N6Rf2JtG0{d@tGjP7&8(cbi3yOq_vf^7G>yf#4Pu+BrZW zXo+SIN zm`66Y7eUrFOYqS%R>Tr_U!Lm1t5ce#ahh;$>m2;`scYccfgGq}QybNA`{{)- zv+>HLv{CT)WuT?0%Giue-fp-UBd}W35knCcWXLDd;ZAk^W<#CIGMBXER2o9!AugPJ z6VB2CDbSb17irEZn2S-H&5{8#(47L><-));SE|JKXr@F5SMWb^64KVIA$c{o%F1b- zuOJL0SlO!Qk|IFwZ_!suwK5Uro_MU@quGG@2K<#@DAw@0#9DZk;jmc{jN#?vQ%8ED z%*l+%6^l_qB-{bSfpxl|W2cMOJ5Cp)f9v|wQ%NzL7ZY(jagbR(n2@5=xvSr zlAt6OL%;L&R`WHR&IfM8=PUz?IN<7w`|3TPC69>`VoP8L$GW(lIW<2_?3()g)HhLN zCjHBIv*0Y3yqvDZz6e6&yjruA#zg({q@plxiyM}b_TKk^Qtn{0={Gq!CUpetTQQ1Le-%Nh-qB`#LOia`Lx{z+is_2l>WW%L$*9SKyJZh)+6mY z%gjVVv_NrlcZbKt-R>_}ykwqnj0AL;F}50c!j8V$er(;GYADd>OX_!AN%&z*O9Ml- zR^TL@@=483cWD==%0|5pz0*TZ@4n2%y8X`^u?u-y+vb$TiXfwdWy~k==YX`YIN6gc zZp2G0^-jsxkrEK~1m|Yr&}#hu78V{OM8c}v&;}RKuLo8*Mg~Td67vxd-!@yzlOV6Q zaPBkFo#DR!geIMR28OceNj+8`b=8S{8K78y-nj>*3wpCZ)myw~O|7h~09jSIa$evy zwOKDLC6JGW$)4tJgi=Sd-quw zcfOF7vu}8KD;v%YJ5PfC938i-w-x`WooX(~?qIVw=WVKWbznJX>q_5D_koY%om4ae9Q-CAklapG$m_ayq+b?2i%;RnVtg8>cwF%I{~FnCUCmV9*| z?5N!?jg96<_4MCVCjrQc-O&qnLgHY>(uWA#IOT~J4nkk;T~J%giL>G1Y5UajYY+5t z#f=;EfwqA#r;y_G+Am3(EvG8l&$@RfTCS4FUnmEllVvwsO1>~me*7RG1zgU5Q!yQt z@*{o%0kL*5e7CFdpt5?R>#WHF93TWQ3!ot zHJ{d!a$sNL)CKIztnX7%ui7=4Ck;3#;Oop&r6~9v>#o;0XY>GtGs^RSDG1;%^GLW6 z?hmrd{q??D{r^~i?QmZM+7erk;m+zUtUDKCl#?B;eN%XB!*DgDQBBP8KONclqd3q% z#3KWd;8N3=*@*-0Ut$|HG&Ch2fUe0c)Bc?2+v9#on!&nU)Up?lmiWcRyPGmPY2L7`DbLHejjB?!8WT+6njTrlqfo62wkOP)%1N< zGT!}>EddzM6xlsmd|$H=)4DC`h);NIoy#rr1uouGKJlVfQFsj;cqnjrqiXZ4h(SLi ziO%M^ghW@N-&hAa95XA`b9?_(KYL|NQO)*q=QvwaeY4&vy>-C3nU(V7<;H!Ks4xYL{fQ)UXSC<&-5eG;-tj3o*bgym$oA+M2J+I9` z$m@048V(If15DUG&6!U79wo2UsPum@U&mt;i>pGO z8doF23_|Q$`~!hQaWgy&mqjJxbw-uMeeU*^Wp<0B)jC( zhY9gHI6JdbE_?Z*FF6`iy*Vyg5-!HgM^W@2Lv4rAVA%T!GGUgWX6u6iyT7M1NFC1< z;T|1>&Dop*_qANMmW}%;6Hv5Cr=;=|hM~r;#hLWw*Y?L%T1y(+oNwv{=VYd+JbOPJ z5yyeL`Jn2#ICCQZ6;g66%mq6@&YU4=h=mpf%M9X8ZSr=YU!ULU#?)zKKWuFEL86Gf zM+Ao#O4SaEREv~U<{yEp&RKd>vJ52ww-b-k*$N@a(?n&exxlXnNbVIjtmqqqXF6=a zLBv7r`0|rVv7u^7HB&pi-QT~_mj~^>_?u6Wy3LK2-oFE@tYyM(t%_JKGLt<&bw$p= z8L<-rqr`>gVH3+ZkNf{AN-ZHW&EmBy&fsU}fx=!$6XCC}1*`w^35~L!Fc#8>q2uXkx=^3m$cUhJC7!9 z9o~Kuvcn^7vK=ZcPYxfreR{kjI@3{HJ%+>qUhjJ)Qg})7!v9-iQ@5MDj}|z|Xr^zK zD(v*g3kmhz!j)=EKR*{HdG~=v|0eszWmxs*L@^UC#f7db}^R&ayzoN`Si| zWd`Urg||ciF&867i{-LnGqo`Gs+x`*Fp3=)X(>-9iux5Q$Y(d(=}BwXfr)%qLeYid zLnQBgeR67Q{^aSXHtk?z8k>2P0t5AN5~^ePyimA7SUhJ)s(Xz6^_8 z;n9z)DKLK$#rCA>Yh1bRN6*RA6}SpmwuVX;2ardUWO%Qco&Z><9CPGKDg$*xLqd2} zGn)md<*31gOeQkP|MQ?`mYaDJEeeVm1-G)FH8;2u)`j)*95N4~XC205ui-;$!2!L^ z)Z}R^YG_DB;})OQ(CD+F7z|#ENT6_klrsct+58inwd3AY{yB++9N2 zN_R;J5(-EoNH@~CG>bG+QcH(ONq568wRD$sEg_vtcfKE==li?f>w-UFXU~~)X6C-{ znd2W^wDr$xM4PPrcE&Hkf#@IC(_Dpr(RH`iX}xiJ$;(3I3|2+NuSQ4mM*iW3h5pIY zfVY2GcKMwCPki|^$kfPZDJ_?j&nM+;z#F`#BcmXJpP_co6ciV1`0A`;EP1alAMP&a z4;{Wh66+?;xH%%%S_~GKdb%XGU3m=1)3zjoAHA>BEjFNLWsUlr)Mfq7OHvyiyUsj6D36LOqLxb3vvSy9aIz1x%Kg|BTP9H!8e*pLK^zG@+@8kq$xdPx78 zeu>9Jr(g}_0^K6g$P>CY?M!_mqD6T41&T*8UEBMlg#+B8JagYMvFh7$tSjm8EC{SJ z);Yr;Bo(0{K&JYwc}qQ%>0te`I(mEr^r$r^jCHaC$ZwRDz}ePaz|=^M%uz6=Z_p8t zG<1?O^gHGqqGmXu( z;DY+y{d-KP|2ul`GjXFvBhnI*i(Q&V71?QgXtb^%#Q4W&G+zSH2Jao;bYY$N7r+TV zA;j>|Cp$suKG#+qiLg;ymy|Esz*3J)OYHb(XcB{JkLhY7Q2N44sBvc#(!(cdq<5&> zw}s7>;B*#lEn>W8H>(?4!DweznwBbB#(23i&CLOe6|X)m!|9cd zKYs<$c@L$@v3X3)2GNi$dpU>RIoNKu*}Bvjq)~b~49*S(gL?GJFfoMv zQswCt=bUZEP2hi>{cQYf zDft_!hUMbaxUC*C@$zujVv!{E7(%Qv$A2?%{)=;eEYkV@}*B^n#t+&%P#xiT(Ub`xE8Q}$f`LLuWl+QV}Re1u>V z^zCd-`Owd4FXD{Q6M$bTs8k&ExdDbN`9Hrg0fLn0&%(lLmwHoHcOq+hOmpGkFOVolq%gHy40nQ-N*&9ZQg$!o`a_-s1wK6QkIJEI}md|e>cRd!6yhw`M zON(%i-UsPMcs@SK_{c6)V?6dk)rz*}O%q0UcL`$CMty+Zh78$48}^L@)Ei39zWdtj zQ=j`CI$Ym|-i936M>dWFgi(tPNJO*GmU6p2>)Czwn+g2ros3TsIVN+efqwIIjz2XXXtk3ks zc(Z<@(HI zDy#aO+{!z$B%6$Zj}fTtZ(;o}X9Ju0>)8RL$&hmU!If*&&`4$Z6yB109rf!L#`1$N zEVt4TU=R2#PN-uabKD$|emW|RCkprN>JYvcf{s)TaO$S^?qw(9SRE&vTrObr6lx?E z_LiyrW_Xh_PoLFfY&$ZmR#NU-Nd77j`3a;>GF;k%Z;O9sBVDUFmQ4ccp%oa zRQD~V574a#WoM*{Q(#CdjooAFe(`j@Da-4}CjLL|R_di@wg)7SvP?9+i>yjB;|1wA zFY23&K=)hCXA$YaZc-(h@4PeHy2ifh@Lta{w+I13hhnURi!p8npzVCPo{BS)*Zlvk zJR#O!O~EzV;jnPprQaJVckGsFc)`%g5sGp6ne*bk6=EE&=1Q?9 zl8W8>ayTgVlA)xWQCl&VJ+ij7D9GyH`_U@Na1pE4nC?-7-tij_O?DHCRy#4wSIeJ^ zcDHH=7RvyTttgu|5&D|BHc@7{la+^Vh#c9CBfC!WcW&=|+{iJ~^0kuq;2#%qgU~m9 zyXlnFgHw%cEN$lA0_JmIe%Nr2w>+iJ?| zGCN~^jfl{#1O`9Mt*1&ou7C{ps?AGit5vk7EE-!2~tdUplJV152x@c^@ zW6Q>0XEV*@r_8i9v&djxL6^ZrrL)vtZidt(}JC#wqRUpgps7AYxyB_0s+0Ihk&2FmtoViK5m)*ff1Dl-ZhKg*`3 z)^foY8j1#hyLd9>-=k!B44}Mee{o$#LC9VRMlJ52$BOqfJ)G--C^Si|c1lwaPgJUW zO6X2v;*;nGddXv=O6)If*_I=5FUkKGDRno~9GLnCmI=v=N2x=lC9jF7zG$lG)0c?8iuhUP; z9*j&~BK`i-+J}jDFU=N1PEb?1K&ZLMx4DNt%t9z~~=JKB>t zCCBkq{dmT5j>gb#Fb=40#)^=(nQK#>nR9v9eQNTZ_T^crz@2x9Ck2NdWJLh4adPsib^oqhU=^d|#^qjR8=E7m4#n z1!a|K@A~4@Y+*fQiuxnOz#|~(yqt-P#DtAunJm;~s(cEP{OZ3|d=xKoaCgc1ela=f zkD?iFMa#Ie@6^3Wa~d3@b)B7;GY)im0aee)!`)7ZzJ(3FYL|UZ*b)~R zGB21<(gF#udAe$~(7^!a?28zkVZgX~@rKS#f;Z&vR{(D&Xc}bQ*->&PrB1BnQiAFx zoQzXgwyQ)pev#>IVx0|sDAk)N*tPnl_2^*As|~HKQ9fyu;Y_bIhczRy8RRiODU1ol zFx1A3cK8=$7XaqW92sAkfV)XL>*x5%Tf5I302J57_F49;Mpboq;rkqLMiC&t#m*oC z?U4j75qCKhqF*%n=)SDL@bqLjIW}Cn@BjPj`%uN}da{HDkoo{;DHs_!(c{+P9vsLX zX$pjIcJ!gR8WkVUhF|Et4ZKNMN&TN!lVuE4Ejuk;<0`R;kEaVKIF@j(8}lzd!0nS{ ze^DgVm{|sTsV*$%3E;6{;{IcH+4cGkkX{$&S*8cFCGpxBkD^LLoojIe5Q;f0D3cY? z4inrM`mWn9qEq=SN)6-DAz>w(0(JaZ1|wgn&1gJA1i+5F)}Ec(*`c_Js50M_N=i#U z5TI-%5@jOuyRPc&xOiD>+lubFfuuRH?FXad)Ufx<9hRCvrL%rYzxOfYe)s;rc6C^) z&}wsnp|MF{XIL-`f3Sz!TDv+c%lp5h^j1FIw`SoLZ{AtTr5jl~V^Qn0*>F>BBcGVA zr72Y4;p6MeLh2?bPQX6@@Hwx}pvSNbIW)nnfbmUT$nc1F$;LX+DnXGSBGBbD{Fq$+EO$Y%*A5eg)HRKwA;LoWgvrkze-9#yHTy~V@#;Tv`0{g(BKh()?&V$ z-qkq@>W9Q~Nx~08ihJv-Yl>jddwO-9;Vk5Y=Hl%Zf5K%gmZcg65(L3+cU6DPRbFIT zJxa$AAT-1I_;Shp{@jgc$yfMJU~NC-0-=#b%RrO6sdZ8Wu=v!|nqK^@wa|(pU*MLn zbhQ*w61Ux;DIZuLRZp_Br%<7mbl#0qb59c97B|985K0lw*PHORbb7XM-@LeLUTMeM zwCzk@x<7{FuCw!t;)-}vXt;h?YmS&?VbLG3)2ISo*n58EwLUad-3QeBE6o~cP-~ME zk{KWA&t=8dKy5fzx;vrc4OW=ZV-*ByX8k+-o1lklhX+sE#S7ROf@JAPpj3Bm@^tNr zs+hytHbns0YToAbH?61%$qsPAbWCM@-5wt@XS!{&{629Bc$^Gg>dP)T|2(d{Mpl-i z7|^U`HsKQKonB&IM^aFKd|9Bby-?J)sF8{7(sgK?vl2o&k2X9xaFm$HZ)RP@#--)r zSv>lr7Hnk?Ct6rEHLiS7IcZb6Q_K&Ojf3!x%M+ zUs$>f=MdTZSY2(elNlz1>5Lo|3wffRm@Qaa9Ld7H!(sI2!f7lh!8g0AhV8gU^+1Fk zbx-cd%vf+{53b2yLvg=AAc;5y2o#%w_>;UyLA(O9&$>MP{8F)Wd?s}2-TVo2bn{*Y zpJ83R8H3W;*G-Dnv&b4F+_c#c7O{+87`>3OY|X1j)gHz-O|-uQ|Fbj!SXS)-Ikl1k zr^g0yUqQ-(dx~LdD!Syzb=mh%j=M_C2|Cw`RUXa%l=ZiHU*Y+RoYxKU`7K^!O7zSI~5xyfQg};x^NDw4j zrH`ZFhMhUCznTysz}>=wKM5a^(XPW-^Y~);VJ6PWsx<`>JNWHcs0aLUxShTB+%FhF zJp)go0@8&K<{{@!1Pt}6qu`0?(8+EXvxIo}O;3%?H^Z~SH4bo~-7~?A2=nUidHMK@ zdRcMIE!X{#l%bJ(JQ}`w}({2>r~8%q5xNz?=pFESt$99p?9rnjCq&ZYW=a_bw8Q-uU@eoyatz=N-jQo^KUUUHShml8 z{AaKnO{H~u(Lh+9H6`)Zb5Q7^2z5fpA+GslwrotlP1j}Z7)nFVX;wum$=(l(jz5uu zy;(RzFon$^pDY2Z5SOWwbn_Sb&2hVZ%dX+R%DuIx&u}>rEv4U7Dxw=OIvy~(s0ey~ z4sfl@ObS(fqbQ*S*uT(wSO&3;nan>qtLi@)2(;NSBoTqPKfKVi0REXtOJ@1MvYwPp!V}lmS&Wkd$(H~*OY@0rC<;_1H{n{2PF3^;FBCpr=*e{bLI=C6M;lSKMy*zg3a;g_sZUO9GK>kbTG-PsiL{&^+isMU72!^Oy<#0IaEN|;Ypvq7m$wN?I?jM^ zE*-P5ym#RT;c6Z2ENplRHs)KAFW?2Wvo#=GE!6>{dk#75QLh?Rug^ZeOQ*79o26`$ zGW;XwUtMxZz62BnR$2J1x;7?`E@Ig|_g?Simm24Ls>}_vM+n?O&9U zs;yAMDT$_3f}40deC=yiets6w=fO;zmi$-o&tYxQyiqWq+u4+QYb!HLhFV}PE-xo3 zPog&14o^a$VmlBmue^K~Df+O=Q7m4ZG^6&4F2df01_=`NI_2y@c)=bknkyR?p+KQf zNVot61GmQ@WWs@UWZL}LOtJf%JH0&a^>&nHjQ_n!)AJb81*~m)vccoY0eV@Nz#Ur< zU6zO;8|fl{ZT(XhvV&SE=EmX)Nx11PijPr+B6vyke;$QZ^r1bA4$OHS&x%1rIP|R8 z!$Vj6<83xuywzUgsjW7%r!#87?L*yr~S!Z&$_+;Yy>&=9yHE&Y(wQP!db zkmwX7Ln<%|?=*+@_VxnsMgRF#TwL7JveZ;hNXlv81*lbw%u?Z`KLzJO|36w-X$ zxUX#K*;6#I*^g0n_T|uN>hWOmh2@j~zH9c!Ub2`i&4d7j8a7Ojs`uD;0bvkiiG=ZX zCth>(f>*Mj@BK!_Yl|d)bB*xnKD@5UgEbB@Jdj__QJ&l7KZW$hUhz*5t-YavFNUx- zuX8gzpIgYyi2@aKu4NxGul7OVhqe7XSON0+f?bFR1DVzTX#u#Kjf<`=2BXzHTW@-J zL29z9Xx_}PVubsK`Ht2+7bZtztmRXTBkbI0Y7~#z7sc^fbu^P!(*@I-NsNJr&^`5m znZPrJ7Ms3MpBkGbFH+>jwjjT- zQQ#TTbP}tdW{7U-XOJ)4ME~ZB;d}*jKR(!ruo%A1bNAxR%7;sk-J95We0Z<=PmWM@ zsal#?!k*7_<$5Ot%GM-v1ZHB_iT9Y8B#U*7E1tQnJzr`uSQRDqOv{zfyn~r6z-}1qEE`vJKa@X^)>Pf2*WFTK0cmTgOqQ2ywM4HT8~M3tB>~ z8E&hDN`p@>OSnwvHQL<@!~{S{q0O% zRuO9AX$0OfU`+%H*0s2==hEmAn$tJKU^3Ag4f=Xqffb;uH_!mfu70YbVb86u@7n14 zB;3D}2J1CytH-7ZV*I~7>IH(fU}V2pm0ax1q?$TwYX<%%A09curxQjEaHyW>wVq~0X}zb&S2@qc6cdu$&%o*H)G$TD($ z1i(BU3JGKl@>75LV%XL+{_eN;?)b==Thw2HNr2I_1POx?R;}!@GJYqpvY7UP0ddsY z*&A1hx-Dvlbo(q*s|&k-eBY*nKU$3Uc%+J9f;x?#^ZAlpu$otoGt^o?Bm>$7O-)k2 z(9oxVmH5>=74Jszl>#Lj+&x8oh~DP6KC^ru=^AF}8t9u}thGJeuPv+^IYX;G$ljRx- z$5fp7-)C(!)2e?&ut?cx1~p`Pc2<;uV|{1QUWpI^;K>O{HO~Ut#LRtrG4u@`-56r^ zgX=4HyWe%USV_-$$ENdKEu&KSjrAzqnO}{@1`U77{5OeqL2n6*eiYSyw(dSLELavc zCk~f;bn$h}wdl9UD6x8XqehTrNF$RRYpm3+sj8(b;WLO6!*>n6Y%91thFtCy02!WaQuwy=87}f3V=mmB2HdSjG?|^McxJaUPF>krCR>p9YVgld`jq z%kh(IA7O6sXefU#vMTDx=^E9MY+U1~EM)i-Jc1y}D|UN$lb=`iIAA$AEKLfIR>R$( zZ$NVuYPB6byI-2{`nByU1!t-9I^{u8vW?;Sj+nC$=#^)6Y&qx)!-><}o1@Fl4C!iZ zdU(U^jsl7j`uiRt+LO4M8cCJm?u9n}ZnBg?Qv)CFt4`@Bi6FXLpUXP|l6x@M z;mx{8kp^_+66^VT47!rROy*dr*lUbV865Fk3fvNa)QtWuf51;)J=R8FS4;={I2Rxbm9X6^^lTD&>&O`O^)ygk2 zJLW;EClL^0JJIT|^r2h~jfRR1aOE{S#=PDSW089oO2%6p>t)s2`~YZ9p5K3K(eAVr z#d7hRg%THLw-NzU`P$|e7h>kMh>JfT#uX^)JiYp?=$HSvlKT2+*(b#~N zgV7uJ0t){5TF|~@W`F%HXkqcVt+lEg%EA}(BKuvGi|<~*A{i+3EX`EvR8ZE1Y<2E* zJqB%-mwL0zTJhy?Ez5r^Cn}SVs;Jyr_RH@fA3aRREzggCA!d|0BHl%5gEy;YKS-PX zpr~>?_yDS|hWF{Kq~;h$k&oQU>bBF-xhv%ImtF&td_EPEG|UpUq=%G+vXrLeTbs;Z zIUJ;OLYo^FPO#r&@S2dirD#pP=8k3mVPX5fu(QQ`A%}B~x8vC@@?9H@ad>ryFLwUJ z9cGp_iUtam44XspPriZL!NY;Rv2jP$a9XrLc|O?!U)w^<6s$NVl`clPo*uEEvbU@r zntOIx-rla96tt2dz$U7wr<5-L(=lfCf6*lQdl)owdyj6!EZruJ#v8!~A1VkBntSr+ z_|Ivyi204m8g_| zQPaD>uDkzgD79`M*Y0M|r`;=w0DrqB;1`O9g+WWYkKJE8%xv5WfOh;+ULXU_Q7A~l zECZdWp$5Ejrp)k5<4-@s6V~eo^;%zrm}@Ofm+Q7W9r$ci{UMAc8uMjGr z?O6wGi%gp9v8{n+{y2VJ%Qbb6L9jnHMq#=*)xjL_G4oS!Ai>}o)7_FCTaYxWoIz$D2loz07BXju4p4IP0mDZUVB2|l{|l&GY>tA8v0Si! z%xl3}z(1~SnBldthd~ydg zAeylxjQva2vyt7W=+t=j=#H_)4KxI3(g3{|2p6n*QOiBbrJ$nERDsMd%~Np3%|hnk z$lhUtEdKBMS7dNLkc6=k3OTs8)@5)Nqb`S1^2ll`jH`qsjMv6;-THoIA`}NEYgZ~% zOm4R8MI5Y;o=BJh?H}dPP$6wX-q_XIP9&i0vvg{tfm!;1Pp zvUCMxz<3~l<6g^-;x}N{!UO=E;e*)#I9RY}^3s%J6P=trhK^|ry<$LE7egmH@Rivo zG!ILIJ)iip6kZkmFGwPq+Um!2DM@pdC|cm158xo}Mf=CSAr5f7P{n&qAPHxBj&0EL zai-zji}}inwjhdisfesby+5;^%C>{6e5Ail)e>`v^(t+qe=FrM_yb@hFi|i$h^&Ec zjDq>Vx^?yJM;QS~=4189n*8Lo9!JvVO*&iuE@62n^1Hj}n8n@gB=NCF`N;?M6Zg0F z>8c`4MOMyPenk<<;R8R+)dYJQPpt!_6lx#XVoqd@ZNu+h_uY2lV%cV^eShR}IIwb(Jf%t}kaP_XAMpk!qG=lgE;hs&peG5YT%2Q_*JLbwquNq^Zr&8A5v7q-;YqOVZe!02y z*M;I|-=9j26}nJe0ppc`Se}>3(45^#jrj|KpaLDH@nhK~kEa+$Zq&WmfSzd?DpkQy zx|reWv9k*5rkSzJ3!a0jPlAn2&Y|G;go`rJKTiVQLH1ry)N1V5*ffYO5Ca)U-Vm*u z-U;G_EL`%t_q^Rn1XYf)O4(o|QaUDaON(IOvB6>nbNH?yBndleBHwPXnc<#8M5CtJ zpiGmgp-Tw9L-4mxA5e@wa?LhABmTUIqM0qdIWnrJ$B%UMHOYjbDm<6aPSl>Z{Pu`3 z<1;AdSS7!n<*FN{Q!M$ERi#hySyVavry4Q3bdaYNYRy}ZE^1O`12JHpQtJ5j!jQX_Y4dS1hfAw2EWwA z75e$WXzY$eK5!JZXi)HdBhw|tzN(Gkf$J4^?_S`eYPd4L8=I&}2z+G8y>OYmXjz z=*U?8yUH6yGnGSds&X|X|NVFuf5X!RF_=g!+!f;NM{rZ<9@e(Sr8vMTm($Xs;~>I|PKOP-C zHGheZh(Az~j0l+uyv{@HS$m?M;u+unprC@Sc-$k4u1a!Fac9yTyjW~M)0 zBkOCpYF~3mh+w`xK-~XPuQdCHgHuS(9f~#&(Y>ADiCeHF@PLH(Y6F~e7V_P*Yn9J8 zWU>1ygbDuYOiiMVZU=cL|2yXh?9D3%(mRY3+kugR0KbG^#$5lD-aCH1^QkiCA>JF> zOq2G6euxP+ZZINjJ$+D*6v278@-eohsI2A~4^*1Ctt!*ooL4mTmj8&*sx`Aj;@!l_ zewa|5;Lqg;=~Nw34PDV={+IBYym`9x4x8TfXURsA4BepWkn@A<+Xh3&vQ{S3uC;dP z_SpoMpK=Afn04*KRiIHX_Po51K5u$z%63Dcpc41bX%C{#a%Ll5OZH2?Hy-C_Q|1FA z#D+Gg7CV+8XYbai)c5opOYm}LFxr+A=22q&Uv6W|fK^^kjMv&4?6*I+`SHnWdbTJ_7%EZ*k!#vug-yI1(iK9{YPi`KVYK^z0%?d z-Mqt_gg{ILU%Z6jfTvkx#K{l{bidVwU4zBuO581aY|_&dceZ$JLkA zw=_`Um9Se{i}f_3HBG;JG-DkoJ79~!B&Sy`Pp6&xFFs+*^BC>rV~{_^P+Ijqv5HMQ z=shnvpAz@}ySc;Z)ZgY7<@$*8bN7>A7Lt{5?4xZUY#KK43cho5bw!< z?T)C=sjKUSq8w6)6mw}~V!J2w-lmcbDhM=u+wZ2@Ol^AGx;>6R(u(Hb*Zsj)jE^4u zw_t#kD+ZK0{lxb^DkK&B1412(%uhV&do9;d(yISd*6K18Oz{y3@*_0mx#N($;LS{@ zQ;jaWK9}!VJ4;8F?H%Bk&=sn)q+A@h{&WI0U2}5mxD`PUiZCT5*HjxDLbirfm>Q1nZ24!{qLh0fzL(v(=gs^e(69WT z=CGRcjilc;-!2C0ZTkP=#Gz{M5jq|e{ZCxz;$2W1RRtoaac4it!@-F(@m`tw)NOju2{=y%l(GE@I%0)<7DXE za<826a^K)e!hB(iv%})yO51@C#}(>#u5S$`i(be*F!ox}Mz8R7rQO!h_w!#VV_?tF zweGLwz8Aa9XrLdTKN4!Hs!LE*ebG(m!x^0+6{_yKnYZ_&DuM z?3WgM9RBvrTl8Y1bgK7qV16;-!cL?gX7SRQKg*tP;XDhEQKS7RB80aXGl*=c-(2`| z;f?%~c9`Ylrxw&cIZ?_eTVv`B3877BPDly<(*5uHT3irl9Io+HX3!#>;aj>n2N-{F z9HO+Z%9<~=+lQ#Gu6~IX)~h-+1^h@$o7&kRuOz*et37iiO9@ysZDM4&xK`ZQDr-o> z7$oV^U8l0e=xIV9SJ#)*OX{43g?L?eMA6fG6g%FiUH8o95c{mWZDCK<8kfgZU06s* zo#IkH(?tSnFN^=3BbEtt6coEV1RK*#pKZq=5Xch}uxHoLNuQ$G>k^Vh(TBno*+S+My=)GeBlBYN zr>x&4b7pNXo|0)ZeLdVguGYN=-PfoId=9GyYaB6Cvfav^&HJI2Az#@7+7)sX6%^66>EK!eR!x z=!3=DUOK1c-4OGm4(TUbhVI8GWrytMqUqjrw^`Sl)m|nsV;Bq#L|MMK$aG-XV_aiQluG&LS`DJzupn{*-ld-PyoNAc zmeXdYgb$Fq!F;xdaao-W=VysxzGe$Lb-_xNwjpk~p_HGcR`OhscCWOaT1{>&o9tHr zi}6S6lC^$O!L0!mx!j*n46OM;L>@|1Dr9s&0Rq3zaJbx9sS0sXa#D(K)NQC zdlz3c&%*w5)s42fOnz3|#mFlz4dRqY(RG@9jZ{SOLKSO^Ckx7M()u~!Rf-xzMt?|e z;li;>{mb8-sgUuK%Ou@S-i~O&3 z9JF{APW0p_eq0Nc)-}AmEOUf3`TV)oI zF!a!yhJ{$9z;~~b_K+LO2Tv3L5;YL^|kUE+mPQ+kcsB5Ap}c^9w#X8=vKTj|>!yP5~KP^D(b*#>g~c|6%*(%+dT6G2$D4c0x_&qy78%yNUv*@#IBx7b7?b=7 z8O2X6a@6^3%#F{~t%pd-YuK{I92WOeceq#My-!6EVQ)3Wd4zj%)9AqbPnqoSsaf zSOz{$$JiQEl|dGdH#FPO|PU zY>t~J4fA-@M4_+s_abS`A5UAHrJ*VH=u#vLrTH6IQ8nOoA4j-kl}g!ZMz=YwEa-=0 z@aql%k2F{B9hfKmc0kOY!LJ`eKfb^^P7#5xS$!++_2e7#L-Td}=dpM)pl!fr4vaph zwaY_@5Ms+!S3cWMfPvj;=N3BR=B1w}zD-i|sY~ta%|x#?pCm+J3KqE_tEeZWkaNLM zIy6v^**kL?Rq?#7JlMp9B*$N#Az!!57YWe;xg2 zFjUk7jQBnFxLy_PPUa7BURe`p7V>632*-_tNNh zyvV^gq$)R5#>wrpCT*b5EZ1%%u2b4v1Z?<#+|}R?<@vsl-`gn>BXbj6rs+nM_D*_N ztPh;zKHvYu-c(4j>D`{bz1~;~{>M3Iq}(=4U24qI`a*gKWNCzifLc$xaGp}k*|j&3 zWqeI_Nlbe?d$P`7T%>FC-2I_j8g=1(QdOu=eFr9Www)S>jaL-Tm5G0Tk`QBOXjN-y zPMzxyQ9?+KgRl3}*9?l1vD3sIb^{O35kV7iX};2?y+E`WWz&6z{3au1FRE8m`w$vv zx8GQR+z$mKfmNqBmr!xP#7ZiQ+qA%_KRyFSw|ybPo3v`eq2 zkJ1_Hkys8e3mWQU4)C3P;;EpXurr%9wi{^WA<9jHg#@0zeDchO#uz3J z4X)Ri`e-UvAQJYq!Zi5b56pVbm4bG1A+`#M9mqevx*_Gnn{G!F<=eH=)!TDi&C9u& z=x27_vK(lL;Zr1ZS4+wlzxKCda8WI{Y?IFDd}gsXUX&YaS3q5edTKu4Z^tpMNcfbZ zN>AX~>uNRZTia4~0jOH7LxWCL9z3P?&fImY>{92@h-vQr5%q#iDk}v|)pMJCl!vO% zuf4#hpw`gZ{A8=qAIR>Bt5;xTE;7t01=Ikyd|?xu%&Ukf$*?2W&@&hrQrI@CQ;DMc zgrZxap;?ZfDDGcc5N#?Qgs{=kkS!3$s3_Y6)u8;QMp`aq*XNS!UJfVswL)lI5Y1`j zsFy`_+e- zD7FPy3dmz%aXPQWDyG7nlWZQJoW_aOj2$xnJI!NCD0Adixg9YeeqFF*c>4jxN|YEi zuu)I1PghH{xKxbbEL*AV=U8|G`eHXYhPU5Z=cQ~lQti>%ha|J}G)xgeDc{zHXBH6G2XoqnD_ z`J-uC?W9E3r-n;N2DYIf+-_;41A!c@x$vVt+{VD%AdrdIq|S>XXI@PCW(cbC8{VSw z_{oUfMY+=y(k!tmyJ>43whB^~yT*=u9B{2hxl~qGP+EX4Tg)R6=&jxTKc-Rq$Sw-G z(GZc%g_d=hhwQMf;q~OKyda6HUvRN%p(J*Gij>ufIIV9t@~mP-_6KzR41P|=V#~V)xHB+VRr)S??MUZd?K68z>`VGC3o&}z}B?!nRHU1 zHI*q9s1jf_4C$eZ0AIME6en;&&Eo=a{11e%n$6r#rV|y z=iXAtqBAOTF^z789K<_>uIo7*K<48cgtka6$eXUR1eS8ZDFgF+r7E@%PV^FWd75w< z8XX6*g4wvE3k->tJ(TRe+-!i^FVHKR)mrKHzT<`G`ujxzTl^LX%Z+q&@;h@y(XQ#-74uCwLa4vpVuQiAYxGCkVt9c=!Hh^@F+d zR5p5K`0sSG1>M?Afy=lm%f=k{tr-ZzmX83vs^eFdI?<_h1f>e5M9 zq{vTe4A7^4vJW+O5#_rNo71`i&HLa#WiAa^92yqh#&|vdkEyQ?i|YHn9z;bHK|&BI zr4f*B1{6j*q#IPayHk-?kdg-JZWy|xTS=LrBqfGs5QcaUe!jowdH;e3X6`-roW0jx zd+mMB?e4u`eV*3lqV7Wdo+d#CVP}e@u2aLM`)$+J8onv_s{D6Lm=D`xS0K+v<3_F(?;?-q|VR zbFx}hA3cXwZ0D>K_0QC)i)h#T3S?~z$H*wa9`lIO5I|ngOs2A2HXl4DZ_Ubi&1}N0 z<2pfbMn8Bd^*7@Ei15VQVWqr79fVus`H?Au9!-1^&hu~EhO{iS_e#-kSk^A00CsLV zxY9sPtWa9;F>Z#(VV}min^o=HcL97N?9R#tYL{iz{0br!WjKG)!6U!QnKQ+Va9#OE zp~h@ev=kv1-H7R)ckD~N$8yaxHBw`h^}Zw!$U?umXaw{7*<#(ZGGBrEGp+Q5XayZ+ z6SphenP6)w#zO^Dwl{8B`+-w3D~U>RvRT8r-}V>zx1N=N#*LG zLJ-T|Do=LP>n3k$_Y*XAm^Tb;t3^C8tIN0e;{|eiLD}$7&Y%PCqh?X{fex@Vd z=du;JC$F>6*k`d^I%z(v?d1Yr+p#p|VxB%jTFYH)I$GR062fsCve+-_!1`;_7WaJN zj^F=s+saRyp!;eW#O9{+2*xcy80R+J02vuX^fxBdQqc@q%b*0sc?dnJt)v%jLTb;I zm=9cgo_W->gx~UY@j82|g)7bXAdpB^xte?wsg=QQUqI;JO6+f|XA6dPXOL%5IHqov z+Vk$LWXiG8Y;mfbR<{jQH_T5BDw>m?ar2Xsc}ylJ87Q6iy~}PsfuFiuxz+O$WOT-j zp)M66*LRLCdu&&3Gv&Xpc`**&oHp#V1AYM)Qy+EseGh{Qvo)1Jg-vgrZzFWBU5XsO zFu5?rA>nuNoy(E9KTgeI8GYaU&;}$L%4}{{tY~6GzX+hWva8umildrEO0xMI&Mlh0 z;omoQc2)xz`R3WP?^%*V6dc(rj+N;8jRt2$f8Vb%enn#}otv|JU7CY|r8-SuMt%jY z4Wb(C!vS{t#sAuZX7hfn>RXZHe_n)*ZEI6fESV^6HE_C+a(1+-H=Qzb2FD?g#K<7c-4nrf_qE@JHl?3xv|cpYH_VwErHrRp z?uO@Y$!KLQ)DWYC{zOS>Gi=OGOX<++;MVSn-IdMNue4KZynmGG{`@9ZA|DqA2Rn9e z|2UhF!L`bPhzae>Jxrjs74upOW5UniZdbt|6ea+HP+o_@+|ZMfcE2zPW_kLD?z?lo z;7El@6|(I_d+QSR{z zSX19b7&$a^X7^+Djx1Orl}0yQ5HmL&3ceztGYoU_i#9DD+YB)adV&*AX3ID$XZW5< z4(sjy7cCd>)P2~*aD3YXYC947wIT$=7M-di9UpOIvTkN->_#C{gAFlLniH@eNN0`@ z2l7{B+c=!^rx{X__(g&|{fW7y9WuSWH6F@ai82vTsqCy790Mv2XNkM<5&w6{#fQ8$ zZSPs_+g}||W&ntp?_t`>d_Ry@4WkfbpY>$6a2AdLN7 zc`?2)-BURA#lj?X)@G}^TygY?k_hniwpo6?%`eLIX0>fMBGVot&If1y?iMfxj0oEs zsiAAh`!1*jNSv`DYg6fFX#6|9_WvT%;rA0u?;604sxN+?A~UIP{cuYQYY|yy`}^Im ziM!;gm4Afs8CW(~%8d|&9R$*2Vz}d&M*b%OdH&syd4p}b)#Y-`;^i#AnhH|(`a#H& zLSy#Fr?A4C(S*$F79iua!Tn1b(^%VnbyP5 z(9qOEoNX)AMb6uc44cPX)+V)ynE3{?l1Pnb9UVYw^^u{Z_5-`IclE{o;z4XesUe~;XacMb7rSInDMcb)`{!EgJ>6M1(mh((MV+4F9#0e z^{%VQVy)@vX#$!Pg_C%YnFMRtgyE@_ZhURnf)nrDdB){@ZiZ|~b>FAqF;g0I6U%6G z*@zL|Oy=>7=xYn_j!7ovUh!d>#Y^-8CZtPy&^b{Vw95p?T;zP+#3C#V9`Y!ECg8o~ z@9L@nbxS;We-jdL`au7JwK%4$bxZJ$f@T}uI^77lJ`#*0o-m+sZB5n2@Md*+J7fg2XwBTzM=`CQH3wwz(Y+3X8VXXD_RXChSdg zHH*Z??>wKxj;M9p_LG3+5w(g}3ru}@<-N2}yh{LqsO?4h{ecJCa;;_!abFNHJWJu& zL>xd79rU<{W+rA3+@yPGP}M(5;E66#u`w_^TROWi*?Mmouw6_5$#`VFm>Piesh zDsa@f>#>iRKW2|bjjzJtoi+jOSzzdMmT08|(r6~Qzf-|*}pH;u1Rn>S!#7@ zdd*z$FAK5d8c5r!@%OBJ9{GCfeMw@zf~|9xiRh2hqC6mBG%F%$tGq6KF43raWY2TP z^NGXM#r*Yz-a3i>UBu2!G|37gMy<@Tpeph*N>@;xe|Ko|=Jp(xXdnUoo};`a!Cp_R z4elP?i~R85M1!UUN;Jc>8tXU?S5d-pgw0alfxO{o(9>0z#iM!}3;L|O zw@|BaO<{(ZFgpL0Uk;^91Qo-2VuB^I$_rgG!Dl`Coh8AJ^_ZyowaCxv35?U}B63IH zfhBT)xalPHSgS-btr}lw{0^gwB8)azhV_XrX4AZS<}w#ND`oi?mS8ihte~Z}D4q~f zw$CXsO!{^Ak5+ULtv;*au5|HovTSU@UGFM+%&AA$?L|hCOLm}kQWa(b^yGOlFnnbR z_iD-N&{-qv+r?PX-$GY5J0umWMD)7!V-rK;Vx@jfjF5bo{L)|`5SiR*h`bjj)10dz znJ2UOZh%DS2x;!?dy!q4HNwJplEjic_UTp^mQ_XzdhOs~ zb>!FW?;k?D-u3I|f>%trx#8PB6tgtmam<#fsR74N!j3-Z-z6Wbd$U{+X3k_i)~Jaw zzuE`sf`Yo6qEO2D)~_+~1SM4hz8dx8JsdlOs`z%p36I1dyU#ahm=G;@(zfIL%ZS1o zwKHA~ZAevxkIXy~N{ejO5yl&^VY{L`rgK+)|Hw z)qKg{S=;7)>D-0r#L%ZiF&viGqxptp(KG`c=I-mX@`MfXR<`D!TE6}CH%fWWd@U#u z>6Rg>U68-D+uTrto6KtwAQRQ4O_F2G{+$#0@%vlCur&ke({e3}|%rMi3 z83^UBghuv!qcN-0uCdc@vf+-s4@$cAPC*S7rz?sKvcf43qb=y)#Ybzu+xIA*(bv~c zjvlj3xGzwJ-r>(XaYkoPA9Z0jGa= zZ=|W?wG50q>65d=U9)Bv5h_n8RkZc!<6R&BANogMik@a)b>D3=mml4Yf*yh)a9RB^ zW9CeH9Fx^IzWIpL^5T1vcbNLKriwQS(Z5RG5iP{qVFup~qX@ugz<`Sjuv?>1wRU$M zC??LTAIAs|%CJ>>h!TadJ%7%!Ow6~Oi;S%%qtQS6ev=>arbh!~7cS~A$1tC~o{Dq5 zh7E~3goUWunBwB~Nn@T3-F%BCi$}R};)W=6x=8Nd2S$6~)TeBGEJ(m_4b`-nGA0~l z@835Mp4jGB>zE4=y<(%zaN2;cm-5G)o$EiUV!CXSue%rYfg8*~0GH<%_$zw1dWtYL zP7*o#STYVJR}G0#FI)i@pHiN>7QXkv^s#Va4FT8PsOyJe8+3U-f1`D+<4`bKj$s*P zi}VifUjAvJPnAZWXBk!1+B$!jeFp-mHd8rylB@7i^3W{U?muy(BXHeOG+L(1OW8*6 zL*{G7*d((J_}~=cV)9Uo`3V4&BFQq9#`};&Q(SwA?XJ0d_bcu0GW`H&*B+mrZh!x- zVoDDOi=^gVVt&%)$eOF@RAXcKOo5lRKBCwdxs0|>{in`H^e#5Z=IQsCn=Hlo5F zgeGj%MuuX!kxFA0$3%m=-eQ<^#|MVvla6ijvDNCk`?c(h`ojb69C0@xi@$1x?cWiO zPa=~RR4#>2yi97}veVz5Ho=k1?4A0xyw>igp(j@yIR3l8OB|%Bvv)4g_E&IW{_y!+ z4R-0+Pb)LIMwnV7k$M7URM)I#Oy7{b9f+))0*rcZNQ9d|?mi+;AN(F;laIY961=a$ z!FXw|mihY6rOvgt?P)%rHePQso@hKQJMGY6_Z9fk{2!sTYtUDs`mX-CgXt!|5E z@#F+{%fwc#SBvH|RJ|z84*QhX`OW2FtROqK(3_B<5`>A_4L$dHKKIe^&<1bK(KZuJ zBel^-8inHpZ!$gq0=bcH$aZ?O_Pbr?EL#NQyYN3%F}gZK&x_Vww|92pBur1lQa-LlC}n<;q34Yb>ZYS{y9*x+R-f!Rt!Qn%T#Dc*$_7C$K-yd{~k z5)Mt8oGvNwGc4#44&!Su8-dz~XF zq$HX($Atm{d2`om;^|Rkb)A~sR|a|_65Q~cun)9^4Tpasy^2CZw|DGr2NL1Q3C=1Z z-v>OB@^p8XhH98>%|o;GBg1dLlAtAwPV5U5X`vziT4St*#EJgxX6NptGYw>cu8S^= z>~o;Sd~Gd`^xDZ1UX&8^H@dC~+u1d#mWR69_WC`&2~&vvHnGWP>eZxKX%{ypn-7|Yera-7h&p1{vNvoDLZ@4vNi}_ihf&p94za`#; za1IiAQ+MroPUp+?5@)@t{EzboY>cPT`{Lge&y@EsYx?vT05}1pI6)#N^D(U zV^9{)Fvj=Pugm%=b(B^yEo~Z=F@jf-ijc7MumEkWN7vY=lb@H0Em<46!0j!- z3ka}U^#SwI);(#ZI@6Qj!TRorwz#9VxWl{^K9-2M&H-G#eyl^y6QbaYHYNF{-%fob z(aBPNF9fu;{h6?&4*5tSC5Iqa8crhF`bK zNCZ_%|8QWSXx8*_G%5IOAJKArd~BZX+4FpkdUG70MC|w-8K_0Lj66^2%~>jkS1?Wd zId1{}C#-ry%1OF%#L9zA;tO}RF-G`SxWU9$^)+eo>jOwrLQ5RGt^RU(?qpGy^w3aP zcI6vR%SWcx!ac#S9@@Ps2mImiEI#L;pn5+t0ZkZ<4R%99J}x-f14cFaY6l z?O|u$BHSiZwKb=v(C%(%eu2^k)K?k7Bn7J8)FWud&|Qwn5KIMF#{d=zVcUqlcV>?J z4${$=q@<)|r21)X74?rIdnqQ1p2mFc%^we6(cX?{XnJF}>&27ON=yNoqr7K9m1mou zf2!O7QzaZO&Y(T?f4u-g+;WKY0t+_&T9ggRj1ta^;w8`GY9oaz(K@w0G_}C?D$r&) zah+4>%x90KuUf~>XeNF>W!Ej1@zT-D!6k&WC%~l}-*|u0hf8*ZJ#=aad(!0NZCbbA zLjygdB8*0IM4XRID_y(<)WeGi0%6i{oO$4H=R?c?SV~8aHn(C12jKR|{)0x`%A z$Y0reLVWMC1b+w9FBGP2IlD7&WS=A&Kt{a+E9EkK4&V3@`v?=j57}9mi>OT^Y1CvO|Iz?-W~)ssFRQ<94TvLVfDfE}@b`YLMGr2|fAo&=Fzpwgo(9H2 zQCu8VD7hz21W*#f$&!PD+f489`&BO2QQ3i_mlA%P)KI@?FH=1IMXp*b%J>eX{o`yT zW|zGr`EX(SQ{hwFlj?X|jvt8~Zr%}mer}Kep`*EnG-Z+7eQ+rq1|4RS2~cDrN@2D1 zjUyX9()i#5zYT=Mr#ZHS(bj2Ms4V{hnX!2Ebo1QCJ`f!ObevVk-1t*8KoPHf`)3(u z*~`gZInp&+=vo@zYm1G6WAqMfC5Z?zfNgzRco*CN#$mGJ4m@jSGZfj zh7ED3vS~H*IrysMt4M{z$e4rQo>^Hat4(#(g)Z|sYi8kI>aKbZRUP5|K+En5M1P}G zAJ)Ax4>#O8*L4HulakO!2dWOoGcR^Djz6hP)}U<++*qG<`gfe3Ux0Jt3$qi+05T)( zJ}|^U0zB1dIP-b$0-5AtExXqx1A%${$#+yu3(Vip<+spN+NzSzg1tWaUpKM+9Qt#F z0jo-_`J1x&fD;Q_0x9b}DozCfbIpkttFDuQKp_!ECV_J8H3rvfwZ&e;a*BeQq`~};N-7t zrxSg+;t3PAzfCia-W@DGff3QFD%|(jnw6t&*xWZXKb}ZSO)tF(GimdxN>1tcEToLw z+S=l1{hK9>=tf9zQ`|X^wV0S!avJ0vmwGZk23y%k+bc6xJ2JL+Z1L!#{lqO&2Q*WA z^BI}>d!aI3;uW)&_<1>o>jomnMVg2!Ux6thAxy$ra-r{^eXr7Twnw!P`;I@hBja%S zE9dHsW^3D(R?<7_-4Aes@3Uv(h-E|xj#dgaePDK_8i7tmLBD~DooSMSE|rtHh()k* zaH!9f%VAV($b9yk^Vd+y*|G>7?%L#DOYe>^iNE)a-`RbEds|G$KI-9k6{x{g{LU}c z%9ly6E>gFp3|01Iir2Z(X|D~=l8vuF)T&%=tpeJsOjScBp zm$oSgG_A*&Pf2u4((BS_mFds@($ZgZd^_UyN<+vM|d$n$|OnKGkN|IOU zn!P<*KO+-A;>aK`lH9Qi>ltZ(w{p;07iJ-EMUZ>eW7esF4f7J9jdK#k7riRw20{NHTEAbM_hV#L6% zIXbd=N3a|Ft$RPvd&t%ACDXQkDs_A&^fSmS(C@L@3|rnW52fom^v&Wv-g$qh3zLXl zgM6X7dI%?2>9UgT`||*+A&}Rp-)el1H#J=U2iCwrE^j$W!eH`O`hRBcZIZX&d@aje_f?1V zJ|sYLt7jSonlyC)8ZSTLWVy)=XbchgYQ*Lbygn0cSw4GUW3$Npdi|lo`tsbE%_k7Z zk8on2t2Ew2Zd9P-9ZT=|iSHB1*pMHRpfJ0Q)IGCvSRJLrX(2qeQI;Jd7(?rOJ`nPV z=de0=z=E{a(4msDJvs(u?bt8P{w7s-WL!usI{QBqKKVd;Qo`QfR2g`(z+o3zB@N!j z(+J9-FO>nK;ml^NBy1%0RP)wfi4+#g^ZF8Id$e{D>tR+}1}as~>}ikn1>mhCOqP)L zA(r6VZ9&7yPWje*r$q^e`-#|0%(hXBRGk*LU>}Fyft5%{I$q`4E;8rX)}@!Jx`IV! z-1*m2eg3KD{vvF=u==Liqxc_**f}UHivk=FXQQLx>x!{7ED(s8`m=|i5#(0Z%zl0K zRjL1F=UMrM(P#VF(p~=f*8~82$;$LSfm0|w8bd>B!%uvDcDDVuP)oq6wwl_u1^*jm*EE)TBH{x#o z$-i`(NJAc;Vn7}6JFXm3{pGPb8|mazVDH*0^o5aFvCbdH(6}nNV^|L$i~eXL>tY#%>PUEAIG7 zA>3FsR=q(3C2Q_jnJ{lrt_m3y8YKVLehziV_B(vy_0}Zwss!FoH4YE%^-w-}iT~q| zep~ofvj?!Vm|>G9CfTv^mQ>IPVz)4L$N@5WN}Ts$Pr#D}5NC?$rNo=Z6Sh0&fU;e{ za$RL*8UEYnA1AOV;WS)bS{KI@hK3Gc$B>`JzCi8KXi}3o(tX?=yFrT}gFBVX3g<#~ z`|KdmE((h_2PH&m9n*vbIqV^Kh^o>QIv8syyhxhydMk7urq1x?)b^A^aGx?H1rVw`jEEHR-@1I3wi z?nkfsY>{JOTZTzbGxk%CnjDgmaP?Kc+Jyz*6UXCS^a*G(=E?Z@%A{$dgKr-s4XWDQ z0>Dh8SlX=R za}AceSG2auKUCg(6wZ%)H+LV}qeCgE!AvTrM4seFwNg%vtMWbIt}$yTi8kj*T4;L- z#pMimEAys)nmoX=;MS0hnn*Rp-{ZCDV`{p)$uZM8V&C*ln4<)4tPfxj;%?@6P)dMx zwXw>Wx#t}p#?JaqYCyNNEHWyaglqGqGsipK_tzfTNEZ_ac+DQCi^%%12m%`vb;TY| z&E0w?r!vs;(d+xV-g}M(0@lEQFL0b^nTZGSR(iN*lj&*#I`%Dv6+e!)m4d zyLR$R+lk}sW6DfB6Q;b&c%M_F^WG2+u%+YmYX9tlYkD=6hO!uaHhySdVBgCkza16c zi4zdbAe>YHb0vb$F-)RcFI>#8^z7>l;pK0AH8A2M?9EPpK&Q^4WWS+Sbx7_1VtK~F zBJO)a(GuutGsbP-blm{N#IU4G=FrxMD^=WHbhZS0%OzKaqP1_)Eo>xruVsN=+os|h z&OI{`eA#S?-`@iCH?QWIP+fP~rdz+J9p62tS;KxeF#>&e`8&xk{Vcox(60EP6t+~T zhjbD~|NIyZ=fS!T+wG13GE$uHmFxQqoiFL^Vu0%|y6dXM<_P~u0x55Vrh3SGSwTQJ zvQIDXXK{l4VsJXKVkBDgd4gKVq8`BVuS$h<09VHUbr{GDp3K?N;|wv5p&g&?SQp~z zGqUC@Z1qS4hIs-akTZX(zjsb_dKbq7-26ZEE15iHOtW<0x(5lc1ZnED<;5Q#*DmY9 z({o?fJD1W3>Y;hIwYapVb;5ffdOwmD^2*Y3TVg(>q){%(TU$N_mr3=h;WOr&_PZLc zYNDhBURhpVUMvZw5&nvG$1Q@Ot>CO->OKT=-CTC3^%WxBoG^`52V9^x ztOoqqY{VN`BG{Gs55JPo62pV|%~HQhr|Nd#t*+C|&AHSfb`%tB{3((-sE%ZUf?jY3 z>PJC!N9XcWw$Llpt&ygXDC~JU+tKQAEWcI|179=S6iXV{fAdDWnZ9W04!W1WKsF4E5w=pv8FTxoQY84PONs_ueQWjeNYnJ z{D^r0Z^ck!^iikbuO=M|G&RIyfhWJRqBPkRIom~~d^&lcUw#RIfi-3t?%uk?6o z380DZ-JWsOunNU)&*J0kW^_5TZOlEp52^0t^f0=~$$H&mc{77fY$Wq`|22=}2Pjbt zL&VdkU7blKl8ZnB>@`l#(XIE6Kzy^|P?Us%S^o5)1{yN(Qj8h|GHE#Xd&2;p|Jp__ zVPLBeBsOM{y->3H8*_|$Zvd{44i&_FIPJndS3Ft-6Z5$_BeJ=S8hk8Z6hzYEg>m{a z{K(yr%ewjJt><-B_|^8-JoST`R{z(M^@TUYL*%gOnlE>(i$+9MmuMfo%>Mgs zS?IcBi@0R1t~R>+qupF+?bW@|)GybT<73Vt)n+?{J%)bs`3+{0`lQT`jmq=FI{&i0 z6*IYQ1lUHmBVgmH7XP?_c%|IIuj^C+6({Pe>4wfROzQ*vEu5|(Ko7+^jePOme6lE# zgIbP@Z1xj2=C{&KiywqQnC+~HG!pjLMtHq_N}32m0|Ska;ON=I z-eTXnYO1JE{d=h;G+dxl-KT1?`u>jv)b_7d_20u}z}4|I8)*j5|O7A45(+!~-=XP+Hz4{dJ&24zzSJ~ zJA%coGo#e5MvzZM2BrJ=d)9&MVWcTwT?Q%WaQrMI6AQu=0mOTJtaMu2(FTTRvLIo2 zz2y4DbKKKi;rI<`#FU4z)l|w6waeyj5iPkt)^|LRbdWV-TFL+ zD~igVS1=RXA3eN-tAVjhIqJ`eiyNc!U%>Y zfhWv*;ou1FHn!2^wLmhkjkAheQ|T8_XJltvK00zZu2_}18=oCwGRLDn&^r0)c`HLB zi8o<6w&fr3Kv8UDP3g1w;}9_`90gno34aGjfd50uHM^WSnHRF{Ep)qbm^L*2rf=dp zY~(7}O_~ZvR2m^>wlVjH^SSwBoXJUfgXuw1>rEN2AFgE{^7j(Q@Dh_C*LG=i7VBK9 zbuyfo=^Cy zawB;#m=cPmbGay3fsiFcQJUp3s3=rb?qAkga};Dtj+I$gF>)D<-ADD~She)l7tzwH zs!D}uDo`bxj`G*m7~i&40LrATO+nxu)fn0VXhH>&JcZP}uX4ya-G>aP5diiA$7^2< zefgMw*oE?GwvvyW=A8Wq2e+$yi`^vR)-nE*mflBP?a*L5hJ|i^s z;h}RL`sr>=qTm>ub>HlX3211VBLG#k=ER2y^z<#8QjMl{uVByJpUJv;U*!FBd%ZHju=)H7dy!(J49YD<8=pwRWv0HTPH$uBPVJ$m=N zi-qjGMt2JPF5=NJ88O;*QbQGjsNd$)wIU1+Ku~2P1#KNL;89_;wgl@^d9I*|d&+M4 zv$)rh2+$w~>YDEZ;eP0|O%w5ANwFo0!zCKk__IUp_b>g03)A(vx(E)C*ExXf1CpWp z>3%C*^!2~EF3DtVhYU0?Wf?nMb{(BoALWVq3g8$2{^=c@bpi|-Xv>8je}lC&WQj6( zznu@r6xOjsVrwGME4dRFPpIuG=Pwq)-aSPjE0{qIWg^vT5IVVx9@_B(;84u-rSmUP zsRCcI!-Qr#m@;gu9T59xMvw4A;T+PQ$T2S?#du{e*M;vMbbHR zEja(wx}2$ky|Wkn}XkaG`td3!6t{f{DtC zr7K?IO<)#^!+2qgv=O!6KK9sAE#bAF6<8v3-+o8`r^rJhaWQqHIN)W#cq&DWyXtm%QEsLs}s<4%DzIlRxC zA>9rWog9*-kwZY^PL0W%M%`p&(>Rwit_wu zwO-8!v`39YJ;|7qjmkkqE_Y@ghu*TIiZQf>e~BOC&XtS!2aI@6$AA(*U{fvwDo1-` zV*`-FriC`3_W4d~&^W}bI87^z+l{GU1h4TiSSLmd5Xd}hyxYI#nH(JJvs`b#l!e>) zfWh`BJ5hg)iRbGL6*HNG%S+|~#D*x`h^GhZ>RNLnYKMq$-L{x1%;|fgKi)Oq?zMpY zsz`E=Vhui58Cx75Jm-+)a@^{Sd`^>-a`7ti9B6!$O)1H+SFjii^(evPpt_+b7^}>a zlOiv?JrYZAnW?VB*Jo>UsBj_6T$5SRg3*YQn|6|^V@}M%l!0$c^Zb*q^ zE^(~{F6s^0HjT@cSwf#+PfIlo*5i1g>B-SANu<_$5~HwFAzml<9<-lx7nc`Gw$n$$ z4J?19bZhhc*lECR0(&y&)Gjke=57rS%G-lyibzyO$FRAz4tdDMwn&CJyo&}CP`zDY ziM+2o1|L3CJw|_4_(9W_eqH#%Z%C?r90_*E)}NIaC}idCdey%^(5&!v6w=ApCbMN? z1$lvvYhSJsgY0)aN)k&-(_agCiqgMB@UEu4t7y7nr9ti;dDE%fDr|b_URFG;xV4a) zy0d9{`)QbvYt3MNrxViwM_lv?U`RSwb@0fA9CJ&lTV%eU@@xYWW)-ol~~ zQ9{?Nk@@M(O=Aj*%_rwUHBfrzM3ZgRQH_{R;b(3%e4sL}UQ(9+lw*mW&bH~`#{>t( zQ$pNgX?zNHgs!)&u9WvjJ6-R)qT8{iz>JReh!tZtA1!j^*t(~h{;&&|4Zb@psS%@n z8|o+Y25(0lEWep*=IcHaDFd5ea!G_+ZxTF2vF9SmSSv=`C}rwlM~D$#+TiN-E(^04 z&F#6me;-;Rsj6d5F+;Wl$@V*z?)#tF72OnN?c2(zDc+|wghYW5GE@@AJAoV@s&izSJfdgYKLgkM@Er%h`}`BISdZ z`P{^zd=6VEXB|qDso}Xy`JCv;JSS|AL&$~htI7%>tp^=gQemB|V&$qK=mEX7W@-9*W1QvLk1iR7D zXc~(&r#yUGgiaR@jtw0(w9}a+XLt2* zc|Uw5)_h1d61{kd8EFEDu9aC)ihm#MTJyJTNs-^Ch4fzx9l+>v<4Pjskk}5s53)2mrAn2-kVH z-`+syNl?e}>ie%3FA-~dK@1cD!2fala<}*4wumyBfxw5KU-;*ses>js4ablGYoPC! zZC%cE-UYNW?zo@VMe%U)-gRN(Uv?@)X39+s65%hy2c&q>#EI~7!WuUJGBGy-N8%j6-*I4ax>g8};eob_FAW@uk_aWcq zaNeK|W{+*c!3oKvfZ~F46dWn{h%70tYYS$pI<6M#t0RUnvTx(*VKT^0xgs#UZ~tWk z={Z6jdqoVb<@leiL?$e=VuX0bswO9UC!YQdHzzYS)34OdVI~J9lS0Ke9Q~lanSEZ? z;62q?oV#naFF=ry<<-$WU8#u+DtQ@&3Y1Q&V`y1;k)i|0!~?<*5lO1Vgv@X76aTLl zKz0YmOUjUS8C|KrvtI|pC@Wl?(6g?sNjy13&d@MU5T8BEJ1a$3xTO&AO**D3fOUk) zmI34ofpP8ec-8n1)!Hx|i@EPI2uD>MG_5^UL;irlG%3wPn<+$&w1N-*r^4mPA%a7{ zzzrH#f5Cm(9|(TEyfxj(|7LeS#&>4_?UOu9Tt(ItGx!#u`R7Y(EWU-H$D1JC;_@=7 z$jd|23^DP}a3s6s^}XjBH@GB;qyPR11HCubo6bl&MnDrj>+T~d1NtI-f*)y7xvaDz zhn2&zK9rNZTlr1jKo-|7y%S2m>cZ}@5&NTDz<5(7WP5&ZH=RLk=h%aex_9|=dtnhH zt>WCWbg*(XJOo~1*+!0EoK5}8gkVykn(?6W?YV#HSFh~~(B493z6&DG3zWsJcn@^jxQhOMP{Kbs4c`USgkwrxL?7q#cQie$A(iW)h zODj!d=fmXTHNC9w!HG1|P50wthM5#mpBP1qA6hBv{-boPK$Y{|Y* z7T976{Gr$`{kYeOCBSMJFuRUp z_8$x==b%m#jCDZbsiuCiuzo|~fzVKC$nD`xEQo`(@v^V0g{YrR!AZBGlYk78shvA$ z4bT%6q0N1~xp>wi&b>;|X8`&iuM=gnd8vWr72#Wx`c3*6pLkr3)Wqxb|NKvjEhxF@ z!9iYY>FpOk^mFlt*Auxz3&oJmW4cD=F3C$t7y=N4j_rOI~Tb^FpEy(8Ka z&DKVYIcW)fLf)K2e^@Y7GSJvqd{*?B&x;odo#VM@SvnZd^QFbmI@h56tNTUB)!yaP zk_!Jv!C2Tg@Qkr&s-E_7Hj9Wf1wgG!KTr03)%-E3#$KzN#y2-PY2fazVW`0E$s^OV ze=hw+!E8mvle0J`eqofiD3F6TQK$38w1EGy_bwqJ;pcQ}$|tnKzM{%9o=#hOFc$^& zbpdtUiYG%0c0zF5SDDtejGfHIpPU-1(e8N|sy4T8Io+fj?Z3+}fYTFrC`rKP3KqB3Z^G;{d#daQS9 zFW1VsBa_Y|!WbiEiO2Qt4u#8n95+Lx^6ies+#377y~^hRgl1;(2bLq93;4Eq;dW%J zddaH4``}=bIF+BLgagwy8Me7B5g1+A)Q5!kq?FLdZ!h!w2#bz`$(o*|5SXA70<|b# z^33afbkFkL97wLA8b?QP^joIEU@+vJLxCXNeOHFdgZj%U;hr?)28mltssszX^#Pcg z>g@QwWMK|kG7$A7&=Si(vu2il? z;Ymfeae7{5L6hJ4Jd4N2|M~Zg13j22;?gj#Vp{^3_A+ChEtq)W$z;gAUYFEFIKg(% z?41_%XSYJYAVUB2CBnnN&&Xa0hs779WcD{hEdE;yLP9FC|GsL-!!zEIDVg*xT;BU^-=5a*7O1xLgE{Q-+p(}^>x#6s=LHLg#CM9 zB{sa;`qe}pHtKl{tYVqvv$bK zi2n^x+QWp;?y<1~lA|OP)5!8j6CE$IYZ&I%F;4z-4n!$YaKV4yz&v9FgcKTzwe^Y^L+MBLB9HTBR0%|6@jvPD0ONy_We=Ad9)91G z7x#>sot~~;{P8kQE*Crt#eZ}7^Vyk&ZU(mcFoz{ggjr&Be!+T84vIp+?&kF}SWt>u z(a0l6`?l>kKde*v4w$L^&b2F+r`;T2gy(S-8G7k zimEE7{iW_jX5clrhXmBpA0&N|bh~cxT~mwCFO5^N%t)wldHEn4jj0v40&3{6cva^E zd2Dp)=|+Z;yn9as+Q9?%ZaNJwDpVY zPD9jPYBSs!ANSN;%;Ir7wr?vfHyDsg$nLuAx92>d$}L&s1OrbjeAH5N5g7Ky((~%v z1B#4k$b;+LiTdU=Q%c{Q;^`elZy@Wjac0KMgb>d|b$Un-pprc5UVO?oU0POCE4t6- zIWJi&8A}c>RUJL7bTcF!SKQD@PoSc7G$&>+oXeeM)0Y!fq{A?4aP@!yARm%0E^2mQ z;2`Lp%`v6D0utxPAM+_|1U!k<0c3Qgd=~e>TojR=TgI8hb_8LX-Z>DKIi4h5< zT84m%h+=suQIhTTqjkY|F1NO+PKg;ROIusBi!$>*7)!^<itP8;i8~qjPS#t>KdX&z}RFtDa!?_kNMe|Ex;+zIvD=}_Ye6Q zVI8o3>#ZJ~L*=l)wSo!E-_pnbyJ&;AeBW;9YX_HN?&hq6uZsq}-EmK`Y-|=L)`H;m z9?NO&^YB0t*TV}F7_UuS0$#e z_utHn_7bn!zPN}K;LlxnHw1nCmI5+zBYyEw+A|!VcfhYgBZtc0VxaVwu>Q)5iQUI1 zWaoZ8Wk&uYg0ST`HtF^NJ-wp;vLlv*=IC7RfOUWE`O;kK+UdQ!!9Hz1$AH$Uuqo%r z=a;GrI6F+4YM(|23ONTakYS;JKI#ao+ls8G$BF-J{fTd9DO6HGwdP)zZ58jku#CzP ztr~ri`#mHc;Bd|(Hu0+7JbvuSy|riWM-$rKGX%^)i?{P_4IB`ih6Hy(i1@irUSlWj zn`HtO+?!oZ_MU`?-A+e4jBm7;ZiAZ%CHv|=DWobwtq9^V%I#FUis;Nwn;zX*CVm0C znJ|d=07`Hf$q#W-F14j8W_0+H7IzUA`dQ z3oAL4a?UV@cPG6w_Szd(C=}wl(!FO%=(2bH=KZ(&+~hAxX){(~{Wkm*;2|rvjITd- zgtu*;wIX%AF-;lA>CYorV;?w$niD;~Aq)i$ke-wTg2b1)1wUX4Aj;c>UmSq97c&bIKVDl?_tKJdM`juuDjV*r@bNUI$J+VKU(nje zdgq0T_pfTm z_v{cHeFk4PxFqyJppJb2Eg^(rgHb{m<!!}#>tthMbt>Dj< z%U*1TGXD`S?fbfQCyC4r*0`shgM7+=1f)i%%c8hjWX57>z%+i2otdK>@N}P68|}99 zo>jjOCeT569CS3_$G>)AT+Uz1jCG~(xjSjId;gE9ua1iH`@SAfB&88SLJ*|{q>&bo z?rx;Jy9WHw3Mkzz4MW$Ef;31D0}LtM-7)Wjet+v-i{)bRhxb1Bo^$R#`|fk@fRKX~ z-cPBN@J%<9!slboMVhuhbM9sqp!0T_bG5B`x_+lfos_?RNyK)^Kr}ow^cOE=Mr<_p z4y!ykO;zvH3|-R`iU%jOeg|v>{@&+*aO?XESI9e~RSwlo@Tg13ZT9aJ9>H`?Z z9eMmPmlKWx-??3O6%P3N@4Z6uiF4z~!T)>?TxUsv??_{xUnzLLlhEMl#KR|AIKU^= z3+%NU`L5-+Gy>HrM=wugNHzGC*PoY(xpz;JSJD~!H6nY*YPQ3H=t0X5M(#S^iH827 zzQ4pnHc%rq59btH{$>qM)Km)-77O2JjBpLBM5ra^933ecZEjy!plTx?HZ0BpSbJcT z>2;e6l8li3NEdzqp_Jt7on^$n|yw#HfvGT&(VQn+Hs#oOgXZ`3v)_)!FEYG7?Q&c$&pw zb=S=1MdB!<=a+Gwy>fM?{E4l<*ogBY=eX@vU^&t2Qe*D3+RJMx=628hUhD!YZ)HDx zLAd9b*V+!Y2-%s*SkVlt@IYZ96)QK(piNzZe4p!b`>v0O`yf<@=gUultaXcpdk-O; zPxY_IjEJ1*G&K|1^gnew>HkZ6s?t(Y4E1qv_j7PYzRfyfZdItNmimSZwTheedh7)t z=PJM_1H2o0O!bGdf-zP+SMFV&3YU(VS+@p6*G$8#V4s(M4A%>Z^t6_RtONuz)cpp$ak&lGgdIOJ=9X7@4)O1KXB*{HcvjK1)z|QeD?`zI_$56ZF^Kq834!YU%!iVm zsSwEBiWb}50I!k1K(rIB33$zoye{{4r&}_A^J573VtvKv8f|1>GS9jY0pThyP}AcU zdyz|AO<&?+U?uxJ6JHFKj4X-LoAo}cHQ6JuJCiYAIV~dgP}7e}TU%)6fhsuS_61l+rk|uYnqQjgg-^7hDKHOQak3=Og;a znUkiIP^xj9tD;M@R>N=+zvNkMBV=*K$Q&5oX#Ts{(7ev|A9_Ovxo{`>6cxShNJW;k zK25FE>b3a_VSLrEA7d@(R1!2M-f_?CCWE)Ahml&InRC|}-N>fhUD$|0WRrlyf-qAt zv88qtb-|wab?>s1)277rv^#U=h2A+XQscqzs-6(NW}k$%rXfEqAKd-XYxi<+`GCPC zFx-B7D=Fs~s+qVjeaJ<|iWCqj{Gf+?Q4)?D`byMN`EUdcttZix7-X$9=`BIz#I!rp#tl>=zmot|Mq zznNR)oGKRXpKXl?A91aUV27>xKFi^ahn!^Y(?%K@2=4+WfE_fev`#xDNN|2 zGYmkUM4U|}9ki_stw9ZYZS;m6^6pd}_VWbRZI1F68^EV>i_cW~Gg9LY2hSgvDkdkEWu3myqS|)ulnO{;mG}&cBE4K_1@A&RYObXM4IwuNO!IOyWZ(* zKfeQcU>iff3=CpmE~So&sQ;|{0H(c(%4)sEJ}~sI7_$+4D|G31<-w8k4#)JT%*dUH zNQvd22IRS~YQNZ-415n2UrbOq;Ehg+90spl%k@~s$O3fSmyPC!x~!=5`-EZtqf6JZ zLsrG~(%G|WwG%UX%Zt0TfT`=CWk-vppm1N7<@_BEgQPz65vtig zf1*4Wym|R#P5=8BzjI%EB?Xy+4r4iQJdN(Q;Nx)CVS_fUMlH{hEo~nM57tE&dg0az zA+JlAE%wDm&HORp5^s<+2o<&CSnlK~fiUn=mQl@CL(Tc}jtRB(QMfw4K?O|f>sfJs zjj?I&Tp7RW2MjV;vnG6WZKn7t(PTbMLcPp4Fbnja)FVMF+3sWu_>Kaa77Q@lvgq2}YX0N9FzvGiqwzw9kDGtiJ;c4krU{7m)o<&-Zk$=pK^)>0Y zfnb`Bay?d(Tru_H6gAttul;^$2pLT)15Ia0Bc5G>#JYc?*ujbZHAXR%ySIgVJ4lz@*D|mjBLn z+(}JZ_b&K@UibogVDv<>yG_XYwUk=!m_1*nDJ1cP8yhs*UhLUSEVk~_BxXGCv1{QA zAfb-<}HU1Bv3E*4$J;?ttNFhMZlx=<8elhR@ zVk@;W7GWj{bu!>pXK3-*ak!1^W~=xOKA(ckN>nG6@`~3#58f5aQ?`Os@MsQd&Q$B| z$ebK8Ti&ucFB_|5BiqYyx{}k=%MzLIGx^<1&20Y2?!TU#-1a$=xf^sn7F&9j)qY_H zWu@O;@*QXVrAYW-E@KuOBTZ30Z?Yn`xJxHK@SRIG@!X8fPsw|1&BJZC8Us=mxi~hT zZ}DZ=G-J`cZH5IDQqlaHuW1^R8$2lyGz=D^+8dI#)$-mhc zN&Z<@xASRg<=}L0^F}_){izrJKfpjVki|e2ML%vJAS8sQ1V?1dpbuJ7mzB4Hm(Sr9 z*D89VeZvm+W6sUs*9e!lA~tv$%{L^DOUofL$WQm@F5oW0W;;F72nk{2>PdbA{Z8F~ z@0Pp`%j-PA*ZOa%!U}%x+?hWP!#S^$5g_c4_DCwzWd2D*PE_`nvFwNOrAjZY|vAV{w1Z5qRQ>-CdFK zsNO9;{!2c@o;Nk>Hl=?pAt59LP-e%9u~rCA?iJ0*CNteTg{|*pnrU_5;Er2&{V&%K z`I_XeFw!7kDRQmd=q~WA`DA^~hG|{N_VmQ9cT2ro7Eu-I`=MTHZ^?FPf5y%yFGjoc zH8tFDAb#J%waYSZS-Z6=F}xdv0bcW#`A10E7jwO6d|JE4Vs+?3kz((<)_iXlFBsps z{V6Ei)7MC?wd@xuqFp!x>(yoHQ(&WWO*yV=L|oe@WVrRJXBfL~-P39=V?3B4G9CARJUT*~{ZcE5T1Ey?x%n(jV9ELnz=JW32h0Zw07xC?wf zWy6XLqq{+v`bCH&=NT`@jmFpC*297iM>5 zNwW{>V$K=OQ?Dk$TKo9x2)b3Li>Rk67b&Y{2FE_bzvrL1Zw2>Ju`)VRqwv^HdycHx zUeUdbwXGX8B5BY+h)^uV#!#@?a^ug8-U)q^k1uAW`t`YmHg=&{e@LI)=L^Fx=QapS zJpr?$yyTRvas%m=Kg9X}M-g5NvR$IH69};_l3aUAY?hZ+)UR)l8u_2?Ptt*kt;x*_ z5$b{XV%XNo1K);%UIfwj>b>^;Udb+qAqeHz%F1YO9bW``BIX`p#D9@gXm1g6wJ6w` z0l5{E5eQLWy;h5m=DL+!pIu5cjvBSl$xtj;i+`YCH-iB#$0#mmPz+ree3qccX28^A zQfH@Vql>_?9583dw!yGuHYYVaXXhP8S4)i02tPQOLodE7QPX#2;YG3~ydcnY80C)7H8s|W&|5Cxg z_i{6*z&b%&KQ6UcD)_%0AK+|xuiw4EZ0T5PhDYe}5^Yf+t5o4N3eL666I~W;F=@v9 z_$oJw`s&gs?a~PiN*W0UVikk4z16Idp5&g2*Ujc=g&y#ljUGqJt7wcX zHl((DfYy5uMX!ObZnXfVIDDz;x7os0Es6xf0EC(*L^o~p8~g=VU7I9sJxp=1 z%NZR!MKvC7dFKn1x6nk1HGxEle41Kfur1Nyk1k)jy!{HYsL^Y~%JBO)Oxe?sP>FV5 zv&;W9OH*X?Y4V?qyBh@fIsc>{fz66xV8Cqje#V}E7hm1|Nr||T$yIT^oPS&BzR^!v zIyZWE|p<|dK@xKA&(PU?X_G64Q6kYxO4dTX6 zTI889UxOI(=c4t>HQ}0?T3VX?d$>+TZ87%oCmgA4%s1vBf%7j2-Z%@JJlX6Mv>oABg4qQdn+b9*7p=B~Ek`eY$Vw0SngBhW<2cU9ak$+QnOtUR?*O6}S+T!^%Z@4i$cz#q)sy7a%SF zpG{&1q1{*d%J_D;XRNK6FE20Op~F;*m9dUgOS5H3tnalejRHj+F(CW)e@U?l~K`^tZP!&}Si@-I305|N_ zvKOuJ<~|=z!0&t^%IDGC2+)^^y3QG&|I-3UFJH?MbVY`(h*T9kd-J;;ymb~H9(?Ac zjrV9&-kw$nrQ;-i!&_ArWL4xzf4{y|F06D0PM4$r4*V5`R*B0;y^-cp+Gziwxgr;?T>f^gtw&P3Sg z>?OW_xm4_w_llc^5LZq?_Os^T7s=tJVFV`1%c)(77gN9DR0T-v4bz{zR9nP`-RG<{ zUe!M1i7#C|I>iXoZJw=1{Px>T2>S;5b$`49tvUKwPG=InNdW4XW+KKlCrnv`t<}$a z*cT%9c*=To?aeO-r^LRbHDtN`wN@?@M@(;p&932rthYnR{b9T?R&S`Pb@@b7%h6gM z_68eY{T&^lL$XEg{5UrYb6v@si!|e~0>yPrp6tmu=9*!vX)bvXfpushlK8h-cg>h& z?@hz=5+AmdaKnt$=DNpAopFE02i0D{o^skS<$sAXQC0egqxNlcSW-O5qBxQ3W8S1! z_wB6Z{I@&LqRj;|X^BbhoPeTw;z>OE1e30fWaGaCoQjWJ-_M2bpzWVZjELxbLA?7RX`0-sX-R9-irMo+rq{Cm-p;dlGi271oiQaEj6kb< z3$nlgW)-4MJH!2EU!Ei;Zo*R7FPpO{yIZ8|i<6U+atrK^q*9~T^FNAHVCy!5RB&Tri9i2=2p0ISiOb0GLLLinQ54;CdrV zEFO8e;9o4$>l}Y)nd|n6>M7MLeF}2fmKM zUu%=B&wNcIN7M1%{qXcdS(%^tb7Lh&ga-=OElC}Te$mhIS>Ppdz5S!7x%H|AR4Fo< zI>~%lJe(S()w)Ji6t=#7&nQ11uFDPD=p~+Qj^&|WP~YCqk2pLpm(8EEsavocNHll1 zN2m>)wV3#}>O)W&G`}9Hzhx(RY95)Y@y*k7^MY?cqQATQ1#EkiLV=w6AF5?w`Pqyk z7>vH88}KSk96Llj#z_M1UMw@bUn?*#JuS_0kw{B`Kc-z4k85+? zHB+kC?AcM3&k`}o%6neq81mgSZ5L1$|FQ892ZtAy`8I|DO?-LnjoR zeVI0Vjb71uem%wMmP@&%R!G_(_2QNhps6KSJQLLRNu4*S*et(Qq7P>XWYNX-D>JPA zh&^)*5I8YH7F;^sH}_Vw?R7lgea`tFhPWUQeZyb%&Kbc@o2EWp}9F3 z5ovedsX5C(!-dw{H$ym+!x_yS*ND&rxz5mQ*l8pLseJQCZMf5hH93Ef)zr8GZo+2N zirN*I$%P{>!_9rl5@nOST9^g?L;XW_YX{!s+BeN-VC_4Yd-}lj+BuvLBAyQFYBSmi zdB2-?kP-!%9ACrv$CAUljqcZ*YeI%$;zLDE^#B&#Ri{JCo`USh)ZB&c>NSY8p3PY9 zbe}mi?C{mEKxMs4w1%DmNB|}O*1+-c%iznG)TMOd&;oP&yCm@$*Aq+24a=_O?;GB! z@u`p`_@D({_1Ieyx$YM38sfdXK9w;c5SetnLzY+=vhP8!;K4bk6k9a;Jysa_{~~#gbf%h7ymT~n`8Y~!i*LzEo%0P8`fD}wZ=kA`v~3)b-?YBWtP9@+h@)5 z5K{!uV^fwf|M5#ZM%q>@E(WQu_clTJVxWeCTzuLD**1N(3Q_dzcpGj#10eE_b7wg5 zrANN8-1u##(T1b~Wz%+5;94s$Sa9|~{}<{^2{!4yC+2u!oCAcfH$(qZ&AhmxzW;DF zp;QZ?ROCw58|pHYcfm2=6bopJ0dfs^A${#n^y|G3z0$?%E)>1)2EFb=BPVh*Vi=71 z?R^p7(|*oYSYBMFyznCC-p`7-a=fwmiJBc~QA5TN6&$lUwHrMVAu(hp=xMzF->Fag z8afdx$n2eO?Iw3JMZZNXyUUMn=?_;UctK!75GkBD^f!fQjgA~$EJ$z-cE9_r899i| zE{i^%B%v2h%lsN*3hG=wRCO+n=sAq4w@><#uP1W9d%lP0RvjCe0l$7Bdv^2j$=;%< z)XNr~p#}L_aXC-yCCT4qbWcHBx)*!n7S*01rPi~O-ml2H5fGBEzoImqC_7#$#y@^% z^~TuSSD)hS6##^3XNSWEno0KRl<8i!`A(yu94xf1gqbaWk$g+R{VEZrNTM5rZL?>S zqx-x}-b80l>Ra3G>4XwB_^JqI|2}#Jnpwz$5Wh{|Qv=uVynL*V0j^&lY`k+72VC{; z1uJXA?}pt0$!-S$C`4SXer*#tKO&%xdyPKVSWQLZx`<%Is{OInu7&fJJYuxWBmb8oJ1J zJY(4~1;++FFM6d3A_5;fz=^rL+->*@+N8vwf!=l2i_D2&;CG0-)?}?F)FpELpc4{JG};BJvPXV{mfk|cV(R- zpdtN+2+Aq1v+XeYVz%jipb?-v-zN!pj1gNs|6X*?4gkaQH5ntNizqXPvod08uKMg) zxd58^om36+%QTrD?7uk@WrgabA4fosi0~d}k%aQe8B|7-=~nJ1T4;UH2{Qk=TyklDY)< z9~R}Nwz?W?X~C?XXKaV_J{rTm$s~Y3BbwkV!UJ9-#F!tr>4s3Est{~JF5O)$B5!|_ z4qLp6k&fq)4KzyC3}g!7QYv2NsN1wWxW_)Npq{Ki5i!4rzQ)u$`_Jn@k!C;bdfr+M zMYi}#fLRev0s&=v`}A(vVCp!=bN0)t|Elh{W8@{12~kV2_m!ujAYGthdw^r0CFoBA zTJmz=nu(wlqd0Wjxdulze%O~Cs3zii97l$cKV!f1POAGX3No)Ta1vj*Rh?Bcrqq2W zX-{)|yN7p8CNL_5isBadvvV2@-Iuce53BDvx?v^U|$3A(b!h+!>qs&&X^G%}HKSLUN2QY*keU4i0c#yh4cQeT{9Gvs! z_xd9JUESYi_v%;pjwEX)w)h|u9=e?b18Vb%g)qB%gnh3o{~jwy^^R78tv`T=Oe4@9k#qo&8Z0f9D;-wMQ#P<|Ky(*No(*sf`Lm*c42F5q&Hc2=Azfnwoc9siEbHq$a#EOUn>FSRfp zpH{O3zQbKQbFMY!;K`q%*sRa$7;4u3y*xgvCSk+g!Ybc~^m2?0ls(&d(#{s(+5h2| z3i@33Os|V3*y}m>{;2?Wky@*rR-cp$?}@Su9@-b2%6^H-xxPqjdiE!p{i-S|A%U=g^$UN(c8ct%+qbrm=b; zBO;mVa&mKKpSkXdg9dyoR(Xt%8`j)8j#ysscMeh6b$-O!A|xz%Xbj*$g1w2b0RdSNnI=^a zie9x$`E8D}rtv+m`KFnf5LawAqb3ed4;~ZA$M3>t?+w5v&jN`%9g%_ft$ae#b6vzs zmPb?~(THb`nNl;MHlda}8N9h+E8=`{Bl>vhLMJaEgu1F> zeFKG?qTw#rlg+@f=iuJ`J_jG~)of7B)^}qos9*WyQM*Nh!y}9^@fo+-N)FHfBVO1w zyQ!4j8#3%ymp>j3?(-D@qum9GU&J*=&)aaHsdMmhQ$|VFL|{IBXU32;Y@Pb%$8O=) z0vlE!KDg)EbBWDiFG`$Q%8utgTN~E-^%i@EIS8a~D>ktM+p4V^V{yOyym!T%Mn@3a z!J|?4uE9b=7$%6XWP9b?WyllFAf`#Y$(~-u$^iu4lg1apHk$Ng3B` z`C>ON2z&Rm=nW8)4wHUHn^|KH0dUFz(SYcGABwv@dPfJ@;ZxLNvKK0DnCEee(XH^x zOqDk0Dn8SbzAm3CA+11RXdjt9xgKjWI^3VHe*GD5|Co72Y{0Oq0r^Ju=K(5uyeede zKU%E>3^9S@DMUqR!<$0QnVi`fe_}>WWwh4~) zs5)Gc`U;CDo9Y}m;aDp$uj?G^J3Xz%zw5pYB|KS|3((C&>Vl@nhxUwCGy*j3ZzFB) zhhD}%y6@S?3Od}g^c*=698p>Ty_9I4Z)$$0la+-p3f=d!&6}|HL<=skrY(rgh`^(T z3|#0o_p5t- zmpSmqDZW-~ox%mlo~fQDCdL}hop+r(QE8o*mK=M+#x*LuVYd1zcxazLsCiXL zE+{REvIS|CCRj}pcaRnX?+IC#+t;SnTZ(^>sQjJ==MM!a<5#mQq1cXDey=W*AM6cx z!|9H$WI~6DWqyJmgi~zE!(XmBp~4eCptN51Q@`JEeV>w{ZiZ!iZ~$y^zS02N^@e3^ z8lZSZl^R}NfG1xvfNS`=b-ygQY1-N-ACFzGo< zlaUMo#&s%WwoiJ{H$Avl&4cKaYZVA~DegLM*aW=L7%o92#^E%3PjjuE+4^I3{)A_$ z=S(v}Jl}@z?NLt0oss%TbKb>v%_}s!%`7oSC2v{pAbD0$0OUu!s_a%p5hZ4rYg2l> z7zSCC#NvqLfQ&eZ9g)VsMT5_QLvRH71!4W@D`KEejH5W$U8df z5<^9_(Qp`fITT%##zYb{smopJ5Hc*ylN1Mbb+720$DV05sXvs8Uqqr{zz&|O^rVVJ zRcrmCl3fAa-tQZ>C2|nsDXbrpKKtE%yLC$$EYX#mp$EZqKr9+%ONP2)n($1qs$#mLDq`!@+&Aj&G&hl|r!qwMG za|uaS5ms!<`gvG9RYdSedJ2q7;WDL}y1}=5cR045x=e@x1~d}97e4*7RGWS&j)Fn+ z13wkA6PlUPXHnCn1u<;(S=+8Fg#$e3fcHQlx1tgv>{|B$)iG;&g@;ixAJ$6{Qo5+z_3Z5{gVOvu?#Y3VCLL^4*Wa=cM$_dI&;;i^&Q@lf-b*Er!42T-uY*Fi*k5VdYsITB zmhcWHl&F?iOM`s!{MTf*PG%_FTHKfEt1dg*cUWk`u+PK(zl^f~pO|B05dhS01l#9) zbv7M;A_PM3gKlE5_kIt}{*fy+SLs0gB*-*dPl*-6k%ANG=&>tVy6oM`J0rarH1rgN ze@A-HRzl2qd&tts`#Ug<;Q+I={uh(`?(+Ds{R?mNISUvpZcG zbEk9vD42*t0mBQpc?JCH;R=4fd;?|a+f9&V&4IsP&QZP)wO*rD*+NpPpU8Y$zBj1g zWb4*VnChF5cXe%*%5l=K^Bsg327F`@h>#pdm^7evdSBX%l=Rdu=}~SrjYh2IrcFP& z(~|I_L$wXC#pLPeHEjlq_^}5MP5Cf%D0~PSF?276-%Z z)Jb%Wg@j4efK(Dl2UPnV5WhEEQz2ak3skd@2oEi#ufKk=N`<_DW)7|yHC+!RXc=Xu zXGBUW=#WZB3>WP!P9iO}i*-)%hQ90AYmI*0AN{d!bk%&~jOIq3VU`14I||Kf2714U zI;!=nEO#R4pZ&YMcvU=^Xt=Q_tiJFGx@w+tP%qry(<8YzC~GM1U3GrYv|~_#y+of# z3_@#kf+tR8(-21dP-8@C%-mpHW82+m7k9i9R+s=DK#ys4Sp!%t(2(Qo||t-H~Wxp{(W-2H999J=U>v>a;^A|F0}yQ%^dF!khV={u&kZ(>hXB7YUPn(%R8<*O#t zz4Zzc>B)W9pAn8NiDKkAqgSrjaEO5bukY_j$&lTRt$W?qt-AGrvP^b+BwGeeU+vy% z*(ZtTe8(4q0}Z@{Fm{kWtW)+LD0J9o-orD=t&c%uL(!~}KsYobpFs!2C=y(RN_+H8 zZd(J?w?7Xe*dBv?t5qOc{291`S{Vz+FR@Cd%BGmz^VNm3;9LsQz=JS+6vFM5hjlyf zy#Au^gy2H-zlvy4b}Zl$#3+Y36#JLr0Fap|IYw}HLV|EEu^D%P~U|9Q6K=pa;6(|RKVzU&HEqYj52 zc9u)Uq;7yZB3}(z0irCy5j~bNAfn)gRnMf8MXXvL!z&C1*IdOgIL)58)CUO8#bqSC z4-V}6q%<&phP?rj9nLk^1O12xy1V0t0>yfq?3|oxnIj}bq2&}HQsMR&F7hN~YgD{T zto!ui3?iQ%a&i@K8xc&9w-o9nla8nT7SzEfFC}e$4pO~Pbi)|n!Vv&ozP%lqiJIzJ zDl_U|(EUxdO>$&A9yHW0_uNP83En?#=;*o+%1>9FO>`%{4$^aecx=E|Wyi$DlAUFfr43$KkG2~owg6$R`hhLpJo6b##s z>n0_}k*NR|(Q9Oog$9bSJxbpXRtIx6BXIm4qRo{Mvb7Sd#0T@E2u$Lr(BRrV))S}&Sl@irei2}F zkqg)M+A~2R+ke+>yF)|r=wauzS-=!n)c2xYD5Mw@QU)bL79Q;E5?`I(A!+tm zdk4_%VVKDC(>VP`rUUa|`6kL0_6?GY79 zS|zY;NsSLMepn?{Jyg@p_qA`bQSL%<$}ha5Acl7I!7BL)VpM|$JqFUmq{Qtr=!SRZ z1~<9FV6`yV_QQv*xGVU;@|a@$ssDj*yAVa=d(-3hDdF~tc{sD>Gis`{vZ1KfLyvIeFt@=-sP zdR^u{+pVCc1MX3|8$zgo73SciMQPmfZWSu2DFbFWD#C=NR*?*GZS z<{h%eji@h&nCna#4%30^yRB4+m;`Hamo$TR9YWu!TlDDSw9p(SMR^+= zSEe7TRCZpFno0FsTNqI&)PRutJK4|hUxCdu3oDqIg1?Lsa7%X(%A~+qr7>!t zYMkY&^$mz7 zf09dKf=83buGw~`{QENw_X)fwM}^r!tbr0x}j7+SD)mjy8Gm=kVf?mwbLNgF zJrR^KmG-gdnh=Gh2&SdHu(7;>R<>^;T=W zkAXPR65LM~|IVzHv$O?qS!#k5 zSO>}-tsb<1GW;+piRCmMz zi4C4=bpuU)hO%VedWWSa*&LY}^3{vARb^qvNBt3VO`5nJ!+sSlt^Nu}1XyoXI-l&X zxaw2WJOBe{?9({EJ4MG2wgThD+`E-jDEM+8xz*G%(enlK_2B?DdoWuMvu7;6wTRvG zuzTZOBGlQuC{KtL7Fom^61XUb%7E6q!T^W44cBFOzF0>mxY+-1FR8(EE)T38n#8ai z*}vi{JK+JqzrW=o&P)kBw?Qd@9?UYDZdB=*v>v@67d|Q)p(=~xFVQa2e{$$ z$SaR8(OrvgYM1^bNCkc^w$s_jDOlZL$SFfl_i{*^j!NND&Y!u+rcS>&aq(MYo$l~i zIyMX^kXnBFMG}P_A`iTZDi>e*`Gzlj{IAN8EpizgYXp4(p%Vl}DYl+0RkhD9teP#f zcT{LMciL;a_d37r#oCFPA3$^kJ4;!Bm3l-7a@^_QYm*{)hDTgL@Eio>R7VnjhK5q& zQ+Df{mD==m!PV6*ExYwBYsE9SbNa8C>@k+!m80N?KvNextt5{z+H z=4Y9;YGG=s!0&E>m?akOL>DV<2f2VBB%a5sQWgf~)FDIBfI#CcC4nrLN4m0{ja+DZ z`Sw~6BJa9%wPlM1VOHF_bsom~pM2~0W`-qQ-&4Z5Skf|+ox3c%&4DV`xn?b2k&hI% zBAAWEH|+l^hN>E7lvE`x3{Vfa zqMij<@8vtLI2^=ATdbZ~3qH)l+rfb~H?3HGmMgDc{rGq?-;R$SG24?@J>gDa9@a$z z+fuYWxmWl~>eF5fG?rX0*zetVF2X}Dz|0J&SOK8P&b~=cumdQ^i0b>MajC%O+bSSq z7Qu`ySmYhrhcb&&a~&|JjQY2|Y5D3>g8_t%9#TdrfGthpEkShHa8Zh4p<2J2U`n4P zy%=>V^uWr% zK$5OkWd7l*G7TwP!I@-PhZuVa`yioBkN#Y+oMdYM}Ldh=F1399Q`3rN0?f` zt>JItLQ9~4q0ktOsD4|x7i@m%cl~DxLM`t;$mY#=4<-ac#LCZqmW>YBd2SnMQp|Z;-~I~s3Z6dXfsu% z0`~NkSt`521jVq&Qv^;*Xbx}(@!dLDeEHYx^~`3|PZnyrxn{Qa_eahbalj|W08r?;*|NA;*vo5nr)!*_PhEn(^WTHJyXEvSW8Bd-g9vdCUZgD zl_T1`^y-W;D+bj+%BYNAh-L79R64e@wcjlW-UeFap*RtZPXFF8E;-r>`Qh{BBqQ(O zK>t(Z8pnL4pAA1qwA`?impljGyt$9gH=d)vMB@H(#U9bn)T1Vu$}G#1wvN?MmYg~I z#lsMO>P;Df-|_k-#Ym==|4Ngo!=&tiXjU+;`=b60vpo?|k7D-1pxHyn=bfOR7mO%v zuO7$79TeCY*1mF?5W*zBn*Jg&Mg9i}$T>kY;gWgF{Ly>7kFn4uVbnG9s9d@pS9QX5 z9xZpuFv+#)9MiT9YbY+L@7>bp9I`LDLuob)Xe+m7RXLCSq(x@^lH~iCm2SpXa8G46 zl`j-(>x@W%$_6oMHQ>JjEh6t7E5I(CciKIsORqm+Mwg9dqQ-EQhn zsNI?yyr1l65?}vN<1zled}U9qP*Wq07cPX)Ss&hmkU1?MsgOvMc{{N^axPU7qZ-O_ zYSV^4ezXlaFe|syYVq%Eg;=yC;!!#_%+Zp?yd;vGb2Tu4qlgC6E`7GvNiD!;6ldg8 z*t2aM>Z;v!@Hf3QneG?HEHhLQ^OP4(VL`?M3xF@C>u%BFFY~4OrHy;I@AKoA_cNqz zE3ZuZMu(Ea8wKqoKXvHC%X!H?3-@~_e;uDCsGLG)vr1{{W-Ej`#Js^vOC5j4329S9 zF)<*gk;(gfzqN!g;hsO@o1kh;B)-7=PZx%l^@~+^bd^3IW!#Dc6dI{X9~NKdjl>O! zSfA$Rv{y5R$-uKE@hPci8zTYWZ!{%eOrHC=az@w+xYnG&D~z(zlbx`lzc6)(=br0g zrc9<(EqnP67k)I4?ZA9)+OJNCg0BL93U_ZT@%fAw-Y;t)d-*#iVCviu=rXwI!SAu~ zv`k0E<$QP0ZDQ3lr&eJqMzIjVMgSN;gk7CnVrN~-o;z-fw^y1gY8rgCitBv zgUnGW2o%^H9rz>=H3uW~{+y`S+0xQttShBSN96Qr7se>uo$_Uu^>ReX#!G8;Ws zHQ2br4#Mu=b#zYtx|z$#>65ka$aBu8kMPq+$K;3W_e|u^P)R{PwjOy?e7O{k0h}G( z5?(v-mQ82$YdX+74j0HCmi-eyiGTK2{gnURLK|JujqwFJAxLmNJ_&2_dWM_vW4!=k z5$zxQMLe!2o%6s%eDKtl&QKT5^efR&OcrIYYkSo#_1Ngk&PSVVuUQ*Nvzy2EpT> z9hd4&`c3XDrRTgAjpGM{A22hv<0K>{e`kmwRa`k=DzAf+ae-dbqijfyc@;TeN_K^~ zkqv2(#4Zzlb+=*X^qQ=sK_c^tKXOJcJ;XgV{j3@qoyyt;-ZwjE19W&eb4A%Jd}pEd zwe=0umx)$!o{mZ*oquRS*_c=i4x84AJz~K9dY}_!;S@ud@n!6p%gqhY(>j-60X+2; z)zk8tE@o^D`=Iq=Q7>#xv^zFxZ}H#4>~Zt^c>CQNo_c-lL1<~>8tu0CS6F?fpX(0s z9&rt@{mJ7dE&J9+98fe3y*oERXdBg)s zPSh#a1hMD5@!#4B%_-UDZf8XiBXdI&=1itH7;&CmZ2F@=#fBT=qU=$oG%nE{DupP**0CUkCJ~}@OWtK0o52-x!&kzP+@gBR((J%bprxiv3oBEjMMdp?4eDBm`-$h^SZuwe z6db3^N_erLu&|Kb6#a9BSII^bDwhwD&Bd*S>M}wc6KKj#Xz;=#G6OivZ#%#*E@Hu%>q1UcOh7GJ#3otMqnzoM;bGLyA)&$kMH9gNFH0 zV=CD69>i@u3;aq7DK)UWG5iw?ED(;&`O<^o^I1(pyIFt zEo9tAb(CSskH7S1k>3Z^Qx)rr95Umdr?<{E>6c`%kGo;jZyW_j zJWR)DW5g(u>R~?C<}FV_UPJ+ZhDLoMq8wwG|T(8^xlEX{?m$U#PF$t>7jNx=8q?(~z1iHVJk z%~{;)y-3zKk$bN*&fs1DFULz=3>Y+e;q2~KRv-{ZO6|edrY1mZBUnuK-BJYwguZIY z@k#3-`;;iiFwGDO|ALO{yZY10v?6GRPK1yt+<_!+>!{7{IaWexfA;`^ zx*($F?g@x5dPTs&)6>Dh0m7VV3l0aU&TG}vM&`t?Z)R+Be28G(H$nCwW%{HnEbWWh zB(e&Tixa&?=sB!ZViZO2JJil(weOw$7bKke$p&^a!|o6=Fcv|UT!7c*b(UPNe)-9{9yqGqTgE9K4V&Kw+jlz~m#i!DEZWAsczj4bMA;uB(? zeSX~3hXUoP!NrdXr;oCj=^M3XOUAI`$w`~=!JRH%j{Lqy6xH0gSU<)uuf{#18(wj( zE_hnstoFx^J)@FE?Bv!eoO1&4!L9O<*3Cp%+S zO@T@$e z4Yg5S$vAEu<{-J!$v1S>6>Hlm<-v5)lB-y)!;c_P4nCHef`crmVwu^R^~0Moi7tn? zENeRstxLpWuTQmWjSfGg`V)%(qp%RbtqD>9~JYYwgzlWav ze_zndLhg9+O42%w#_x?dsEC+JDpN?4mxL-*2SFm{_CEI$LsZ$b$Di#H#bzb{ucoVj zYU=;u4^Rm~rIe7435dj0Bm^X-JCu?VknU!nl)z|^6r{UEYLp-d!azzGFghKKk{t1W z`15~ucFuOrdwXy9d%yR7?&scjZ+)q!sk1<4z|re?!>+pNITU7ua8+^8Fg1LP8EQND zhm5NBXjQt!gku7o(MEMeR+2@gw%($Ne=~pe}FujLeIJA)yRNEJ97ZO!09Ajz44-V5>- zb$r2bX?AQt$9X(j3=W*908}$Y1VnrqXw;;Ckz1t$OYsBWP7?xmwEzNP3ErkR>a z+_{~K8kX3nqxhH;yX_tv_7%yjd*o zUy0l+Zpd-cSx%#cl6C68q;;$(t5V@};^mc?@Z6mE+UQmBb4<^Av@bZ7=}>lIf3d%V%uU4i;#Jl_m36E2g&M_oTcC!M+=4l;61-_ z6mm>#CNxc~gK&1oF!y& z7E-*!&%%ia7Q0XG45?KFUV)Z%tn|a7ban9X;};>rk5Pm7>?v!cK|hNC(Fch@i3349 zpC6`HU8)=PwwNJNsu;B+d6%?W$OMxT$)>v8I*FemeoL7u=K_w4ojyBb2i60PRIg^g6T9a* z8S6Xfcpn7Xx;6#&TpVY6EI6{ubqELTlVJrUL7{U6UY{yZpH)d$`9L)K50wKul16r;MLI)dI^??7nvl{USs$GBHI_>w`YN{z=3=p5EUSYw&V zfm`HwKqtR;o}fue)(^D-?Sjqh0b1!RlG@Z9*93CMM^)g*_kG^QYLc+zUkT(VRdM*; z#FChe;0p^+Qsl7eHiD$|sNKAIx6y#f_WV7gtU=aNC>J1E;GsJgnq;Oh6^lFCNjN(T zJCs~00RPV?|0N5F+**~M0fEOJxIgP7Ro;u6EAQhuvu`du+%>WM0ah*$B7^lxqq~km z82kG4I}Yr)yvyGd!o7*mO?|PvI>~NY*Msl;PzQszPl?PN%x??UNVJ}4Y5#mW%lm;_ zXw~8VuiJFbo+dtfSih2CkZef5JNethd4~h+phxpA0ya2AinKZ{sjIVq=5RhSg4kCap5`U2o|}YA5?j&!1%r7 zj6L#q*VpC7R^^`UE~nKE?)PPgY74KU9@J;P4}JN5;x~cMpP{+AcV5nZ=^V04FOvcd zR2KLlmLcE0IkRs+3XXPwb(x-=i}pj&&&0{$tI;L}2F& zsLxgaGo6cLh?(;z!v|rH$n)NYbVzL+FV=T-_quSmr7#ukDL?q+HY>M!I}i_KpH z;rEi`bC1Y*Btv3jF3aI{zEc;I*{>@Z2p#Suuvh`uUP*@$0m$CN8-|lDun`Xv6B8WA zWhAx1ULJ;SKA1d|lM}awb-hl>NPFVv)ez(kwGts)&7!a?!pk-nO!eammU2|12^Cp$ zFElRKp|0Q$Eh%9Zv?$^x5qwQ2=cY;~GxY_}qK}=PeCxtX0>-AD7graLR_GLxCZ$B_ z*N74WA6HWg^7)*N#wzKB5Xk9L|BKU((LvAwE7*-f-f#5l5(81u#xx8@0jHC;l?0Mt zBV?+Ysg0%W7Faram)TA?JoCJgQ6wgf9o+Jq#4MufuJo@s6J`>pIKZA4Ki<7lqsHF% zE|Hjc#P4J$nb)8D=sg9++u~0dk90yKAVvsN9&SRJ5Z8euulofUAGa;G`+8Ezyn6Dt z@W}JNSz8gC6NUHO8qzK=LnWDUv6b5zy%CHHw^|tTjf|9aRe%>Rj@VW7Bu970@af#S za6aF{!cHi?C9~LTpavW&AdJePMt^vadFW4mbE^3VW^XNwGBK_-3RQF%Em z43aTID!E?q^Xd7$yj{B3+v8i=y(D~E{XTszH~N*+Tc0) zVMlddF+Y~%d(;c$o;ACS`^K_1+0Pndi)9Ib7GE$lnGRFq!UeUo<`k5qjBj}_!}+Nw zqPHwKHA08d^hHF|swbg$>P?GnyM@uUqogdVi-6_&W0i8(8RSdRtt%N}Y3b<^oU@=< z9aUR}=zIwzG3aE56u%MB%4l*Y4X?ZZR=*Ue$9lgP4pZsKB>Dm~lQpyS_ zSuOU3a*qpAuin}FGU!5_NvqoVSeby_(v4^7hq6J%LpgTV=i$Fu!;4(mDmXxiXF$85 z>uyavo4HnmAXgGWyV?sI+1eAcslorr$zuAqWOi83Dc+F{Rszbj-mamJ@`=W2y zupm!)VLLDruYr-qv6Bnj`$qp(1>0Z%Vba6EY=W9;W7uKfdJrVJWCX0N*7kH$V=tRNBBB5bxJey2 z|FBCxeE80lvE{z{J44T8+e5Vig?qgX%6VnAtKQJA)R5uk_~nW>gGjwHhwC?X`?3sM zM;d1Q&I)|D1L7b1a=OYh)^DQ?-D`%fGhY3VvfsdGpm7W{ook9NSciUw61;;;P+a#hS=Cjh35^yre}BP2X$55Ji?>wqFiHL--a_ zo}}D;PD4CZUHH0SH;E(lhODE$b>fSj8~Xl{lu9t{1@r0eoHB3#;TDbymb zbt3?fQC`*pc=Ym}2oxq4Uo~yN`|FN`^>`+e(eZqaD9_%OyaqkcfzktJW^>xl!u{iQ zk^N*<20ZASbsDSeS%dPKvAP-N}q2H9iF}K z?M>5Kv>~E+)>*njA1S~))YW7~G6HkPgk8t(<|H{CB&~piuZ(}nm+#HU9ewcj%+vv( znY2nC>X~Q1ai+3?xSTXsxiLxTVvh%!L43xnh(Y4ai?k(Eqmi5u$lAg1lz4y zb?4z>-3G8}H@&70lm|n?`%$hJx&!j;*FtEhzS0MIHJ({04%cp})_WhIz&`Hxf382K zxBhr8T{so%AMTnxVo!uRc4_>m%t;0mYw!LZE8SFcyf)|>68PE5BsC@VF2a4UxV7-v za?a}9S2JL5EXB-{!F~8M#hV`nyY$PUf1F$S^{lCq=^{Ba%50WNcp6KfV=4D0yy2J^ z$(VV zm1xo%S!)u-=`Z!aBEArweH;=A`z4%wqNNQBwFIJ_p5x0;^XI_Wgp{3s%!h`BhM7t! zbMd5iGBH_V-8T_4kN*N~#NH|&nB0?N!1B86|6UFX>a1)%!9*`c!Toh!_FSBD=`h`v z$Nr*MA8$Jq8#Md8lTu<03~QPb-IH=wXR4C+b>_NQ|5NC?eFxYs-4iS_AQ<$o=lKYI z* zeLxQa%o>A!B$83$Jj!$psB&Ml2a2>6P5gF*S#cpIPclga%Ms;;WNP3o!1HXA@?rryrn9$6Ju?mKg|4S(Xfb>M{yIo+w18PymSxDP zbVg$`kF10^iaQ1pN*P#x>&A28(eqKE!}%)ur(W)cCTIPe)@b9J+1n`~lW^(wE8I3G z_t(CHd4Te%YYn&C`auV*X(lWv-Zp{KbRgc=3jVf2h>et+IF&xbE}cPFg~r9B(EnT; z&zI!flV09np*WSAm4e@E)+u9rD3>D+{6H}-Q>~Z{+~U_|*>@UQ7W)b5WweOS zS)#5!DMr|?(LK@WbCoIEK?LOTKCr`g9Ua$Ag>0*1CJG~6Hrje`GipU2j}+z#3S(LDvUi<@1@lhu{0;R6O*i&*6>&~#V(4*pRF zNe0kFS=^bb0Fw;cZS#7y%a-0#!w@|EVrK$7v$^OK_WWS$I!h5pzA*&X{}1j44>nAj zToG4be@S#t%~1-D+kbl^KVDu2%n;D$2lV<=aIJrky=#2}s0-N3{(+$KbW1UR%Ud{x=zH-{3E;iy+<-gPrVrp$| z8H`_t76)SLQSuv#mfqKz?foeL>t797*1~g`tNUZl{#>`Qv2h2|vWL2l_YPHW%>G*k z3_Qy3*3-39FM1##KsI{4?<&@QBLM&sUnUdaKgaRL!AzEBKO7irRvK1!%1_1=@%n8Q ztq^BUt6l=CmRDeE%q;>}tOb#i=L35vbe;dE+}X;CvmwBnJC@yFnDdAZ%Z1K#_Tv^N zd3eT+HKKtu$NE-Mz?iQ{esvU-2^NW>Cx3?zO_pRYpj zl(5alDnCz+1^SK=w5?2v1x&sot*6(^Cp>L@JvOA z>yy~FrY{79yROZ8q4kWBm`HgKz0E9&S5EAcyua>ZU%yJ5eAN?7{(PkxoCnf5btTB>JryrHtw zGvdT)j=9GNZC$gCZAu94oP?SAz`TAmZDL*TNmEnPw^`h$kTLm5Y%|wmI-AN!!9!h;KKiRkcJ_1FiP!B#;5MXvWp%C# z_eiq)5*A%ME}QC$aa%CB-0DOqIW{(?3<+5EvH5phXCWLru+15vWcz*d%JcE^`puk|Ck;QP+FS3b9`uEti)(>QLqU%!T6i(WAU8?x1Hy# zf4yOajD!a#IEo$|3h0=#+gigVjr|qpfpbm&#OoS%6yUUa*oo2u5#!tUxQp=+v35>B zGu^_>Bk!CJ@Z-~Je+lCt9`an;Eu-l<^B~4 zP7y-jX`8hqr19d+!gp?lg>Ccu7#)J8de&U%zi=eh9slr`{WZG-h3HSizaT-$I8GD3 zqCvhA(KmT#4fi|<{vGz*U~Z;KXS&=!esw$wbh>|d&wV9oUq$=xnNN6vFwnuKjau1| zl=A_$MT`5;XtLF5Ug|$q<`w|5|Hs+TLIkK{w6E;_<7Z`-y|pWP<|^en-EF54lPA?v zemo6(xEeub9s=L3#2+mO)PGasew3hX5T$GwztpJP_Sa6e=8fX q%3=+Oc+%qcb7hxq%ZR%5Lc-^!W*UPxDL7UP02E|ZWh$gzz5O3V?9r_N literal 0 HcmV?d00001 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/out_of_lane_stop.png new file mode 100644 index 0000000000000000000000000000000000000000..fdb75ac0c6eb8bcf93eb3341f7de19e2b6123da9 GIT binary patch literal 183400 zcmY(r1yq#Z7d4ClDhdclgG$3t(j6j*%+N4HgLFwZ5+b5>4_zWd%+M)~NP~2zbc4h& zH1C7{{@?e$vv9GHxp(e8_nve1-se07t0>9g;ZovaVPWCPzmZYH!om^3!otS6e;fFp z-0f>c;M)U-H`>lvSR`+Oe?hk-nkSX9u%2Vd%e>U^FxZ~;bfI4JWIC&^nk(TSWca}J zAm%a4%ZGPk=qmZJyW`m-euWjbM$u0bF0rUaoro~2?kXhKU{X>0=mGb>UQ2VQwAd}v zk3y=(pitwVkaxoork;ch6QqoUL=_*@VxP#D-ipDZw9Le8*7`kBW%q{8t|MwyM=viT z@Cp7v??{v2h8Nx2J11Q`JndYFpcZZ^2n!DnkBsCE9ix2u^!WVp49=YHA$@I!euRgI zK0f9NU2yPlaf#C|K#0FT_Gp-!oSe+hXLU=tLqXug%?xFELP4SQx~~Q! z7E~V4wt1Pbn8kg@KL(^#dwH~pL%=< zb>KUS7-DtHe{LA7{LF+fI^NW3WRX3b)P5shHQ}i-7cg}mM^T#vQhh2j?CgxDmrInbxO-b@aHS_fQJsjXeP~Q^iRwie z!?7_Bm-M!o$<0ErK&Jf{f$ZUL9lGB6MTDU!-CAn%9faPc)-(dkaBcPw_Fv=Ck~=kK z|E8ECz|E%9hsf=(*T{K*zfM6A?HJe{-r(e9cb#^Q4kc}TGF<-m3SePno^4b7&^m`a zOW;sZrl!zLiidHth5vL}t=)h8a^GR}F20#UWf+^R^Z}HyeQC+&z;h;DaR-Gjl>YM; zRwj9;Ay#IDFjtYZ6+GH;RfG_afFLp~>_w;wj*a0xsRmHlH4ABE%?zj8Xfq!bD1ciD z`H9>578X`XY9&|0;~!X9LUD(Spr!d#s+tNgzl6loM>yde{)sgPI40|FM^V(wJWs>H zTBAXxwq$DIp&Q1KQp?_M%>^v1w|>A!V_|_;O1|SFpr?sdca2xQB(T-UO|psZ-wpAu z=YIX*?(H1r!svYyHr1v3I9OPeQGtYE(9?(7ls|sUJ^_wa#7Rw$pO5d;5c0OUg+)@7 zbXtu;`<8%-se1ZZ(Ax{&rqc1UH@;0x0|^}W8G5$q0M8+Qct}>p`H^)A<8dqKEwF%Q zLZOWQvGO-ZP>##X%U-x2A7xr<>PYdEx3+gpdY^Z{54X7x8(_KX?=2By7E?e(ccj6`j7pGZ%#?GlQY(^=U6|PfBviW2Y z*-mIEAfIId;WjrjGJ4t9ErCHw($M+=#>D!7!%R87g_TKKQ2>5V6-?56#www23=3L6 zSn1hTW98ZCJvtxW1_AHHlDgwOv<>=jw%}IIR;66PzRf%|QC|86!DT<_&WN&YI}6bm zi7iTSeUy0fZmdk?W6hU$w0Rx4SMdWSTNA5Ztb`0&j+~IwIq?fZ6cu}oU5xyLhDs+E z;cUG*2bmd`ir7vpOC@4?kB1ol&cH&n2>QaBnr^AURjw#G#;3rl3uV)@{taCF-tNxM z`{ionhZKPM<>chhY{<)JS1;(kG3FK&J?iXhv z>H7BebOTi|)JTCckR>FF^Tv2sQVowg+S?~8N5|M4RPpfexM!gY4igo6h;|@|J4301 zWyUaC8$fEf7w~d*;PmoJg~c?3YsfAP)4{hI8ULrUq6C8D-`5AM{m*vqBWX7Nc;vVO z?k(cKtNjpdx%Q_e5&ClfwVd3SFJF>SXIEFgqJACYsw_{8BcObgh3%!dyAuoO!derb zTjA>F2rjsjF_RhlDW7@T=Eg{4enWl|jV^G3Z(d%Nb~x<8+^8~nE4aFXk`fPxzi*SD zrRDvOfj3n4*`-ze8pArqch!}H;jegEIYNe%9&m&pUuQn}H~csp6&CO*=)N#SzP+O( zoVUvB{rf>8OVi%h9Kw@fc%S|Il+4W(m8>3V^s@~|s~z`d<0i-~Slsw@h8xc-DC^%_HiH$HKyfhb`@G}hregERxbXPF2c3C}CYdxX ztakxyar46KiExCMR^Sqkv6;SmAma~jKk_z#v8&26KmU2a!^WSnxwn@~`C0u;3fBs7 z@0*w9J2Fk+L@6NpnbJADQazp48L~6U4SxM?v{U_~s9OnygsqIb1SieMng>B5 z4_(5#K|a8BS}M`hGke!V2g$vU9UvOSLL^*gUCEu;r?T3E-tpmjA(pa%@-jB zoZ&-Gv_%n>vT)CmDjfc5?9MH^&AD25P;zq0J2U37V`7d<%*4b9W1=GG3B_08RcBXOJURQ9HVFZ4L+K&++G7SH~g0c#>$ ze-UJo2H~Tutgis8H^fVw^Y-as&M(7-oo5rHU>nahh8~dLzgt^vXKQU|V4$O8pu>}& zXZ%E=;D4Y0;c}9$h?0_$2M4%w9);G+@y+}D`+)o8G$U^Tn;;$nK@Z{M5AqNiQ{7W9 z;q5DQ386z9>%EU?Q~wsDHDGZegjA*H*Mh%tEROToI4LR zcQ8amnAYUr9|VLU3k&lKQJnt<3$>jAPyzvk;^LFzO%ng-1c$_WCDXC(%I>Y z$Ez*+{WT4J(q{*-UO1;2QDU^D^v>l9PMnY{2!s|pslHZbR?}}Oi?u5PKXNnJKa>7` zE>@?a+fJ8&th)7@&LWAVc%1Cf?pso-Rsg+~XOU`ghq`u=$H2AUf2Jy)WBZ|Ue0W&STpl*d zn^rS-9~+iK%UW)XSDi9sSYh$XlrAb$l^Xzy8WUjI9KzMxkvMwK5O&nvhYE;|G%+sb+-l{ zS+d)I-IT>aNI^1vRU060(<<*$_BU9=VfnDz`8M|JGT#Qfy5uhj@pmV_HV)Q$T{RRV zWPN^a9d3-XeQB5aZ~0$!I-mmY?u@AG-lo6ts#yTu-+0w7k&uv(mW3k}8YoS>lt!vZ z5deZNZ%?A^3v>i>&2%e7=7!2RK%8^Z#@PR+a#P>>-pz+9du0XBL7_y6gnQU2Z-~sD z^xOU)uJk*SQd9XN3II~`l!C&DG_A+u4C=c}lA)%0m5F?$QzgsTuG>DuVF>9-zjJS~ zlwA6M1`bGWhqlAm%jmdDNOf+bL;vQPPP_SU2jt34xIJef1W0y;{BD@q2I;?rG>ahh z^~_=gnq}yA(M5x_f$>t8#y3a$sI;u)qVOa$B&fZ;ee?JFzvzNBqOh|F0O9T`Gf{DI zpPP7BN(cbCV+>~eahNAek|)?3o_)-i7&ELw%SYGol2-+r1&k$OL&(Jdq$Qt-s3O21iNLeAD{>?ANr_9XENhH&oo0}0beLSJA#k+e%TFnCkD$gIkNUwP9_I>`ps3>)r zZ+F9J!W6XtknvLU#PY!imq-QUfCmxfE5XXtkY1)TD%!mNTqQ4k6F7r*mfVV4oBZz} zzyHS;Ix6oLO+LumI}d&R?M(4ew#I)O;8$t;-o0V_!7F@{=#7ktiAnEcHsHOKa=8GV zL*p%d04-6Vq*eaAI4ZoOA*jtvJe6W2-P;`i93bwX86hGE7WZJ}KoGp?U+yFSTmHKu5RiQ7?|=Cj!C9#0+0ZEd z9>4x<^KkpKm*C&nn`Dmy7~T8n+}2~;+Aqs18|RPfOb48 zzW1Mv>S%NK-9HG6tWTN6TvEAB<3gPH{`;KHj<6Abp@y&ySswMc`e;y!^8vxY_f z2h2)#*&|TfGdPy;^mGZj5ShN$-v;?bdp5h4m$OS59UL5@nhqb@2hCQ%oJK4w!W7;a zHoVL|p@)@w0qpa?g=xu=0{KQ8IvWSGQuv}Z>f?f_y8D`rkf~ydOT4GryXq4zla^+i zscDp*I}f4gC~Fw^4~6YJ$1RsFl0t*z-_MFJmy4Go+3*BSc_Zd|TMip&1ezV`yE z@EX=@qQT!eNK|}5fBS~81EJr#g#B-ljVBw`rUGEa0INKwTji}p{8GBC^j7zvH;M(u z{a1qKsC+tmdKSCdbbTHci8d@2l0MFV6Ht~j6?F@iAvqzax2C41uEvPRj@gic5O!ht z(uXU6I|4rA8xh`5K4|IUOKH@{PAP;AzRxpY~n3UU`P$W2$AmCVQD z3WPQ|5lSrpgxtDi&HsT~G2lP&m1zzNK^=r^7Qa?9g15C!{GKG)Q7S07V-Q-23a~e z#Q?wgW72P+Z;Y_q0tjtPEAk}M`rW&CrlzeYFFxh|vk_K=x)Sf(#{Erl?a@x(?MVom zJI;tYF)BKm(7d69PWL)%a2>eJUSl^LNq0v@=Rc>%LV%co^EaS@v;WQIN@XatQp(by zN&g&${vN(G^FXmYE-c*pjZAm)gZi<7t+_4l?W>uv{8Nvy#Mt=C!jmtySOzU{t07ZS`LZd}vo?DQfXRtEnTy zsj3&NK6tg=g~~h$e=Ya=^~7jpqOTJ$>+GxlCUjM%e)_a8;R)aN%?MCKdV1r(gDgv! z|0<@fCIMXAW=}0KV9cwR|BXR|kIpZ@s&r~`jDA=eWs}c8I^Gukvx(|2N8v5ePhX1L z%%c1A!dkn@zl`p8J>VyPghL;FzfOlnvCwu~P5Y>3^kA{2Wt-%f@ZSZ9pd(WC^OGb7 zM6h-t_j)fQ7(#=xczk@k`n~lgL)|TyxdwK?%cTdam~()S@dyZ*KT-}4do`x2gB@>a zYrC_tAj!0eC?)<)Bm$c9`8`mPnozRp?14Gbvi&ee< ztB4hkZ=t>J_)7sg@b)%4p`xN9VAuWqwjjT^boUuwz~+F--NUl?5WCgtB4TMPNtK~7!$>+5tJ`GRA&s#N1~Eg)oj zd7X{qtA~b$PGxzCBr(B&K=kU>E9^yP;4C<>Z{MPuYVvDJ*8NpBZL64}YxMmuWcW~? z_XCfxO<4@lrUvZUSne6&1FG6UyxB${g53VJH)4n1ukNHlrd%g@|GYh@EH(^BjofCl z4!gOG+UAHy009Od!t(*`4nbTE)F0ATMeIdTVV3pv)uq4GhkPh&suqdmBR(dijoZ6h z8k~>sXA4}Hl`n$A|6jLqNPTS+IQ$VgGE&}aV8Ik&cC<&#FL{)W`8{~{roj9BsliD% zty;=0D3GGsCpR_Rwc(6sXKwES*fOf1to%&-Nz|lVqZ`1C(8|gvC~&^c&dwbXH0SGb zlGa|kNdh>@@vx+;$T3+4BJIQ9QsPz#CmcRLN>(ICC*Dsp2x|TsLnw%H<~~Q-hBNaF zfw(-IkAuc|p@S)@#!C6o5j{Fu%ah=#Vut9~Mjs9!Vn5t;t@mR5_=Cn@in%V?P*c+# z@JDTzJmZhZPUC~ij1)~-?uvaW`}+OYKmyX#Bs;%f-rmgLg!Mtkn((wrN-ErNY5)6@ z6RHN15On|F0w$s)@^rk;2+deO_lE8{1VM)FbgUyWU4W^Wl(J2pMIyf@l34lhib?Jqz3U}7lN(~^jXB) z`d4eWmQJ7y$vf$AL(Ja1vW%>f1%_X+9Kt_7H~94`^k2-`{wAe@7!qt{+{~lud;f<8 zK7E+zvFa$t3_BYFb(3iV+M0t1Wrco%l=_>SQ+bB0z1o7fj{>E0_M;4hsITls#+@YV41$50 z5}APq?K}(HXPqc)6Zd!v3Y#KwT0oI5GP|g!+vq5^6VyspLW7pKJrs-$x^v;D}$X z%3_tPedd7gF+~WA=crOZ-ho$UTDxNa@y2|SeCK_lekJ$bk8epm7KyUQLy)$aP`td4 z0#cuU*+0n>z)GHDQbS-vN2|<@`NhR)24(Qs~HP5B4TiwX-YWzt7A@FA|kwvsxaK=k{n za-SbYnS)nC5>%k(mr5+ z*RxCLQIr8rlqZW#{xQSLKhY^ci}_CoN6VH$6p{2$!$8cP)`uooxhsLZM`pU!8Qba@;J<^b%UpBJ6y$uu*8!6d1N{3)!v4yHm*!(af zmlfx_Q9o3&F73^79H=>xtvjg zuBdO8=mTy&p{f`hSr2_o2!z)UuQvAZ5*qjz|Hd`sc!=vHt{mH}1NKMIH$?8UeUU$Q zmX9Faq14G*+GTrQ*iOV$pfdO36B+r50S?=_!YdYv6;UBi4!J0~ZSIV@l65N}&;aAduE$@ScuL*_|jhaRW9NSg68i}p3Q zgmF+}SjQ3s(i8eHC=a*SM#Ed^$~rPTufL<(69@uVMS}rbYjShVU_$&8P(0pQ>-C8bzESq^JE5Rhs=n`0^Yqk(qiLegbPJy-c& z0UjP1aly>G+f3R>Y+BX(UHj5mrw)^Ecdgk(GgLFw1hd4-6RL6#!W*G43>!%7Ht%6! zp}PV6QSw!{fdO^?S!g)3J8tapTI)wjb$Bajgi=8P2eFWm>i>EHxVjBI-DiI$F9K=s z7b5gB^j&n=zMN6cAkm6^LL!L{4F}1IAyrRY5DPz=ea$YGj4ladFjA~}vJ?AMT#3Xl zMu$lB?8vb`uYq4ww64BBwYV7!SRV+rfcv^tJ|A|Gf5s6^ z3w+*`S&~vM0MHuFX-CQzZF$psXd{?r1W;2+~0{cfYCoo$FNq%{QRRoU5H- zDXGujAgPT$_y;peCaGH&;?TA&froYDvCOLdm;;;2vvsBS5_>b*3gxK zExSDOyb@DKl|?9gV;n+2T0Yw~GUmZlihS<_UaLr!L_*hXkUVrx9{5GyEKF@xSD9V% zoAn3h>Wz7Q*9Hx5f4FMeC{m*ikdopBarKIE010pb?KL_2G0ZBiaa`Hw=LxxOX?80Z z4;hqqtWC0pewcFB8bNg$&)GLBLtUJm;9P2TdZ2OK!Uh(wf5|>yH9C6!l6E3Pw>I^` zwb1(ovtvS;*3h}jjHPLT@-3OT9@ZooyVEl7jh*fDVo+DsXTRw$XAg@7nml5<%-r%} z9)oFJtUPit>#nL{K|}e0rntnzx3DN9;-|9v(_TbO!npda4{b`#6#_u&I&6Aw<3{t1 zUzYkLpO6@siingV9p=$x2{}krl$vTpSv@k-U5`YoDH>TaYvc#tK~A(=P>yU zPBSh=;B<+iEBPJqMegwscZ`InqO@Wz#?;Ig6Dxe0u4+Or4;ves!Qn-?WLgy5CWSO2 zX$|JuaHPQB0Al$if72_+kU3TgV70ZiwPVy~l@Y8YgoIonKW>fVVM1rCANr@I7N-RX zMKDM7v{|gMEo|3h#Z?UCRok)vnIK0uSq(jMeOquHm2xH z>#sVvhC)}Xe3zAO5Sg%n$`IrL?#^qGuAJtrlP-xmS2AIJn zpz)wga3!vsprFNN+WDL+j{R1m%ApoIA~q~bc7QzTZ=5HkH>cKaHVk@<=I1G7PjVSH~~TU!`gazK;k;9bvTzSgH=H=0S; zH_LAK^!IdC_d_zF2(s##_G-I6j{0K1FoV@2 z86t);#C_sC>3>DgGiAet9xwO(97vr~dTh@_y6Bm%uVbW;&y#<2a6w0iSVA>&b;awh zdhNbzI&f~Zn$B5q4%W~~@-*`FyfsK76_97SEN!bJ~Ds5V$WzNl2pU+Tc}20 z0Ek3gR|a2!C2ZtjA&*-&060;cJfp4MNXYzDiS;}*ejKUQlgRD3E70E@aE6;>Jj4j+ zHld^QysEG8tR8#R$=98~?dGL5-H^us#^)YO*ua#E^(p$I-63z6K;67Qkk!@I4qrpk zITk-Sjh5j>1rY_G937qB&_uhhDl7mU?JO!P(S^yHW2^V{^k6GVy2g>zfn{W@J)aW> z{xs4}i*L!<5AJ+#HR!z2WuHBN)#$xK!>_c+P(0v@I$w>f#atw>RegcCdSB#T zs|WWNp8Ww#`QsL2O`_+Xg}05$eLd!#@1p$s6<6F|N#$HjhQjUML`p~Z(*!+_ecK!M zc++Py6q+QG!|QYWKKQH$fS5wm{Mt7CP@XjW+UhWtSoIM>7C#vTxN3%{dcQBxT^+Jm z#(a!J$e2>)sMuP$_oV)ZTh^&5&-+t-IwFH}qVpeU=}E6QyERP1t;lCO6uDO>M# zAFhSpA_l7ebyYjg1I0K?**oAoGqXM~o{kckO-)TcN)8nh z_#1|xP6D}kG}i_xErX>R{uWd z_^)TNMRm*xiOLNJm@dr-)?iQDZ!z+wT=fiw!m=NiC(kJy&-Lr;pkvI6_0R1l>0M^& z43oi4r?!qgwnJ5-*;|rvzDv4b0oNTP8`ZL|cgvN_sO#Ugr{7X@6=!@C#Fs#5f}ZwF z&@=Spuhn2E6Me^U+4eq>DigeqL48A}n=flfrVjsC1wjP^r@iDg4L|Cjb?!T*4&zcO z+a78ZdRz4@;VL3SRKssSTp};V;QXHJJ$T_4ryaq^(L>J92=i~j8gg9EW?*&P@c_=4 z7$~$4a~(hpsacsZZG_t6&Y_wLM$5oR%q&;blnt4Zv0R48P^UK6uZP2X9*vjX1tX+{ z(Z?9A%RAKF)q4wz0U$-6o0w&3)j>NuxK^;Fa@x3+gr8@puwtfQO3F*Y%QSR3DfNDw zoVW0FsX$^$`+a+NX%hc+)7xiB!6wFXhAUQeD_!e~`6`P}0E9*V&pN|@_Q_3`&m?P6&CjrHui zQo5%Nnpp>;1XFuhgMdh9CSZWZ41wR`9va)5TxfLd*|zz@j{nd`T+fQRCL}@0VzJ9b zB#n_Sp+7dgQB5dI?WWwwLlMawA7=Yu$mTkV;-o^u5L<5Yj`E9_@Q^wOJ*|e6gJu)p z%Qapr={3oz9nez+p{4?u%WcUKKN z)r;n`$&IoxnPg2}f%$VxNXjmVVbJ#4D@F7Zr$y(ti%A_DzX!nveWy=P$^!E-BH%=U zJ4A!q+Xaq9QNF8QX$XxQZ3{Y$Wkaf=`r{uOEZ0?qck@|+25>sxFc9@ioWEmv0$c{* zFytbr^9?jK8H^6?8u=3;?PX+!K(;MUfr2^7#G8%^B~8bJTREy9qzU#oTa=M0gYpqG z7n8k{WZCI=il_(}!Qm(mf9LA`MmII5nvRo;u=;{4ppZs(p zHV!09>d0e#wio#+R3`Z&^ir}%uK+9N6XfOA=Kx$b^mKPi*u9Yl&;VHo;f}Kg&w`<5yQy zyx{Pkot%^xt)D=jm*`@6*uMJQTeIqso$uk$<7?P5r2PolZq%Rn&MS@%*lv437xRsbCpW_# zP2;pvFDZQ06nDgPPMaf?PAu*XNpP9H)yT-gWzd^mJ9U3_(gmV-sF8e0|4&kfb*6`7W>)j$ok@&(KO9dT8KvN+IW;D-=aR-PeF zb(GiY`0JixzgO}ci4km|OuK{RD!ItDp{|(5b7ZaXZ8N`1YhyE`aNBTgr@(x164Sz` zGwQY^uU)<34KFlPO1`Fa8G|}%bi-<5HsIckGQp3&#^iL$$m?_C!8N8>k&VY}=N3)) zTB7fIS~7ByOaCb2M@U3hto>OOO&%Q&2h_t&CREmKAi7lE72xL&l>X{k-&dS&U2$uy zEENnr&);0k*_uR@B?{~-Dl3z*6oSM=d|X^A9E>X?%wVY<{lV#a-UjkqrPE_{6?>2C z&VrX#Hsn2b9}%*41-@HG&$M9E4$#}x(I4(cPDIi!)>GZ}t6sEgs9HXFup?fywdYRY zO_va4K6h$#oF$I)z7V!t?kq7yeECtf2|XUCWI;+A59n=iVBTSg0nDl&hXi5>$X1%H zFCz;(??e(tpIi{Sy&~MCFI6*ij@wsPc@_)@blB}9Q;Ire*XZ#)f7waHYk<*KhokAamt0{huaRr9)8>cH3s&Q#Agen=Z^UqGc9VftAaO#P zFO`f9uxs70b$ZSg-Juzs}+ei6nMN*Mp-P0I)%0(zTV9+=C z<6EGdb3Eprl3XX1CL>X>dajbIt1Gvw$X}(WGriChUQ&fua{Q8nq=30>O~bVzo;3CW z1}M`m#DR-Dc~EZ{j2EIQo;~A=B1a0nq>Ip8=IhdSo7Txre`z#~chDC6&h%qkYHebu z>`?C2X!)MtO!Wnr>B2;k$H|An*>WGal!oxr4iEvuzuk85?w~Hh$N=a)U zZM4m6pHhRJ1f(_COkM_&H0|=j+3VfGuLETz0F->a{JibY+zs*G&WtJP+5jBg&!{M% z71_e(1_(yhT3to)biRwM%&;*_&~-awx~Ev(R!QBSuLaGmp5hmW2&!WSTLO+NINVAW zPQC@tTeOsq_~o!Gz&-GDnZI2nKU*drh$xUk)k0plWYS2?Sx-;Qo@QXv>v7G=U^vWZ zxnS?T1aLMFi8}zLVyMzKLx&AHr}Y`Oi>-Ij3$%_efpUtG4Gps%JG1li_6sWwG)L$i z^Mr1Is+WH!4+kn^ThK}a_0hti>FJ(|w4qIbX7H}4u!sodO%HvhmF4L4&5P<0j*Vwf z+z4G4tI|Qtx^Xp0Yw%O6Y5j}OI~&!JJqgOu4(Olo6YtgDM8PBNjk9y1$a$eU{d7Mr zvXnte0%7IQ-}A5Qqtee_UVORMZanDQ{4yN~Z8BdzJ%nA_dNx{#mOGsu)K>!Cwh&pF z1osR3q73iPvwCz3m0V^7VhgRUV2(m$)G#p$mrJ`6D4+5;otG?!==%&B?o`xLg4hey zgaykgHv36H!+Onq1r03~$3=hmn_1&8Y8zjtOJ?ldf0D$xA{wZ0{u>k)pRNDJ=lm%r z`mDZ0kH>Bll}XvDC1{mW;LDRw0cgk++;~4@48LumMq0s_`?Oize*RP8Zg;cc=bHoF zY0)ba3O;LtnoCG;61JtKrS>iFrg=kUY#2~|Ed&*In+{c`sv=AA87u?p7=OS1YFpaL2(GsUKAx%o zc>%#Y-7Tb0WgznpwPJq~9RTwF(u}ZAj2Sc2Q6%EeND=#IDq*PU$o*ml2!Mu#qG=|=)KhCm9A&@$Tr;2O5jVKdk+YFMb1*P`!9-)*4nGW=WJR(^9gvcOuO zyosMuR2e$XdwG`ObF5u<))*W3{?4Uu)BIlXulsJNL(^byrS)2<<@}G}r;NHIG>buR zHX9P=OX#aTxXDsr%P3a)dxV6ntrqoHh+~>ve?$IuXk09b@GG`wF}n4M53X6tO_~NA z%hz`MIs7n^6NSW4+n57sTTCcg&$hWDYQqxbxn(ep0dW)#Ex+?FG`>;L@$8T9Ih>bo z#*8|;b2CG(nNpL4R^K0aeAKVvhk#K>sU#EH=WDoSXG7IffaJw((93mrmB`MvNDYuBq#1nnY4txZ>r~m%x5z{!G$V=oPQcX{nRsGe#aG2bplCS55Hy4CEPy6|34pZ;<_iGfBamY~yX5 zG=BP++ew5g&-w~kukNXEd@K1wowv;2>{WUk=BoGZYdi5=2s44sIV$^@2r<8)!^VRw z-~KF2ocQ^>$h7v`pzV)I`}>Y2JTZG8=U;Z)+!={fZMwdygV`5E-}Vo^%tdYi9XHVM z5X(XyHXVu|1`S-VyBgEE!>?j6oCUK~=pE7oq;|@A47# z;}YUE%e|MSkcY$eX)|J(e=cJDQYsb}^YLaMrr5oCFKFsHcrsG5Y~39+G@X7geafLB zvFEXB1ey7ifT8t6^ScQQxt@%I^PZ3j%pHq+*S`m9h^H4HUUWNPu4^`V2?=N49k;lD z3<_$TuU(nLo1Ve=ywtB>8DgD%wa$?1w};d<^o-G`^OAjBQGKoYRgzjrf%#ggI*r9I zqWws&x+?xcCR*7)4jj8v({WjNZ*mk@2vy%XH3V`p7tIdTB(t^+3^<^C2tlO+AxJCY zm(WTI6b~o@78HKg<;4QM{Z&Q}^Xf#TG0_Xd>UkVCR)@h( z&mAwmB22fR9`ZhXN#?lu2Hg~vep>F{IP8V_mgP5%>2Ems1qcTFHSppOK|!(gyH}Bd zKHE*s>0|D^`+k5XLPtki5_?!dg%KOC4ocaeB1(SbTg)UPf~I+y<4PJtwvI@Z7J%c{(=<9Z5tu z=#yjB=Q=)jK}J{2Kord^AXf_gGFZ8@a8Abgcn~ui<2%ce)aM}PmlRZ+uUOru@2QJ< zN;v4f9V7OKT(TOJ%M11X5f|GXI%Dg-%;;8X^rjBKuDD6?%MvsnsJ_#K`tgoKtnXC3QjwJbdSJ*Yt=YR^NU)@ckELZ^UCN~yHPGu1M*_9*6FXm$_QdQo={hd?#DcNr`MX;A z^>unRKu%89_0G6-vF;ol<$5=%==9m+rFPAOx3|Dd(P`$V z?nz(5A4mUoazFOG(@+!2Y?w%W`Cuhz?KDRUvR-cNY;iI9&F}PV+-YA}&B^7IBcW%h zam7DjwLd)VJlH!J&p2e{czr)o3~1_et?XZOlBr10H&_uk-E=%z%xyaRu^y-!d7W`q zSUi6mbk>e%qquv9kr_AiIlMQum=r-(Q5~Eo37uLgp=>84v{hY;)IUV8ax|qX1RIN% z7pcAyp%d}xVYa|Nyv;4%Ru^0~Lu+pq31ft60_4Xn{|R=>cRd@G{TcT{zCMM0VH!e5 zvWPXDiR__1T+DV{$4gJVy#p=Xhp^LMq#+vKbn1!U9dkEwL9AtiOS&H$?===Mn6uU6 z^d;`UF`@=e|Eil8C|i{exnL}SC-2hrp~gFrSZjFXk-@fR$bq`G0-F8sEO|(n>MRn(Y7Cg zWyGFFj$WDj<=|$6j_t94N@6eGRoHo%furuioYcdf0+{L(Pk~gC{py1i*8H`njy-mc zJqAh#QCTdL8g>iPXTSWq>d(LRCS|O~7e{5i8K3=G?P6vz7iFQ$M)4}+F4rGhU2*qI zd}gmQGNqvDy~QwbJ8CYMIR-TKV}adenMk6HekEn|Vuh}Roa`OizR~4|b4)_x={Z=S z&G&gUdHtyC5-lC$p2P(P1vE3ecM+FGlg^DTbQP zrHr+GUKtpGn=V6qi3uG!6jPhvLB|a0qDi(QH35Ff+8)>Kh8N>$J*&K}5@iSNb&8Gi zHnxNIJcxJV444^a2|tD+xtc6S61^I-g(mHh=77BOY#m{yK4~8WT2+a%2SH$$qb3+=KGm$4OU zI!3fUkhEoE4i#X{RU=u~{)UElDwwVHl>#jSeEgN0vd04hNH%XMx*bJk;!;Ss4Rn)| zT6_d60VJtfETzFi?T`IZJ(W-wLCRi7b*j>S4-JY-rM})L_EvxXewWO6?H2%W;a)hwWKNI-me?mB ztHBS+;`Cr$W-Ac;+<6H*o!B-{9Yh)?B*opLFU(9y+!=lRF-Cc&KIN|F@dc!2BJ8UD zBbY8(QKr(pM?IWu>!D$OTPWI(Zg)!k3$2RH;;CCgXwaKWL4uXbQpv3v<#)DU>9X?Z zHpnx@%{18(DG1^qmq8OU~+?Z`;u1XJ%WQ}{j;?fk!gK2!gnoa86Y(nlS z^^d#iF~FnLw-w#}s#;KuIx#naxoCN}ZhlHx3p$$jd1;12Y0?KCxDut5`a4#_y`_b| z#Jtx?nS==jcs;xArlX4${57Ot#C_B-;w)qR_3mfqF8hBRo?jUdMc;taUJ#($7tOr~DlMxcyiP~f)Lk{+(jDP(J?6R0r#az65tkRA zX;M-0U9HZCduDx2ak^nqyL}(FP&iy;JID1uIxFYjE)?RR3Mm`j>+!%48~QNWw@JL? zNc#~RS2n|d-J5(>bwCV@6|v};DuzAv7` zz@JGsge!oj!VHWN{kJyxfEMt7e;2@R!ZxB3=ti4zSHGnBdQG9~UX}grE|!w2j-IRx z(9$XwQQ~*xHqeW{ZTmr>{x_eSp>@_^9!*5eL}R$VPAK=Ikk{YF%%$L3<-JHygTL95 ztdV-gdra<|$FWT(AwczM5&M{1W^q;D7_$?nh?u_s?Cn>W9Ul~QSg{gc%tv$9zx~Zs zNIAu3?aRq=`ccY!Up*!ZCfoMFuF)01mt0==uBB#b6PyK|(_u>1-EBE-!;*@M7wT?j z;bp{ZKzR)iS5QS9D2#D~*?vnBulh|pV}lcla-K4#3>^iPe@xhw;wHjUQvRn(1#}bk z%`03jb`p(9fXssSR-+=7xTVIQaR)_ff?lVy*lDb^{xhBoDg~7y|NI=+mk)+Zcnp1Vht z1gwF9<0z<4sYJ#vS$d1f(GVb!4mal(cH02TYS0^x9mqBg6`R%Oa5C`or-xjk>-V)h zu29tOz!TUW`L~g03@UhIq~k(%7${8NPs9GZq5(SR`+PB|mQlEWn> zj&B?JRIq_971+Z|h*b__&vk_FFdP_$r>1@6^s+kcf z)3-T3J_|gY(|FTB9}yltNz1|-V1;Emy>9E`aum#*-sxU0UD?~s=Yyi?o25fjJIs2p zP~co5a@FSit@nB%)VQSfcEFT!1kr$Gyvf2K(hk?KIZ^_ zvgx;Z#p%7GGP})#^#R!%+68o2n#-ou^s!HcG~x{)GrP&Bfxm5m3q06Q#Y+Pfq6Qu! zbCe-T$-f~Sg>gKSV}PL>TSLYKD;7N*5AEEqi=$GN8V}+CUpu}fl8Oq96$6S^3fb@; z(EcfuO~m^g-|>)BZTa0Yzvsg3I^hJU( zVtP(Q)Fhs{@tvkzKUB{*H z^7-pP)^Fc!c6OvoF&^yYF@C%@vqbpL0p)6zd+NnZZ*)iq7MAM#6V0e7Q5OkB8#MdB zSuE6Hj3@GkWav--D)>=N0{eJ&?H%|&j>^7S+$2m{R#+)k5U3ic-kRks417)UAT&q~ z#K=OTSUguHN0+N?B)g(klYktLrqAx(=@PL;ZoJ|~&Q`>l%qeHuXPE(7e7-Jc!9Hj2 zt{Sgl!ck|Z^C)*9jyr%91B7xZg~x;^31nHAO*AutfOIC+AiAFpY=6dgR0wf5 z<%q@BDk@W)ZZ`4ZCgB9o$1mkF8|C*a4;xZ|&ub<82ntkf`jP(B`mi_qn z=%)>>=FIR@_(;}wFq`^zZh#qDflo7A!93Q?kl0H;O zog>FT>b<~wG-%y>;%5sq2)#YjC0~D5Uvu`cEbB(c=9DBweXQuL?0QpiwLmz&C37)w zvh_|~BCmT3hwWbbbfco_E--&*9G)Q0j75h^Qn4iw6CYuChe%DnwcHWmKtrrv#bDp6 z5F+1p9e-86at{9;^$YIUKE<^ya7`XTxC*}wv{Vuq6aM!3bKY{6HQzZopK@BftQzay z{KKDB!-sgmz$E{4T#b0Gr!)6GbPgO!#eptVQ&+~u|2fFY48P~!txV5vZ(j*>hT7xz3;*j_GJ8X+FzH{ajUT@e`LfJL zRCbug@R70g9&C}}`mWZ#`QzucWftnsjZ8=J>G8Q+iPxp$ZQ^r3-uj+UJGaca+n&)q ziXA&{=?PPX;z=$9BsaJl+6-l`k4Xbi?Pr1TX^kpvr!1NGp~7Wr7DF+O_oe@FkF(DLUspOQGWK4d`?`NKwp{2FSlF?l*|-(dl8_r;B9!h z`fOmQAjVI1X^M_b%FBQvbIibt4=|>!AKQ=GE^r&SZ_M`N{~T>DjmY_=@Ur?o=3mFa zNf`KO^6_06s0-B{{f4UV5)|Q=5&3}u8ks_hd?}8LtF5kDb8e=a!)GpL@pG!$`MA_O zO_=m4X7}NSZU+yFnqw44D^jH1DCD@FezB0ljh0=UjoLyl!=Pt`N4B22+>aLi))7#r z&BmYZQaUO$t(Tag1etSz}3mggtLjNsi+dO#*>o8<1CW!f&?J<=u< zleM0@NPNVvRxwlUp@~}4wh>=1)xWou2p?1g$SgkxzcvNOEf5s8=_{`|05!de`OeeV z*Iqy!efiuI3x#Hx>>G%=k7H+LWn~jz(8aIk-;_DBfud+lOPy(>isgD?`ilSx!l+}C zx}m5zTj%MXgcQ##Tz-f1rOO{ccAG|+&0$_V&nb^uS#YQE;XQytfY{#&i+-j+;9~6= z;7l6+O%>q%CGHZIm>zF1$|6<%C;}-kWN5TIB{alFoRwV^w{O4e0y61pE@FL(EwgN6 zVF_$-{Y|kRcZ?9~nk(SR=4>tL^27gg}vi)20X}RT*vGv{Kd9mfhk>Fn2#hS_0oAhQ$SqU!7z6nQvO<7i;zH{dm zGa}=C!k!}VtjuAAP2TZT9+mD(DB0 zX?#+zJhyT*D-3pH+EirbeduQckgyr-b-8q}7<~hEzX`Nj96()*|Gg25Kakv zYUJBfL&tbEMLAVw22+uh zIvS2g!kuDrFNeFDl9fE+Lq?k9s4?0;xTeo3$)SFh+tFiQypCMvefWMTZj0x~AH~W| zj{%ZOXe6F;0!K7_L-lT_Woh~Lo=)kpQDcp_f!e^kj1PBw=@PHGzq_TUSUlipm6e&$ zqBcgs&^Wx4Mc-%}C<$>Pw(g@?S)j4ori}Jlks6(4wa0vUFx!&_$E6W26as>1gwbg3 z$^Ce}`n;^=` z-^mASQl)?-!E(1lk!OMQ6Jx+`#5-qvi*mv-t%w3x;d4=_HHi^*)r2~v@E^mA_F8JA zPc~HWjul(KEG!;ql-I| zr1ssWo#dr@k(|8bjsCp>^)W!O+U!e!;P5I?-#0(Ga~uQh7R1Kxb3TL=-(45i5u^gSs6CB5Dpm=QGweD_Y1?^1n$SkvF{=fAxGJ!BnI%2SYzWs{3A$uG19 zZmQY)QMphQokAAeJMS;mb;t`G#*~olDkCS;kDtTtx%@*gAXD>H9m~v#8r~Hr(;d;Q z&FJrGi7<^;hPw7F;;M!rrJ+i&L{j%PO>Lf8e0oWW1ZMRBDO-bs;QH??;?+-61MSM~ z7t2c%=Js#ty!jk;Zx7arXWOLsQa&eYC^Q|CIiDTw`>yTOx1}{Yg{-4**{r7V0Kun| z$1<_SO*@m!&%D3h+BF5AA#)Z!zDxeHQ>fysNaA|b`pxHoWIn1QKAkdJbJf+PPZ2Qr z&O?VfyPzRzkuocjEfcQzQ@gJd5#5Yh$=d?Bi9K=zgHa+j4BZg6?!1N3D@olGM zi;RTaOsR{Uk~*t=NS?5TqS_e3*g<=b@B2JDY4wDa7M{2VMw)s*8lj5{8;GO?pv3d- zB5{R)%GW_x%PAaonpS3@risPy?-Nk_E*$UrE@JVJT2d)IP;77-(=gj?^veL3qh1+> zx(g6M6C~NB##~vdyp4NWBM9_IFsTPD!Tm&%GNzAF(PdxeTm=KTtWv?#4Pj4M$qFFR z3TyOvhgtUd^YBq>A$@OriC;I1Z%g(V>YMikg>6}>*IRn$q-P+SG&K`0&9u8I=u~{p zI?(1LP?lHDLy3cJJ}Jke*z&!{EWJc~!6P|d)_g2FF24CvhKdGzghg)()0~bk*KD=> zC3RB;i%yv+ALW16rqw^#5c!$*iA2MFlfQb->NQ;nRSv-P;}KzHXZJ-HF5(UX)<(m+{@yN5a?VlLsI9xPR*I62y|d>I6G(4P0bE@l*;Y*> z1i;D7K1*6!<)-t|dw)-2tWJJ{ct@dD!y}p$&!p1Z)97cirI@}x(ssoq&MBEkFZUQz z%T-?(*<<*h=ey??op94|WHbtMb5DJ1HM*&SJ$2z%8cYWyHjETYzg_WSQ^ZH9w47{g zoD|zq6#M?(*Ez&1&T$7gi1hUhiTWQWJ~(=ECP2BDb2;s$>Bag};?{z-WxsW;UeBML zk}oY_Pr&6o9f;nu7^c-Jk@mfJ#Nk=?vi}fUZa?zz$^onSNQE(o_q~yqfyUE#Y?KugxC{ByQ9!NGKo?$3D z1E>Iq?9(>KHBFeV9l=a$gPjIM;j?-k&B2!MKOM zchEO5f@mk+a|UL#GOcKL6m8{%Q}P!8UrFac14$xwuQn#SeAP?m)vr%aeZPNmepcG9 z-}3lz!x65udH+5l5t$>S*)w45)km^Z0%tYP|m82W(QLU@?4e8gIpF_|E-Qt z)}v`%o9l)NP%xZcc4T=y6GM*yb@?S9aRk?m{}z|v8gca4b~FnQzTifPo9{IkV|yUh zoqdj;@VK+Go^vGd>Oo$(CF^LAkd-T%zn3s6p>j$Wu1$J}Eu%d{SSLv~-#9WzJH;ax5G4lHO3n)Xe)Be zrq&e*vol(JuBahRX?RkGvZ0ag zxU>;)%EwAX*{E>1nKnTFz`-tvl~dQYy4&1nBw_GHuKKOt5CZ5YJM+pd_Ev}E$s5e7 z-&m%uh?M1YaQoY(S=aiNxAUJ)KZ^BWHg^!a$T~eq#jGFzml)RirjZq>0dx!ex0*_gmLDyTcv|e&cXypeulb)mPc-v%^E6 zBU@Q{Jxhkr+w!%MIYG~6W93;x7#Rii%||wKVZVsH-Y4V{y-3~w;LRC2WV19eh{`|RJb6}F+c_76=2PffM-^OObjrH_# z53Ozgi8VAf_}Wo#djKg&28g)X{YbN&OG#^D{(UWbc`A|@a2j|bKz8!5fc9niZBK7E ziAd#v4-V1HjH{ScAjQ93Tua9r7`0Nit$hgF=s7Xw!$uNY?`ckvhcBOWR4ihqE5Oek zK@^^M`a6e`VrH?D2N}};F(;H72peBZK!1i)xNZlW7cH|pSkeL2Gc<})+<*#VHEdJB z%*N1mv_dWsQNs%}-fIrp$h%3S#;04g`2w5>b+rVieCY^_tH;4Lsw{PUzfTvsV=`_v zb@M`y-XIPG+F%UbR&f2t5&5KoS`kzb6OB^*i%(8pn}duejvn+c2Sq+6ZlvClV#4dn zu4Mbfvl5aE2-Q}`ok;MEy~wv0~y`kVDqE6aFIvO1{Mut zB_>o3qp;13oND)dY)|2XE5;86rR%2TfBv6DtqxAx=KoR=H4geErMyyzOpSRxM7~`Jj$21OpO9t8yni* z-aMXr2TSMSqGok%A1fOZv$pQv=zZ5U$b?!*izb1Bu+#0nr>%3YUhkc%X(){%Py$LO zoQ@p1{5e*YSR54cBUKmR zlIk<5^y`77E2i+seYO4kBbe3u zuZM$&nK>7Od^`v3+=1VE;Kt9p52FY}AJY>ioxLrdeOAarClscx@z+RgWQ=ZqHSrJ; zK>aQYxR?8PfjuY3@WjUR9RBiEUCepUzz{OY|LyoWK0tBBj8|mP47Ix{g&A3e3}#=C z4;}#opO1bC&!!_8!`@kW_WWRy-@*R*LaFV5on)VO;O4xsU1Jq?hr2=KVSXWmA$8+S zaTOT#GCnfF?LNSc8lZ0gvoiHK+VVW>HxKHU3n@T%!VqY@9o?TklD{fvdV4zIwmA`{ zXGOlk*WoGn!X^5z-Ln^_XIRdD=D!W?xKAELD0UP#DW3xmo)D3*F;3)BBXP>+R*u2Dg4|p2uS6)QH5um z?X656vkcYotKLl$q9mP;n7{VumdQPCmmia>$lGc-8O`(Op++yv;?}D`&%986G{s;i zq5M{=dKs@ji6b8)qa8cbD=wFUANSz^wSrhPm!ns;?|D-Ymi6It?E@6$s-gLk6%;X2 z@*C#aDYOuEgG|~TD0N2Du4m0H{D0r>H)eCHMRyR|Ql={lN|2owV@K zyTf>b9KVE^1<*fmOZh2w!)+^N!>mr_&-nOw!H_2)iswBhKc=f0*%{-+!P#MuP9v3e zvi!`6`Q3m@{J4!ChVsslCX!%$5a-oEMojKzrUl?ja+&No`U#p*4QZWGbLGHH@a6PN#DSp|NJh$oZgnhL1 zzghr4b+%UQ|Dtxv5^&CR)6A2VvY&kjfN$C~J0yWSA%=vGu0o27oxm@YvV?^M1@(Rq zHjC}px$jODcN*UJEDeWCj5=coefcx)&^6auTKH1SK&e+oZRNXNsM`_$XX_^oQNh4# zVn^0H`7A_SX(VBT_yA0>ntH1h7|#8kvPWuG@574$S{KhzOV&UEz-AYQ!MSgC+m!)} zK%CYWu+8On=I}8k#)7wV1}-0s5}4pV^R)NdvUV!9;Kr~2y)U1bRd*UtdEjEf-c_(N zMML6nn{L1hnL>URHC*N);X%6c_m8N9@HIoPY9WSmLs*7hmr{Is!r@mO8>vhwK;^}2 z16PnEllCB{`Vk^r6Jv%mF+43OVt^C2@!D5gQzexod99?!5WS0c@8(#z+&P(3v}$N# zzjnViS$(UGwa3vV*kOhcqHBn6ws-SwEqjdPl_7AGQas^I8BzZg9xTQ*ld+{zbFC1( ze@blo*Xxhk_%L(J(R5W;CctAM3QAh~79;yP__y-I82r&P@P=!!Ip%_U!AH2ac0bvo zeAA(=c(~tm`9q(y@SfLoJ%Bcg>M4}cp__CKVfuCbrll2f+r+x%30j*+v5 zBO=+3T|}C(Xl~dX3}ebxTa+%rtXq{U?t<58rSJ3RNfV2?Mlv4$(45SqT?MA)H~e`uGt1cHAmfUA4#p4?GeW^u zZ(KZz9mWB%^TYuDO^I!C!SCeFzLyL(Vp&fxybdMESjHm4mail93IW74w{!5{O}v19 za9?Gd`-FzqIDI_H&DAsYmaxi#IgzMF?g`TY!s;OY{kp#|}h0qMa+@N;cVqXWqQ1b;Bc#u?uZ7K%7h z1@jU#7|yXvTln+m`y&TXc#*;9+Iu!!p}v+v{z&=GB*LkI3D2fb+C5!~P)OrVqfS4p zW;n8pl-im8VNGgh6n!PrtOXq_xrJy@ksJAte;4VYP>DgAdw@A=t?SFAGMFoFUC-uv z%+-_a<#L}^APIYE3pcd+CW-g4sf9a#Sxs4KB&z$j2oKQK zEN)Sb=;3R>;$2Mr)oU>Np`Gx5Z&5qixe?|UCc@2amK3J)aXM#g3h%`09$@6x_yW*% z>ZMH^lbBx5OPF9@4J*dG5O;1g|pU-;Wy@(HZyxFQH zEOBshVkbi1UGcO?RMH~xmFMKCx4$ZERn8!A%+$T_t+3KnwCBGiyq{n)9J#-aY3S9z zXungZqTOt(;tEvM{?75|GI38Rgr|1kMAqj&?7l)r4I*2XVew5#PgX!WGQ_@x#v zX+by-OLbpcXM4{EK)e4>-@a|8B0`sKbj7o@?ybCK|n~gvoK{a1PlHSHW0%k1H`TF-zc^B%U_q^KFR$JCR$+ z>k4NZ;_m_R6q03KYGNfA1iF=iYo$oh;d)7*8G>?rWTtt056!(uOs{@vQvA>D|Df0n zwm?G$o*o<=Y%@}}xT!r2kp2rjW9Q+4loh^xo$(@c!PZIGh?z%-(cQ=$8(jY-~s93(ZyNkfVPwk>k1_d$i&o?nWrb4kd9DVO%Wqzb@aYP zBugC?XPiXu@*l510MR6oRLdjd{3mB3F_)ba3J8k^|f zajI3f2G@zILWymo%4B%r8t*aZG22}OB}RkOC+(^tdf=<{pj-oLHT$v`5rCZ96R*nW zc!~L~g727P9n)nHg1%veY9gbTVToClz_J1VWi*{H*N`3gsQl+@ujcz|WsczLQs3^L%W++?fp66+pErTkO}TZjbQDZqIMivXK1 zl&6KhTcj>QOwh>bzgUA=4a`bm?%-8pfJFP=o_~%DlMaj_)49M6$|`;I^>la*y3c}R zj9hS|?fn{5{Q{Azd>XGXXlC#vFZML2u2oaUx6_1SfNrXxnby-op4O!w-?#I-GXq;3 zOplNS+$(z^1wELKS=)Y(oxyJ-23>=M%FT_1bmEjH91aPD%svhNGI$BX9rt#AL5#|bM zrqe4b;EDZWtMaxpzc(D5FPA;SwnDiY$3PXBWo^z>$Y>1cWpyk!e8iwR5wjg8EU~n& z@0`KxM0w$I9=&GS9YDC>c8#%X1#yv~1OVggOGwFVh6UjYL^I#!L@r5%^4`{v^bz2- zITv1<2sz$w70w-M^z+Ld=dD9}K0(|YF?U_a-3r^(6)M389p3?A!h&J%yAyy6`Zqph z5fWv>-Ld+KvBjD4R3Ndyk&xfTpMm;12?w&o335wJNQLBdbO+Gb{kb(R`s>ut z+)M%jvN|lxKh(At%aKF{$=F=7n?mG!e%$(SDYeJ#mX_uLMa}3diAE&s4{@7jQRM%Rjl5{IMwmvNL>Z`#|1$rP72PC z2?*F?{a-w2zZ;8OBnV=H!m{x}g3j_*t)r$^wHpIt1FMrc3-Rz2%?wYEO@kdQs7p)j zV$Qp0G8&XuG65n8L9+&HW0~gLELbocv{l&e-v;?jj8M4@k-*HhCbKYr{m)n~LL1*M zqG8a9Zt6Oho}-+R9xA7#s3?_-JLMI3-dxk4X}eJ(&80CDnN1Z{Hq3nNi}2kdq4c6a zgYipAEb*5T;MK;eG3Oe%H7dq{&cY#MExSuMiKZIy>@^GVMYAt^PQWMxS}AIC=;9G?cs%x@eYtcQ$4vvD0iw6!54Sc4kD(BAo z`;mQWs3NWJGO|$y%fDe^xbZCj$S7K<@f-uB<^R>15KE)2hOz|yeR@M!1&#@lv$e0$FL z^tn*j3{b*yAK-FM--*bi@s{~>NxD+uZ459`>BdhqpSg#<5)eW90xQWe_JZJ%*eR`k zJG!Yyvd>aPT(&~n?O@wK;p3WoW{U$)oR|y=2e_$6SXzu4= zV(_*fB8YX~gMsj@#7F4$lRHDdK)V+<3A&gXM+If_Aq8bmC((+~6}HC~NV^;G#oh4k z@i7gZ<1a#uRr2`*1U)b9{$9Nn!&kA979ZlD?qbT9`JS2tY zLUZ^0_`cO@l9Q1Ygx7^L5`gVd1_f-LoUkBKWq?@^h|3fd4DhiPNi7-fDH(z=Cx88A^J&rU`|MKc5$CrJt5;T>T z;QSXI?Z#t|lt2=+X+F>=H@AbGlakjT2W(jEjGym^i&UmVKYlD_B(6POFtf6vjEMjY zS1Yr%gv5oOAygK@Os4AR0A6P9*VD-gNijHIixE;zDZeiTzm~0fT7iUW*XR35&`9Yg z2x$1b5s{A285IZ=21$E04c53^UKV77fTZ-Jo&%yE8QqxV5x7=JJ>WBmw&V zu1b2XVSr*mMAhYQUVs&f%Vq$&Ua_xcpi&X4 z{Oj`M+#QSag;9;}gP&FaEhF zpXEiFrYzfs3+ieUz-cX0F1-&3?>zti?6Lwk~Fxso^>=dYMfUumnJM8qR30 z1+O31(V_->(3A^b1MjCc`ic4_fg;z-1km2Eqg7B>&NE_?3|&@Rp06{rlIUkDW%-3CtI+5pq>^({$*- zg3Mml4&3$@Af)~Zg9wpcNjn9GQG`L>DH#H_s6}e&(2beEK4I6nV!@sYx6df;GSLAM z%fb=UVUe&y({H4dkTwKtor?HX#;%A@yJErSRl%W%K<&`(VaswAqZSKhHXuO1#LWr5 zDqZ_M8vW3ISq3sf`m5?DzK3L@Iz3}_t)!{Z${ZzErIf*+|4a+iQDqxlw7CVKW(KZm zu5&C9=!>GpEk+a|Uf>?C3Vy?T@4YC2rz_|oywNcZyEHF1tC0jFe`K6SYh0>sxAR?9 zQ?fR32ZYPi@FabNsNAYGKaohIY;_yq6JtqU!yt1CP^w_g@{Hi(Y`@u;s@rSvo`L z1aQ8;30R$25g+5#tKG4bl4kOn>YK!In^vx8QCXOuCBZLFX|;cNgc$e4Gux(Y7+S2U z6sw@K69QONqo!RT0WrwZpsbtU0WeYY<`6oyF;b61zrkN!z{=h%ybL0F0mHL2zX@Uv zd@Ny7EKNi#$=#nx$FY)C#itZ}4la_i6D%L>D%~B_c?v2ll&h=X*F=CZEV#iO?Ce+( z5{bmJyRV9!eBE1%e%T4ThZ%8XgQvC$&ee(ms)mZm+!tj~x?vbZ8)DCi);`>e|LrMu z!5F5db1_^pT9pJ*1+F3pI4+}uB~kP;#+_|f(M$p;u^-7Rm;)~ z6kdm@9&pk#Fo=|rWM%Ayw!@k;)OgDU+_;6r2;9qz!rnorpA>fOqkTw7NQ19bN6V+( zjsX8{DhcZOc)uqd?H58e{RyISir#s068Gf7Fqc?y+g$$?Pa~w&1{)^@rhH{$Oo%e3 z2XZauIf;-1zrm9Yy?he%9r2Pou4f!Iu*6(u`8JlvKN8PDnGH1TD>$Q%l&vE^8PE-g z?7~grS_E37CV%%HTY;zW2JgU`?xt+_eBg9W`in%Uhdaa2MvNP?>Z`tZKuxtV;=&i=!+@2kwGYrg_XaJ@q(3V8>VDR%3;wVKO>Km%WJk4^Hx zlBQ_)+gQM5HAxZA?!f`e^{G#;N?CR&IxbVIivp6M`)=Z;i6EJX|80#&UgWtpG>7yq z8&7V`-AAL(F6{f?7|pgKx%`y_pxMn!I08g5^%}>HrVNF2hm(HE*2XR;9AZom29wms za!rfsF*zUSfhgQJz4=4=fox2;4?aLh@T9-9#FN^}pH2M52s6ymzMF2F`23KfIa?!i zW07yc>|oy)9GX$Zd3O|{i>PO5`0?%ov}H1NeAn3sK{TQ(^R`d?a8@92VM5H&EW>0} z`8_j{-r;W3#>MD_W@(cr=5HS~R{fm5+NNgDP%(4#JS;|Rav=JKf-*~D(4gnW|6McX zCMa^UP?=Dn)P^OO;l00APEqGWY@wK3Nq-$gOCyvAx$BR$sfC5*Fb-4Kj$PQt?*n!0 zV`BM3a6<39j)(7|TpRdd;g*ZIP}`$6->dreV=53mOQmh+`wpWAUYZ)Q86U~JqgUAc z+{tBUsHM-60S%qCCNxq!l|DkiwAx)abAB$dxKREp_w`Xb=E}`0p(6Yj+;z3?4a;P= zsGn?JVuIpi)&Y+AJvoj@>jHyHV7>Pd;OWIk%{kgZ7>MF^#_`R)g*qa`Z5?Exro`x5 z=wRo9^&$b}4Iwk4K4EnX;XelrbP-DNp=ic1DrJ+}W!TAkRY*P&aibA$;TZb#3?rTmN)P}jt=z<#=Y6zsnztvDoOlI! zd1Wxl32E7$e6j_YkA+2n2sBR`n=wr@(BYJ8<=~=sO^AytSFsno`!3%I7>xtJ9|Nd^ z(;vcl3J?{^&~ek=omIY9v4Dn8?v$?kb=*_M^>=i1@K_VDb8|~zB6P2vSaCP~<&`4u zwA0F*lf<$+S3wYRs7L#=RzgTVW1uPEbc^(4uvN(H6B3RDTNPrcG^deS9rN_vz^vFbp?#q5ZCsqzq;d`zm?+ew)I-M% zkX#0`vRk-+b!I1;(k!;S_Rnu;08UZH{Q4??Nm?iD)bHd+*&lL;Pt3o#xF&h{ZmvZ( zgchax)x_&P51UlszqZmqMgvEIo)^p$$I4~3GZ*2_mDp5qTK*6Aw~^qXA*U0nmYdB7 z?A)Hu^P{70&b6$Es}Mgin~?(8;CD%gijg@}{h}f!aI9i7o>;|hZDVES`HKE%;e8=i z)%1@;JuG8CL(WIaul4S_H-rvNr6QZt$)KzLe62h4;D!>XRe3I74{gu|ckpT1}iWOsG@F z?)wceQF7Lq#?{#J8p5so17}me*?q5}n$=fjMyg=JSA=W?^f0PVecw^-5r{OiZ8+r( zW>#F^fQ+meRzf%7@-)+*Cz-A&B*y~I@o7En zL+Chd2%Gs{iX{7 z>?+YY4jOAy3XG($N|yQ0j}-F?9bdo{Gk8?j0im;MJ+kTyGG} zhCaqNlOiPf^!otJ;y;S+<;XQYV4vuL^%C>eY>&38Lyq66PDN{=k3Ndaj8T+F_N^g{ zAmBzk6EZUa=7Hh$*zw34RtDV8NgDR%JQ-0bApVX)uCZgz1~hU_v1AGR>-bid0^0c^ zhKVdj^HeyotG_=Kh}k;xvm6>?q4S`891+eU2LT{(T=Q*WGEXd`B8v8duM^47{+B?6 z&xZjl6Zv|WQ_f7*DqnI#dPAlH7&Pmm4k%C{p!h>40PN?JpJhy#ykUbZiVOe&q^eTI znR{m6L~|xRM2mra5xe5(I%*&uoK@HdP}(~=jkCX0V`gC~uFXZxS1Uf-8k@|85IsRr z5HM`*JgabPG-Gm04g_E;VAb*prLdlTzJ+~lsELhDRdaJPS#5|~z59AUok^uN5c$jZ zlT_>R!&T8I;(LOk)bD1Apr`myJDa3%2;;+z5+5S-hMu@$#c84;@pEu3&z-Wep%M6R z<{s}IX0{+v@=#4s4>N`;z$%VUtkbHxtyqT;=E)XVn`mr;92&qV@?f7z6Ewr=cMW0} z6fA}B)yhJxD)*fdZBfTq53CL(OM%x74J2v1vc-~8 zTEyxFzsR=V00f*6k~CPRz^x*{NUCG4O=g6gFi>taRkE|#oG7>K(%8GZ%epgIVUyCf zb!(fW1<>jP_CqBObrvHN$Z#-|B7o`_muF`+e+&adsI-YRQ);VNfeN*boykHB@=@AU zd@C=wF_Q&YWt1Tp&(5~?9X(WCPB@=5eEKJzp!-ee&5d`G`3FKigaReNlGG(Y6uDKR z^$L=pjy^^oW<1(=Q&Ozc^4e{+=W1>1iU&}fm~r?#_O&txKEBJ3S4Tt}ePk*xf*orM zis_1~vboW8-_#o3)vC}-7y@MN0G%%$>c-2mbR{|=4Mdomr153R#V}%nVw@K7Z`ZjT zZG4bKE^fApuQ_U|=;RDg7fby*L(7*2#v;d=@9}M+!#D{%#Vsd9q0YLy5}!R2YovR_ zN&IAPGizKc>YQd__ds1`@i_`3Fw1681YP;PMyHv}3Y2N;PyC7q_N%@brKOX*he2!| zj^zqi5vR~Bu->S?zk7y-D6>E31Fd`+7|RiZe*gXrWWn;4D3z^{H!D?nN$c`D;|-qy%iFhvj5Dqe_cyaUlo}QU zVd@s_U!S&Yqqy^-1EN-)jbB8$=6m$j_W4>>3l?hreSntEg4Pw2IGDNJhcekW$+$)x z(U-TJ{Wmg$I3m?9Mg%k)`qMIAmddE zDOhX@I5pZi-_YEhH{=`rSelCSxf+gDC86pJ9oxzc2X(Xz51!V$d;PB#;J||k{+$5h z{Kk(Yx`Y`xgrZ(trmnl^1VWdJ)H;t0!E;qGQug$xN5+b!wQEq=%(vK3)^MW1R;&C} zG>vvlS0o1cY1j$UdXvEU>OYHtP&K_WCo+-J6Z}b=lzn{kMgbUCCvXtvIGy-3&B`!5*Y^YjCgCw zo^BfMw(2JkZQqe{f#{2DmYpb%=G!ASOU_hV6=S(MO9CAR=9gtPxIDGYjn1tu&$JRT zo@w1?s2u-&+WKI&lRGfOu5zaXn93$dcC@#bTLA*5^%+!ny12Ob=`Chg&|Ab|2x(NQ z*_1L{&S(b6KPT^r`WFDTMJr_DKu zoMzgF=H5Z0!H*(X!+ap%sbG~k>D97UYVGgOSEFE(X)J0MFFAM`8fGmXJkHd&N$NwT zZ*(BUu()o(9CVac`1Kr$kMIXRUc8kS{t)8;$gw*UiEG!;;S+uUiK_m}AW#;T#qU07hb>f{1P&ufmJNM6;~AQN zjv+}Vj@IAHU6OCAx{_gRzO7z@(SL#LVD^ZZuSQ6P&CV3y%WrnuS-&0IxAcX|2WUZkLOePZ~bRx*5{rj-NBs<`CrYmDKv z!!)Cal;*-HRaIP;iPX4a*S z$dnGYrwXg^5TYqs_{lJ>nq+UT-BsnSA$ey;4AL>1peQ4iHNFnN{<6nY}MLz z9Z=^Ihrp7BjzrO6*m-2uRsEX~#*4ix!6E!ymq|MHi{7!&&PrY&?rFeaoy@Zv!jkMk z97bDIiY{IP>e`yC9E%D&HA7}BUEqnEBKg4Pan7%$ffz#q! zf3%TYvB?7a`5iSI0~pq&UDDqzM~?mhSrTt1X*8@^-?fdo6qx7=6=$+4pmh@N2#|7nx4eH@0ULGX{|mft=GoY zLAibOh#Gjx2S6pDxMQAz;P7ousYJr)OuS4J&m@C1Mao zdw{hMCTQWi!W?@`4WDia32~zn0EV|!-xh0}KAF^C2f(aMSm~J))Y|jh>Sa;I_o#E# zXxmx;2a-Pxfyl?v`Fr-WDN91`QfcwaDj#*F7zPrvpUXba3|OtBv`4aL>3XmE@fR`L zw$iuSRW0k!Dj2$&3(&yVZ?`APanPE7)WDjetL%`vuC+aaW0A#*L{Nu5r6*If+jO^$ zFwkYvDGv&=={G%-OM``mZYYH>I}j$?@28xv>qfwDyY7}#NM474{B3cWm|vh9ndLKs8m zVcaBjzu%g|crvS!*A{53m97*o!luo?Brm9rkzdMHEjAfyI=syNW13?6rUbJS;SU7} z$Y~8uDMJx9jj*?r0>l!Vvt_t|roJJE`R&T0IkB+;&OStt<>^w&L-xks$IH^?^2fzv z0jHN8#lF>_Y1^?u@gQNgs5=*yW)?)m)07@-m;#Ln*Xesde{ZwZA)-70motFB-s`del@oauH^sYa^UMH+7LJb_d= zsnRy2mB|$5r@qza9j~KOoAWj^_wbG9`LrD%he4YT+n%A82i4!<@_#L~4-Beyc)pvt zw~IkVm<*F?&Q@~jWiahjG7I!EZ7mRg$8D^IIO(~v;4$ta>i_&dn!W>^>i7Ns*ds+E zLRR+Pn-Iyd$FcX!-h1y4Le{Yfna3X4Bq5HGz4zWBgz$g*{;vP~>bi0X)%&@hd%Rw+ z`@SPi9TR`wJtl&_=VwN0(+wORbj;&bl17@6M(+5OOzb%ep-Zj$7`-5p<<1)BU}vXK zmcQwrJ^_cY8oYT`-A?o(a!ZK3f6hUGWVCJjUHlO8^&iLej=aSxMD>;L;(bw(CUwoF zz-kBi3CY;biwTMva)f$+7gH*BRQg8$BQi>?1k?9^+~_; zehij=ue!ECjj8Jv&#nv-dW#FkaBQ9R%jP~ogV3Bj#kpOD>+$Lec}OY2P1bNlLQ+8D9}^$jP8Y zdy!9zr?3h6ZyV3RgtX*ZF1hjSZ@~?Z*f?(1#1uno`j?4oM@<%OqwZ8N_t zwVcrqI;6s%u?IB97xiOHsrkh7fI@$U_I%?#4~JkfFcZAJwrFO{rBwW zcrCX$=T9Ct0RJH_ZrAC$A=EZHqo+MHH`!2~pYLRUxGck$`*dZ^;dR{TFiJ4VSLT>~ zL6HzibAA1&g(H+;AZ~z}WTHyz=kDeGz@;PF8O3(G&XeosF5D77*x!ET>o{~q+=DcTueJPAqm#K@1*iny0;LXZ9bXyoLx;N z94OO1EAM{V*mpBrC#=E{`}o^;3+gP*&VCboh>L{4I%{SVC-Ec+UVVY{GF&pSj zZ7sDfDs~6S9UTcXQ%mg&&Ki1pq;1|(Ll*09Zl^86QHq}$VdR=+4YAQQ;R1bss&#U& zjmtFE0=RIWTX1l2NH0gK26jDw$^_69jXt)OwJ zxl-nk1p$QmG#|HrfI1gg%ooPeOF`ph(UNM(s$~M3?t^(M*%ne{xI!p`B_$zsWO$X1 zCV?tvN8i^g(ADt@m5Qg4b!dOt=|&|I<8ptB`5AREi}s}4e|Qx>9T7A2J0wj)KcA># znWZ@-0(W_#{L?eOfbo>8bgA1oHy>ny4hSwaiT{`d<$VKCH^UhLXuDXVEvD`F8kS9g91!e(es!^7HjI;2`OD z0kyS{k1$!>$mnRvgdO~dHCI=)C^Ii=&t_F@aXlaZL zt&UZMD)R@>|M%g;&o`FIL3iE%%i`AN{iCwzwFX(DCL1Jr*opbc2g=mTG<6Y26}}y6 z8mJm&n*u}SG1*sGjYOKvaZbpR5Yv=7DytP!2J99Zd9^z!y04{l;>_5CuXhioM;S?j zHd)B%gu8##TrOxw%b5e8=CDOING+$uK}HEL7kxLq6m>y}{Z!jY zrj<&~<_lAtq9lYMwpj^<)FF@T(Uxc`x7pttFa*HN9e>!!&BqbT!S-5$TGi&uLMOYN z&rAUu;&m$Hftmt>=sIX@TUOj+m>*ITU#ftLy=z8oD{vskPre&gNq~G(b2z?_84IBp z#iC8D+UO4m2#5;^uyzyfZ*lWs!LfbaaCYtQcX{}NlytVU&?-=JGAOrai z>3aPnV3Og9TR16P&gNEVGoPXZuz)Z5g(3VE3*%K{MZe@(yGoZg1zU0CxrW%`_fm~r z$@B3GRR#tDs`C=Fl+r0jhT+ZFOb?LY2G^k9O%4&8`(a8vtPdPQj0Ns~uH)kH4DR`B zJ;S6!+RZo1e+UfhLM+AKRwrYGS))U!t(*fd*VoEe$==0>mnJjKC56h*q#|*+E&;$)qS#B#ej=n zQ{m-+e?qIahT2=%Hf?&(#acXw^@)kI=d4OtzCa6;i)gdiY8VR{&a|)1Kb-98fHmKL zV41G0q~a^f=;6bDG>8V_;zTNiA6n^>2Y=p=*gX}>%oJ+M!g^1$NlG&?mB+(6 zU%vG-X2+*%W##8j5XQ#$eor9)_R=8&9svP3SX2}JeRGrUj@O5ntvu#`T9=1_-IJ1P z)Zn(dc4M4!t*$1KuP(;C!mOGm8N|}$Z-OMW#(}_aPCru}84dK1{C-4G8&U|VqiR;% zl%S{wbais_=HAi4HjA&61F5WNz8mDQby>D` zkvL`98_Vty4qg@ShFmbY57yElbHpCBB8}o45~$tJ{ubGLmhbuHolqGU{C-q>+szSepn&rwL7JLmKd_GSfP9-#l+5lTNc&76(8k%T3{?wcb1<@ zjSV%}au=zeI|PRl^ec?YAA|_)DlmIc`ajc%x_qz?R&W`pJAC)-h&93k2#>8nPGUV* z$cD&9Y~xqjUZyWNt)Khht5uoQ#%kT+` zQ(iC@-Pof)VTD)T5Un0;iVlapjbT$&&tUgQlWi&+bbCZpv~xofFKT=yY;k|a#7Kjl z9z3R&mZVr#n{wuKgt=8yWf3!&)PT3~Sq!Fj5R+_FOZ?}}bUznGPsM=%Ak2o^*=}__ z<>3k+P!*t2tbO|gQaI}Ut}UT{)yX^NnGQzNTal?x^~h&2+=CoTw>Zy94B^qSrg|P8 zF!G;$=gN_4w{ts?0?13sgv#1PH=*515B%xX+Uj_8lwea-wk(VvJc`e$3w51N3HpI^ z1q_;Ta*&!K39eQ(NwlSI?hb~OiLMq=uLW2;)vr;OqPdG9&n$H7Qnzu zD}QX@qTW~j(`L*i#o(8-1$E>Y__!7o~vyxV7l{N&Rg26K*6$b|RkU+9?WLUcB@Zj9Nut{LX0 zlz(f~m^{{{{qBOHqVZIEVUf~QiP8!{uQY#=txM|t5<-+S@)CsG653C)H#oBP~v6Du3rwP~^d^p<1MlTOp$B>3F18oum?B z`9h1g%wvo}Lmu;QcyrwGs(7LI54ONT4M>sd;HPb^Hi;?a%LXGiHVwu%S&9NDZzQo{ zn8X~PTtE481(Y86G({8PwcB%~4If>ihvoLYjhe}FZS!f%+46}r{g17UPJYnhy9G~2 z&Bxq4uc1-vooyf>;}0sc){VBTm&nu9wzjjieQ#&(Ez#7Ro{^)p)Si)%p?kY<{OoU_ z{+qClmbtO{K`uo{7q#q+)5T8q;bAH5w4YO3rPb9=HfuMxx><4W2x&e$KHN{2g(qU626xGgbF*JLf$J>>FIt+wJals`$WBQZpps5s51Gfrp?%F(B4y9 z7mJ-p&zz>LkfG>nN(26-F}+D{ZlZ3&9waDVt0_e`9c^9z+=aXp74bT`Y+DkLT79wo zeYvBf#lSsK;%P!j8eBa#_NvM_FE5=-v5gI<9FIJx6Ubl?!gSmAUjj?q=8kfopzj|X z>dHh&Sg9=>={GaeK!;XOVg`NHU{2c6`|5xw? z+@lbF+}RxVWfjmLkKm=y2aKA}IojCvt{{ zhUVtx0RbKT8!jR_gGvK{9(QF(mM>C&^ytxGb#7J`(yG39v{B}V>r^xXaD{vMQxtlB zE(i2lSxsm473U~wi9^dzQ|2?Sv|nsJDJOCZUrgK*i9CHu`08^!!HMlGA1AW@_co+8 zH2{^ZZO9G9XUmbvQVQDe8B_5wk37#1y7ipR6kNU-Z@9+3X>Z>Lfb}dTo=^l!&++M> z%&Z>2t1rhZ#w*j?kD^{0Rj<0&v9U&*38X^V+1N&hhL)STMMcQ&R>R746ybEb_4?8j zI~UhxZJtw95{EBIYg(3SqoZ3Mkw_RA<+l4wRJ0>TPO4WLk+(nJIG~~7^|PX}MV9w0e2hl6Qa-WpzoyDdPj7QE zzZ^^*8YfJ&l>V0LK+EE{%}wwN z>*2SGAOmBWUWt}`Xt(`d{R2d&2XtWodPZ2v88a-QscB`o!*8a_(6(yRwdK&Wt!GV@ zgs~ zKLl(I*g2oG7m!aEjsin+0aNI}O)oY~k{Ix+8pnXAvqi@Rc2k=Q3krI;#CaNH z>a$VKZewCXP4R@h^C{B;dueE+L-}MNuAZ0Wb;W=b?*)5yjBR609s%S%?WPC~<1?I5ved6hhuI4n>y zL8<00PpE?3&jtPdkyw^HHOcY?PD0 zE1B2HlfzlHm8By*^ptThSxs6e-aK15VPenG-u=(rGW@xhd0|P5PoTelpc!r20D&&1 z^rnIGpp{$3hm?UI95c`)+5gz&T?JU93)tk<|NTbTXM;hRM778`GBvhQP8(BBDlG&V zVFQz1>g)U^;p9t>ZKK*V)6>mndq=X)f>OpRnW#2E@La5M^<@}KxoL;+PpY_d}NwCKydtOvPu=C|_VZCy-2LTd|t)&xdy3 zYn2LI>%jF29;GInttk?B&?4;f#)0HKAYb*o%1}H(qV;@C&hX+wpoBLWY=5hr{3dN{ zDmAjDBjtSKzr1Z_%(VF^LOLKoNz@B?w1@7iPLeJ+tUBEc0_5mlkXc>M&3&IGO{8Pa zxjbBCo}7BB%aBCWgSs;At)Ra+d0beM1XcRbL6ORPDfe^=kDYvt7s%6^1EMP^Icv{oy?B4 zJFWal2u#V$QWzPQufv1HdL26p=!;OQcz9xZ9j)67C=*!7>*eOgue38WB%ASMq~|D& zj1=?rnuSDdj-Uyvbok4`cRnni$^yOY*QejMasDT#)u2r1s^|6JRlPD#;6GX(7R_Of z%*gxqXm8&!Zz()}{CLfIcF!F%th=>UB%$B!_VNgvvLeIA)om&dBF#8Z7rte{!HDGP z;iBk26$8RY)DTHIxqjfjaA*KFU}c2~$yT=3(9i&mh?)Q!GYqulG&Q9WxSnqhM?_%Q z66W=9?CtN1`2YE}k??wXVFB2)4m_@Z1AMcKiwiL3fi57-1`ldTFrm%G#RUKiIe;~D za-vP^)OJffkpoI|5XD+$svfOA6C6TRN#Z)?=d)8`aWHem=?#qAvEelY`CqC&TTa`Bn! zj&^}-dh(XCFCGx>&ps)F;q^ERWG!vj%(v9Q`#Ty$#b&zo-F6&Wq(*|ZxIvC zGBQY^kKXY47qo>Ao$Ty(_s;|b>qR|Sdh}ovgUE>zglM zwtJ#-ZZCbjj_JZdl>L4TLdp5|#4$-^&+dXeF;son7IFEwDfZtV$ zXpK47Oe^n-nk9Gr1NMNnT2U|8P!^usr_N5NmCosez=qoD(W%+=?3|pqw<}#f=K$LU zt#uiSES9PbKdAxd@P9i39RBx!3f)FMuD43?mKrf$UDNGpce>0KS8F<%P zp%{bi++qL_W8lTg%5Wf#ORdBS{Uyc4gwf$_NN6kAzyM&V`T2RUXmH^uUWQiG^RU_D zVEXK=uFjRV-zAi~p`n37#CQUK`vLGY|PTMATyKUFLgYKy0WcX z1Mb+^*xv;6cv7kB@od$s_;ukCQmH(>)!J~WG!?)k>2YD$*%hnTQLJ7&O8ta?Pg9mJ zKCX35GF`622_DdNnbhM20t8sC5)?rSZ){o0jf{lf-!DI7Ta*2Qe}|w z;-ymWMiLV$4Ic8$v#q|0|VR% ziyRAyl+>)&Khbl+l0#Fa?f|M*Dj$ zfxlKoTNbbVt}bH=0^C#bvMn(3#VM&4TPB+9NM`K|nBtdO+}6Hfm|9uc`072E6;4BP zaC%idhuIu2k;xLlDqoeV*RZi-f1B#-E1~qSx%g$O&6Tce&<=8$C9jo$gZ-(s=xE?( zHPvuwV^@mX19>);ZJ{ukrPn%(Iz$nY;t|wj3HUg$_~UivA@51G87{TzHxn6 zIbjS3u!{_>%l+SL!4-dvIis@db)V%eR}U|*ptIyWcOeSkSD|4t7 z_5x|hMgU97QTl9t4M78*5M=JJ@Agzy{7yQ?#>N(!oMH<~pSw*>O zdsN1%4br3fOnEf6eiwm-@u3f#M8GczzNNjNo;J}=OIPF=9$M`5P8_gmoH$@hRHCa= zQ8__h%~qB#%cBjap(9QhUFp1gsK2_bb;-WY**WpqFG8$j+1Fj)GpH9VvmuilbZf<) zCB)H*Ho2fOo_b$lh9M7Gnui@c@uNH(IisWqhK?vd9-_v+;p*-VTC#U^;>qB3R<=r# zKlHZLe}yzh!r7wXGBN9R$+nlbb{;VC(Yw?Cip)Geknnpg(nDFD@fv zE8+DM;AH|dNV%tHW=Kd#%xh=MG)nv>@@5Vmqk}N{b?3aSd{88l7fg>W9AlFHad6>E zq0N#i0Fr#*#kU4iy~Q|?Jh4iqAB5vUZTIZiGr($wQp*&Mz2_4F(i2h9Sb?-tupkr& zYQP@?LC)dPQKc3FWcmPdyxT2IfWzTBhWYVy@=0`H-Ee1TCopI*4*bcJCzPU|Y{>&4 zie`Cwy4au{O~Bxm!86syg80JU^mIXt9cedB$gE!2X>>M_Sj< zO-+42JiJ?Ia}clj}I%(&ct zqgx%|1j#YJa1*u2~SKqn0Sl6d=-g2^} zty2TX-_w;5s->|T;4s?S+Camh>+PjO^-QdQey`~Hyxn)lqb@wVkMooJub+W%)Yc|S zfSrQPoCc+kv@w4L;Dt`V8GR!7d-VES`IMa}+(B5BP zzdGWgOMNA+uBrKPeYz17 z903lv+oVRl3`m8ChlUngJr014G{))bjPL)e6$Ag!^t%9kX5u>wUteDbhoha>4=py< zBxwRa84N9%?Y6hK$(j$WDl6Ddj$R!(SU6rQgv6=V#6}*8n!`BKvSvF6F(8L{6*$QA z{X}cIsh}a1j{gWCf0&oltZe0Kf>TLosHAi4> zj*fyE{mMc=O>;KTCNiBEBCxL)#1~Us?CiveEiIf;!N#6SLc)qbKpD&zmD03~wOICw zikSMx!mzk$5>0-x#yqRlp(Y@Z9kXy&6gx$~Yc$q)^;Ewob@Xe*XcG=hZBUS*oU|gi zN|kA7WMr_;ACmujsV&3rDhA|K{xAX#vV9IqUAU!ijxr-*G^n(lh1g?c>Q`bJgqZT9m-s($wE#}jzA!yp1hRCmQMmxcEI+wDnx5*YjJV$+qZB1 zC5&4PtQ_rpeCBvWKG|dz+kh`l83!epYs=!of++Afe0*R8zjv-6WpJzl`TGe!#meG2 zwHGGro<8*NwO_d@827znNQNtSck491CYk0MF2`#3IDq@?)_z};8F$eUIe%JAsfH15 zPvDBaN@Xe*OwjOEaq3&cWN3Mx5QoZMqwN&G#x1H~$kzJ$`nZ8@AuPd`o*H5jf9~NS z8yWdo6&nK0P&`}iSntEE?ed!;D*C8coR-GgLLrTP-X8psk@UBR7qWt8#%swVJ;Dwnik;UsnYsm22EiX<%nKrWy`qZ0PbVH6eW#%>HFcNVTSv-j*(j_C| ztCPzPw+_Y8*4%S{g${WdZiXEVIfz+DgM8?=rJP{<&A-jgt09N6c6zPZVy@e_izoTv zqvc|!N`j$*!6k{36XE7J?}}ygr+w;Nr28jV=%^6YfgN5})?W8{Deb2)cI*1<3y&6^ z&*9V`xchy8DQRmX3SBrxm*rV012ILZk=jf>Uof6DM-zXSKVjm%qSJN|L zMe4FJ_R;6iJ;@@T&o=jFiKQ&m-*G~B&wYBNf(hMvTCp2<&BcyVFD>A0%0)!q4$xTTWvd|Qw3hl zbJ1Hgc3!QENe9_NweycCQJ4Bh0})5LOEYNGTF}>8_I6^t8Hzj^hktqViQD_mN6*9{ zG6cBbTquJb!)2f-lIZWACQb608%GwKuxy2ilwUit43dtjoc;ac*V>TjXePP^EZD`x z1|tCmawY=l&>7YU>Ck>Oh;5Pjh*hfV+*&Rbq_o&3reLs6ox?5kYc%c2>AMh!lg8gE zwln@E*#%xhQw3VwBy?<>7i1^}YiPVTR%Lo#%Vrdz%&8Vqw zD2FaWQ=UBIQM3t0OT>CIC$Z+MaPf*1VluU_Qjgq!t-#eMHX79fW{9kfudccU1gO)| z-rR+bMq#Ys#NQ^VZv)*bNOU(gHpbp-5>Z|9TJjuJHZ7ms-gVtAbb;oC{yrEi)lyfD z2)YN5Kys&cwOe_Cq7H;U#g4vnaqJ5-|LhG^Y#)dWWJv`DqKXT#glh9u*7pAS zz-Ll?_@_cI07{gA`>jx1?!hDAg7E{O=wwE!Dp z+o(6Nl>&{J{*UJ6;5xUWHjE{hpx@xb%4nmG_TH&;4nf~tEyu+16fyp{7dqT++!;819DP=Few%4uRdbgOu64l1vWJQ z0Ux6-6v@^B1$OE;cVK14?Sy(6Y0#c=3;(O#3g5qk_paR%wU=i89Om16`Z_>7^L0l9+;N!L79wk z%{K+*Q%9pSeC%>RNJmSsZEnKaCRLiPjE=|I7N0Hj_7*+YXu$n4_@CUf@N}E6q-YTO z@KTn_6XGO7X}-8<_U|s))HPT(yeiS_D-F#rvHH?Ce8Rb@(NhRv^qGAU?iNDtSX;0C zZ-kCk8k6QDv)M)Rp2=!a&!snC=V_q^R?vM+_4 zSNBTta&vRRO0wu)vx1eA3-1)ymYPmwzPM7BqTf~JJZ~mJd9|=W{v_(|zOpMRBumf< z%shE1;*KqFTSneA-&=8Lvc}2qV{prrB^9*hFg*Iges#QrwBS)RY-w&te!gE`Z#_DF zlXJsanKM5AO|ngH#Dd4E^LiWH57T8;D$mCoS?}lcq=0<>xRT> zLo$a*8|GQt;xC7$)R3RCKbiWO1z5@h)uwEDE&H>}CFo<024>rIlD;u1+KZOqw|RsP z2ea4hfw+sRRY^{Acq=Fl1+IMbRXkc)$h<_6G&8-7#uQd7uKAG5p%j^^opWEt{3|;T z%4~7j*B>=3JaO%9FtUT_r7hDU*1gwM z!=?ew!yc#0AsL#Yt_z>T(elfj3i7iXhXJh3!2u*U1Cg*tO_pG+QI2w3>tNAcgR1t+ z*MW;-laoAO_4F(%*mzZc(slhTFn8o{tH$zQtoi$l_tL4^n)rg|*H&m^oAG2+<|CcWMq{)Q{R5;vtuis2-O{k-d3lk_!5Mj`uQ$*=}cwhe+ zi4#6Mf=?F@eD3uz!CjVdr6v{RwR;!apx+uDs87&rud|PKpTCvC!j^@LI7It0T|wP6haEV zodc70B!K+-ZbFz>S76zize|W@1dpBj&er0hp3ns7D*!zY{tlG4E zhWiTT)#*z8?fm}qyR&|N_*u^SHJ7w%y|=M=I0O^>#agrDhYo=W5(t=0R8cQ}K%V1eu7_O@-8&3uXd0(uxJJcbcIxoDE4ZlS6r0=}zXxFVHywtk)tf>*pP z6n=V{i3j<*-jnT6!rB%>Sip=s#uu+{I4(@JuOur)N`*x_r&x9Rl>M)TtdYj%U|4`O zX%tsq)){*z=EL=^^UC}h7|wwbi26q%)01LSOxh;=_?7$cDHqu*pkoH)_2xh_C{PQS ziq*@&`{}wr=(_*agRjCgc+ST5MgHS>-T)6PB?Scqokt!TI0~S40xG)g#}Eh?KR>wq z2Q~Yx|HAP5%(u+LJLgRR1@yl@1B4ks0e!Unbd<~-ejs~`rW$0l8(Z*xhQHA-eMfiJ zNAC1b)Nw(ld)PqIuW(Kp;c!|Bb&} zn7%QU`t=7a6g>Fl7DoVO)hB-Q=zKK=OnsuyA{L`u-dK}W!^M>C%Ih6G{7TV}Bdtg9 zbwdg@fomO&C-Gd9dxggYT{54gut;_S&Q3x5M}F1R4NCl(@7*5L*9k~0X_v;zxfWw! zSCF@N`qDG*XYN4tn3|euR{N*3fTDX=DB=mqUrcG?T zzrj$nH=l)oGXMUdNPR%;x6Zix#vEzm(de)rA6jxLi^kynIqi2A<6~xM^f(2LTlsv_ zwZ=h98I!r#%D4)f;i1mIoVjVCaIm+x7qe-wPFJjRj0J-GDFwNu7+I;)uaM*=YKVdD zA~xzL*53chKJYtY(A7snyL&oU2HU*d&_0GlR)yQp!S!WICo% z9$jXl;Y8yb)4@Ou0Rm+mw(V1IEp6?tzh_9m$W~TY*XL@iOfM!$W}v}Q%*R4^#Hv0*nuY)yJCp!7;@esclZ1}{LT^!zO=neId(6BlrcP`R z{ZY1d-EEatzv=Ah?A>qWgRW(r+j0YCW9Q4U@A<(gWBc_#;xE&gU(rBd6PQd&&(=45 z!0k{ zX~MSsIHFZfPUF!dgJHLVFrVd_DPl^pvq;c-r=-RwTSkOi)t;5n}${v(G)nB5ml?5X#B$tw#;6J>_ z7`}c!e!QVvmytb;onyS7BB8?ZgmDB#ngc~pX0?&BaCdV&i19KCek9{9WuQ61hD&Y< zMN_^FgrK-IftE9X%L~`kUYke^`lAWQMr$Z$Lr981M(sN(Ym7R`VXxblsa`^?_#JG| z@mX%}%&31IgoOb@0k8>8uiYq|JN0F-!BixTXIEym&7GYRLPDf*Zv!M&z-T1UFLzrpC|S#czho2@1*lhmZy4)dih*)gf#q44^2OQMlrY4mBr4Ag=EU< z2lsG?-I+G!_3fiLB-3-J`+Jc5386R zX*jC?CHT-=U~aC?B*Q#aK4n{`Ibem%_@Kvu)~_brV4GUXLXYm2 zy?PNNk!QFYnTqS%vsN`WrfN)ea%zu2kYeX2Af~GAU)sx#dO&UHvy|A`G0eFOY=HKd zs@!Kr>1HH9xodVxke|l8?di90CcVZ|EjQqUn0J`Q#$Wu|BrC=6cA5X%0U_uX3O!9G z%Y}J*vIwwRMG%bJ-C)y9*#h8j_*ccfDlCYEMu7MVti|l^2DhbKhm90({x1KZd(AO% zqxtyw7&L4jyy`V`@$m4lu&}VVA3C{8h>x$Ts6ao zK7dk&7On5Iq+L!}7+&lnsxzbJjWFM@R~}_nAE=Q;87OAOk>BciuG_n=TpFt|ByZ=- zVMHZIjo~h}SX;%6a~0`AIhk-F27K>BpQR{2`z8Z>-LG`k{s!l5#z$i5L%!+2@CyT{ zF-ADYlh0hZZC}bQX9M0It=Dxqk8T>C3CSTKk{T(C^LqYP3_Lv2e8c2VYc+*OY3@2C>qD5}mYjF_$S8gWt zQJN-*Z~x#J+=(eyU4+)-*HnLj%`PwR{%L+qM%-?6fGY}(F6Mr~`1OfO?A8g#1f9lw z=S0h$>jxvD9WQTUcXzk#)W(%p!Uq{f2HIDzQvExVM#=BUq<61ZdX~7IW`1~E{CdI6 zhlkBtBqQ9WIZ$Z}O(eWferT{ot zXwf$wEs*Jqy>~Y^?`a-|NgG|>`cw=0{xW%pewJ9vod{A!diigp2WxB_O_O1i^&*~! z=|1FNfBg8-)OB|>`ugGTco7g=S$TSTIy?XRwblbJWUY?9zrVk&Eeq)9PnOqMd(`hj z(GOJLiYh8#62lFcm;jQn3`I=TkiK;j4vx0Z86`PITT972lFurxTesHsh&@le7u9W~ z77SD4-`ukAen^LzUj!#mdYX;>;4p-VAC2mVaPwkhS?%-ZsyHYo&HliFny17&F5}p< zmg{7dJASa+>CAx$zIV~usOk0XnI+-7=Y6covQ=EF_1|v>Z#AVVR$lE7qRkEe-IA&V zZ$DT25*LVEpd(3(|1NHrC7rU7q(9YXPF#r!QG)Rzkrbo&;-~0pFlHjc_T}ZWvf1vi z%!1M>TMtAPA>9B~+!G3M5dcXg=F>pmSy@`14yOJdPvZFUA6we^o;pJzGcF@GK+$Fw z$UpS{9b@{eZW&f3jSXEx@IXz9cR3f>M4joPv@D4=-^COxLcX7>I_#b88Vh2JejwZ1j} z6&d+-yS!H>@XnUm?doRu%sB924b2)mk(nOSTDOnlPlQAHo9k5Y9~xe*fS8Xrgdg%) z(f?DH4eXp7Rcj}G24)99iPTi$4T&_Sy9h~>(x#?|@6iKkMoF|!)mdQZAv-?t8+7t! z%Y%Q|FV3g+oqWXpRdwf>Jxa$Bk4UVntN=6$WbHt|;^lSq{rmUmXN({gUL4FQwF0d) zkQMCy%m)|#8uY1yOfj?ghyik|4~XPqD`J3G69BC6w_n4-Y+Cki(L zB;&6Pw+t9Ms9gRW@D1grGZzQi)xc+3miFef_)5zjfHf`SCFX&b%wAsIAMMLDY;O63&_;itxxMY&Qzh)^;~D+x;GWJfA)u`Fxqq*-liEz0n5i!yWN z=DE9%Ft;E{mruQyE6?o0f;bP)d6XM1?IG7rNbohy1Y(| zR8#Q78D|F8>RvD}vK?j1mzFf!Is|7giJU_!sSLGM~QxVXHy z0PKBSLP8pH>*B)W9KSRSP_`^68`A$@3(yZ*8mhbQTWcYz)~>E~U0n}Yum5!a{rh)l z=mS#>Fb3d6`V*+If=#$cV|y)htF#cL=J6ZS`Kok;po?!PSw&)QX{oZ@mM6o&$jC@v z-!09{YwlTQF+!vUpom!Jmh$8xj~!y)@O`Ll;zqc(7+Bwy%boC7<*KIg9ytX)Y2(sh zkcOx{OA=HxvLnIcsh!CFg0uF6S;F>x{ahlQH#Z|R-?m7*k|R^FwE<2_gPi?~Vp5Sm z(RNpw56ScUtuH-!?a~ASxtv;-Dq;mgj7L=(fEek}N~1202kzt%bMH?8L^&ajx%LL@pZExPn4UBR@R(%**Sxwe{~<>1(p#swxhnmT_M0HA{`3 zl`mMK|AE$-GoKdK(&w?EGe|dshs0MUU8ixy2~42@>!*>pcDhREQ)O>EE1{>l_1)D=>YIDaQ5ka1JcxE|g0ErUzi6_Z}Yn zsZdLv7gp1lcq!1L#>PhdCdU9UeFtc8UC6at4sn#P;1}l18Y20?Tz1-%MvT~3tfK1Q zXD!Z$BXTB<@n?S>IAN)eGFzD852sWWeNyAKT&FII!ahaWWEw_;NWShDx9mE61lh!H z(J_9bij|VVIvDs-%PXKed1`QWyaIuk@8fxE4QY**HGlL{A!BQOTzHZUC9SyTmaZCU zGHk7Op&u-V2`wvTa;;5o_9KCCjXp}+JlQ*q>`C7VNW@N-2?y1Sc!*?8)66qG+bLUL zo4xxSQ9I+n7T=Axwl>j(-^dhDAh0Rhmh8BG?an`;X`hz|27sUXR1($O7*m#_2Vjlz z-HVvOphiuWV)ebPty`~SY{~={WP5+l^!-E;U|5)-jRqrZoc-2T-_XzwQNUj(oXlJ_ z0n{YXJE`_s4;`5cZaJkaYOpURvk01Mlc6aX8&USxfNhq5_rN`ZtVf!Q2Y1F+S z+h9xI)Xw9#Q=i~y*yhdNS))3*^KRH5_US9yIOn!sd8XD>LAkV&Hvh_Yy(tU+A{^69 zD?@P`sD(iC0jkxFW=U!;At9!i^}Ce{8uU2Ka}$)6JZ zRo}b2H>fh1d~jNBZ^>FHQe=38%`M<-UBk%xJeEFrAmR1>?7@YyiVB!c1)3Y>lOiJ{ z$M@VN@<`C72*Tg<%}-1emfH$s8S3d}W@WYcUhMz<>rb0ldRN%o+<Mj}c z|5p$6h;2VY&HR)JH#XLW(krR)Ad$6udmzo|d?)k!w#YgCBDnz2sS`x>jWr*Q^q?GU z6+m$oc%bF!kxG1(LCM?p#dGId#J9e_*$I^^DhUd}G)4B@zt_Z#N_Ev`PD5rW3Q!tn zW$0#QB*w~?+e{6}m)l@N+$gq%%&AzRWqM`b!P`7d&if#7_j`A5qF{#g#vj{Unds5L zCQDIBrN;N-D+5l3WW7cZqgpO5POb6daz%DeqEADOai5?<04}T!Wm03k_V#r+9U1OL zq*-lSJJM-sZ@ETKi^+BnMU~3da}-9^J^CX^UAdp23lK}71C&n!EFD8K&eIOZ(mzxxx+<`be=z9ZZR zumcY!$X&d=yqJhufgmW39IQPpiHZWeO3li@)kY9W0nma0lpH`K0bBsUqjYt2W@l%q z!zb?nO*bEW5z+DLQuf8CXn-4MM&SYc{blM5a3EMgV+LR1bdPp_WJ%{-1h)m|KUaq^))rl zppXQQ3@zZ{)v_|iX~ID$&}8udvitXZng5zBzNfNkYr-C5Qh7SU{ynm4?MBsXNI>R* z9Ul9D8aO(#Y*kWAv2Dy81+?*BAbAFY`^7~H1d6+1Dw5JHKz=5nNMvt_NAhKqLF$NO z`{)<^P6ZwDLc>1mx}&?9gEA~&-rM;By+NWpJb}lHrA(26qYm(l@qsiX5`idd2gjZ| zZDl2-wkexP@U69%gfP0%-FN=aV+a877WW20>G9n?_t~U>0WHaTJL7j8i6I|?ZLmv{ zE_v+!vcJxNnS>|9j6?f}MMGtYR(gR%Ogj;ZBu!%T^+aBk|0xC|w#-D*)v=|vHbVbk z`}YREnb`MpON8*bauKmMM^I~>I!h{m-&TSi?#w_N?DI4@ygX8t9ECb>bfv>jFcrGg z86ZlE7gO-#Tugxl29VUEBxN8{bmvIBH7YuDl6}g@-qtP_wDt|yO6bZ1jyg~SELTK%VZQ328MmRrG?w?N)`p2~VjKneQYurNR zs5DC(2RL9+PuXyJ!#R6rvZ0Br{5@tGGMXaI@)T%B)7(cDI<3w>-^5yR=w(KB zw)`cUzI?G?{sSqq^2y#2&s}PIff0O9vb&kgN0cv<9L>O*swnI>+WdwGjmkI-K@w+f z^$#t@Ul~@|`~8hEkMzretA<)UE^3zVTw{5uvEJi>v!Cvy*&i!J8MR=|gVlMy+1 zX9e?!*@%gj;Z8Jp75?#I)R}I`VR&@(;ME9IOcHqA|C(AVY&0CchY+Y&YU#$r>|Cod zZ7#F7x(jU!=D(~>7=*lBe`jJmEcky^%9BI$-q|l)zktH^eb0I%kg9cct+Y`7nm)Ey zj`gP8!0#;Vgrw(ALm*EGA%AYw&h=+Ro%J8)H_OSeVIG+oJZseDdNal48n{Sk&$F^#Up-ARtJGBHi5} zC`e1}0!uebN_Ps<64EIRyL5MmbV)4T-5@Fbetf>~kIR3IOI_~!o|$vz%$XFA-=UQ^ zwDT(&X8_^-mj9FX*7_op#S@KN_ph` zki5H-pk-LcK)z8Tm+;v~VCjbXsAFslC;v!D*OskqZ#Kn@nQVgxbrP;DN9i~@WXi+q zc>H>Y8ov7`>OyxK@i;`z5BqUrf31n3E~VuWrtmhOeztC7%xNNQ6SzG44tm~JkD^Ms><-h-`m zA_Z8?FT_4vqeb@$a(6q75~_KrjOi{UCk(cA-8hyK=8~qQrCGZ~=iLYa-Uz_ny`kK+ zWGOKHjG4{#p%<0<95MRhvqK*o03^oIMfbi5H_lf>@dqS*!d@?qXt$H_P&=}l_1*}e z0-}tP6X1UEAD&ffHe_`^WTxA)z|#?r13~fl@-S%57eeXzIzx*@xOVsehP7^f*POKC zsrNn54LAD9mbqPvV)q#p6x1*MvyTSL{g5?bFgGMk+i6$w{1TklFmCmDdMEL?9bowq z)Fd==zQ*%@*O8JdYlqGmPa;`PE$8i)M1R>`hC_%e~3X^Z*{@MqWd1F`Bt}=47s;@N_2$1kqa@U%}hDfFo1!rMY%56OVcuFCuj7sEM@C3;#87$G9s+k=`-k> zZd$Fep}`r~I`vXu^R)cd+cn_j=X~VD#o{ zb;$}kxE=2TJxjD-s^}HSyHy&rf*I_sAB+8z3~}%#;TxhKcw-*%A1zZAnQnKwTWkq6 zp@$p);MTPCNpoVq(bb^o7PFAB#qQ?uO%jy`5Yq4d^e| z7D^R=uf3EM21u0er}FS-jkD)`b!vAB(2+0Q!r#lj7n_a}T;{}m{=DMP&2fgFE;Qh< z`5R^8wliV3tcl=`k&KDQ>yWvo0`jba_0K02+A|Gr{49(O4MCs-Cx;}mo ztU!Iz7O`sdy*T92g0yMq&(JML%dufG363n{GRE^g(d1acz-j4Rr>zU>!U~N9Cy;&i zpF;Dh@WSdi@Jr2=!C6m6jlep@8JQMIvQ)H_2o`40i!Kz}9a+&3UL0Guj3Xi@sms`G zR|l>&-NCJ@H+IZe`z=SDMl--fV!pIin=e6%Ga^e)(eQ%0)&N{b_t17;KCDJ27lk7WrUX&k~__3lqV{}hdN(RLkr@Q1b zZ@USznOue+uo?UF)t*RCH#P9^R_Xr0qh{cgY+@ExG!V4N_x$#kR>1ukVc2`J&@dq_ zxD7wxq-af13_kDK1FuzRX}+h!eRuX|wuHp} zcF((2z3j#@__TW)dNv|;DqKZX`4d^LbImu|qiw+n?jCx_Al-O!-8c3sGseZwEq8+w zl|SpX3eHHux*-fDW=a!1n3#lB22~QbNw_u|Vou--?4SBX6U}tK3?26KHj`J zYlz)~Jf_4DhBZsvk}`yY`~m0mql1ON&mAa0zXWkXmbGv)XDS;vNHoEzPW;kO_d+hj z@uBLd%NtyQ$k>m0gf}S-q9e%$$#wacqD!#{mh5am0ygY!#T}j=&ZYeN#osY$p*d7+t<3mkXfvaKFw23KlFe%(u}UCc9x=L(?MZ3YDk+vY@NwT5k{E?> zo^rP%F%d9Cs#RTDxR4itH9#G#M4-=c77|eAS4lom4u8ru=ItM;Jal2TeoWK6`wghS z>s|Pf_Y~I&PMZYz;IwC9URw7SfkjqK3C0Er`u5*qjB}||s$;xAya06^{c;sVkcVm? z+5&gM6+a3vF`stQx%VEXu<7F*S3e7?Ft`rPsd*G|nU&Qt2y&eq_y+A9d&bH^_l6DH zIGTNZ)7CMP$`i?j$bd3as*4%kx&Z&Xi1dsoRR8UVE~m$nafxJE6wpfA@}v3P?Vqd1 zq^n`_)J3CvzDxn1aG+xF>u{)!>M7Bu&x;rbq7Uy}0X%o*8M=dDq)!{AHZvhq_#N|G z)@#={r40f4YOG`FhYaELOZeBzMH+{w5TmQbCej(PC^mV$UCN1+oWNlkB9LK#Eb1sL z(L2$cDBVRF-h<81ZY8u>wr6^m9JpC;fviSTR^w?32wBZ=UVD$m9(_+0kQ#_*(Nra)jp%CFb6-HhZYNO zzCC*n-=&;VD#HSC2fgRCqcOkWroG@TG8R;Mn@y{V9Cfh8Hkt*jL`b0QR-3PRq2zry z=W9n8r}BFRWmVR?W~Q0j^csipa6N}F{I`kF4^R0M!)?baG`k6I$<&F|k>kQb>YL4` z>_cr^p+ghlMRPCB4+3?U-P$ECZ!MT34zhgmMRKbRmYWpND+MnI*Cj-*KRNj<^F8p? zJWv8Z!*VUk@D5_n2xBRwLYev}LH%VvcJu{rD@Q*lhazb~cw+v0?W1c@v7C3;feiGJ z(cC3ACcE@)K=W?nlGea3Eih{Mw^+Ti^!wq-@87j9g=DCNDZ>(-&+-AsMH6)Yh@fOo z7aXJKN;h7OqH$UV?unDasr<5tbg?mZ9r*O9uK;8O0hj~st0F5ZxshmfS#x&#k(vfR zL=UnN1sVp;fBDWsc1jiOQT$Kqy6=j2$C%GvbX>g*{!r%nvKqb&KHT4H7fw^-Q#7d! z7y<&*sXXIGVdHD2Bw$akLokpvGCsQ?%a{~Kd%mdHhpPH)gl(v(@8xp_K;m`gn1Gz- zO+f4p;?!pT`tvEKr#q7HSGC_!pBAYe5y)~THvk=!YF&~Hs6z(FcfQcV&WFN8+CA4R z90X=`-^1|wFMO^b%lmlIgdmW9%8i@_8KS#5t7Y*%&S4{AWmj2ZvJ&0pVj22({TotBC-v_Ry7Yt8&YoCsKl6c`RyVA)Pa zzL_e3UryCm7HBjZ$+FG3XkM&ji7i@{eU!iG!EE1XceD{+<9_PmVP;YfSd9{OKcM}YzF67(DI^K(P0Y&xltfnxG(6CHcP9?pl7L-q6Lv?)N&czK#3{1mh+DY<-} zxxBpU*1#T3HT6n|%~dX)eJyo>NY9BsQCYv*c!r&=bQl>J_?1PCf_t72;S zo;7HM3qcq!uGgzG-Sz@ZKWASYIZ0a0-N-!iTF`xYEbra2n?49kxQo>&L$$>#lX`t0b(;HvYhMzV@6Ui%00MJnvg?zg+Js`$ z-X-e|hlJ#ZTNODG2956Ls{4ivR)MQn2>kgg zyf=wsaQ=p5Wo$5V?GuURfo@y!w$4A22JH+B!_W3BH9((6HzHoYJ8u8B z4>>OsT+obUEpGy0q3tPW?)4)P$5zF#1Cxkt4X#uiM zNt}^OYWqyi>+{RWH5AO<}ZBFB26KIrlG35aO4u0v_x0<;Xd0CeLD@dfWV@9{DYcymOxe&&c zJ6E?+A_p2!vuf?L-%Ed@`UGCbhH^nK)@~vg99lePGXk(qz3 zF5Sj!+SHuJ`nQ}zpj5Be`f-f57kuAd1mjLP0desKj7zdzdQB-7@= zW$6?tt058nuc|?{h^Y#vo~jmH&{}69o^x^;_ay?Jzd?$tVPAbFhwA=&l|u5_&bZrL zc<)x#8a>`=9N@S3il)%v;l=4~#g!5G2ihbTG(Y$8PzW>_0qLAKAVd8o{n}NJljpdj$b2w_vS{-m=Ee+ zn0=wD&{@$r#{3(dU#wjO7_l`+Yh5rr@-WGGdjUCkG&;9!8Fq%JD1|?Ft)XdQqlc4bc#%%KAnOuQ{Npy zeRD%(t4FL@!L9dbOKsvK+tVO_tGxL2CwL&kPLm}+miZA1eDJx?%QRk1i<+y{AA``zD!9lYnc&?M~f zi(90;>{Ir2MiUhEMe}qP5LA$q=y;K@Vn;RpF39j~pGoQ_)vf5lm8AwSxK>8fj~i1H zfrULep)qd4o_cAfq>6ib_rp-6J9X9E7 zF0lBfJdwQm1W#i8SzS{%Mvs}4mYiWo2waWrvp{5SAPA(Fmf+8%WGxA;Oa^DmK)J|W zf?uq7fci-Eso5FjS%N0(w0gKx)d+Uh)EMTM>4nmAx2kL}47P*dd%*N>%by-{V20XA z0dIaL9&QA@TY3}kl)6Nq7mMXR_y559UnypeB~l5B5jxnA;NAQv~WMd_w%NP6J5_T4YOY-fIK3_?6lCrqJVBHXp zylWQaDay}y{{0Zh%gwT!=U7r%KI61w(-SDTH@kugYZ2B?g`}^YaFMb@GEP%ym1o_uuR@oPvy^L1jW2I#n1MLv=*B@OT9#v%3sNSLoAkx@wU=^{-U-X-_&SL z)+FjV)26}+3^!Lavg=RGx@E&}j2sI;^!FELdRiqV4m=T=GvwTPQ4N=c9@6&M?H^yX zqWgf-%B<|{M$R>h4l8h#q996(?bIdObbq84C(4sB<=}gPG?9P2G1d#;kHvp?`!*%b zP+L3>(X2&+pGpcEFETVs{|SVA^k1p5qhWi60=o4E`a%2>_Djz}pxM55HU-V7@olV8 zrJ}hMcol03g>o-IQ=H0IWY`Cm4ixh@Xhb_Y{$Lferkaz79#EoMhCdn7zt!E@C6Y1% zo%9&X=c9svrdMJ8PmN8vU^Y~(h##B#yVbZLDmL|>I(up^qR_-QX13GzDA^as-h*wU zBPj#Mem5L-QGB%_zi-_1F*ED1StUdGpuTS}%DS>$ae+UVUJTQVfFJ={{ zU3Y#Izg5Zh{yF;8!dZ2q?FTS7+T-W_tB1iu43Pf|ZNk@XAYW~- z;=;dWZKE89v=Jv{vw*J?Wrg|&rOs8SWeVDxM@@WG>8@V2;oZ(~{b^pMC&|xFjPFLK zy4pVWeb>2-mF4tZlDDZ1P)K4&7RW8cNhZ}9J>sH(AY*U{Nw}vJ_^_nVS{RTDIXZHG z@2^>d{~G}ZR30cab1lEIZdpWrmHepAM*+{|XT-#GkGzNDU9VyGOEaq*g zF`W}ez+BpvcZT7SO4L2>ZP@D(RV~AG&xKAx7>Yn>r6EZk&HRti|C)hzis`jI7R^)z)xpZeb40yui%OkE zyvgsza#esL?Q3am{Hl?^aZ(#B$(@B-BKAr1cz32%Rr2Dy(OX+peu4oR;P~AvTwI=3 zl7JW!uzJsuw;P8CE2eZD@}7fgd*Hr6^PYm~HvS^ckRL+D#sU{nikvkr44|rcvQ&$2 z{Te{we%(v_-NcI+F~KTuy^;1%N`QR6-@R?Gr%$#J6!4K%f&G!(+~Qp63XK4b{IR?`>yY&)0zdT^&G*x+ z%OtL>de=-i@BWWDl13hX%}j85{6U>)7g$V~E&TBKZNmm_WT~uM`+`Qto$N{$~2kR2jT` z3=Rm!dE;pFGPyhcVHh5AL&93F%;W8bvrfYlHIvJkkMrNh<~M-mj@FSF~xD2CaM`pNSg zHosebp=%Re!2=--PFYgYBzj{?DliLz!ddKDd-UIHZG?BW5Usff37MCWlw1qT4qP-d zKa(AxI_xAMW{v}O|{~*lc;0kVCJt z+hPF03s76_NpCF&e6eSqk5rhmkH{z&Ro70}e++1$5(Ej@j(lex4iVVE@1rf`JN0)bc z@jRLiTX5BD|9oKh(*Ako>io~SK}GC~9@(eaL|I>t-kf)6j4e@#ywL_j0Yy@3Uij79M$6iuE0Brb*m^&_4! z@Vd|O%(X-*INZXwE)hB%--0Ojc(zuRH&U$83kA}o%>oPGT&svEfw;7OjtGvghFE#K z9Cp}H=0ea8bETi^m3r1`2-k<+1i;~_ zlUG!kh=0x#z-Be%%Y#u!;?sg+KuxJuxqGXUYhL8i#8>&Ai^hf7=*6D(-_Rso8Pxk1 z)e&W&4)O>Te;T>>>Ri2OXOL2xfh9Fp;dgFa*$P>|Z>Dgb39phK)inE{%UHBOa15JJ z4)}^r0`%pTeCgDi(t?S{hmh0Y`LV}c6I9k0RU-Ds3;a%wCvhjh}bFBl_OJ0rY4+xeGIg>}X z(_HEfFxHES8mT;PsP&&|VVQH=L&fH&#^Cgg%s}HBJc7COZ;Z)!7jCs}E2pGj1&v=% zu52oYxptr3riz8>I<&Tisxj--JB)c4mb4JF5NNqpROMx<;nNJS!j!NHn|{)Q7*T`G z=ZieuUtg=M2WM2a*!kfv<@Tp%Gy&7Au`GeIQWVi#^?t1h5rg76`ASLDwo?1c4xbRV zhB~lK4n1$U^Pl3x+Fcvs#99Q~aEUhLk(9XK=hMC0MUL$to1Vt*8_nzDTpfuT45Nin zhqB7KvEJt*agZOdW5Weiv1t_lMNBeT00EO%IKr5wlEU*jwc%{HH{~wik za$9H3UmAw%@zJWB++ zbXuSroo}V2=PQ-O0Q~Lm-jvsOG1xX|+s3k#)xVN`O{Zs%ZmnJu9^fOsI$6Auz!I^= zW?qGnfI=+va;?#+3N@bi1snvw>J=cGA{^kY+FZVkS|v_;A`D!#Aju9wtjeQoRRdX$+JQ$^Z+Xi-DCmns3&EDu-S`sAY4D?B>)+U4>BW|Xl zC)Cz$Y#%y7pD93K1il=cWJuEjU@n){Yms-42dz4vDYQM8uO%3GP+o>A;L~9Gzw$SX zebT=bGi*Nk>Ba0ss;#mX&t(l!OtaYP4yEk@yJjLUxo3sYYKlw9PcHp=mfOx#nU8pp zE+wY3iJmqkiC9Wr`CA`LX+tKc1e()w+)1nYws2@>SSv7M8o--Ex1T@tmpE}(S5;M& z3AUp@+tWaKQdPyR@^Jd-!;n{VV>WSj?fuX`<0^fjo0XofQN9^XDFT`5z7^`pJpK&w zXN%8K_KPj}^XDO^NTY~K9`cx7D+RU_m6%~rPA{_8DZ^^6E}VdkDKDu&kq!Kxy}fl^ zz|qZ3JGrvecvK(PiyT8P1eBb#8jW0fS$H;AcHw~^h{ESJgd(2h{Cz(Yr@`U)RyRSE zjBNF}rx5z@Hkdm4wuFmIDP*=H&LK{H8fTrvk(|WOs!v3P4dyEGHaR~pslN=a@4*s~ zTLu!`V;-AE|B;J$<^QfL^BW&ol)2Iv*|)f~5`27DR*EEA5+St~%uYbiYTg zIb(Q-`R5-RI>mMQ9x61|UXsc{TR-kb0Jd3y`^RwmQI}fBA_sOGzS58@bm`ZD))*BU zYDjCuSNMWEd%v3l44aP$_8yFf+#7ItF+lB!ytzBHiZZQEV7Tgux=vw|FQ5 z{I)+;{FtLr(FOgqg}R=qbgwKIbc%h~dBepbptb?mwI`z21i#D#bVF!^L4v;WKPgDG z9{(=;uGcNpNL3iUeN!9k`4ProK~BruvJkT|z| zbdL8UD@$Jenyb%IxfNXg?X(1$5_WEWLfXIwB7e4YJHscz`Lq_oZbAyA@(3;Qio^Nq zyHJ>U<=hoB=JT|zru_WEpdmDEsq~}c)RUlC2T_0MND(!c~cwU?mk51i8{r9z*bY=Kd;Ey+%b|4Yb}) zk2wL}v%Ig5^_zAk0Kx&iD1dp&AVAM?aOBplP*#2zQCO+JD)t+pFxi&>;Jd(uxQLgh zE$N|K3QY{v4Z9LF-53}JySN7zCSp2V*LukGdq5YPw0Utc@N*;qkx>ymcd>&dLx@t>U)awRiNi@Y_&74Mg3bNQIhHN zzAeE+#^D7Bb5ioy-Zfe+DBI_y-We+S`cIx&m1&X4`t2r9BkUh!uRO`3?uR0I4Np13tc;_;N~yxxPL2%(N^U_cHLuBp*jtUbgG9h2YOkHFS^+96k( zYx1`*VE<^P>5GNiC9c2yLh{bx(`|YhdAN;KBBnN-sZarCqLAppC5CU#z`G|Umm7y( zwp3p6^rFB4|M70x9~_AGiwxvE{CnLFyR_$wlQTvdU4>p6O#>?Wy~=_-@MaEH*}FqM@h0ce$^KzkSr--e&C!=aIZRWF~kS8sPkz|sa}llHK(_?R;{%( zB=TMW-+?ej;VKnH#RlrvoKPZh<+qk81wMbjT4!8s0O-9fjr{c|i96kY z7=ICu+Tm#F9as~%4m%RXlW8;+&wod0fJj7}1O3wAF}fq0=A?weCEdo5Z-}_RKTO;W zx3BUe6Mo=dg-Iy*MkuSjUO@mEk2Zgh#a%RufWRk#z#Tu_c(XR=T@B? zJiPZFd)RZx6@cO3{-D1N^rW&1{;Z$Js}MD^X9X%?4Q^q3ekQog6~DzBBIym#}~ZzsVZRT58ZR3DKYLhO4{C{GEjl>kDnOd9P%t z9A=@3C{23clSnM02<8y*YS(`gcqvFd5PFszp=Ib6>i}d6N3uLv~e6Rqf$kcL@!}BN{rKo)PQv&H?K5r z(^5DnR-&ne6ilsBU$UX_r4GQrFd~H_D}mgE21PG2tV6m<820s#*^z&KdLf?VpDGIacKc;D>qmCYBDOSrGl5eLP1Fo;IqescbbFwF^_V0kf#YCMsnG62C~5>kW5|Ft-Jc_4LIF_heI{9`A`5#&=eTNo-9MX zo(t6k%TS+XtXcDtMHK|YfNrxO(l4{_d{(WV#iG79ot!iuNd zpQ<^a-?+aXlZ8=lR|m18agg-jVTZ#n$K{irSiuMO@E3TX`5(Z1B;;NWX5B;PJZ#6= z`9}v5H)GK&%TDq%WKbZ<-P(o&qna)u&p(5=NxXE+JnmzmjI9&d=tkPoQYD3kpEVU<=r~nOc+?MUfH;k~2 zh{=%2)wcfls*KOhA{X`|RRh{h01vOc8U4QUFQFndemA=mk8+EY4xr7qRkVm2r`~@Z zlP#Wsh%mP93J)OFpdT19od*mke&uK#;YZ0-cpgA-q?i-b(8b0QndzjZ=h4yY-x6e` z3=QQgI9U%!i^L-O`{h<;?l0LZ*8$__;0 z&r<((uihIGYVifnE4!oW(Yui)zD(N2Lc3TK$PuReg{PwDxTd8nQGhTIvTf1QN){ZC z#w<<=#TRl-@$z#$ZfWzIwwq-BdHRl7zJ`I-G*H~HI21zRRvM2UInPRDT0F}_-ZK*& zK;)kuAKX3cwoQk(V`Au<7m|I9D`N-Zn~Wll$vo-&5&v^L5*@~2M{yld>9UlX(*u_Y zPu1s2dOuFWA;Gk1!vzz&fG9Cj*hT(0;w>p~l+Mnqu{%#bAI|jx9j+wkm+Y17{LU#8 zV0pNsquTx1VxC$t@EDt}6jd~z{xBUaZ~wa#NLCsiETE)ri}N?k7HwynLh|wF&sAs7 z{l(^SYlqK0H;sQvY_ElU zQ%~sr{rhrkjBDG7p65-Q)_Upgd%6J?Da@c60^`9mv%T1{FM6kPEQyv02Ba%OJ-DBJ zG2Cf0jz-M$UkOf@+;I478AJFae--~4_jSXj#PKv=@@>7X5Oxr%a_;4Mv4jD1_hx5d zwli9G9q^>e(#U;(@DpKd7AY%c)5OZyR9PmzRQa8=M#qT~2QnTQm|Fvh0X$>fZ<9`9 zqDMd?oZM75D9=l1CaETLx16WOk26zjO<*r_9pAgVKC>dE#qZX7GhLU;Y~;ybcKKch z?&tu2T=+6w=5yeEIGbvxL3>AWh(qjq7JKlHVJ(CGvh`VpmbGX~{uBQf8K+Lb3Fm5e@qVjeW`h6z z_%A|9w4uNIKP|x90J95SJh}dded`xjiOX~YAf|hZ!NJTyNlZ9%sd9!eQ5D^982dp* zuv$r!R(gT1Vy#1MI58L>2vL>o25nNVIbc*3JfDM^%G$c(57I0WY@2eDUM>QS(;K8R ztdvt?gCIYVq}#&yZ`bKmPmr1C@4LicB9l{)BM#ErIL1r|W|gn@-Yz76TSRr=AL=En z7lLN$?Zl|5XZ*`tM!I5v*ZUnv8@7}c%t;{*$0=m*^z00PkgIessRzY*pq%0%a}LvA zu6>ju&>X!m+$RHVIu-z=CCPB-& zJT>`nf0unvux9X@WkSbdBa4y;ur;1>^gRMeW0sr8P?I?fk9_^1mR&ywi9jH##&g`>gHg z$sRcFjCw%2_I1x1unZ-Au<>eMDZO2O*GGPo3-SuKN!pF!ukir~N4CP;?oIW`Zm3$D_ob6*c>SbPcy{x4`s;^Zf&-zRbU;23_Di0WAueqAHJI+U#>S#y+5>IG9w@y0{q1yy`4N6%cv!C^j_*#=&v&F z&lP=0ZgdR3d6E?6L~WNUYQl= zDEUe;H~P<5%YBSm2HrSOILH0cFDlc<*3f|--U?IClJd^O!uKRp*fkrsn>r@6D7Bla zZ`aCbN7%;Niw8n$Wy#QMNzPuJAin;YFJm)MfVluBg4x-$Odb{Hxg@CrOu|VayLzQz zGW0yT5L98^CTBS^?R9HEU>n)f2Ic@QfzQUqQj4D&CxrN*7YY^x;~yIvqEAH_efl|N z{E5mZIMXs85UpMkiutJ{U`+N`#9mazBjF=H9mFFcLdTnTn6x}WL*1V0Z;Ags^O>Jg zzUJned&q9LWj)`=`^~H6dizCq0OE854g<}6znWR*^&Q&Z2Lq3NEIy0}J1*<9>d_O} z36bx48W79Ps{ZA=1~>r5?UPX@6Kblu)gGoexPjx_znJAIAI${9C3*)CR>b`rrddCK z8)xq#SrHu~3uOD+wAucC7v;lJ>-W*e>dj4ZeNFbjjIVz(E?uu$uPqgomCX}2$B=yq zGK61q7P)8;Ie$znEk$U>PAya1J$Lw;FSq~xb^pxugZ$3`kma5U)ZFCrgz4Ekzp&D* z+6(#&7GYeJf{Fa@y=FBb-Tc8}*Ki?FSmdZ4m7Fkj{hyzzFrl zRiW?abM*1*=A|7;GnX;@)Pkai{nvTqI|L*7e>Zr0we*DJHSw-=hwY_^%{^YaRJP|z z^38os+J{@|u4&S0Dy{cn&3~^yuDw>y;A1xK{VvN#rldf)wU9R!pC<)F{se;ftD(Jsti4ngZle%sbL; znPU}B)^elr;g@$_r&auu8`{K*yPy{Uq@7m{4sz+rd?BIcxl6?vDS+xX4^Kq*Onl<0 zzXRh%5naAU#VY2tLzM#i#z#UhMZLMC6H%ksa9-Pds5IC08?1ak!0ruW&Uut}lMCSe;L zHx6M{M$lg6&Sa0aV|I|=;?sGa3AUsN%HMm=jrdnQjK#mJsM&k`&}^?s^#gQq@Oe+; z(e~o6-H%!Lqo}F^s(OJonjTWR_@gGK9f9Nb7|L|&4c(1WD3!}#bn@0`b~g$Pen`=` znp0NHmy0RKekTFTetBaJ+G1)}z1ndY+05_yEEA-KGjJKtZNZ|8>ilUqGd#$O1dAHx zi(V!}qqDC#X#zIMp65G-`%|>P5?^M+>GE?bn>OI)q-iDx3#V24BGAOffMfG!JcezF zgA@<);AEtyya0&|wO&Rz4p~pQ?X;lsreW(j!QqiPHa6RCAw?Z`8-x87@s<$)<6yuw z#QSo?{88OZ1BSfjDLMWi66;;2aKq?gE)U|?zMGDC3x(USI-^;8R<4nr70ZlXaAKIa z@J>DKj~h%2x?k!_%q@M}+~n-R0>?mn7gIR=XnL^+!(UoX;nF+TFh2>wZ6$c8kv#+y z8OQ32Kbr<7z$h5snVj;7ymP=uA;8D`ds=4;xJ`_ja{Im>!UwOd;>u^BN_J*=KlHNA z<;ri4j2vejdEtO=^t<}%zp*Zord^{AwfKl+6$YKFHiZFH$2Mu&c1xlIvjDK#aU*+5 ziU5q;t9g378`q)6I%$$+W19yLX(|t$B;)1;q-f!|tj=xwshEQ*GQ zcfA!}8(+Sy;>omK;DBcRZieb^g$!h2==nZ}hXRg`vyKHcsWB7={fHcS8qc!jTsHAP z@br-maqIuC@_X~;>Fd6cd@o4}o|_ikjxN@R@?{iBHz@Dm%p0?+NmQgCb0$}oqXO!o zPW{f!g{{}+%P4s~Y&IHDG7pampj6xy9HEl%MHNM1Vf?r(D*I$}ZPbJN z!jY?{f`zG6YE7F!P?*;g7k5P5eT8b$QbQ3~1~742Q4qK%5rQ$s9I#yoeR)%--y=`p z6zD9hH)W72k}=pyzYl-V8O0g%=AJEORgRU<52`^m%oJy%j4;S?F5LHLuUgp4Auk=s z$XHI&P3Ss~^0xMqJuXYJUo^4_tMu)vU=*92sw)uoK>7nTi>0_F#BGqb2Ep8q`i>!l zQ9U7RB3{m9O5Bpyh;je5b}nYu3X7;q%oUyHaj2<{h{^nYj}z>IGlJSLvfXvq;(I(j zEVjlf3b+z?#^Po^Bu=l`JN(!vQP>s|=>Zz6YUpA=A`2aBS^OzoK$MBeVUVEa({G1R zA3=#@UyL87Er>DYZz$c#9P|9o-?UUp{iwkrEF~!FmQEQN=}Hr@*2`xK!(0Kp2?4u( zwv>f1#h}E!KRLeevGBJ6|8X-dbBLt(F5oHB{Yj1twZZ3L*hO%5_NZQCAcwER-q@S5 zZC&mbV@Of9AO^zm$tgl1dNA>4_m)uZK98H8JC!8js`Y(1gsP zKl_KxRLvlIpX!117|!JR#N;Ul%q4f|N|lXjPnk}pw@(jofA3t%>O_Vx`dxNBIp^v z{mgFxK}LWmu*+QbAqZBr)`dp>SGz#uP=0j%45WC=_G(n6)?!zeCub}3lNFPsglH}h zOaIA;%XJ&$lvTt$>0~HW^S_n?f7glB>#;VA(XI@Ud$>ue+BEXv|(QRdpmU)Xkgg;J* z)RME&)s79v+8^1}RdxwB9&~ehkV%IE-K^RK{rzih5GLPnJaiWYd3tW+=`JAjw)mm?lWRTyl;)#7o=u*>}Kfk$X#32_p z@aUea?wzZ)P)b07`~?1~-jdfRYD>5;z#zMnuf*X0J_s)=p6|!4s#DVIa{HCZMm`)aYl7^1y`OAC5-$6&asnx>DW zL=p+>h^JOMhNBEdL4+ET##2@3RT(j#RAdj`ydNfeHwD)E-m~{RTXPKyj0vod*_?PQ z{ln=VE1^|*`_eVK~Eh2WdFFlAY9Z>Y(s({P(avQ7LQrL=9jDgS>w zeFZ~QUDx)|B}kVj0@B@`k^@K%-Q6H5-3SVjLrV7y-637l(w%|`(v8%2@P6KJe!=W> z_E~%Fs}?>nCP9v}+SCz%(KhRJXpJ+br*GisSk_#VJW>SSiOx;+ox4t^{ESUGg*CmO zf=x2L{zVWI)ZMv&8$O)`RXmvLTxvfl=MSDfF>#H&~6 znlx!#UzQm$0g=}cFh&<^aox_o?=!w5FmRLph=)t94pGH}Y1KHeR?RBhD ziR-4fquLv6tJaX#vY3Yt0npX>X6~W1S8dUAIN~ZLN7-T>G%KoS9NsaGPLzE3PEKMmL7+tWmW~5Z?rlBFI zP3EaFh34?n?^Tm7hffcG``^*-T7U@#IHlHO29^}JW&rFJ8wO}Eys2-Q+g;zst$l$1ay4exfXf z9O7DE`V%C7I5P8gJ##q?`|#D$N04k?`VH*v`}`$^c`VtB6X&gIuv@X)@UXGMn`IiB zBEeyu@a&r9aI6ozv~I7nhFe%bQhe{l*43zo)jP0 zk9i2S>%bg9XwYVF6#dG|p;ot?=V5QAa0SqEgG>-{YdTf*fcQWF6_EABjOhnXzOF9E z7=!zrsB2C7L>aP~``au2_=?lzF6tv~07>!H5o>EUgt}>Zd>CpL*7oRW$V57P(QvT5 zF=*J9-s&^6So_h_1E6gAsU4t?%R8=VBi+JB-&OdDgas%!?=TB%Pr2dJE)X2EbXqIJ zv7Y+=F28`L*BT<;(t|yn%_03U92!!JQFt>*cG@!nnv+Eq*qbD}Gn$bFC=f*fYEd@X zCAJvpkVKlVacjgJLE>vjqx0hW!;_$M5TnXau z_zB1f-A3M2_v{W%WA{}E3Oi@&7ln1(AIXFzzW1qLfth2q1OfbLcYCF^uq#<<(1vO2 zhKfeOYKFY`73}owDY)8YcapX=0SIvBQ|uh7wS@3x#QcKc&YOz9)}(D3pMR{ap$6o+ zU${LBa?~hXwJTT0c6J0B`JCS|bTo$vJIoYJ?!DdtAsRroo9He6Fdn?IFdZ!klkAaC zvl0$HsMQwBJ^#{`ot#cvrrVubnqcEmI8pX8Oj}QGmwE@(+X2mYOp&z|&d(-=A8Dql zLSI-@>d;KWEJlj)_y5c-B(u)f$OKU1>%PW#_VJ4@@3knZS~R2Mjeqj?YKl>VBWXl& z^(THkT7kfayAx?M`x6ra!zDXA*$K|~{~>+{Y)S8mfnUa=7aw5q8U?;FV3az0R248q zk+)B7v6kO&YefMt6Lk8lU=l-A1khYEs&&VnlSll343NFBawups^f}pcSl?fur~iw^}+f%eS*Haatg-4?Kb0 zh5}3s(h3D?28+4Aa&7D+GF5E@wu!%`BKg3(?OMOalG9#1$SHw+#9kVsa%XYrxOMh< zD>p^eb9 zUrHr(f_7BUbzMby=O!j<>%y!`kO?ER{?LBr93XAg@q1k^TQ{_^Iz4UF=%%A1!L@J= z@FjYi{uD`=NH6DpM7N(=2VOdVf2*v>eQ9u2#u2kzgDExv%l6GDIITMnJlb*HAr%If z=ED<`get(9*d@$1=+e2y*Hw_v+R(GZR7}u}f}6tO_St;|rwOV4GxPW}>GuNP7Iyic zK`^(-euj(w!J`FzDXXFhx4znT1QE+6xPh|A7XoWXN{>RuSE}MfAORaqHUoAQrPww< zK~-#DhO-i|v?_znxt$11c=h_wNK!hqfZIhE?E(IBgin2C17RZ`z*pK8f6F0zvz31oZ39ShZPWwhSdK z-?s2f(SoVku$2X#h}$@=bpWY4m`qZ{QiL;I7F8e6M6IAYur zzqEmxTy}+)uC9*0;A=vAakr?YVeS_jlocL4NOztIFvX8^T)Rv>rsEzn5Xf}Rp8LBP zt^3KeJh!YDUg947qMaP(plJn()Y<@DQ|90_C7_kE3UzOA#RQ3$D!FLN_cu6>X(U5m z7krG1zVp+VRQOJ8S?pRX@x3_0FCG_*=+bI0+ML>qC?K4#6VWSH6H0*)L=owtvuPUQ zhN+5!R3(7+17-5TkD2JviZ3))9{;+{I^xml>Xuj~(z5GOju)qu6`qL;Lgv6gFhOED zch1_mvn;%B$vqY@4YN{c4q_iHv2CB9MK!541?7aflQiso@D&bnxDlVCs)$g=`69#0 z`1$^WR#)~}B-v>-8B7(l4* zY@1Ui(BfTh`0j_Q)V`S!;}N6uKkrHB)vZLZQ!weV>jnCR?{7N`6;rlyF|YsO+8>q| z$GI+}9EIxKvi{`A6i*}eOUz)g5D9RXIMc3B5mQ=?hBsu zR2X0{T`di-+Pyag;~e-8`anLzN7Re`;?1d}0)fKZPTq=$-K3YOO0*xI7oUi)O4jD7ClPrPkbcGbL*Jf)=olv9 zpbiAQDC_QT-MK)Y!?aG~4Hg|w`)DRV+mGOzn-=pvxNF$q;k$%j#a5N%pSsvCr)iW{ z;dRz-difc7OM(&@0pp*FUU<}vw8RQ$@>-yHMYjZqJ51nyt`9vU!*|x(z1^QVs5y&J zbwX&7SAw;FU(Kfi>G!WY0rMjwBdtOksGBVv6ge-r(vi1*;HrdPbrd~4S2tm(-k!2C zy4o%|L82oW5+A4l=uY-vnrgJ1{W|pP3)ygJz_(>7Iw4!WX2L8-6X6<;(xm)FLbe0q+jOq`TC)w#~II7TR2d9@Z zg(vhzRAK1e=W*b4Zo)g1jzy;^0L8Wh)??<{Q9eeL+6QCwP6Sk!$42#hTS%|qpPRQ= zNS4wu%_vK3VIL$vY;tAM;ku+_sccbhdVl5=IrNhBmm~g69}`0sZ_jC4*zaey-+V}S z*)4%P+2dW&_EJ$%qu;*qR;%)Rfb+dL=-B6KW8;OfN1$SUu9Nq;*@$WXyYwQ+Hh>Q@ zVi-K0+ZM$|*mv6ND;pk*u43GcMBbX#szS`6!{tdWPM;G0PrQRolo8!#N%T#qOwlrk zRtAz>O@fYhG*x1$pjXgc+v=jS!Hi5$O1SDrAmL=kPSl5pkI~OgDM3Q61vT03 z?EUX>Lb4#f0s2cvA0zW*K3$Vz*#~)#sky!@vLym9vdt7XF*4SI+fA@H#WZ}s)+;tw znJk5yZ9pJPGhCD4n46rMtUsbjuwABLhG)^tR|JL!f5U0UY1Xb z%C`au;Gnv0ZpHL@jieQFc;;1sgTrUTx4`X~hh~(tN@^w=LdT8tY&akCYk#2@_#%JG z+sr@^N-VNZ_NxxRXx-G8x^j2l&;c1H+Z5shI$HzoXP3{nf|Kjv+m8;gGPB%aZJSOp%iV zXWxDK{*};x=am7&hy#qb0D|pP(Hg~X<dWYy7Tv|)0)80y+dmcg&b6>amihGJ!~D`FJz3L?dQZ^kuQn~3DJqGApp{Y zyFT^+qgUlzS($yn9JX0>=b_%-FcIW}DO+y_3OPnm8`8(UdKBB`D6IgaeA@rh0$7sW z9^hJU{){GTz@FOYk4C1NobIZ8VL!@|sF2%e)~JoG4l1b`uldUm!~c_*1_3J%!1v=! z-!MQgsjV?vzkByLta#n{FgP|g8}6x}vlV*%%)%FlsTLAE+JYN58LKLQ={n=13a^M6 zvxxXUTl-q6Fl#G@Tr~`UjqliSKxava-$|UCSt6I{g`68o6x?w?i-Wg_i5I$IB{6tA z?+UkHv1WTDA#xaYG)u}??(k+je;I_Z6-2Rvr*|VS*O#WHZPsm2{Y@1N4 z`8&P=+Ubez;t;~X+qODv?mqA{5|z+~`T=BO+owLGC`5@~Kw&Ao$J%u~RSHVkT3Dri zT~gSBY!mRxetwc_Q|KFs(b@99jvP@5O33UjU@y2SkayDlP|qXx>P@N zu87W|VZR-j?CVK!d=a6BWPWjy{jl5Oe_PP?%L6A}Cyt%uUUgLdEi?-{e12X$HI@BJ z=p0O~X4_K=Z2RPocHzd>)k5v;z`Ts=CMG$}sE-}D>C4_HK$m>w*36LyKCW>@O0`td zbjA?E&|Md);OaCvUD!_qrWqV(mcOB%hl`o8n?qPZ=Whz+7wyvT!?iyR{-)k>i+;&! zv}Kf;IP*Cn53}>$uO0T1c^RF1+lKL^{uTuy#ig-PKTQBu`=O7++-W|i|4rwEXM{RP zC6gX=+K|asFO^)Wx5q`zFAnZmT4XCk9jQqr%3Lh>s)QP;ufr|q#et{4rs#{oD`M0o zfh#}BZ^$ao>}2~JxEl@_%fGI<%=3H%PFhA)SBmF-4~!J>03nnt9l zzKdwfDRLQ@9>c?+1#6G+$wsLHJzm*iW!@e(@dDkaDI+BzagD+A@*vTDoi;8kwa9qaV znDM?ZX?deaa&OzG0UMs&oCFUl%)he3qQBP%-3~a!?az#--~tei7tb7!djJ=b*A3jHNgM z?Q4@`KkPJamTTB5HmxA8!Dcb=5}!#$J~!}^gH!Yaw~q%;HWHN`)is`oqV}*Eh)r`4 zGzUC7XCJk5t4@Mp=sgU}PinU#s@G|LXrtxTgAfmp5lmrCFb>{phDV0L%BDv#;|2)G2^p;x*bCkuQE( zr-)-p26kVucH^Dv&o^_+nET!`9o&RbcJlD=VReR+2KFhI2lXs!NE@%N>I{$5bs{VP zjEja4^_R7kk+!@E8=H68=q>UD`{)4^ zqk^+z@B#ae$9`(PH}__h_VdKSu)s?uPm!gXzZ=sap~(KNNs|>u-FuSPFp(5>VrHZ< zVkrgx=$?A6oZEN%h*aI8qO_;<)Z}AE54|~gYd1-fGF>a)+i#1Xz{{Ba#K{KcI9JCx zmkkuWGOf1?@PrXAy8A5ui0ZiUk9CGn+;R?%t1YyiKM_Y|%zA_Gj)!Zl%TSGoRe|cf zJU8n(UYzE@8x8$r-I_rxKWLLjW!#CDKQuNw?~z9a4-fa=`J+TfgORW23XC}ar-A0> zi-N(2tZD(VLUu`rHreYSn+iK)s=E5`%`~sY;8)GM)IjfaJA#|v=4xFDVkw#`88H&A zI*8TMdNuEPOg+SuHC41A)P|R(uE3$Ab3M-a{&NM%xZ8s*8~=cy)VYld;vNRrH(0Fy zrOY;+9{~e~b@qF`*v|ngDhx!~*>b2v_kA8e>7S@}n>~4`LyE?Tz-GFlJAz>t3r|$t z(7a~|?v-daGSz*A+HB<;j|t3JhG)+xYljS*Kqkj0l!$?IoLcuc!kt}#F1P%~Sr2P= zKfchg6=eM}yFU+#SslH3-@kgcmTA^iyE0mBK49nV2)D%@Y8neui7W2cD*zj`UY-Ep zTTL?7voBgR^PAq6h46(Vm(5-bZ}cbnNu=O)eQCmRw3Y%*Kp7hfI_OxFskU#}l;<_1 zMw{W>r}Bppbee~0tR~tCoQ&{SftD8+=P?a@y{5I#iuLt&h7Xm+_;+fl7`RVL@6g{t z(0b|ANe59urg<)gfQm8rx7MKzqYd{HuuHByjJM*s?FtH5squV)ig{D!k<>jiBZlC> zJF2_nS)|09ML)jSpou>(XRF%+lalH$JLMi1CV4LQ(9QsRto8!tr;Rc5u`?tD7c{ zYqjxa$&yNzjly}rzO44ioL)-1S&qW>Cmw+`BK1WXg8h>>->yZ&(_HHanNnw>8iCN1 z0oi6qQW&4cKPtwC`dGi2!nJva+r{#9w#S>lDoVXW*zNGkuRUyeYuPspUPg7D&8?_X zRWm$CjLJzcxx;Q0J|R1t=O1@F4smD^eg|CkWNlx8Mi8gM01%na&ZWIyKy*%~1Lyr8 zZGdGl)_W~$PWwJdJ=YRwA7ydf9e*jjte3}5&3bV6`8x$rj*PZtA^8xGuBkU#tvfzp z$qbCNCUx0*N5RL1f`;PZac%X}BU5jpi9U+Q=3q@cRO z*tNA6cF_GiL@|EAj%W};Ir-NgcAReRU`+Lk<{Km=u4Xo?g5q|T^>(&As9Qn}gLFh$ z%F`tv?o1`US0^g#Pb&++=oP#IhdS>8z;jR>x?)PiK<%3<%eqjyH`eCxb3<{0NMNOIJm9vX|0~6|7j#Rf!c=NY3|1Kkp;Twqx2A<;n zLQ<+2y@zAbf6JYY2HA#(G)n0&9X2%V8aM^euK&^_lh<$>C%ZQ9FChB~IM0$(@VY(w zB8^RRTJcq-(i!+NtML#u+V1NSFzdu)P8Lwg6aRtVU zMkg%=8gA23J!#dN*WBB3f9GrJ!Il}aN(7C*&GJYG;JLTRm zAMn$*@E^O> zCKXPW;Ld@<*`tWxExE=tnoAi$&9&B7!1h$e6u9a8Y<+vSPV1@))(nr=Sm>^}jy$`K zMv!mR)yfzUEB$u_$!f5J!OS3}B0&=%k=EDK5=8>i25miSH0kBB=W|YqGt)W*cMec# zIsPqBh=w-Vw0x_fzL)vOk_3^7bb5cg2-0VOw!mftVsdSHsP~vurWSnQiazA<2mn|T zsdcVa{C#rreg*=V9R?xLvIh;BC%IEPTs8iFo2sX>?Lj6tn0_&wjKWrwuTuV7ReOH zEbXx5&n!DX?tOTMw4;15UR*Q^3M zxoxhvfVMwgJ~Hsgammwqswiwq`wvvR7Yp%JcP#R4Son<)#x8TxgZF9bnOrn02cI&d z-&PWb#+T%f*tDmf3#di5LV!{lFiY*RT7!Xb3ShfVDz#he9KonDkrX#?UW_4XGpqpA z7k>u)W29D8#$Q0bmlc?jlW~TLq@~-B$VQ4{r%IPG+V2Y-sXAMVe{1B6r|cL@Z%yqv zUivh5_DlNPn@N?y+5vFTm7o^VdrKC3cf+^lJ;J-aQwHA<8VUVCvf z`-rie>D{`x(~N$}ls)=awEk@4EpP@`+a`)2cZz4v1YUClU8rPx8n&W#23(dEA0yrl z5`Y|hS~%8~t4j0`vH$^Ib%t3s%YMtE(vz+Y(Ef)ev6ctTx6J^S0(AuwmyzPj5iUOJ zSZ{7DC4!oi>Y0mp@@J-~nOL;-q}(5d2VPVyu5%lvfD!vh-UBoZdD&cKVdOi>U%p=U*P2@qG87jarTK-!dMG8Mza0K{|4OO&huZ`u}FO;<* zCXSINl@y+0e!r#R6o-yWbTLi4#yX&?hux{w8*k9cg2TjAX6E=K3P|S#KVs)t=4Ni| zs*4BUMSZXyPx$T3bk<_L`a`d6vpDk=@^^Xx9yn zBz(>R&nz&BBJ-42KD;-8)5co;?^C2@)Ye0Y9M%UoC80}$%C8w=`|IB(qM?MqVdZoa zl=bHXwd@Yy&%Rj;lPlZP7|*ACwl;Tm6ncSBKSV+_{684lKQ{pZ6q+_%0#fMcdvJp4>rky z!>p)n3f*yfvUGbxY>@S*Vto3%sLvl15$WmZ3Mgkfsw66n>c_G?7VqW5@F0k{ResmC zV>(?EOf!&!T9*1XPd!0o#getWsEAsQwcv(^_*ik&xDV&wy*-nw8$Oe$>Db5OU?Xt& z_-2y)s9~o^YxmjA!Z%UguA*(18k~t%N}MAjX&lzcj1y}b8L)P z@e!iV2Rc4rh`e~FS39wY_i69_aqYO6oA~s9g`<>Y0$Aqp(=u0->((|BWx2GnfU~$+ zbS{>=;V+xEtOTf=;DACei_{&8qr=OU#lq6*j9GOdUaaWDJ; z80CGNTXYe}ew3ekC?sSu^ZLV3FFI|o6Cld}A-3}qHl3)!@u~u*NtRPb`1;!r$a&{1 zs}V~R@sLQGE@d#uL6&WT8?eL#R|fvzWD!RI3DmgDy@FwMS%C|U(^^?v*_vS-7DsDn z$Jd7|3*DNsKCxxy1E9#+BlCs*hH#s8WR z5LBKy>1X-S!JVO8_f=1u)UHu~O_-+d;1LKBka&0uBcacqPLNluS+kgsB&aTF14L^p zrJdgM8%%NXtald5C}lvbnW%3ZKO#&?I_+o-=%%i_Uz()3&Ug!2kli|FzfGmY13-2~ zgzn;JKaNZ=B}A|J(9O96kGsA%ZmF#U4hlf8!4%scV^KuykQdPNTch1x%e&*vFH)rA zR8Dd3ZH2>qUC{kgSDZKrPeM#}Ys+zV!3~<2a&!CHe+w!q?qeJkRF7iQwBt2a#4l@6 zb!dGvgJRfI@?SraD`kt4(&*FEaIg0oC=o@6%{V3vCS?_>dL-r$E!SM83^wU=BH0vM{L5&LST#dGw&rUo#nCA}z5NJd1i z8m|!<1{KDG$muojhC&0sy|^Ce>xYq7z+AbvqdvmT9J($76s%wgqhdCsNXgJQ7U}AZ zg|1D4yhNti6qJ-+;z?ZnlcHbi-M&ooM*mmPL`n_x5U#UKOZ;t7vNAhfAv)a9j1Q&v)N{2SKIZLMCMoo5jS$9st4T z!EW6&{`UhI^=GCe%JNf@7$A!vVPFl_0&VB4Gc>KSLrug;VVJ4--h1B)Y-xY;qWb#{ zcfXjCZt1d;0=ing4JmhL^xYN{!db2fIDi4s1ndv`U!8mLl3kYqy>-#c(f23V-!n|8|R=;)wTtQ^uB8E_bb#Rh9Bo<2kVJ`8) z6m5s$+#vc_v7K$va%AUpjw$dJ>^*!HcsixC=xe`^5KKDC2{xi5H_xSIgYX&3wiYX-&RrnshV4d$)Odq<|f+Rt4|6iXD1}( z(%a5SM*e$Y5Q|$SG%yc8)$tBfFtJHb5%Bo0Gy#@n(YcgLmZhG{$6#^6^O;Ad_1kfF zyy91)>Hfzc!0i4(242RUY+MIKVR*~J3-F-2{jyutKEShadVN<8bzRCH23I-ow2{mHHc9zckHj>o z%kQG0i#M&sYQ7aIeaccevp;m8M1WXW|J8%&9Qs$`f$>7skdMBF901EiyNzrm;+Ag;8 zmVb>@6ifMP{yB(hPiP&0VcSLL3SDEq`>DS*hdBK^-b8&XZZC$Pj}wE{ekHYo8*=ge z2^LmzlG-tfF19lL;&q??EF{uqFT7aA9gMi(T(vyscIh(lerJ02Po=*{2D*;F`!?7> zUBUwb2s|&`!%HlKV)}8$J_?A-F%ohU4d1XovrJ=zx~5n@3yst;e6z(+*p-L*9{GNj z<$WpJ%iV;?oEFr-Xp5KxlGRp)uM-CajIn^qw(|U97xJ{bW0ZJo0b2QU5hE+f?}eLI zJf8|->Gntel(@ENON`PzsbD;yURH0*H4&X{o?&Ga6-GBjWdfD(PEck?x0Mw9%RuU=lL-Ly9?YEW-Ui6fJ0$1gEA zd($6&gG|(phySGz(FM>JI!dV#xlmdyI3+bFg)wC?4P?@**UH!%SV7+iY_hH3eu8h5 zEXB_}gY?g!D%{MViedq6zI~d_B-oIx>b*~e{=gTqtD`A1c5{c-eSgt>6UWv4fF|T+ zMARVKs}d3|MlXWSL&cP6X|rF)8|dO@sPlT?{%WCdw0JoeX=HuBkdIZIz-|g;S!!^! zouZpND^#ObOiz|=*OC(H2Y(E@>gC4)+J2ii_*b6@)Gbc{=jqNT73S<# z*KKwjO3zr$cSm<3L(`CS;{IoD7K|yoySBD$3`}ER)1)QHt2i0SBsCQZ)t`(2a`y76 z!$IeBlO2KASKDggjbeVxP_RO7^Y?;|m!@+xB z?|N-j$iLNl*jr#_A^#;IZctJ2VrP@O8APOli9E_aHBe0$+ys%Cz_a^7Tr*^YMJ7zk z-bHQ9Ce!5bzGPv?3w8eHcw@G;-RqPOiQT#uNTX`aP5oTlss8>IJusxp+ET{9us>#g z5$aH=WWD3ZwYz!>0-lr3;P|vn2&cVgo^qkON1DQPeEQ21nz)th`|^%6{nL`?rQ%=C zVl+?|0LK??4D!0S6MRcHSVHI&m1blbmW-(St5HtYhB1jtqn_%L-wdLO;xyQC`ibHx zo&vZol?a`u0wUS7R&>uX^d4G?_(H;x}xk27} z7MAW*aR5&cFWsy_o7z; zQ=@;2RbDvuvz3VG(U$ZlYqJlLXSXPRbuCr4sUx>BtdC&O*39)wA&j;@*hu!ZbdSdO zLR}uAPn`Jj4fw|m#Ia?8e>LxzM>I#jWT-Pzl8-BumN+BlWg&*rGyIqdOs|)wt)-l1 zf5>@ir<7@>ETv-n%Y#XtvO-cIFLThUjRqOeq#jbq4^wYAR?Jq^xAk#3d z%zF9rHO$H$k#W;(S=N0ooyUFMqvfyHn8PL^iWy(7>gvi>t zv0@cDv3tUxZ%zimC}UIk`OjNhs%k=@L^r!!QGU$%EYdybj@abM;nsPU2DZ~E*bo41 zp?-dI3`{ByYy_uW=98}1`Geij>Nt^? zP$kDN4chMUI9i`zKRSYg47Uoa%NIpO$w5j1;w%5B1;E0>YVsVtz&HL{IC9@)L;7)X zahb+n!s`8mDB?O!;Soy33G0U`o@~NO5<%~iyxUy*rw>(2efF@FcA^EA774@>ovm2| zVRJ#ok18EmWTPfIHpjqI*?aW5*G;<5_VEcW?GV2R?HO7Oc_g4*)5WJ) zp$y&)C0NGtZwiCeIp^6Qp3aLD@!!1rS^y8U9+2X4_D1LDcq_VWD$ z1Axl(xxXpK>pFbQpP{&V7|?Uc^u1jANYL`M(cPRmEUrd z2dK%Oha-NMX}+a}U&h5=pxz}#FRdqiUi<2o`@!~gV)e*rjc)<|Gk{apX1G|vr2_0V zBRs&3s5ygwdnfO-vtfty52Kcv+|trG(c?i+G*wtd%lQ15nITaGz)QW92Mom*VFec) zQbqh{f{8L(rmrNn&)pr zDS0#1SVj@qYpw=^w#Daf7)IyE**#G!jWUgN@oG=VbM!Ju(@=^q7W!{ks7~E-$vBn6 zvfv--GshJX;IF`dMqu$2O`t>Pfjuy3(k*&0zfVa;Me!mC$iAz*#3?NmwE{9A_ulvB zev9`i0xk`JPyie{DnqJfxy)H6URCtyF66;d*upeJEAvx%3i?yG-CQ|ZyVSkz-F2B2yWR1#IwIIRv}}6 z9C}7Z1b$|WFmngUG<=NX*jQz}|LCLM3~_soWs6rD&G8o9QUC%VfhN{?9ac0TT44IZ z0z*Wc!-MKuN#0`^qCo7~UG@h*zK6rddoOytakBTtrdvjdG3_3*o=+0$%TvP8h>cQt zCh_Nj!H|po$RFyFc10rIEYEo;vz(;|mw3Gx4(#o3^@Bg&^pim1hwpn2egMJYE55jI zVE?+~i89`>fb`VVph+P;S|~VE#G4D4%gmL=yIqc0FhgEGtES+(9=g9Hdh4_dG^J>A+sW$A{?}Rt9gpvNngH(KoACO=c;C;$HIiHP zwfYaj4MGK7k&4&c^`5H3pdC`?8(EQ>vXvq-T$r7O8bl#(sMN<3$9+k*k*H1T9}W>y z^tv3Rq@+~hulXhc2-`~oP@{t1zc+Ww3E>3Ny6j&fj2#MC-qE0K1q7$JM)@J!a$b|5 z8=|es*=8$Hx;0GkP%hH4ahe5_y1-M7RHe+W&1uC@jMn{-o8EFXZcrlp_C9CdB4OOm zCuTUTqNv_{gd*E!2}>`n0R9YbTH`vgNvHk_ltT;gjs2Yv_hj}orT%g_=-c(?^#CRR zC)3FAh;LFb#?#_9avs-CJBR(gv9ZGsG7v+Pj;`ePZPV~&9cz~Q2R1LgS?s41eJ@r+ z-ptIb`;1VBcp=c@@zCCtHpipir&llth<|w51Bx5oMnzAfl~$L5QA-lRD(?ewIEBD- zm1uFTE|LLGF&nz5qz17IPNas8j-d~Em@$h*Os;X5D2g-aX#D>nQKR8Y`r#P*2)L_u zK0b@0S5+*p1Rm|o-qAeIdPFfhbX9<}TqAX(3F@NZn26^7iMjNQ6K~wMkNdSvyk9OU`Nn;d(bSl~|T@V4@5NMCaNYzx!d$ zJ9F^H)Nij6pU5ylVaK9?d`4WD<d=+yh92G2yaP%3hSlP=_)e>fihOi4RV zCj}|UBw(BQCXK8N;tX*T{gq?tt~V>S9+QMh2#LQf65bAg`sw`l@z@}56Jr#qmjZ@| z^2}dafxVWt9F?8258nn>xfeal7G-J#3&g|M6t}Z#x6_gYFthNAzP3 zIouyub8&iL>+4b1kI{Lq}0ggk2dkpZvC$Wd?yNUeDCb>>0N$+3Ua@zUNAdb{E(9@6(_o z9%8}|gE!CY?~k+|J#?H-v3}Io8x2u;-;k!-SiMKn&ru&y>hWFFNV*U?RRzS>yWwIiEPbSwNDtF5IW&K;|V|JU@^DjSJjzl|(eSLz+@H;^{ z=t1miAX_l5Jb1RACZdjIV9beByN`(KuJ**kafUVJ&moQPk<)<_ zWOJ1Dm(z-YFq^n%bAzAdFQ*ad2Kgz>w*(0$;3RZ0$&lAk{2ZWP6V{Ic+@i(fyNA2p zS3w;rxvHWa0ZQhmpao;Uj*?|K0bYYgN4IUfoq` z`nEv@o*rKW^c6dwzYo{fv>u<8!*I(V6p0^MX|fPram)@kqnvYck77MOI)vHq z4j+i_l~Ikn`8X3Vu3W1n_&dH+)TtSa9!a&(AD#EM;u!(K5u>++Sz@kHBwx~0u?qi` zN&QV2jAOThLyE|+t{~;1IE5~!AsXSsLH!&MidFb*c$4uXBSBL+1gAy%G=Ps;38G!I zAUQ>71*bQ!rbm$X&}NOVuvoFf{S6qqUKTC0H5fAb>dR6A6V86Z9x0XbUTfZzYIene zWq<|YN>8KX;qwGf21b4Sc!l%)NYv-N@6VjGlT+=C@v+jEpFblHO!`I4^#vM0_ak4S zlio47<$7be7mU`;gR@MM3u71kL>0`@F)x>u4w;smu9=D7YpUf;@N}8bx(43gBkg?x zN9KI@IJduE>hyhtDnAZ>uv6HKAtfhQU(ILIlu;2J(U3c<(*joHyy_*MO;VFYI^?0v z0M;I28#P_(lt?&R>bPH;9Z_;v;`8umRlAH4Se%$}Bz;cW`sd79Wq2wO0@ozGqCpRN zjZVF(Nr;ckA@a>@>Lc>nMzfLC^7^F)hcpzz&OK{VymFY;AEk9aVk7M})0){j9shi^ zQ47O86gt^#WV_)*+CRV!ETrHsOj)UR8wR|Lj>|>+(;0InsL=aOX+tOT>YIH{969Wt zQ-OB(2ilW+m>?od$nj)?824FPqp`O>%bz1WCT$`Q!Tn)tf;DYfqgf_AB+ZB}FQgxtG`A5n-?MlkHQ<(C_QcTybHxyp?~Rc-JuX~b-Ng&B za>cc?5$ldxw7Rv z)7aAGnBN2Nw*xO0cr7729sAGNHM@%c?k)EUVig**{q@z1hm{PQ8jEF^(6dO~dr38F zd4u0<^wH541JN!m?lX}iCiGRAXSDsAj~VDweLufjL}Sh*&Td}GMxGl7mBPZMh)Om8 zeMKS-0*`8sZEL>3@W|HUVx<^ZnZ6_4#2%W+71{{y%nHBBTF||H-WlAUaWpwTg2?}D`kwRfSL2D_>=^SZWtyHe za~dIcaQc_^fvgPUJ7qt8wK9uxSokaxnkatLK$TT9t9u(L6!LPx;sO#Y$QlPB@FhsVV=rm)bF53egdzOz=|b-VyZeLY?M$f-Tx7~R zMJBbvYO94=d>32>y}}%n&@Bgd9|eR=NCwg*MCazSM;vN-}diDT&>{&4!FQF1TV>sV|ni6q=k?pW9Q(|LxL z3dygB9Ue}f*9kcg%`$6w&RoFhJLsJQM=Kam(b-uCh3s$I_I=m`7REe8QPE8vNrVs> zq#~TkC+lJQ%?3dirFJ42i+#1sXG;aAKh?e^ z+2h`ws$w5_bBN^Ie}9oBF+eeW{7Yp2^K#EnRZ~+=UAcz;OpzoJdU`~07n2TVd|FmJ z#(HTnPSZd}+F0O1^Imk;OJlDz*3gsb1Vk3Stjs?HUtVH;h;{#0UO*MLutcZw z1WH7mnuEP>@1WFKGR=b~jiw?Py;fF-A}7c}%^&V^ZE{^{jMACvm?U}1A!Duy)tS|# zL9sOpJRIdHAbo00$(DO%-EuoqQv^`>!&40n`QZ0`C-7 zLUWmeJm&hN*Gf*<5-T#y;;5-)$AtA$SXrn@RZEcw@@_`0^P0kEdLAvyo@*#9!+-1| z_TYS+MjeCcA;40VW=BkY{#to?!8!sZ4Atlp2CLd+M!T+COdr}AV|eohuNSfu@0vUMqlY}9e7s{ONjA6F0JVcMDZSXphLz5ieVZfD{79YJc(u2i%uBD(Ac~-m5bwrsODb?&3 z8x5*S%-I6!vaAb6*^6%<$w1_;e(V?yUH8(pMoR``&J3Alvzu0Q zyiAR^E=@1Sw^vpDff@kCNLM&OWd>XpNTY|jdZG#YLd@coyJ*zI(^zZ zl3t3?1pi$s7&gsrbrhUs5@avZfy;iYgZ=6S4O7=Kfu4uw+x^jcT6!yY+M6G3P4n3M zx(3KDt;J{$X_Dc~BT78xL9J$-awUj>>9Y0cdI@$;Ig|#IFQoqW+DpDLNrl)Xx-z%9 zwUP8kEX0pDGij-Y+SGTRW!(tB|5{+qW%i>bb*~e(3Yx~bo?Ae+OP)y0(Ixl)nEL9l zsJid_A(axO6_Ah|8tDc>N+f6K?vxr*Lg^4BhHj*0Xr!g2q#L9W>F$#FU47s8^Zh*! z&%=Lk?>*=2z1G@m?_)-&tKK}K*PK2C8~z4M$>8Gn_6zrRWC!^fE<=$A#!xPB4`c{y-Ba7>sTU0p`*8U10xNO5o?4hO9i-^%}IX-F76bnWFmI^_dnWBofM(i}U(n zA@|_njX$B1`^k;b##91?ePiS5Y$>9lY@{iLyKUe}=E2GhXd|K>L27AbMG_t{J0;68{iJ05 zCr`T7`l$W#37&XfP>nR@G1j*K2_Sci;cul8bQWmV9}^nnFdp;)R%w zRjDH9vDgx0<+4p_9L3by3K)3lVu);yF3e{shU~YPW}N@7@RuZ$-z)f&B(kH5=9pQ% zmZgQ6+dBI5uwR$~1_F+3ZtmjsFxLA~Ijwo0nuxzFq!G_7>G#L)pz(AZ03Jw_pejQW z?*1a(57x{zLFkVe78DQk4}vQlyA!>j9F^)oKOn;srM8Eg9~E668N*M65t@%y<{3|B zh|O@e`=(sGh6<8d#&4;akO4s6!3diM1Cl{2TU%Qvr;74&m1piA z9`{w>+9q~)Z_TS%Q%VXhW@CbbXW;M(iA&C}htgJsI~<-8_gBMfp=YbIKRU` zs`oNiI1AMJ6>YfNa45EObiWH++#2~jD41jc8Pkf7q#1m1krFSydjCGZFf(K;XQ=N%YZgz`ZDP$*)k>6@Nh+fm>-*LcigsYbME6{Dn^Hxcr4^+241lVm ze9h;^Yj52-E(A(hzv7P|fU5ERb@{!wt*xTLW0MuCBbh3v4U`WfI!fghFVr6IU;#m} zwwo*aR;`|~%;5LtN>?OMf6dA=+T1(4v@4T-`xa3raMr8h=Q39d3~h5?YCX7>*tRtN za9~kZ_4!$MOUI?*kneLaC?Isc#nS4RWQ>o;gUHIgfn763G(EHjPFWjT?H3KQWq5}o zUx)nBp7E7Dvs|pLzA!#my)n71g2Cv~#LeqlsC<$e2KFF_#7<3~L)2E5(v9)U!k3(0 zhEGs08}98qmjiZmYpSbHR=Q5cu50Pz!&iJ}Uu!mJGi<)0HmOh#v1ssyy>sW6m-@l0 zU9P?Ea3)Y-wW;!F-->CLasj#_b9`kb_b?N@6yo9xH!F?s!*#M*HY$29z)Ix<5Z7wG zT73%2cRmM;FYN4+UZm(X%$Mr#_iFC~KUbbNo^;M%^2{cxFwrO;>~s}bh*8JjgoA-p zTLAG*9=>l)84#$Nf$~A9gj`l@rVVjM5T{Gze~AEr%u6BH^^Y&C7B5dE?oR;T30%2U zjBF+tgfgl4DV&gRZaaQGuY?XKru*Iwk1m z_T*6wpO$JFX=78~l?Fa&%0qLK<@ZdvitlQDFqk&q*nuiE2B|Y)9-FjAvyTZZ z@Dh#vWOO60KtrTE$?4J{VQBOgu8OhzJ@z>O=uX&>y4u=;9{ZFwkbgc33hykX|M~#M z()E-;uw#DNNp4}GUVcji@wHTCjvNOyv`6xt^1KV1PuLW$tF{An3s54$5&6pV;yksM4u| z$0+|;H2@jIuHfGtpyV_FrG>FNGqpqiaNFD10&ObGN@Uam)dfA_D8^S zIR!p)3O9+g^ZhcyCvH-4t$lmuzl)G27%v3$nH zWSj%eH2hOpG}FX1KW?-t1DPhurhqaNW_|b7u>J2w2c!NCy)|Smh~|$ZL?9zV7mAJ? zDV>0QTzw!~D2FQ19#w~%N7sd&YQ3iLYvp}J4R#x9#nVYqngCXJXgGSr&>6=NRUrIz zn=%MZTi;EG)!Uoyw>N=F@%ryc}~%Wz0QyJ8Fq>Sf{mc@`oMJ9a5= zrf|Zu0ygEna-n}m^`Pf(ni2S@a2lSCsE->QYJ%tG z_XX3dmTL4>F240eu^w_jslcVW)$U4kehTbWO7lS% zZoDxdvD9>2CX8`j_OaflTBD_a)dX@K#>KN3cYAB7<5~}5`-w$-xuqa zfmkBHfd6|1dqyyiUWU*~x9f28;k+wn5$n@HqYdGJPc=Kj)uR4`L*U#=Vg0^QqL3>+ z*!GvPQpg7kW2GSAQ8RY@Fj+MXt@*f*GKJ}8)4~3X6hV#vv;n%%EC}PSr^@7r02+I( zHNh_e;_C)cb^3g^gG}k{T-=A!*g-_PL3NyWhgoc`b4gx5lDwE1eY6%AS}tE+K<`#h zAa)SYO=p3z%(aStG1av)QJW@5()znGt{^BwP!}T+C5T>wuzj(PqCXv+QuXI@TMOxG&ayt^V?*PhsI( zaEJWp(Q7&HnkcYUM(LJE?l%Fy4T^2~?x^Bo9srg zEWP}CYsozQl5z}x3+{54h=suBhP)Nuv|s8V(Yr*AyNbIDoactH8wO1}D=d$90HaO~ zpfZqCh%jqG)qEjML~a&ygxhu%(gtsd$#p(V76C%gRKG66NPGwrXNIWD>UaL~uXSdn z>g!6i22g(+p@nqhQe@7|7pe!km0O0~X1@hNoVKa;SsUq|(7*=G^?a;Q`-O@{_BO|(pg{pS)-pHNVYMrDsa3NOo=vQU@tIMMoi$1o=4X~S%+*OjYX?z|Mo|ZS`iuQn zoV&5G5Uc@HuTW(-gb}3%(^%0Du)&0-AAlneMpo8Q=z44MY#W0!jo^>;`6}h2iD46y zwKa0?)9Y!cyte&)=KgVp(;-;<+w}X7jM_+0ROc_>z`#C)Sgn|KY9kcJn$)zU5C4qq zK51NvP(w=BuOOtxMKk3$Im-tCpm5V2;#t#`h##@AZS9R)u}cN&)%>eHqaP(Z-Q92| zgomMnJ1`%QG(1|CgC}USA7<~Q3P(WOr+_IP`Z2&LiJb>JCH-PEI!yVwr8~&5>vu#G zR13y@{QL#4!MPj)*ik&*8`poRa?kw*^}%H%)p5luH@nGRB_=sPUes(43ybMT)2>~v zLZIiV$z!D4Z6-rnh}%3x$AYPYFH(Ed@@pinWB$jgKcO5RmJWk6YOCDTT<%)ejj6>D zHINg`0&@4=vdThY;^ki{R?40fkc!zkg#BgNM+W4x9V-Ykc1#-@XJHbSUUzR@wv5d* zE38Snbs2!X^^8C~V@GAOgNR?erI(ZWnSdq>EgcZ#r5dWv+{`sQCy~E^4Z{>4F|^YU zpcR)mxoAq8b&~osD%N}WhcEZ#e&@aI2co_XKHC#p8vCUPrr+MsI;hYo8>a^Y;W2$? z|3}@#eL?`URp67a+)yK~kBR`9EL+8Alv4V|P}bRD29?zS7%MVif&Jp~lif8bfuP1d&@VbyIT(}zSP9w=s!^gGr1G!a zC2=ty>z+_)FEVyu>{_(q{Z#=75fcWt4ezc>jqcCxwKE(^qwoCX_BiK2cah`v%RLIq zLpM{U+pqWl<{+!xecA%eyOfIH{StZpgsg3$@ zo|-F9D}pk7=qz#`^Dxom9X2ykd(g!vMi?g-dldZjZvrQhCrO*m>m?CMGqtRnkcAuD z6MIr%#82{DijMIFnj|NC9&C=P6l>&)VKtsQumh9Q{YGz#jXYp77E7N&j3$;pYm zcSV;xZ%12x6cC+t`+A{Bx2oLg#7Fb7`E{0Z3?&81NY0g8jCX@Jn#J?bwj`+s3=d8nkYL-K|_x2xoWqquRWt2+4#sa7JZi<_UC0K zVJ2F1qXL4-$(j4SxCX9A#2R$+uqgx(((bzPV7fwz&XBlDgUaN(topDlRSe^aswriS zQA8>!8hvR*n7D~~71!L~gQ!MDM5IL=NRmo{B|2a~V~nxvp|ks-%MwYQvQ9>E9=iA` zmqc0*iH_6OGoiCekg-$Mj@`x;Wi#Pc%p>miejbvB2~iDubbX6rS6)tNKQ#-xI%^Q( zMK|8RyX`pJd^kZdg%U>b2z>B3z(B-Z^jAJaD7=>Wc?o11Fo$fb>o7P)AcItEUZ^5< ztM>X+qOzcT)~Q(ifH1wTjg1aP5o#r?lxlH@+bRV*DG;*k|fmw5#i*_FruTcuQ$WTGLfOuhN z7ks0u6B2Tc$lG)VK+4rmd!8%SZqR4kUz0AhRaKt4$j%S-p})#RJklWs;hm(VFId~| zi1-lI(BM%-N$zbaTeK?N`1Q@o)MM$%tB-ii#eKBJHV~XzW!-OQkEV#I9>|S>4@x z4T{A)+?D)3ycO=>oR_(p4M(Z+CzhKEz7qf4F7u(0=4m`3bx5H3m&2wDF#qzTX-@aPo5o zMn$(hS6B?yfD_$&NH!UATI-edqcx7pXE=21T2jbY2rXZ@vG?`*M+&XU;#VFXVp(4v zGb>1f0H*t?gqNkGsLPnaETe|f4!(TX?9*>&XWC$8gVmO5@kEwdzo`=hvxCR7EmgfO z9EXh}{v~inp3**PV_p7cAM6n3juatf5I!bM`F}SDx*8RG3_2LVEv>Aws38p0gICm5Jg4?s3b$AYnJO+QKsmdZq3p7ALip61a3}$)3n(%0!AKcSuMvO_Vxj)r;w^FVYxm;3upB$|Xki+P z67Zh>#Rwt-ZwK4;;vkZoCg9AIG9X2s{N>9%LbQoeh)Gd%mF+|oagL$_s`$W02Mz?3 z_+%%t=fY;poX9lbGvx6+b4d-JWPTe*}0^ z2Z@+M$XpL=6m4_1h}fn3Ckzg)pZq)%&w+S7-Td!7c$Pf8-53hXl|uz`RT?Q4+gyWR z%{QFTsVZ!@x(OqONR4AFVB)T9X5L6GM^V0O22fo>!KgU~X8C^{3E2LSDdIrjCxHE$ zzE8`sj=+Frgb%gHn54>8>@$$W`MX2#e%W-$mz5pAX4w_1@eu&o$@yq=u+~5=fEp21 zu<=}2C(z zjr!5~>pzQM&)jWKf`9E`J_uykMv}iA%-G=ppuT&;;_pTe>B&f-)4*aZTO;F*Cq~x!J+5 zCN%mwX$4-yw>rPfz#PkZ)5;~}b+|qV%CYUOrCvSJXh?TuZM$-EfsY_Sp0*IY8fff) zL6Y|a@YwzCWl4i*uO2u%|9EW`Ynm39*7n4a_CZ_RX$?okHZBzKr4;AY z=C+pwo@bw$B`W6U>NG8;UxVAf41Yuo1yav*JOij5DsX>7U*~WXX^X4a{fFy#lZ+bR zTgl;>{ora0C{0IOMk3ug>e3alN)-u?M3+^ACj%jx&^@2S<>lW89$vj+kLp|+PADi4 zv&4^pZO~HqzmwviX!rcRg~)WELgn_Ioe@g+z6osg2g0Rkwn|?dAkCk1~|s#EN;zhDlCF;=c3yc>wXHF8x3zyfluNt z&Ra<$0Y#~Opd(aZ0bBRj*v+2>*HwHl*|8Kmo32zb8R5RrkXQDp9^Rj!qt_HS7_Ez- zAup)kt~CK!`{9220{&Z8d6=8?g`bdozM*WM+9@$qQFbEZQv{q@|1D*eU_RT<%g2~* zyPDEP;9@OY{8%G1L_Jh?%7%&`rL}wuakFQa0$gLW3n2 zjw0_4j=>`r?8wb&@vdsR%K`23`ODbwJ-kORm%j}-U3Jv^TKHQLE0b=<@SpmrJO zdz6K~)qViX3ae}MRUk?kfZOvIV)2ghY6D3Y`w1GcG{h;ZkpMCD)_y(-z0>gGyiPh& zIkWHie=27uV|H+C+w#zv;tuk9N9zek#eu$@br+%qcr3Fjyn|O@225s6utZKDKehn+ zDlTi(F!vd}JlC~(VF&pTxWQ6AgU=8bkpR?1H@zgUOcqkyrvVyekPnN^0WMrhN!S~X z!-INnG#1thbGpCi?PR3 z2*&>}IHvXj7Ow^4Ng!TJZd^~cN%#olG+_s1yg>`2_S7}VQ7iujlMNc{Vu9&|sg`_J z-)a(}7=G$ys9EA@BW*(VBRgrXfUc+o(aiNGOT3g78p~c8XY&QaezwqEOn5WZU`E$_ z#u(0nJ7Y|;s0*9mI#03eyT$Pys@<@skUXCkS}?ZQ`;qfnS4qbXK03!LF|g3WE-~?> zrsP+6XS`#chM$A7pWx)2kEMe$83KfB8yD}%jpo^w?D;&w>(~F6cLq1~maYqgZB6O9 zvRxO7%TsL>PdNiF%UesLd2PA1oiXS3g47dOZmSD36qnSD)>mXLlEaoIkwLk#>q+-63JW{l5liWh+xrud^uJ+8)f zsMhMlOD+u10oN3^odkMq^e&rzvRq&YD6CVziZJd6l)6&EXT_&sL_ zY;vkAnW!5#e6eB=klI;f5nyNcZ;vQoQ`3xuoKnqhTv#jusUh>85Ziqfx(G)Dy^zfEXj_5wW@Z12T)#vBLHTUx?3OL0P1M+9O;!nF%sC*^-yShJR$K1p zKTLi5d&mH^xHo@R=gWEPu%v_%0WWO(+^_OXn#7olf$1RdyGSEGkJ*o}ml5tyDG#}{ zg-ixM0S8!5!@Cgnn=n_?bn@mJ917H+5&a)gcOp5c6O>Vf-@R<4SP&n-Ih#Idtmz{v zWGRl}p3%iHUMvOeU?Mk(%e2f(k0%h{9zv9AZxZq}(+ zN)2kp{l&sqo6A){ceVU60QrwH-2Cm)MHTRAF|1yIW?VN`L&IMG^v<6G!-&gNZUH^V z&+OMIpd(;&g4xTi!%e=kylMa7ARvvD9_=nF>iA#2QC|PU&^`lpCn3y#TmVeTqqCyf z^+MUN?Ty68wrWl0zdB6N2gD$z1!cziJ06d{uerBPJpTz9-z{nZbtTfmO^(c zt0QoBm3(4QnX2%pAD_fBt|L=7PCOw)b-t9lSOk+&=Cvrk`WGlln3Dg9zmZ)P8L&e( z&mhMy>H3PMd;My5lux)Z2*h`5w?^XwAm1NI3|gYxOj;Ruln1JGG!mhqj~HlbQxbjB z*I&!tK+$uo02G3%jSY)Zu=(|Y3!|%_*>g#I?7)r4tr4;qpKj3bgMA3!wa3%{V!aG9 zIk`SJIv}2@^>g&o0IQAb7a?n_C>C*s9R)e8P?U$UMPG^f4xOZfXVrurx>Ct39=WsO zD^w~Hb9w{9BXLKv$zMY0Fk1UYG6^7wrl?N9wXe*~&{9Ns1aaex@FTCxl(OvGP<5dl>F&t#TjmChh^ z_{<|B)3W3mPrP~l;gB`Qsl2$g3yKXdggw8ulX2(^7LwrSg!eXq+$?yp>2!Y|#}7L) z6G_;?Z=A|aBqLBTvnfglQ}qgMg6g0s-C5!bBkia3E^nZZ{hiJ_l0qX(bqtyHe7*N<_lw93j`byZ-!8M8*-lN&#Fi<7I z45XFJHKme0wF1VT^;RF4f`s_r(*J?sz_MzkI!aT7A=EAZ8FM-za3j zDAHDpQno2^JFR?s$~le zaT>@KeLGpPDV@C!egk6ZaH0o`VTK`Q`_4p-;{kV52?R(Myx9d{dV{0-Kj>=$N9BP9 z$&%$iTLOJl@KoMdMdjk?SC&@DN69UHBTP)Zv&P~X;yq32{J12cz#5<7vL>T=e_*^6<*mwjlN;(60N-=9a!?+d+VD?vzLst~iabbP-y()8Hp6Br6LI@0yG? zwu2WH1ET7rp+CL>=+%0z7bd;gzo544d_wKO1djVnh@vsVIdkU)`h zqT(O>xRgvhu*26v@t^FV$NxqU(w5cOwa5A$FwE9Qk-4ar9gy}0oK97eQujR?W8nT=AFkoLYS ztHES75V!l;%S*IS1__SN)V_M(<9^TqmkhDgQ_;*L#A1ALkFBp`-i4 zMR>``xxnCrK`CjZ@w7w}C5$A2W#&yj8%~t@nD;uy(5LCs=LTl#NqkHRbP`v4%=rD3 zpD$@!UJZ?`r8iPs!N~w`d{(k5r+oP4FP~BljpBAi0hCEH196|%s_)FN)6_{-?BHh+ z7`+waJwM)bc#f5R*6-gKo;&yRcV;377(w>8v$@}XH{MHDYrP)A7dK5ztt*$Mbaa|n zs6!}i**1)TJF!~_=~ikBFqMlJ|3+^a8EXgNQu8}Q%qmGHZT}8pR*5If9tXl!&)mUs zMTE#sdT}meDo?yDU|8y*xU*E`pS4Ez;Jz8kCnm zrLeQj;!(qp((31FlwP%}5O!K_TVBoO3fCUO#a%sb&Zd?T zs#?t1zt~N`3PCvwOgJ--fZMgL*iA|a-*F~U7~9(&k?mA>Az*TJ#kuu;w71O%Eb)yLB{zWclWBU%OT3c! zC>5Nd)~`E_*ry;v3jz>8%0HDl0vU802)(M~T_($;oymqFouawznUK%wSPgQd2>L>J zbvgy*rl6{7iQ8kuYLE+K`>>&CuP(m&4G5bq$z4n{cY6pHnwhDY0C3KrBqYJC@#NA{ zmOTB_j0Y2=!{+fhNGy{4FF+9oP*J^l&v2F}wO`Me{@?;|X^exGmj2qZTD{@hK=_cA z3;vzwn%r|c2{kQwUiMoiB2|5$SfFA%g$U^eG}y^+-%xds|I+nZ<99c4jup0V{_K@S z=DQ7pE&X!hq^i7Pp`j|#fpU(QYQ@QVl`crFC)qyM*(YWkG4N0#MIX9hD^B@0KOe1Z zbP)Dq=zMHV#*`9&5CVAWIAfrL>5^u$E0- zIBbfiW&axUhgGU;db!g^5*lc}#sL&6NR3NS*O5b3*7MXRl1O?|sekFUg#`LYhF6i} zC|!;s!6$`3EmPL_-}p3-w7_M*%3ecNS>i^FR0Pvh^%Fk|b zT}dBalq8q2YETUioh{2}zc$}NrusQNRr}Twi)uiB%&X1!J|apsEiNK100nR7Q8{U# zOWm%z-2!Y*+&&dl>sm&pHK?E$BE_ly;M2P^h=n?tc}`zU!KCKa=4{r9X|yi1&SY~6 zd}ZQ0dLSqyB&0C)pVrg-r*7Ey8L0D*-ZbmpNW5dx*4ngl(5X>N6bnr+W@KcZ0uR2) zFVb>X3*Dw94|AYCpGd~WxwBgNt}^uE`dO!)-1Pp+L)>_%p*@Qs>0YWrSGDT`C!ruv zi~?9sWQBzEhbm0gjmA1GueKgUi=sfQCjAdDUF2$LdAI5Bh*n(6K=>gfANN<0$?lPQ z0fN9Aa5&}LFhEZF=5_v@g{#?{`=@N0>@(*nH8rGDnp?A3p4@WmI|_rklbht(chxWd z6Y)pg|3uM8!|bj}lT7XUv2Bi={VHC3avJYMGgMO^Oha(sUrUO$bh!G_L66+_{Dsdu zD^A!@hB-DaJ8iyOuoy${LVK==oxg9(Yg5@}KZZOwkm{1hduDHcwbWB*$7boWNCS{`1={6qlpWJ&M zmW$aT&P)6R1@8^yw$${Qfa}{c*1~QQpl@%GACd@lM?J{?MeZ`8P+1@&|K_)s z_maGnLxo`UC=JGvP#PwL2DSXMG3_hMC&7XL&^CIdM!UjXvVEPbR2dgjk{)=uD{nlM zvMF*+_1P(|>m;_$gET!U3{7k$>bAJ(PlU~_J!96W)W;}C>X=lxL?F>MjYuAU(*{-q zv@Up`PaiAV|6?@zk&PH=K#M^OMSHx(-x8$4Qkc;!8>!!s<#c{jRQHP`m+~s)4MQRnC8PgNJ zA1G+W%uInv9JH%XP3^DG8#MU-`Yu?ewSWq+%HX7>85!jxE%&1DNVYgzsDW;h#u~+T z4dk^UhsR=VhWEi9&tV4W5Wr)-@Bo-9$N-|y$;XVumg zDE3s>EB5N<9&t-Hmu5Yk`Uif5DYyM!tx@ysdYe`I8cxic?rU5PX1+{QWvO!K^>#)^ zWhIxy@e4;0k>gzXpj<@;#&ozQcusYa)lP{Ac4)Ej{n^pWd#ejdaFdT?9*$G+IEUlR zuNK`&5@xAOATc@sb&cGpp5J64KpWzHZ`Mt?C@_)e_V%{gQKT10@O}#Va=^`O(U6@! z8#3G2o88-O6`*S%4od1oA9t|ka-U(dPpk_!0m<_uu2W8uY6*Q_@+tqVkrl_pUAiZd zEH(a4{FzoEDBz88I5$YCRxG6-!j9zo$S~*rXAu#j+>#5rvj4@3N-T2fR$gA-B0+?= zcVm{%{vV=zT(SS!*(Zi8nE2jksV9nem$&|TyX2O)~#NuN{%p%cw z7M=IkuU};J=Y*_)rOxLY=Gy}!WGtve3dkyVaQ}GR3Iq}6!h-&tS8ES(AQ7>MIj+v6 z@h{{wUt7w*HJ$&Hvmer}qyy%|-gu+!t*yjRz+Cdfy219*4aaWt0H_V+(J7Z%LKm3{ zwUt6!DK*I?Wm4{{K9L~LCv6T6$;ED+%*;2PwTT6@COE`d%`c*1I%O_03^e1FOx1#D z!&X?@Q<+D}TR*w&Vke3c>gS8|Q)y;-IU|~FsIk_2`|+dy(rGF}mw>Z)zM`!4@mcWyod=cp z=kKE~%Jcm7!>t(H${BckyBF$Hsfa5HLBR|jgBl{1z|2`|~I5U%sn zu6Q^1@V8sh7r2|lD|IEJc}tf0d%i8gpcAU|Dx31|OWUaO)O0jMFL9%9Ze=^Kzy_T( zbkYoOTV9a^4TC2Ez?7f}8QALg#|GWDST%N5kVs$e4q;w9_!rDW8%5#HD|3#E1juaV zFxQlQ(>b_kx50JfH~<9`3B=`gFi8O5VfSW41MNdE!M5`b#i)rjr{_*YJ zIxdCM3)AJ>-4&X)uMpf|`82UA?0uBV{x23uZj~$1)H+@pG($JzR8uV$N^BHt=bE#; zY(H|!#g|r}Fc8*fb%~?sMWX{I`0rklfU?DguvB~O9SG;r?v42K(rHaCie%m|b&Ub* zUM3(66@1=HU#fFRYw`<+O(@{%rlr5GUmIF&4~$IJXK+j@#3e~jMVP>pKW_;r9~~r` zmVzBdK}9wHxH3O@zCMgXkN)_PU$YFg1JRDMi1Qp}FVN@N6{%0sZ9F+5x(uk=-QC^h%7y0-4P{0QxFQ~HDx9AIp_@k>%pw@t`~@r#{yFjl zXohiZ>2BCHq6+WYrqS{uDZ>ebkVumRh#Tj>87gP2Ko;o%{r9Z`xrTr*D5&J$)`7Il z)O%!$7qG5CE5Cybz0)&yPFyah5K+%WSCE%efHRH5A$rus0L+VQ=9Q_ zPQ>&}>2#OcqZ>*U*cttbWN$rvB1YUph_J4Wl7eg;Lv(Lh>VU? zWO%J1@^V1!GET_{1q%(CFSj?>4Cl*)Er-GU#I{?tAzWQok>QZY@;7yTwccSz!%t#9ddP%hP_0>aw%jf$( z8$RS_Mk_&iLeo*veJT74U+3<9Y!>&PQFm2qlm%y3RZ__=r`e2%<=q=Lf2PDN9vo^9 zQ!mE=isb-yadN>x2V!&@E1k^pVIwld6xCF0>aLT=_!(>Op1q2Sw5W`1VcT5#?It(u zG-H({(LYBVts`2}^4_jzMOS`qXy6jT^WiOEXz;IY+LVGR&@>sbn?q0} z$xBwfX2grR%!f1j6|j1I@t;w?yT6?n${1q=|FtI{?RrzMyvw1QhbRCv-!$E<6#KA# z^6_mNQ0x3heCsTb0~}p{{I=d|pLDSqp!u{`m*jQzD0)R|xV0#A0fX4oUT9w2+@Azu z1x(T^l!@J)S;dE>6kpauz;Z#yx}+}2u88BRs^7JRnftHtnlk0BA$`^T{UJ&>$BW%| zo(!2}_$X{?WvnB{cWq&SyRex{e5C;d5Uh>Nx%9-O+JZ+e5T2B;{`W0Bnz+!wz`)~< z>)c4ew6T7&g8IIf$hfCdtPv=vKg{Mplu7Yi+TmpkQdD@*+pW|7B#ymC*9QS>&$X~} z6a^r?1I2;Xv@*Y2Rw%fw=AvuB%O8u#{&blznwS357D)5#0btZMX!X;05+R?>Q-X$@ zPCHnSVcuTd)mqr|4-+|#u%jaOSajeTC5Z1ReqN-oW($3B@e>IAveY9+1h zQNaD5(R`z}o;SMfwHs=BLyGqaWsvC~T+wjpDoR`UngaWV% zaA|G={$nZN=rRyu-R5d>Yx6yo?VCXds-~Hh_RjFdibuV=5H(zaHrgQ+Tt)1p!gj^Q zY~gE8eNPjYW566CLi8e*YyPgDOL~%PV?4GF3R`I2=>(w+@Nol8@&AQ5DN#mVTEfes z%}tgfevp+mh6k1;u8)J=gBMhMiVxLmx1a^Q3w-yV=vMeZ1P?vS%pju&@84GMmfz@-*v%A2pye2lfS7 z3nBc}SpKR9)b9@8`+V#-p4=48j^?=dLI(M&k1~bOGOJWH3z!pnuxvhs?s|2T`Wy4Z zb3Pf=2g(`~nlQi9*eXSSZLcZKn&-fR^0NT#{FAk6L%*P$fNdlJ{U1*~%`5la{pH{IlUv%Y$9L|-f$ zS_3^TNEVKuf79TRG>MEhcd=gTKh}s09*brkTpR8?FSE$IBz_~2G%^&ShZ^R}Rj#S> zaZBQl5Jgkd2YCeGNF=iC+nwdpl7HP`WBceAB@gtFqBRKP@XtxE#F0qKvGmphj*r{? zQGij#CCl5-Sxtu(7Yc+apcg8W4kPf8GUo*lCJ-Uz?ZsnsB<`e+J&-wwLsT|+%lz7W4K0X;QFu#0`RH9L6%b6zQKaSR4boqN%LwQD z*4&OZuRQfr&|lc<^blcPeLp{S1Kb9)=>5eL59DIDhu7rv=uxt~FID|KKe{HI)KB6p z=+;zJNdCuXm7_$6rHBQALgkeSAoh>uaZdFc!^qUhRaFyUP`rIM9Gms65(fHbav$R` zSXB5Fji#uDBgn1PyWp;j-_Rg#(j-%jfG#~VgNeICv9%>aE!ybcJ0t1%+2yomtlFno z3j?k=XD|~&rULm{Y>1-zy>UoS+_nEi(_T%LXv0+TB*x7`uVR&OPlug>(B zf{T_^CssfqkI~Q%h2}z<>J`@$8DYV@`ea)Y!+O}32a6;)z$^L;i@MsFo108?Trlnp zwmiUhU#!o;?f0JF4t6XB>jl61FX0dZ9pn4N=41*Kv&Qc}I_D^a*ND{~g}34Q5pUW-53IJKX7hGC(KDG{Qxv-*R<5Pn3s>%iIBm+D=4vtazZOWLV*?HR7+Yj1Dw?Z761MSX*Y zArcX|&rD)W2~rbxXy4?W_K+R(X-z`-!oVg-52IY``t7?L)JZK_%C~zo^u4(y%LMin zw${&7%iV34t`QM#Vg|^nA>Hn2JWUET^opJy;oZ{=725|#JNlfgnpi*~@~xAf!*6#l z>%AYe5~q3i+Tpd(nuGShq1Uf6%>%N6qJ@F60;OYb*A)H}hMRvdKQ(7lX!N$8nI_bc z%X_>%Fs{?@u2)1XrKpy-dXuehVL`^go`cuR&oQJ3hKrK*{47~cB*cmKu4&0^kvMCg z_$Z*2RQ`Lm_$HWXh+YK#!r2{5^m~VSCK=~}sif9310l?1+6@8Emi+=SAd6{z3%3@M z!j{$%J@G}sWF6kyk(I0&XOtxSg%0SHlU*Pb$`$EvA#tMFGGa(0_fVGOkVu#2hkN zrgvqBCd=>WM5yb66P{0`YcBq5}^3d`JoUB^uyFNGi$L7G_Apn1A)mE z&wz$LI|L-j+}cU9!=XDl8QVR0!*&mMJ|a3V~5_SULkmaoZVkJ)pp*G{f;_d05k9KAV)j{d}-^yF67Y zkTgE6Bo`mE#VJj)t0{}}X6BV4^WLWar$CDJyU8CTsqOD6^p25?!`uPyF$|2Ly_fZM zEJ0xpC;K6dXy2s-v~xC(t$|ZkNVS7>3s%x3p7TqS}b z*}0k%Zz-g8y{(YQ_J|izB|07M#jXbLfFbIG_9P?rHC5gVz+uBrYVQLH7;m(RRJ6pl z_#6bL)d36W_Go46xH8w9Av!RQs86l9Gi5UihlvKMH8k)0DIQi8~6I#Jds(x$_<491M)3$K9wBZYNwjBEPfO%yt)IsX|wA^0gO)2|k)VEf zXmTG70#|;mjmt#T^9lvz1)h_%eMFTp>mF>xKO-7wCzpH z{MwfXI>RlQlR%4~u4w=Ya~I@MbUS$P>gfiEFp7aMrJM7%FmFPuOTBpc8bLc$Zl^+q z@rs?bwVpXKV+Aq8t_RY0;kV`Z4@i5ytDtZXR!LxpH`9+?K5KL8%+7Kg9D>b8yQ5_b znC%Xy1mh)#(vTR}f=ht_X}`*;xsY*kfOLm+ z362=^=)U0`!(YkI8)tqQovh+=t9%?E)c;C(@>jqU~C@6t=GatFbDJJ9pJ z(Cojv@5L+SO@?e$=^vo{01Kx{Emnl;LNJGSsQ&BC`ci9ejJ_Pm#;bzkynag&y8UMo z*JoU-MB_ySeM8Q-7VIMo=+Yy>oE%N={H96$9o`pIG>6X8d`CwIa4!^a_suqyai^wrXBpS3l zk$`rOSKb>T=4L-2$ykv?q}}(+qDvdRg9iDwZf~D0^hs}-mKUutZT!#0PXbov)HH%% zxhu^3%um#*=5Y8VM=VZkscI^eKp$#KY%edP2k%6UsX;a9t01{KeJ2Ij6)GQm3|FpF z6%Fom2aQd0<1avdvZ$@>x7fwp%CTo=BEc7q;(^vpHa=P@8{0WH;c{JiaUf~1XSJ?Y zH|zm?2zc*mxy|4&>3WDf!+rJnC- zoIa2aDd3VHNpr0(E;IICAUKcC4O>=1wHQ}TriMgVWfy!h4I?FP6?6*bgXCSG&Rj74 ze%mS6Yr{SB-eYYw0iRn3E-931&sMPlvZu$HKIcSw)s+NB-CC3?`SZzLzo z75C*1mN@u1RU@!BjrS_@@}2?b%f*XE-+h1lB4u?wMSgrO1<3!TTCm}=Am}G zzP1c$zm}K!Upo-do$S}VWY}}K)}00ok96bMs;Q6>AFqW5X;~{Z)$s1x8X6=UBu*X~Ey4 zG=s9N+44Xq1(e`j>}{tkO<{p7S_DI$adD|RILWOnH&Zy-Ht79xF$IEmPuj5{fa4e( zk;?z|d3-jj+vlMAjkj7e?P)zjzw?0QYPCekuOa!B-1G)c|cxk1cG-TwRM+X zH{Qp|XVIgPZh5A4oVOfzrl)%>m472Tb6la^_Bk=JNze6)tRrw=NIB~2e40iQ-8~^& z*oEK9HOmSI2L}FnfV;^mmcym&japdvd^*)scH}4<-X0vZj8HJi@YT637~sOA=ANSX zX+LJsEEl0jVDG7#haS?^1{-56pI***cC}2*@13V8(v~ExE}}0%9uNpE&?~2~99^ye zw3Ua4ldzCi|6hW1@3Z+W*ZDyj1$QamX6Ir_xpw!=_UTGn1MAZMO55tv-&j0vw3hFI z%jWSno*TuEsS=V}%Dqp@vL3D8lGtk2I*^QCzc`!**58Sz;8joF5o{`g%lZ+H>$W26 z;OACOmwT=Fb5j02)bF&@8MKie9%>lyivdz~!xZ-ER?YWr0A2AXqE-9moaViOyrnE~ z?lCf?4UPfMHqE5yray!T(AU;=?PI(P;RhQ^JZyd~2QEQH8Ytt09VYRR|H^vS9%x#( zCm>%@Jkj+p=Wi5unD(9ICkMiWUs+C{P9d1e*f+HIt-^|MHdTAKnXQWzUjK2YGAJuj z^70wS9IF3?M&TcqP@=SJ9<3~OE^GEgo%MUK>&blyimc{^#WWJ)cxu01y!l&0HJ}AKssQt))SaNuV=xGo1E1bGhqV$$6ew zw<*aCVqNkWgR7(yN}-!?X+GBE5@V#h3pyTPB*k-ncx&3A)TEn2FEN`EbL0+KaOXBh zZ7w)p6G+&|M-YAFd_@8j5CNh-3}OvvS;LxH)g3{^eX;Eiu6y_Yg--lz*X%EpbfBwe z<8(*L4A-i#GJ__N&tvT(swUB&+yJPrJ*9kh9A9KGh|`HRP1p zfb8>5ZhjX?%M!kpSmgE5PWQY*idX>(dbs*;aOYYp*eTcCb7%wk@LNjM?ZXi4nXctB z6{Y8|>gK^ZmTo@*8%vFKL9vrjjts*j|6pa5{T2t`Ys$SCT(s~OhzvuhT|N# ztw5IF5vOkkBdWSMaG(3Vw_=-B?yJ51#K~-QVpGwon2TfTXO~I6C1rX_Q(GK0_&88s zt3o;_;uWzSMQ-?R(&b4(V@XD(X`W;aeJ_+V%npGPl9;=8vlyL2w0uUW4>e zh`@Fw+K+CLv?|`L*@O0jFV^v?m6~&X57e2FnC@frxi6g60$f#!1{%qM59|#o@8_9% z|EDe9Gm13KH;`z94>En{4eAs1&g*sVwKpGdEK_$OiA2Zc3P#t}3ZAR2{|U-@d#TEK z06sEb2t0gdC5@8ojJw`KE!Eu}@hS&yP^Y=>E3@u=xT2AY5B-I0i`6@gl|G8L9d!1d z7?@L3b5N1GJ5V`8YOPcI)hi+UZQkHf)Gq(HfxzF4ggT5)I_I8v8`Eyd-{x}QlpT@0 zpF+_7V8jB*tu(hgO&F?S#_?(t5pAtArKsvI8sj}PD*}9&e^4CzKb%cDvu7bN=bJA? zFR*LwRYv{kvni#`?Go%SHoZ7jD|D)@2)5$l66hMAcKOIqE^c`l?ytf0X7(&qcLljm zcIUZK>oXX`hdM#j*5}*7Jd9wb$$3c45-M^`a4IZ`mK5#QiceSnX z8JJV~MY?W^YhNvn^|2#b#FdC9DR=LYFDnbRMDD9MIqkgrU|%Kvf=4|UW#}k zWrRhrg2RVpnjuO_$X}IQo(cS_GtE}6+2HsG23VhQNUL{>`wn2(Sin!Jv~cfdj4qfV z_Tv`FhUW)k@%OO3t$04A+@Um=L{3||e1T@8SLn5a$F8a0zy18$$9^T+jw6Gdk=S3YbGiux$i&rSy?m=WZZfY{pwc! zra!crh9zY>oi-%TLG$iH>YMzb8CCM zhypHG|3gktP;jEY{klo6p1$^Q5>I2(g3w+gg89|c%Z+`jcG#*}lVRxxl(4O-_n@VP zID4tHKm#js7=>JRovHlAO4E4yl<62zA|S=PacNV%Cv@+|uFpGZeCcJO!?+r2Yc@x} zla6=oGUKl;CB?v^FO=SH`z6_ZgThN(%j3MZb$w>zP=a>#p)-T) zX_d_)d7pN}7fBF`?MNGWd4`m4DQlv zeVqgS9rEt`*=q?RZ&jl2GXdo)8OVvJ48bFVdo)4^J+Lw};gi~9`HQw8XYbY;n%7NY zQM*?1*~9@NrVx_wD6cOuA$3J$wWq=gGOo%3^+G)=Ie8V~=Jjn>GR!R8R3}_VI$+yGbv6kSMSuT`qC~Mij(Q=>lM4L5mKcO& zxr1ds4!;+{|5Q}meiEq;d|6ElEG}}CMSlg2Ntune-9L`(4@D~|)R82?%4!W~3i12u zdlW@{Y_G9*pOq7P(G*nqTw>sCkVCW6ry_m%KI4qe9UlpI+b>%cZMq#p0zww0onJrw zeQl(mXg;`8w@#yzQ=X0jut3p19&$i~J67c<%Ji!yxPh#&t z+zA`Oe}{?nvLL-6_B?}^wXEU;BQy%l^(W?o zUNDd>BLqq-T;H9Zjl&N9^b#|pB{kR*aZBI)L^06Rdq7m=QZY$!=tx_oSM;Be(b3VG zcmA91D^HnyCWeT$CvW!zNmQl7k6+=dwy#8G?GTO~nY#;N0; z#xX9JFnAkZ!7W~a$F6X$$1Nei&E@*&cu5?M(R`@c7Ra@|Ma;;o1g$)qNC_WlG}NQ= z&Tm)ADTD{NK8uW0|0(KmLVC$Hdp(*ge{~!qDS~Mpvd~pLhk-NmqQ?ePdi6L&t$ab( zU;bgCK}$`tU7d!2Y2NH|>;~PV(_?xvJH?0MT1B568v153)T(x$l#e|w03frx&;bfv zM*%AQ?jrIqm1n^mHCmT!oCJIu_CPu$e*av$@JOm27OT3z)Z4&Qo{5xxv!?jGDy8dM$b3 z)~H#9ZV6@?>`DxQ3$)w-hWT#}@vLrYeUoxidt|GNZFRicls(fC)EZ#*4d=4N$^7ClRI4W-DOq1z+kL=rpg_Z-1INQUwu*mswrqtRQ7XxaoN1W z1TVFiiB+{Mb5CX~)~hi=Oy2VOt)^$|RrpjD%q#+@y4{bFn?FPp_r} zNhkGh&{78E)PpZItPXMcH@_nbr(`FhVKU}8{FZ7oybWDykNyS;@74GmT9 z$+!xvWwdEfpv_fC#}HnUEFwS4c56W|_t82|c8_K|$wsENanvoi@)G9|yG_!YO?UL2 zb-KrByf-?hidU0j-zF1iDrHDAU^u4VUp$f37;rdIXz_qqnf0#G-|YQ{d*|5<*L)xB z-DRFyzOUhUVFlWffoBE`%SM2_S+``HMDvVaY?1XmKKd45F3vHl6i4qQJJ!~Q@w@MJ z%ixK~W1jY?&Zko37P&;-x@5wg)Mw~tJmC*hnidTB>jbn=5QE=6npw`K>9pP+m& z{q}A!521bK8NZ9@^^C*eMf)lWL~GRD>@raf?!;*VNg*30DsA2{wF^9%kIx2tOe8NR zrcaT}Z|X>%mM+W5$N*Q>zUMuA;M&v2HC59YJXefej?8NIMQ!B6Dz(LjDbJr?k$y_C zbqvJYFc#EKhpjc_d>oAEqf=2`ABs~?{0!Vv_-sk(M{GuIU0z+d_kMeF?75v!gd6(H zIg~pzKQ8AgAx3QXBR}Z7MxlVz&>U#1>bshV_XYC}#)br^F({QVG6Z#g>*c#_?FYm_r_o7HUBN<;urN;jH!SGrH>Sz)vY*6~{{+6t< z1D(Pqf1pjqYS6%X;_esq^TyVZzPkP>TZQI93toOAgS5TWQ>lO<6*98y*x1cu7pB;c z^I2SNkizYsIxWyFHUYEgUUPVLEl8X~_eg#f+L7!nRlCdryM(b^WkT|)g4RdEpqM#V z0w%kkdDz+~h>{s7IKuovBe=S{y5{!HoPA;`i|rV*K=gHHv$Tm8740V{Li`_W4sKsn zi%x6oT)i&}isH_&xuYiYTjS@+iow@ADS9R%2F@#Q4Lv7EO24=ioPrqkS?VlJO~1v2 ztgNm=>T4{`N))d93*`-ebeY=2+lIWQ_LLhoBPFl6__#`3T%T3hL5&CPBY(-g9mPWF zCuxqF*Gi6FZEBN#L9%b}{TmnTjX>b{>&bHV=XVX$nFfft9CY*nOn|5Le9sp1p&+;{etV zhiO#S;bwtYyS}prH-agiF31e;Z^peT!^?6<_r}Cc>c|9b!yaxq1odDSvVg-)cu>d` z&&raS*_A!!C;zeUwgmP8p8#!A0N;#w;T_8dM6J-gm*YCOqsZw@|g6DL8hDAl>5oRzxnM>{=R%vx-m zge_o21>Nl!GR_5aW&?Zfek!(PJ zbIE#n+qnNEBR&Vtr$uPN_G!BO8tdDqGhlMNyGMtGTCmy5r88PQv8HLA*Wv5tD6={T zWzWf00V$M)UNRF0!By7Vlj)zwm@2t*-wbAQlUEVwa@#_WkA|PXn9(8XC#e8h)2cw+ zr)^~Z1;qw8Qod0XT*~Hrr#E|r+xA-P-m_qvKetnWzzwrCHC6W30SHe*!bdABu6KK2 zfajFY__(j`eK@k&8NmWSPYGf#9tpXM5Vdlw?Nz)iyqAY&M7xylsS8_AAdB>IHu=L= zobBPKF}MClx&fC*f8U(jxy;LT)M!NZBVd>^{j671za7GX2ShEST2OGP zWGO1WbgyL&OvF(o4seUw!&&%FT~Pb`0yZTI01h!_ zD2P63gSe00EAGFj&JfmiUP=AvY^UNqxuFmD;P^G>zRu6exbH;Ioy+PPHo8GYxrj{q zF5CJJBznS#D4{GKnOD5?*PXkup;GsETU5w>3LJ&16;k_jUF$9(@P}I~rw#G@8G)*t zAHYoZVrR-HUyl%+owp+#RRsK5gtB?HVAJ0X64v?ZE!`QGHXA~Hq)z9wZGY?G1S zy>W&47Zr3lS~(m%0?^i@*JQ6YYPVkm!Ja+Py?HUW#_*q`ftjnJERK3S{bL!<)qN1L za6{ji#1QGoBZr2d0NqCggZthZ5J>3GUSI|88yPMiq?F9E<9MGyLzDwihmSoRQCirj zNcXcW0ipmvhT`{tS`aDVX(OGWY?mubJx0z#hX2w$g3IOD%kc554|M5YpMjtcKC6x_ zZ*B<;4S?WsI2zh@VVH5->FH3#pon`r_(z{n866&r3=)6WR{i#67c_9Jg;t-PhcNf{@6k&r-DH{ zxH-5TMkR;|bGPZm{mFq~HfoMB6uQ}I1*tN% zN;tmRuRfl&jXjwJQvlE50+OKjwZd5ov<@Xf0Y1la@iHZ9NOuwK^a1HZ>Nbtw9^B9* zwsYT+M)0E)y=f1Ucpr@C9B!U;tW67N@qWyrJPSCbXcdvE`>M*m$D;k+U_kSY;x z0sO}J10lDSpk6vTD#7{MeR|_2O#3ETKWDz%uudU4E!Fq-y94I~7OXtIMMaLe;{HEV zQx}y^HJjfuD2pW1(jHU2|Vt5IZ*{n znsSRuI&t3q0&)E5>TJ0_%U|aZkPuik6ioI|Rr$Q6Q^Ohc3s}3@=jHOu&eSI-$<52# z+1g_pouVxuZT6iqDa6gfm)v#s2V{ez&OHf((K*ojBb{fqT)j3G_%8>zW0fwBX^~1DY&1@ z6YAN+i?bP&lFxl9R>)t&I1E?@=$iLi^}(;B6ORxs##;CD)bj649Th=d|A%E%xM3Yj zy4=4%0@o({qSw+A-=<&S6{4L>6r8WGUO9YS>VmW77)4SYJ}bl09d<71Qh5aVg2Wip z>ZT7u?`K)o9VklzK)y{!k~?8FmWUru&{)Wi7;G+`UEIZ}EVJqzaZ4+Z5JAmxBO>qv~&cD+#B2#Y$ju#Zfh51u) zUz~2~ZB{J~fI@X}G#14|BS<|sy>THgOwGvp>j06lhEbE$18nR`SU|>6q0plx_b+T> zJs40Sk3A+Jm&vVeX8hGkc5qNTy>=%rIE~Lj1Je|96&yBO@9yT-fgT|`CU%q0{2|Hb ziY~#pAlD0C+FxRh*dTcYCRxD$!se2I6pEG%t1v)G1`i-+PxtID0PZvg4pY)r#y!fe zt>f(L^N*0f`YvG{K7a*^K|Ye8%Y4$y zXZhz(6#1PVQ_Kh_;h^~{x?=Mh!7@=AQHexbAO_2=Z8X^kpvDaJ!RmlmU6q-@t%q}w zhAOJZrFvilMZL_YxLI*t{hMxapXeUTM%~lzDs1W7MXXDI3h>y{bS~nq=NAXhfhUK* zY4cMsrJNJ*NM5dI7LVs#;i%iYgJEV%!6Y5~)J{P>P4a{v3+pk)TImu^V_`;d4 ziZas=W$OH0l8)GEQk}2G^&$ULGQ3*vcXL9(zpQOz}OOi8|V~v&{pw>Y&0T$W!RrI-lkSzjo>F zh%`S1A$^#zMTA;W=6Z2bl9;%tH#pkqu2LZDkaf0F+@*aMr~L9eb2igcz6YY4zXQoo zeMP0jk!dn&*^xm{ylU=FxE=l|^12V+iAos;3FxCN^72_?q7UJ~`ZXzDM`Wfl)4m-g zs|J1f4?=t!5D%bw-M_YO8R)dTkdq`n>T2?u zm$Gqz!XyF;qDNwV21bl2EPmspTpQM6>~>XSYISaqvZB@J`50{=aw2=O;Wfu}O>2>_{KjIPaaFN%!YV&bI(6N04 z4Kp3FBmol=utTo~GnJ^bb709eMV;GaV%7~$oq0r4fLV%E5e$x>cx9H6k>S>v4R8m` z{Gb3%72P_ylaQUmW{Y{dhKwKcs4$PhpW;s6AmYzMd)BzT9OP9QH}FlbOc>hfJcK%5L&L{y+`#xuh*9*psZe5v87*>LQ^x0oyo z&jgOgco3^^Dy5EQCL*&AFHz%dtT)R&BvCv?K1ENTu7U>7waChV%Gmp!oe& zjxK$!S|K*F*sKqcs&;%gM`I_yC{((93X`i-BO@bo-)^swUXr6DNZx~NK%488teyZ{ zwaoM+_~=NkhB1ImK`US!4buH-WhXH}Co#)=HsGRCq0WvjmRE+Sw}NiKQZbV6Gq zc{Z2&YJG31lWZgL-Ns2!f8mYJU>~l|mtby>)cY}A<9_LseEzwP!M5DD?`tPnAZU$_rG3%HAmeY=lFcJ?1HYrN}u|C_x_;cxXxh(641|lqy)#R7VxH+`#2d=_nQ)&U3gpVkE#-g3E_5cOl$EB+ zySON&E<{G12_rmV@~%)NmjC)0aAAYi4YkHHXyG>{xLbH{2fL}G2$jc>a?xa=UsS?f zzcg)59=ml}$xDX{Lnp_#uZ;t&04ACj&+DQiHkeaUu$}Nkq+pac=7iwfN;?Xk3CO$Q zadQw(ifU)v8$2-{AE}3YkxcPP=#>a$cvw2y}>G_`A zh>S|NJqDzhdlL)rJq6(RL(wgUjx;lkB_T+k)}0Nxe81w$2IES z$MG%Qx6kG4(Evgno|NhH&=<^sb~HC}CdO|wrc|V8I@f#z3J3cJ07);)lCkW?a1eiP zxOqI5SUP{*_=SD$$aeOovSFwVM zhf@6@A8jr}$}}5uw}Vsm3;lh~Cbvjh_sU13*Al-H6ONSieWSe1l%)j zS<^8&Yhzc_g_Hr_18);@cec0m@k7RRe=9cT6v~F(yV|JKdp&J5dif|BD= zQ2n}Q!lDEuJZOAcsTspKDk|(jaqg=(UPJLNH9fa)hdyC)ih<#|^WfBTf9bqyWmK-z zmUhFd+mM@O;X9WpTGJOX<=}{Q@_Fzy!y;b+rG4q~OGJY^DdEDVFhK9bwgePDhr@hH zR;@?Wa{c^D^5RJ!5}WKR4XZUSWW1i@U3x1-Z-(dWqTVqdB4FcEni5z1{l6K+;@UJN z)T@_L%KDV~n66l~HShd|_UpePOEbOi6m+*)tFS)E@0$B0p4g4F#`63^kc%Xj>rqDG z)%slWA+eq-I9lb=hbH=p(H0(5&Zf)6-NTOJ>gvjgfuCUEnD?_4lz92>+$U+oc_LA1 z5$&}go(gntqUgUGmH}eRiuIoT(bwOV5jGVtF>ESuugZkcW|eCym~Q>R_GM3e_^#-Z znDu#UDW!WSI9U3;a;p0@f^R|MSI->ieDoG#B3=b3`igS_{KwKBe0j<=g)e%oXzWk* zFE_$^^_%C}<2I~6zlzz>8)>;=deEQHeUB1Q%reOi@&qz24OKaNzcBsTuO@P~X}(@$ z$~%a~>u&ptMRRfz8xOGuj761rZ@%Z`B=^MFP(h$d}M#=$AWK^ z*R!HS${G)LvFkFR=nJ_k({q31+@|x)mW&|WXA-Qv=}}o!T36_?x_wT6wSZ|Z4Rck9 z7iE&DUb@3m*ERlCJ(+j5I=Np=ujefbbqJTJ2nzU*uVXc5y>ADJUAvgCAZw^u+hn-Qt?dqY0<1pLiZ9EY?c`0+uk_Lpcy2AxUjDkw_*OWg( zy!9u8JEe6q7+^pHK^stA=;?vqA0fBdMxifN6b1V1rhN(XC?e{>7?elH+eZK$G)p(off&S#cNzm0F|&w1I`$acPaHF)%P;;?B@U@v?fh zx`#&{ts<=ckOg+lf`sa$;xtBRk$xyY|HB^;y?qZP?bBxBR&B0D#Y=Mym%!d1p>v4p zGs&r-o;5Z|d9`a!l3n80f#?4Imd2>dCUz1(X66#t%78l%T8|5b|H&y?r6Fd8SR&ME zag6Ig-E(ORyPX4%u9qOSvvm9iA0?MX;ReNUV&hRLiWuf_*$mZ%xZ<9_tueCL7>0U* zCFzmnhbM}#eY>kASJ+iGx)}(DF!c3QEhsICY#_3#T>M^H{c*$Mrzhf~dtf7g015Ky z*($1!VrlL97#@G=Oub!bZ`gaPOu6<2Ih>cA*$7^CJnM{PF;~5TBLbvn^8glpk+{6! zka}bUOir8XiBNqH+4y5F=hU)1%>{7&G*T_QMX~JYpfF?1=F8vtN}s)mh_g7N_gU*0t|#BH9b zr22i5X z*0vHg2Za)KUD!6ei1+%Qpx6=d%i9|dNCx(oO*DYBcABG+6Zg-gS7>4nuG`Q2$|swS zW&rf5W*;?A6(bmVu&VWZA7wyXITw{|)r#Gp+01iHkdD4x>0(<@-w;@4IHxkf!?p_7 z?PPb3av-bj46W(~m#tC0DaB$*<^PQ4LJ=&guTK7^X$j*qFcfNq=7rFF5K0LRJ-Sah z6^MHaB5VP1pGZf`B#&|$1+I@yCj{<=0rvUOkgWVrufW=5!MwC3-U)phD z>FMI+qD8hQFEG)x0J=ymB_4}gXp>*%afVf+3dXykijh`f(4%8=hQ6Ajpl)2W!q7`8LzF}tb{cfGK)nCAL}^4aCON|wC%Vpr>3vVQ!> zw*8xOGA@lS3XGY<=5qc3*vUo8oU)?b{x#=HiDncFTz+A5@h|bRACI4ZRIz&eLr;Kz zWtGwm&&znDCf6lf;-d4YK@g!~n_mzFv{kzl{_;`i7Inq@>El_gbLByjy3zRneU-!t9jyP=5&)5XvoLh(in#`gP>k0Jtk-$5z51a01USa$u z^lcQPp`zwho)*6*u7mb@bKPs2Mk4RG+8qoQA}8&Ng2sU6j$Fv{!b~zN3qKjn!(TBZ zH?wGNOo%d!0ZhI2E3yB-~c@t6URZ>o} zmu$(&60T&VV|i?Ujy{fGFo$3LR?x#4?(qDkdD7n^Q^B2?FPuS~r{+$p3-4Zb!@GIW zH{CLdmRFCYu=cW!KDA_3e@@RO3-2j80=BV4!4IynGbuCtvs=guv4;w+srj`0f#!cL z`azizE+oocF`kJFNq-pGpDrc8UGM1}A7K7z&E+Edun}h@i(%5rcT2lx<32W+BjSQM z4;0g5*LI`x6+G!)IgNwc^NjEtzswJL@yUc9pIgzl}+Qvqfe)h%6TM z3V1FoJS673*{{c$TG@?Yk4A#LLB;?g#z=Y*fC?D>IjenB@!p_@3JTP<|D+4<#N^&R zZ~Z5e8Ysj#M6Jd6RFcY$y}1NutAlbDHor^8dvoqwRJ#qsd5{$atz|>MyUE^WgoA+< zO@UF-S^c0aW9KQU2akKRPCs&j#`xcuJ{PUqTZ}WOmrX0}l+IP%{PF7@A$K1%9lq^R zWdaZF&L9`A+Rg_JOWbH#!aQmb0H#H9ur1@P!Zexq$9Kp*$IIqT@)gzOuc>Yx*YKyg zq%#oL!;yYKu!rf}klP!$ilmR8LWQ1z^&j(gdz zYXkrI<5zc%6?IjQL*c7EKA99&zKLIlEcDT;ZDUM1C&wG2;e7{oGQedG|4`39LwDr> zpIcWsOoI*QZQ`Gu5MMyJ*mRVNS4bahpu`e&=ObmFE{RH1#3`Laj5*{ZY(xCiCe^Yp zEEs|q2^7t+4R3&Ey9#$~#*2X~X+%x~pzWosX})05`u!Vousk>d7pnYYGY-3aPGvvS zyZj=n(fD9BOf+Mi+hO}n*OHxvHw@_=gvDwBVyCb4IDv~IBWv(+crCXO@JY5D9rX^%sxo8?-FxoB)`sU= zK3h`y0kXp)B+Es`WMq}ugD(7}&r-5o-Xp+FdXgIsZzTiJaMSbz_Jokk*J|G$ab8vj z1aRsHJ&+ILeI_q+nC)-1ijaGuwAK9TDD-@MGk=im`3I!prjs+Np4jw^R}=EM;6;9w zjLO%mUw)32VnVB4W%vL2Wa25);Bobk{|L$;RJ&SX$-1I*h2r=1Ovu;SkgugYNKN)L zf`RmGKWMsIm=mAOp;2iL&>d=+CTsk&uA7pGh?U~~ED>$}-k#rqJOgi86^K?uW2=W{ zY49VQS{EgdH{%@p@f$ekb>!^xn-|_ra7&+F_XzmWJ{P`)bSuHJG-)TYSY-ZtY6HlwC3Chz7x!(!@+>?zqEx2ln3Q{Btk@k zf23SJfSJZzQv|FNq!87D;5Fr$Fsitp_7qSs_YB?}nfTfejhmQFHMFx$^~0L9 z#fG#6+q5mJi9{qR&r&7@?OAQPb<%IJ-^5qFT&zlyYB2pd}n`a z@tRVlIEZ#tXRE}P%$|*raI5gnq$D%qo95}>`qv)k@0)cz*&@F z^s&d2oty->7lf!^{6ZgO#mi^^7%n~Ks=@!RcL$&CsHD+aZqHjX|o4_`Ho=i1O&%(xT&sIs}`vcPVm2h@m^01Yp?z1z1^ zw|!Rl#ukF`>3b;%-2%#(y8^#mfM!&8yo-gcM$do%KCSPsGQ_}BMO|MJ!eo?c}(1EYvUJi81D-Q8pHJAmr4yj3rLYW5csR4awP9M0oOk!`M^V{FN_?I2lvjV z;kj?-Ko6C3>KUPaZT*cin?v8mI;cvu>lLKt<$^=`Syi5Ir;t_ zef)2tuCuF5DV^Vb`-A0`9{|s86w#3R=;GR4@?lvd7$|+agw^>z2p6P22e#*;%*Cl{ zJ)pInXP7kCf%RHrRap^$n#UrwtcreH=LZmw5OW6oo`{Kd?r+pN4yvv7m@$x#uWIjG7lE=HuI zQ|?t2y2tlu5!R67FL#`OvzW0O9&|jH;b*Ia@lTFvt};NiH07Zk~N0<}F`(o%jyzhg?1obZkhKOl>oZ*5J=ORidSePz%(E6L~vE5By zdPtcd@i{J)ZML_a**!1j1c~_$R{EyaGSyK)?!w;E(}IyY;0#E3uVDNl4E5LNmqM<7 zucW@+rtf8s70hj^-qmefKmvVclCg_Fn*8l(ka3c%qgf0H_JoVO5$D{KN~asB%BJ0 zg1@#$Nj(hSHVe8LIu=2&cjr=wpupfX!LI>Z$cv5?(z`{v$^MShiPby6tpSUgUg%%H z^P?8zFJCcuJPxks^Xw|;VH*-?C>@W5j_bgzY+r0n6r3XREH)?_!TvLLcT5K#>EEnV z-+9K|H+L~QlVLsEv0!W|v#3e%uqOpv_=mv56TIHs*-^{GEl_ePJhYfq~`|qMOZi%a)5XQp<~c3=+EU6e|bxUG^$+ z3(q9g;&e>2BYek@@VMMm8wmyw)9DjOA zHR=p2P_Y&0AR?xj@I_T=^8DY<46V|FtsU)7tupo zKLgW-a&`wVur20!N%Z}PtI|dsfdS0OnIv{Bz_x29lK7W;lPc`nLAUB9g(oG@1z9*6 z)wQ=Ffg{p`l5Gr}-?h9EDCQJX)Goq@*E1@bTs7;po-F+MG_?6aq(j#7lqr&A+e(!q+^{9v$Zt&G>%z^Ds6tQ3jO3nn`r7ty`=~uxarJ zeWoGG@|JEAij!6+gbfCoQnH#U=REAeg=@RgufUbo5oiC1!5n9{bQGS4N@y!2k?za4 zHes5}9oua61t!H>30OfMb^J@rZMGE$_FhGi9%{Q_pqn1XMY1tmQZ!UJn2%jXLwL4p z4pE&&;**t@+%?6_-sUG)&UEi2sD5P6CWe7QZ@k@bH-9t!aVf5}g;(*}y!OTynyuvP zw4=FiHJ5r<1imk+E*DQs9U{Sh>lZ;ur(PQ$j~lhQWxvqV30{5Jc!J{XhQbxt&UJ=J}FETGl$5?`?qink_9Oq;*DWN4xXA?9c{>@sZ+p3gB45)YxgWS z8VRpzd}A#StM3lUSJreZaz9C(&2xMJZFMVbT%`aS5y1Fucge_2TzvmLES+u%s2z874gqU-KoN{g9dx=A3h)?{X`@+Oc*QC&t2W;x8- zGBU*xozGEvl~arW%!*xaYLyee0NYdC_O~ z3q3TdMcg9+q%t5l4U{;jhdQqjUE1jV!xA?{?H-7W*2=C1!C%*j#3^E-O&l(1!TjVE z$t*6j%Um$zqu4@79EpUQB#+5$-~nweN+beotuD1)vxeDP>k_R)7Cb+`@~{DYRx)L5Y&Nk3L;p-lAwQt;%){bP}7z&B5RaW4=G2H`)oSN+1gBFackv+d` z#^~Ek|7*;AU$l8rKi(90F4F{ek@;99LE-NJ*}ToMci-z%{nIi{cH*Fx>N}p8T?vzi zegTf>&Xw*T$>>So3!RZ>hdoAr(0u+qeUJCw?K`p_>NCETIXc>=ai1~V1OE=h160^J zoD>~XZ2cQMxHt3kJU_+gswLbrM=}Xl`(ywo(X6h!N2Kv$nYGo%#@rI6Uwue>?PmBe zcLu6gSCf-$U6kYq9A_O-xfh_=6MZ$`uG;Kctd-y5RIAr)HtQL_Ysz}~{Hcn!hDLz7 zhOOj1|B{4#sPmeQmXkMGfgUne|6W!RQ66=GWo~(>^35Rb)vafgkP(^_cU$FeCMOW0 z&WgrsBfE@I7Y_xU9t4&9o$*sSe;CN)E&XVl{T_iabvW1@FUBYU{S1doFL{8TvL)k6 z?|$0M_kE-uvJV^HQ{}$?*GIp+1H8;m&%iy`ad(yjDnKWmT1D6cxxa12uR!1 z{#%K#l--mHuVq$;RL7?BX>+{1JD)nLtcR>|9mA+7Y-9$Zep7v$t|&nQDq+pP>H}?7 zcJ_09YJQ5p4Tv&_9!POy2c93ku}LqeY;U4;nT`&7Pgq&b}UY)-_3ciVmxOhX& z`z^@AdU0*=@%Np5smH!s&v_B9A|El(N8NM_yLeJuIn%eX$hZ|X!P^TMl3wc{Hrl*DLW(Yc3 zYWq!z@p2p>JOZE*!ahuR43?}mXLQs^oq3u+Wg8MU)WeYp zP##&Kl6B^BMILxVzca3V&#^Pd{hI@Y%JU?QyadyKiBa$(tUn@e|dSqIrCf{`%_+o%*R6t z#X`?L)#E+}4a!)!@mc}*<-28Hf%oL;;Z-$_wZv~mZrQb6jL(dWnm>gIEu+A`8?lzR z-kffI(L@ghqbS2dSYukLE7^St>N=?rXY9(?V_;O%B9JNbtd|?KuCRUmCD$6;y+X|< z)~ScNy~({zy~XQGf(T=pkd$>-gV0?nlZ{)yWAimQt2~E5iTd-4pG1V)(h(6Z!b341 zFnmJgRnl6A2aH7BVg;7dYbEgm1@v4BD3UEGnoHlpU&t7C^YM*w=>BEy!Ym3`Z3y`Y zDzxF7w(L})?8F{b3YdBtXT?fsjLRl^l-ziJo@ViqN-pQLAvl@0q&(};YVZ3fzbjPD z$iR3HT&Mw5!rKuqLHl=O4-G;d>*lWbtrz2b(6|*b->0DDXP<&xP4;KqyO&Zb%p}}w zSk!SB3z%HVL&yi-#lWBXrr|w+ia8iJPJy#uJ%nlF%T+X*)4pN-ke3Vu93D2lH&0W0 zU2J2h_5&`|h(9YE`;_!1+&cHRniKyuQ(A0+5zCkCz6vB(pVo;-GR?*N{+*%<+_Qod z&*%d&FETEJxA_`R2~V=s$Ba0$KJtv80_Esmk2BLXYB5(ItE2#Rmro6@Q>?b|YmB)~ zNe$vGAST5WYJjAgA)@Hi8DrM_=$(#v0tVw0 zBI(>K{d3V~kC)eZr3*7@j2iQ@dut)1oGB^94?4ZM1+yyt^$*F~EQUO?R{%YV$}N=y z#frv8+{g7c{oa=2Ew2@M@SZ?$n+yHW5|E@QT(K;Zm3HHi^D;MAEpVu4$`G?C`eIjq zHcyAUWOE7X^d)i_GaVniUS-W&tmk6)Uf~AzDqLgvz~g@*45YvJu=R~xB2Pr$DExo5UPS(uo)k=Gky|gFJ4nY2fM}ILeg7jRu#a!$%Qa6%W z+gmoV%=4kkg_qFMX1#NB3*TKA&_lQ^-NrPn0c@(QIhB&X^rf4lEJAOVXwXa~;^8Wp z5+)NcA|;@7frv24B~3`Qf)z+E2z%lJR!`}-;L%ZxVBXlZpZ;1GWa3JG0*M+WlzjI z7kcGF0EE8vZHA9JAH{gwS(7ue-LeaR@_Gpn*2q`_G8Fi=LaC9t)*Vn(fT01G4sU69 zySBem&7bizuCST5)A;0k?dsM_`O9c`ks28Lzxuh5;AjImst+KeOi2FR5=$fFAtU&U z+KoFO9ZEm+O!Yl{wK@(b}v>(NPi!?|c>zxUQBq=`m*Gt2C&b>BHES)LZANalk zxn}^h6g(l_5lc)R{V~41>gSQ8&Hcv;9Zf=%(&W-JuUWKRIl)HG*UXMND0o&6;E1 zO;I>2h-IuYAmvKV_w7yQ4QAlJ9rXb>nsV6s0DBoeg1I@Q?2PB_l{Z^0D^o8hhmulh zd5)v+@!W0c;zBs66m#!$`#pXCA@00wmX}ZLw1sLqSWJ&AKe#t_>g*dk#65;{{gtX- z&=FBak2yIB-~HzBqMHRn^#wkoFUscwUjIQ?jo$L>f`!$4^l8CDX%V&U~*=V23%&;1E9rQZ$-xCDwmIFvBBI?UX z^$|nkWqat4tGU<2Q`*3Wm^d&NoTipwjnFJrjl)=X$kSW-Iuo^}c9AcL z00+}^GE@un^>~j9J!y}P@6bw*E>yKmV95c9uIRZG?fIAEm>;^T#o&K&%jYqTuCpXO zLz{b*X&U_}G)(S(-l7w0kn&Yvas z?<(e-=(Vromrw5l=R#*+LO&8=Y3wQ0^|jn)l>Hqw1|GJllFfW_jg#X+{N#iaV#2+; zo5X#*!R3>qbBtL|^JfNQ{8!6Pg1258*uC}^?-JRT*^%jH7_laG-KfnvjW1fh-?A1Zr;%QXL>j075*zhi(aNff6DgY?^AoIqpe`b~YZ^7t_?T}7SEy|Zgs z0xDTwKcmq2Y;X>6C@mg6*F7Mpy4}~W0Hs)*YCDgqrRphxZwh_uFsSl7YqdJbr{TBP z2k7;?mC(L2J#j}P2P>-(u(tz_mbx;@M`6+`U^9p{-;Nk9z-G7`hx8qAo(!w;%P}S) zsvaA-azJ<71*`1)0?XrQYjn&`p;~zc$it+yXpRJo|JYbGGzBcNuV~r#w^?-RtK;kU z{T@TZRN1ID7g*q+-a*<}wSxOuW2gnp#OIv}LDa|)I)CrN6M76XpMA~+7((9w_ZY(x z02Az+5Lj5a;KJ0Z#`!fFVr4a3wqVd*#Q$_(3pY>#?f7Fm)6HhN0iPIf3B;xZkk=lr zDXSN218zCmZu!9bvJLDg#eDIk>cxGC?UWZ+u6ydn+#Gr0e{LzP^2U|p{6?8lZ|^Ri z9^*Mf32p*khU)sO32%dZoOd)N*ptaOhFfWM#hH38IqEE0_=;yX)wXM;u{Pe&w_W0r z!q*Y^aLGo5iQ}Y2<9K)?b2M#C>p$Ku1HqA2(y=?2g*Lw3AVyX5Rd*%UT)0`C8A_nLOncD=mI)i@&S6wb{BUc3qD<@WKOnd} z8?lo~XZKgj9!H1{C{T_fJ;rQ{m2w~<%zY$fS>=x41{ehJujQ9eRP(J44ebL@u|7cp zn0Xu%67HKC{P^Iu1uU`g(KU;;tOvii84w+Laub=82(xL-bpfTnh`44wuslQ0(kTn7 z3IA77<<^Zn^!=g^PKuwO1-OFA&d&;fHS~&90ZOEmaC@r!<@Enfd+B-kDZTc-#j6tH zc(zpQ_v!XWSsR@df5Cu@mfU$c)ok%sxvOhxJg~gU7FzfNfuq7eI&XU$zL>Wo3hFwk zUsf9_4de3!5+98V-z=B%wNHTEN`S23Dfqv9X5Yqdi2+hj{B=MAlDGO(00DTcODc?V z*26qwWl87M2G{M1#=F#^fA5eZA$WAekr%u3<54G4U53J`t=>~;;zcpf5T0syP|*Hz zi;;?hUjM15Hc&+J5v$60(c&>%#z1z|t#`$XjsE4&AyY%oFdxJgrREnO^d}gnQtn%D z!?&c8{Z-u2rcz-hGjduB2u0DHfe#9SWh_BwuwmHA(u>RCeW5yK6eediBHNmR4`6CN z7Z(6)>BErF>oEjFGCsGxWap+PtfO<>4~g}DEl)%!$H6+>pm7r}F7blp5Jx{ym8gRv zwXMAN+WzRea8J~izk)62V^8LS1-*VV??L?_a4EvN3O^A?Mb`Hs;LZkZnKf3F-SOAy z2XhJB8N4?I*8R>a8aL7BM1})97qB|{PVS}8z!?ky&hS7YXOlf<8zM+>U>mP03c4V+ z^ov$)KyA&re%e2@{wF6jT^+4U=1ubBwr48^9LNd+(JRD<1qt@+M;vt@??GlOD2q2x z<$a>dPDu$7XCwD+nPqY-ej>aWeM9$edY!>f{O~j*EnN2NzgAV+QQ*SJ{+P9X&~z37 zO6EcP*J8J6WwciE31oisFVz&7aiYtBF9ji;-yx=s^w(sw*njT%?a0%(M+<2Z>}bwx z9UGfZU%oyhRpHnN;P*oet?G+;UqEfUW~qnhOVA&J5->zK7Yi3gM;}22Rs?G0)t5h& z0sE$Sk{V1$YsF_R_&TB)Khz8HxY_=8KYrjAZ)sIn0Iu{=ECPajl1(77*-;GdW&ENjdC>CHdl2w*@r@h7Nu?vC2(qhV?{h0(M= zkZ366H!cVBStqtR7G*3H`u8T>m>)Z&K!VNjYd9XYz@c^yq`Gl@uC*IP*RHpl?!x$2 zQ0meB{L&TgCs?2F;Qe82+>|@Y0p2cuR7B%P7BX*61wE)T@EnEG%Cv!L&;EV|6Byja zc?5T{UdQ5vJw2p@gON2Z0EW(~E@PWRL~5DzyA>oH{|d-9qRr5I?t5`3>hWP!y8C1{ zRjaZm4vdzT*32XjCIW}*v;0Tqz20%OdC+g&=0>hc3y(3;yUVeWzxj>nr3R3+P_aj& z&q7&p_t+L*{gXnZG4JK`^vk;lJZh@H{?=f*`5&c;)~YCKkpPrrwb_X>44qO~CX#Tlvs2?Yf6TwYa?~q9SiIJ)(6Pe|+NA5r2x%U-1C}Flv4J zf_r_Y?qs?bWQ_HcNBa0Wrj!8hbDcC2XzxC)m9ILs2>(wcq8|JMD$m#B+ljGLYg=(R zjsGU(WCQ#o3s6I%*~xx&7ZUaNcPld18=1Up1&v1H3PVmPUE45(?Ug|_J2gm18(k-#=GbviNA&AIHxGn|8##ytZw%9NCK;G zWwq)Dw}BUE-vKwBoJW>O{uH`ZIRATvs`?OLJVN>qpQ_ZNDokX|Wg~;!Z%4yBPA3Bs^_tZQE{(tRzUHw%T=;_c6SC=9~Aj-{bR!S7YBx^Iu!v?hMlI2C#I(f!_? z7YQkU82*w^ft!+&lNT9iH*|LkZx&coZFRpxNk5T|h>fid&%-o}!79}5T^aur*6?nh z-|%i5#U0?ZE35I8&>}RT=;_1c$%n9V4dC>c+1?@TKX+DI>v6E!_@~Ts^vq8S6CqYl zW^#Z2?+Zl^bmy{ggr7s4KnB2O05!V7d*G{fp<8+Qn-Hqt`4K#|fVd1XgJ&zlJ6RUE zyMfB(ng?3mU@O{BzQia2OjP%W95W>cgGAE_?|Y5%3TA6Yu~%JlnY{5K_`RTGx0mPxz%{p-|~WzpqNh zx@h>FH~2+@2?^IT98_=8YF0Gcg_+ZHb1Mvg6RcOy2)mB2%O{K0=M0`1Eac)7#m{DATWlnu52)!i9Bb*{;rOPv{I!-| zfS0$EMNi3PGuq#0H9RG5`C1xk_2VUQ^+m3q1$yxd)Bwn_)jnAa>ODgP8_1kp&my=Q@4N!(wl1o7szR>lLSC1iEGYu0U!j3 z+W7j>zh@2ymg%l!wNXVtQC}?)tnq?ir-CM;zpUGfA>{+s>7_KtJ6yVcv!#?6F{b@y zm_9$#L_58iY#5*Txqr~VJ8+Zw)~);nTY1mMg`%5^LiSj`*|zM++R%ftG5=gPQ(*Mc=Sz&E0$kp-Y?Fi-i$WUykYxZnc-3?(Fh)#mBadEASP_%D@0| z<%1@CZZrKi*?G#`B|l;yF#82e-sObv@$^`(8}L1Z3<|VC*14#gO>E%cfluRV)a#5l z9GCri-E2kye-Wa2ii{OyWky?2p(=+$pYLE9@rt61PTGv4tC42XC|2AP`F30}f+&2+ zh#-+l^@Qo7AM7+W^TT$FSHFf#nghYd9yq(=wKp^G2a*sb-?)*=mPoxLmO#Fye_^Jb zT;q~O2yt1?RuS{uvu~QvmSc?B1#Yp!A$w6t%AKUR*?uLW?e)m}FL&>G5(Dji38iol zY(-Rj=|XrY?k5Dq$HtmSyd~E3!$7cFYa_DaU4Jz`7ScjVKnv-Hm;6jKgq~Cj5)<>{ zT*dXKHM{h(W%&g`>%lB>Q(gtyH0guN&dU?D!;kguwdAljokJqnX zl{}=!%cZ3D&EY2-3c+yFD`(F=5L@&-GThGgiyWh-m<7X$ry9y4a(L^pNwx0{#RZ8d zl{|(8Jy%FOIO1df${tZ)yRoH8+ah0Q3{nR^bID|1I^|hz7LU|V#SocyvnunG+;{fX z5Wd3l?Ifif@0Qy_v^FJ;`HbR^$%3W(hClrHJ*KDlg#xgeIk|$j%{Te>+fsD-tD#U(PWjDsNimZS({6rpS`x1UvOE7P+b&Gu zi!VmPE1ZTNP@DK;x15-Z?TJ^@4K=_?$V3*>&~@kfDyO}fPJFTR%&Ou3gGT8l$FR&| z(Rv|+Y0C)WsIT!k7V`-n^TJgr??lUsE@Za&9=plt=_0&K9)>I1Pp_=3EG`;be>wsq zJB-*kfF1E|7Ada@MrEY%Du$VKftNOACzBk@36&XQdW_2^oOD4QwzXgQid^0+3)5L# z7=S?ZJDZi|V(gx(>Wa(;)-u6C0`W(>+0SU(@^Y1Wr%Krg&nA6``c#FcHe~Rp&fo1k z&-T_6TG$EM-h@W{{&S^;?0L|ubJqH2f!pTTK0L*M4@#a{5DGnKPv}j`LY<+d1W( z?MW4n$FfvX^70s~Ryb!0PI8}}Dk%JD*qoR;qPO54gB;O&P0h^k+=@FY3T_T3IuEYh zcwjf*e6`=pQKJziCqZ%Q=1^~|1a3P3{fXFwU0kP{1^>L)eaXVa&T6UEMhkmdPZTwF zSXh%f9JA7mRXiT|(%?<#SPXuZ>2y?+j}h0n65nw+R45ti(_#c2tx-1?GXw3O^lOmh zvv;$6^ZJBxOe0Coc~wr=hi4f#iCJ(|3g5~fwIZ+bUE!7VXbU}%&RFajig=&tXZipxy6lT8(^@s1Q$}*vF zJ11HPSZ2hRRieF3M&{MYP~6sM4Z3M}v&OBbx|=n}qx*jh4ws6#kmr$QIM?V z_rcU&4%<_y>7qfAzEXfFGvmX}|I&pKh0iN&L9VBAkf#ZH~>Pp+4-SUSf|t z#U5=Sw)A77GUFl;+AfU{gxHDOTF@GL-nKB3t4icc!C67b7rta6TdLoe>@H$iD89SqU*?{Noa;D2c$4V?`G1(dxy z$l0Of{fk}Mt*`fM^+0-gs>)hAcYVRJMe>{BtqreqpMPiK`I=!jg~9MCp8XfWXj37o zj5N64ez1pO02>E~0TBN$C~kCxKiO_KzT)#YYJJVy00Ysl)&JVEGZG&i@!5``Yq*#? zv2NVx0epFVV}mij0+KTZZYaEpr!f+cW;EX;v}mE|Na5S~1z_ z?#-8RV>(cfXi+=_9^{TIE9$%w{_*gYr`e2^L(8F6Ybf-NG_Bx*Gcp|{QIex3SHZ(u zv<3A&+rDyD2bzZuk3I+PA%@UoQFnv)CbbCV@iu#d|;RmRM5ZeFD+LfX}$jr&n#i)25b|B8q2VPkZB-`%+I_ z2YPQ>e`0n@?48!L-2R8UO244!0xq#QUNVP~*MYAX73jsXrSCs25)6V%t?!$def$(rBJexA{dO1M%n-M2`K`6*<~X_$2>NniiH)M4NZ|4KCXaO^k5Tk$JRN%67p`8VA74D{vfC@w#~4deMw7vUZE$00N=f7^tI4$Q%zto`CTzi3?_cK5?M zyJPx>23VNShEj|3Y>}<_eq;GA6Z>XA)^#ZWN$}=++FzEtZY!L8xnM^rGEebG_+-LY zF>Nu#?nhZjbN22vOkjU(tT8Bv-D;)z{oIxyIV_nyDqehXII#T*oU>V|8=WaZ*riaL z1>D=VFdm_4<23aE=hF}LNsv(53(`3z}iNWG+k{| zW~CJ5-ax}{eVgSgov}S~x{qxZzvXVtjYLCv#j5twZ;z9KimC;Z>@OC7FMmAIzb2uh z9?Y%q{f*0dv60Tg`CLZ!JwC~nmc2Y29{h7_D>W!btspb8SIC4a$t<&hq6S&EiMQYP&eq*w6E^bZhEJHzLUpMG?sM8G^t=c;jA6 z-&;&;Cg?^-Z_6Ut94EB>ZN%0(-sXvL!hkrB0(qvsE99Qn7)C|#$Do`Mg|>DFff*4V z3Oqdn7l~c4N~)j(xdUynyFPc4cKPmVFcjVSIS#A%Kz@<^*&uBS7E#9l;CvkIoA&)efk0jUb@NQXt3 z_>g$!Gif2@@*ZGp7ck5dp>bMy?ioBpYt5OQVao-q$P~X{-9k@cknCte(07n2%+RlM zszSh9(|P$_y)GA&y(n5X|Ft`hdtAO2+B2 ztVgSywtWq#;35BlI{Yjnj_^Tz#W*1So$bwm;H?W!2i}nd|wSOp02T{5y5{zC>v??|5iVmwzifwTk&Gd@a6Pg{HHEaY~i`6zvK!HzVmE` z(MN1BD-t%C(^hE^$fS%ZJdJ=S5mS+IKfBCTNYG{-hv{wldU<-iCq?#$b2;Pm1Nj7m(Y8PE^k| z9nkn*HU3X~9MVFE=gfhO46x` zx1YN#*XqnXWZbuyt)RyHWa>s#+Jm(%@^A;gw$d4Pw5I-T-Q3yqSlYLnI=|89qFS}i zbvqe#69Q)wU!P>KD>)FE=5m?CzCydrDI?g zf4<8Y>)w$ZgHyfk3j$kihLqn0n1?cQd#hkB7t44LjyW)$Eq7ne8 zm}8B6_wfPv9c~t?y(^@aD+boT{_-mF6GdsNg=i2o*=sDYw$O*Dw7`XWP=v4gb9wWI zVr1ULU?G1^&rGK}H}Hyop1iY)E{V=+G7R$&EZG{2=*{>lE5LT4c;~+MD!NJ8X-6Lh zo|zAcvPDldd>E_pdwdZJkor@5E4M|DS-NIMGUo2cnKXQ0pIZtnxS?Azmb;LX| zW$Xy%NOf=nUy#+L9D%n5$&md8cUwGlD5dTIx1}E)_JvUjV39v~TomBZ8mwi2Xs&sX zZ4VpAh{I%?6<GK@Rd{@Y%m=Pa|EulNo@7d35d0E;q!#G#x? zeBCA>={S&i&j(KLCOOfp-=!upcxiWStokvO`_5V7iNn(#M$rI|`oEeaH8)l+O#CC7 zO3Tz_Jn-U0h%7zjPI(o}U@B&6A|Mp6P)>aorW9aK9JB9GgYkDYauZ1|_BRr=)FpZF zMKbEmkD?t8&01=_FJ`}h=(7L|`g5mnXmGg(xg-58UyC5{4RkBuYAQ$TMAWkas=9Fv z?y8_+Zte&wu1G3Ii_#G3?4Ca#i?XRFsIyWO%Z?<=#qG2AY#shqyIkK|#~0u!+Lq91 z+gz~*->b}{j_{!!?h8^ePxLwsZ#0pzpMliowg_CIS^(BPYpxKrcUfhq4W5TT_Hu zbWs#t=N``Z+k(;IVZnn5a&3Km>WOofc9AO$x@@26bM#GU?KgVhMb5NT>X{_UMAPm_ z!2r?CD2G#bVBcA=k0|qEU>BIhb25&3xiODi)x0`+hm_SCbaTALFy0d?Tk52ivX`z`MWyq?ZHn!icmxfk zb8%0xod31A^7BWL-F+MGR`WWe@7I0t%D*w)|7(jJmtmN?>8UCXr*!JB_o}dTK`-m} z8-cBllGTVnxk``?qwq=6YDSA8wLlg!;PF>}=@W5@{prp{A@XG&#Po|-!=kl`=`X;Y zI($-;6`VO$gAv;xTBfVU*B;BSPg+m3ntdg6=alZ;zx9QED1?o+CaKekv(QnmC66$B zLWgWiLa6vgUTRui)Maz*H^g;~c(L%q24knd1YAs|Oexs(W?p(2#aSqkq`a!0>*YCr z0x=3Y_Z^xGx2m#=AFQhJ6#dEfiO(G$srnB!>WxzvC{<@@oOR<+EKGL$PN6G8yXp;F zP|$N``vl@LkO)zvl*y3ns7RG{m3EaT+o+XUu@rL|TQG>|w}VLBuf*?Jok`a<_r5Xx zqi<3Wq%#!A!6s5a;bNaQCzRP&ND&KUc5>IPDyu8g&?Z*sARVHOh@EwsWL1zOjmAkb z$yH!w#eW=LZYC$d7SGVk(aZtUah-K4sN@rF_^M2^YH5wIa`9@=F1UMUMNb{sVLY_9 z$IYvl!;s?Ff$WOYlrRtD!xa?~MAZE-b1S}+$PpAk@RM*)N`z2mjKw<6bGh#a#gZG9 zjoI>t-2_`5HiZCe#i(Ae0 zUxR`KXzO)-oloo>P1;ZOV%RAfzpwQR6(DUPrxNxjmkJ&SirvgjN5fy+7fWT_7|&^| zgV?Q%)$~YbWMpp9KOhvGADJX%jP<$9}WTKGV0TLf(pUhjNE(&SAHCyXh># z>%{hCXBq(i2VsQ7d^k-tb%J;Xb#j%vE*IL4N~yC8Oqs4a(Bu!nq-CHVY7-kQ+C+ri zgi&dvh5l;7b>OehEZAv{wHE7_waJ(#%)b#l5`Q$PP2)M>c^)i zpUviLtyX2ZldQJav0fI%%X>6H??a6pm9)zXpXOTmQi|BvQWqld>KuQpbV(QM#>dOT zr+i$B{Wulq+0-}^tciyVKRK2GxC2F57#`l$iqj(51Nz(EO#)}$2z7&jl@^xKe*g6K z*N69xj_n|QgH7(ZUMpqUE zC=^XSJUzde2)+lvJyD_HIN_~rfr3YFBagpcRYX4B#c~#5T{BgujSq_6-guVw2EEv= z9x9agLOXO!SPM*`M^#i?&4}6DXKrzi$4tog;7G>k=40#}DSe6Eh8L4Pt=BEiCe5))vQ>hlFln_hgZsO z5x{_X$~P>DjP)cyA=3Q;`iYL1ZjN8jH-X_wXQ{*hSEcqUSAb)p7|w{oK_~G-556qD zdO)IhX33sD{wyAQXL#izTHdVBByEeVpb9&Jk&HAed6scxEg#>hB))|{;Phe zSHV-rG9;RCJ#?+#XOwt+wI|TxuD{`B!rb&kyXu(DgBHe?(-87OtsrbuKD&%53tJLu zZil!OND{deG}-$;5&(ycYhzL0r&0Vn%%9C~sPC9Y_?VyW{;;1+gitr#>95d5-fR@&Gp{mL9kQ>KM4)Q&iT<30NXDlej**R zI8;3B>ssm3wgha#-RHFvCm?URkHnKUW2bswlGETH+skAkcS3?}6|$enZ_ls}o-j}< zrwYE0y<~l?#LgBwHYP%-(m`6LqgG{j|4k@88EIEjRiU1vHzT@2==@FFr`?dB_^Zt< zv`)Vp<~46BfS7Am!1nnVa8=_A6qCrFrksLGk1LiD>Jr><1 z7D_P25wd6HsFlRl^bUUoi9C}pU$^1kL$I_lGJ*nXN z`+oD6&9auZ@Tj!kQaT+|;o~Avqx9xrlRrDlG}}4q?7jjg2z%Y@p#@@S-4J=c={3>` zCcbz=tK!D7lvrJ%U7arGoNjmjM~n(G-EomP7(^Gfwq=8#&KK8{^D+q@ z9`2c|#+ndFwZ|R&k;y4XznGApW=74i$hoiZPP=^3{oUj3X^s{kj6?1pPF~-l3tn#yH>F3>(fUM4KEBGu};b}L>1w^a-xk@8;vBgjk za{VN@Ek8Zwa}hT3|2d^+_EO?01b6yH5u9g?+^}Tz^`VIdp0SsWUy#3-mWW-KF!a-{ z&u-CJg7m3{a)9fm@1kYydT03c-$ygN(}^u_K;_&5Mqw-bAZXVFhTp^CQ?mIcCke(X z&9hcETn$Ugx!1_Xh%$eL)jBQ?k57TQHOI}m5 ztxwSP+18;IpGeWfWq?}MymCY0Q-7n(@n3({5PS-@mrP#NpS29c>i{@L6`zm*SEJEw zaD}rSjp{5BSJ6WZE{->>Y0E25uW+MyqyB7IugJw%dw85JCjf~u5Zm

!plsER_WS1P?J41NfhQ@d&Zw_i!X@H(Ij_91ie`J|qU&2p0H zu5`|KznvFIuJ)tYQ`nD3Nhv`erXXfi}no25IVMqO~@vW~b5a2vA}@$dmp2 z6LpBbOG69SsL$ekg+=_JpKg-jT8MAjG3U9l-VY_pe+!_hu-QQjtPNb8sTGyBNh8sQtxeL`x752CZ2GqA=WB zMMRUvcSN`gyhf=AT3AJKzTi*3tPonPiu(`ApFPOyjD~BXZkq`!GfpM^W%=sgMXOzE zQ@a;Cg-FOh=+^-@Dfl^@tFO6wtEJDEi#*Wx!@3y+2Pwp#EE(w^iE^g9zu&q+%Hb(3 z?b&6B)?@$3x}p>UfGFowSmS-M&^>G-%E)@&B@-=yjNB!}0vO|BJ zsWi>t+XuRSacoy_)_!WXzuUV?lgw~PQo*JK6{2gB^;T>BAk3|C z$7dROMfMkHkJ!+^W()utw6Rjd=C(ZWx;{*0Mr7-xfi_IlZC($6XAJ$vvULL$gLHQ) zwIoY`wf?}3sa%Hg8l1Ru5yLM<*?+#`B#F0KNkRo6ZynQj3g;9~A7TJa-3C2qVL(!H z@2%f1hm;t34nAD$>Db4}Prx1k(!~RfpSvFo*Z{-Y1jHri))SY7d$979&)mzdyM ztB02*Fl%)??f9V7$?$w2yR{I;c_^kNUB>i}TjU&9Jcp`QYEPm2*ZQG|jcH!EI2-WV7tkg4tL*-*WEejsyZkMQC@!6Qu`o{-HknX3s zHNL)KPd!tG0>$ZYoC_wtd==!{qg|-(vKlSkS;KQg(7TK5H%Y=dS%I8a?$`-6uTX{O zTzu8`#U?FRZneyHI8uK&+pMZVLN~-W`6iAe_rq$j+sAF+#RpNc_7z&_?THYcSvED# zze6fHC%k-8u!2WPeamGvx|YUgS_Ihst~*KpMN~~d{v#O&Aez^C-ovPThSqqzEbi+Db z7GzrODz56$E3#gj>^G4a3J}_56R4}m4DU+0eEE~BvaZs-_8sn**>oexZ%)s+UIivp z<)$e>+$d-)tpsO-3=kVvc)O+sdM&<7W5s3_A#=}Q$%;DK3Dd_sww2)X?%Jcdjf$K7 z6DWDM?NAcX800GZ9(vV%5vksuJE6$O4MR;oc zMN*5$-Yly*6;ezNiPwxEiXp9HF?HH~*Tb(&=;t4RJc-N6ex$F!csw{b=qHf43r+BI zzHSFJPrChK1IoW2s>8=p*k4mcDxdjX+ib5d>9wl_(Pq8XmXg^^&gZFMGFEuRZlr8Z zL+wgbFYf)JQS zouOGM$S+a0WhHc^)-s3+@^F9jNXk62)XL{;5dDX=|J4F4>S&m%$V@$NmQAH}B>B9q zfM_tsLi5*ulGG3hz?V4;hG!L@kE`~&qEE--{tnES<#1*y!!Z6heYc-|8M^jtk>zk4 z$dp8piMca*&LNdHG!DOS_@}HklPy5Q0rnDu-ip|1$mov}=0~LuF*aN0k?=c=#EvBA zJO78?blk4BoQ`-aP!o<>lQPqrtLAoDwvelp2g%E)FyX%bvXU3}J%jM5S+=B|F;fAU zX9EDY=84YQ<~l-daj7!w^@Q{ON?zsKTkY^aJ1mM=i!2W%osO#POPU#JGe*wSg@&&f zkgIKIe8gZQSu$H+<$bV{0RK zYl*Yq*KG)&7Q)-YT7OchsE=A0_W?8i63#`9^owYT&V(w>b^RJUSfG5AJ8XR_qMUf( zk0wNW`;AMqrEfgksXy&ephu+x9Ag=8B!PUBoR`5BilO&x6!{Opp*?s8y8{Brd(zu| zM#|K%uu;7VPSo{^B@RgQG5>!~^wynnR<8hyg62rVNr|+j$i5RT%xvcnI~0-7{cL7t z(mKU^zMROW(Z~aW-f5uS4MOQZTC2Ne862;w4XArC$GQUDf}Yl=Ho{BXKQ%}xdc5XE zgHjy9p5Aln&XKnYjkaI`e6Zapeo(jrjB4?rB$qthn_ba{--!%i;{OJB-}r432_R31 z($jxQBD65vf4}n&{BwYhDhImTLvIn8LzPUk|J9+9BraH3X>1`@Jp_P;a~l?^aDo} z;f)sf#;NNa^J)CB$xE8LO0FP1jT$@l6ox}!8zA2WVy6F|ELEe**kPe$Oo0Qa3lF&> zmk1PYk&yYHxPD;l!HN8p|E&NTr{urCkw+vk>BgP-H~&&Am0L$69;xZ$a~B*LYN@z(uV)g;*e6-Xx+Bn(?T&Y^1lep5EQ7$#B2NXV4M@Uu z0m}2Bh23YpC`|Ku%diXmYs;MA)Cttr;J^W=Yq>Ma09$Rm)UG_oUc?dqxuQ%gK^Ta)>5!h~a7+aV=k1Gbl7Ln+@x3!H95B$NC3BVC zNE`%^!vKu!)yo24?&&vwH_cs4Kf+OF(eRPz|D)=?!{Pp(_u(Z$A`&f1NRZWAi0D0n zgy`(*y;~)Etg;eGbRv51ZP$`uiJs^sh~9!A+UmWo{VmD+^L?K4hl}fSo!2RIX70IX z=A4DyEv;DGNc_EV2DF+TYzm+8$5~&+tD7-!+;c%4v1pnlb)d8;c5bZ6_2fD56D4_! z;SRUem5w0W`9Z8WiZtvIaS50x$&m>`@H2h?HG3674_|tEIR`+kRz&JU(-hWt%HY3B z5pd}x{8extmS%Z_e^5j@n2XYX$7SN{%SDs@)5BkOv^zvzCW*F!z6IVTCitgICdqUU zbfq&zUSj4_2-0-qz9250PT6QDiI#hY*V=FWJ1}Z~UYjD|;e1M1!*_oc25MSC%_w z?`6l6mw-t!hs*y1@NC^7q-zErSX7g~s?|L+qO!8%qft*zN&z$CQ|90)ue;ocp&by<%gR*|W zMt%IBS3^{hj!yk>4G<{Zxu}NInfR_$pUY_WGhFR1yI84drF`@Qk5YyTgZht@2LL|H zTyDi>9zkyQQ@+b{75HYM)T`q&wXkmBDkA7>R-I>fLLA<|MKzNRDCtcjkd?S}{XXbl z_8Z7>sip*rKXtA%86*UHU9L;%%3m@jMrU@91771tZ^?7MO8HO1_|>|f_}Xw;5&)Z& zp8e*XkZeQbXm_IK;9w(41I&*G2EXa#QPoSbnQ?)2N^TbF+VvRt@wm~r<|B|)3K8Gmze#+F*;cvGAa!tA58@RTJh`?FH+}s|2^>SNw$U- z=kwx}QD5-krGow@c(a^~d?3H#)YjI!$0g#m#rvDjjNU)n7v|~?(KA>c&VVVsU^pfT zBA+_@bop^JenoVt7fze=VU_phwy+5ozY6yUb1r(CPIrm&v8%A-?OBKz%?k58{N`OP zuF_##?r&wj!Uo}es`v6*1|ppwE_+cygdN`#%i}fEJKwG7jmCbT9nFM&mx(7QKbbE( zo**)9tT>gyHW@aJNt%ajzE`y7V#C=V*8OJLT;lz%Yq;x^FiyxI)7!`vWow?b-z8l3 z`q%p4iW9ZVPT|9qk01^C2cxaGbuO3lTee5B7=}IkmN>aD)K2)3Y5nexp$=IYT$#bm znO%%ff2vFJB=_lsAomZ zlawdKjAs($b$D1CsmV37JC>AWaUyJP%02T?L|1hGotHasx$;8f`F+B5rB`0B$}aIc zfJD)A>!8=jTAmL?yrwK4{$hOR7Arz0CSI;f(*8x5>z;#7<{%oAYiw^3hjRi*?x%!5 zfXazv+TBC?mLzKjUaKQ- zojMFmlBQM_GFoww%pSfb$E(Y>&8%>!L1D&@4Gi23#zxorLDpiLHq)@x25OS!i4pfY z{nNx81>G7Q!#hvf9~PQl3TueMlyk3hTxai#yv47@)lbzEaXFSt~5bfEo>uL=4WJ5$Pe971$iHY!)8;Qujbwl<5i@82HeGFE$AYG=xYF`9cKn? zsMwzw2|dhFMg{ObW51K99KaVGur~qolff)LSF1-I7o!(yOfM86iyCVUzMze@Qj;6e ze~vE+y1+M`iwVv+=Ot*zd+gcMp`Y1=cS(wynktOf?(vewg>;h3xVRo}L#jJ&?&bX0 zW@_Au^)v6OshtaXF@oO8BmeOJD|{=g%|cviMaU7-h{Yv;+jzcEz6?1`Y^8w6L%XY& zE|f04nX3ZFp+CFIojebWWhj9D#t6ful(_2LV973k4ckPCdf> zoJ%*A{Rim)A4+iP@ltVLv@1TXZVyDRP(C20 z4NkQqFCVamS%Dfhfn3_;p467ATT?sDswtE&FZH#j*-0{ZcUNg83|AU*yiap7E#u0s z99>jaI1_(Nc_dk8^k^z3gpxQkWW)Ep?@_F6-*`ekikcNTZ)aRq7xSW3OIf~qQJIZ8 z9E7mV)wJ%7=bqbz))BVdf(|B*bYd zYGZgEZLk=V>o|7wwucSb_GLYoUfqTQXDvE)FT7Ad1L3joFLpl$!IE^|Jhs9aMjuZG z4`ZLNaaUO?wYar*7uiYDO*_VQUyN2^6mB5FEP1%str1bi@PD>XLI;#Yyri4`%DMh6-{yxLm_$Wwq8#bD?nvW#OixYeLjB>z;l_$1nXOxM9 z4g)e@Kp&gjgnG?&q%`-vK?0`z%WvD2$3jV*{=_l+_;vo*UC~`TArDwTV{vTak7oYG zzZ8e)CB-p)LfUkZ|Ci$^f)zuL+PuqX+hRTmURmx}S~QYteiIZi?-2KSffSnxwjjBD z_bwy7w2w4Za)(RM$Dbnj^3@dN$jNU8x8!di(Xm3-t8m>tlr4A1fpOW~bD+85lrrbb z;`WOEVS1|P6XqKt@c8bCLTbJjs=`}~3&{~--ROw)ki4oE*PBZKg^VBh@~tDz<1_Z& zZm2rG~2Ye9@G3pAFk znM`X&A-zT$BocDth$pgzd|0Lii@vHIIx+L$Lo-C-bk`$>4e;>OBoEHW5Ey5;msUxn9y zbMQdr;oe~j%vgJ%7BqeKizwOsOqwz1DX_FTu>N>6oSwI!d%Y@@L6{))t5|{IbF8Lkp7wFwhOQFi zcY;}Ylv|Ia#=KE08y`YK?8sTG%7Y#~p506T$c#e`ELL^zELi`tqjFpEZg>t%GXgJFSSigIdEfUm zISjhYzN+FdH~f~k*sRVcx1gJf6PZXuO=Ck@F<~_d_UgQ&@QJ*`OOnc`5=C)c$spXW z!57waCr(Sku=p)jLWNg|TRF7TE1xd>x##wE;FMO(WMAaA~f1)zS#wKnuS?K7P|>B2n#*k@()w$@oM#!i4Dc|_{Q3ni%;tZ6c(VvV4R($6QFRA9tc;`fQ)f zj1i}!R7xtYba)A4AbQtHK3_C3btrmq=nDkfRmOv%Xz2DoqBLTwGt++{n}&gqzkpW%j3%9w7_H`3c#4!CXin} z`9X6I%FcM);6xN_q5vw<5Qr0nHED6Mif}nE8ZDi}pnDR9B~Ot3Mo~#eO06t7ADpa> zl2j)5Z#*^&E8O}+f9aPqWsYg;nZ)ANwh@+SKWp zs;Jm;vvhY^sf?x8SJvlxofStsSqz0e@X(V;nCUHJNmi+?O^e90aDK6`#tt^;#twA* zrpDjq7BtihMcIA%+_>tq2|Gk!PiQPeey~r)hcKKg`PyF*+7MiTx8S4xw-+3j)zgz# zZ=oi!DU6kPtm4?VhEy7pM|Xv(%Xn~AZgU#S=SfuM!WkuH@nF=U=SXJH4@lkP*rb^0 zlg4YQ`Qo;qx+VoqT_wHNcqYq9cP;wqNxZC7_N|kGr`b<@MrzFa6 zc=1S$_|f_)=MVS2!=>hrJtC+2ahrhxN)R_GC6shFHE>!Fb{FM8z73?v4Aosrjf5_o z|EhWSpn{iud}~uklHA8A;M{et#W&yAWLO&Y%Gn>Wo1EW&(VjosC%We=BgS_7-$7>H z&{fBtZuy@ap`DHUyIa`XoTxmV&gR-1>fTEj#BD*ylfw(rHF(4*RaxeN<*Cn;gM5@Z zS-FyKRr0)yBgQXgK>sRdeY%UHx6mK$Ze z?t`!87Xzp4o>QvrPZGicpgSitOVTcqM{?-9gZqs&MYn8Gm}ckHVCxf=BJ_;l#oUR$Wem8WcI^>)<>`%<#90)@(` z{d7DLKrK&*h5Z0l#`oxuS;=cT7;`4U4io^ocy4W`JhT*uHUo{t76-ocm~cXb5Bs+QT?{f&PS2vI7xfs1O%y-%T?vz@SByTWkKO&!ouM z0?`P}aYNQv9h0Sjsp+h5)se^6PYhkHh>t(i`nG7-w z)s09~44w3G#ip!>F8}2%LYJ*pT1H~e>Rb5vL9Z#+x9Ko5v#-JzvL)FY6>zbh8+6i0 zoTiO;_eQgZ&K9Y+5$HjtG+lJZ9v7}Mil(z$1wh-??PKsm;ZHM1cNwv%he0MEuT40p zV#ylnRb`$R!}WdHr)8{98V7e%zzf>Q3b-D8GCY_wU&Ko)h3(E=dHEk`Ne z-Tixqrwb2OGK~$^6{=|$xc&|>hkp6x&J^sdx5<*cZQwQA(5m03CHF#fE+gnR^o!;+ z8tHUfMB4vY6RffoSyh`00F(<{`>3mLkhcdUcqJ?YJ&;dWs~1UZ81G{Z&MMl4+5kUS zRUQ86T43*&4*0w`diY9PJ|jPv_?@N>)#AeHGeAF^dL>6k?!|Wx8QAg8w?ntYkrtC& z48~s#Q11|d2_6`|9^zS-9c-#6Ja&n*_7O5Ye`$dD$7XyC!y4d|~E3Vb@g zIBkfHZD?pw7Zw(#=_s&wV16Q&0d86X(y7%f-QZxWC;O?jJ!yg`J&y=bMVJcQ zb@T$fzy8MEfV!=|(+ETj5@t|%M2^7Qrkm%gC9G#<|GO~Ibi!D7hM>c-ru~hDebFb2 zU5;9cV7w;$I9hr|Db4rjhJh-)!)O^0$Wk7FyB=cZxo~+W-mN%9yeiEeNDW?FJ{mvk zcQHUy9K9c|hesAdoMk{{ix{h_`Eufj)z4U_ybl;(caimk1zczP{DxfH>K=gn<_}rI zP;yIiZX`AH&ve&57=^rtlsIIYk{s6tXoiYb2<2c$-(_C(?C)F*$#o0x9UW$bg}oW) z4}*T$@`rt;>e$)Y-&tL?zpIct(8;bUT+Z$DvpIPX#}8JPc=-s)%QMX{;Fg#C@pk_` z9Ud*@oZSsGfn-@L0hu&g_Nm;B4J%6GqOce%QIckU#F9@2BLN{{`*a9bnv4IM#5CKf zpL6Rh_kb$D%wApU-(8A7i}6Y{h(^&<5Pl(ca%>dMEWxdaQhGp(msx0rt|0SZkR7k0iyWEC9e6Du_8_vuoHnE2EYSJ3 zFzCQ6;JOg}5&B?vLjLW~6zS{jw4$;f*TWkNcNf-9%sduxuJYezj3|-o_oAN~_3(Eo zr-|?#swBHDZyxk*@@GGMDr~I!6K()$1^qf8&;m=7m6V3iW6yhHFiq+>za4J z7iwD=l=Y{9y3mjTG&{+o^i`j^9(DY?^}kW?%*WMFD^?vJH?vHabvycgtzo!>G0<(# z5{!pmV`|qkf7th+waztX@*VE$R|kdgbvbt5LtGPtzym;#re(@H1gH5s{*WI7tL%WV z9FA}cNjgK?H>S7I3ZCxy{T|yd?Vsx=Ue}|irw4(kt!r*{++okG<(j&;#qv}=3rO+q z&5#&?+*JF+Q!oFM_T1;Bb*lSt<5^Sof2%DurrYeT!l)Qj-;MXqwFDhV{8NPRs6?nE zRvnMyqir&N?swV!6Z0Wuo#E*dXd1m4t5OL(wkywA<1_5E*!`qoAPP(cAc(4_c&wNW zE&)hMGi*DXR3CHrEKP;6` zY2|4~xTLD%Rw?jhHe8aOc-KDn4T)lQ|kP}rYv$;usd1$`|Tuk5Hi}Av(FCy(4c9xl( zeA-VbuE4X z8d9J!Z-(8b$vmHb=@QUID7-QeJ48wTc(i+_f2JNQ^j<>gC>$OrHzL)vAMQ-w4*I*H z@?#E!)o6Z%Xjah>^|3F}{|w5R5_3}NKlA8VTExZdaUN>@AD+NyuxBn>!>_@bamcM&_B z-qJK6vGzu4*_*M!vez$1VO+2vua z4+h3)O^f_?_iAzl>JPG`M%~aU&>O@yG0Vg$#b8XCpJM?lgJD*jXKpF}Z9e z@9R6IFaE?vig-&;Vp&!QTq3+-yL&RyqW%%kX2vZ3c9p`RhKX3@!ofnUCDrl7tEzIo zySmdwXB};AA+zAD3jw-l(&##ECQTwEg91uO375RQSyqkv43eu1C(1noZVLKS6F#LCZ2qSABwg)HNhYM3 ziKK;T-+hHGR>`}SCL`QC0q_Y=Dr7cbZmfqB7XoA&B^18lNJc`4;V0SUxs|K&@_=t4 zLK8BqmLd5e2*CvcjjW}Syci1{`A@(^6s(}G!-&e2_XG4(brtI@o4mUBstYnMs1*h# z57o-X3>j`(#h$Y^7x(*p+e>?5<_06$wnAo=~P@Ri6ja|F> z+lYW54bE|WD{eQq-&b;;YGQH{Ej+N?f`*Tod3#q+{S$1{*K6Oa05?1=zY3|fH*kn& zY)(*To>@)QbL?he{CG_jJk>NjKHM--cs$&rUjmpFs{?%QEH4?M0Vw9iz`={%VLFXz z-LPwb8(BF&L^kh&|FdW25J<(8hqW3Xhk&w*g<1MgGu0Htuw&?&v1%MOUZgu6yK7+r z`mH7;bK~ioF>6=V+_|5;^l0@@EM1bRM{h-Ml(?f4S9T+KsT|G+L4iUx)6=b_FZhC8 zsWhFIj&Iz&IvLRPh!=Mi&1rQ?*E>Rm;Q8W3vVK|%kfLrl9SI?Xel5LadC#XXzt6?l zO2f1G^lb|V@mr?NqhG(os#O+nC2Q}28X!j9nl*p6-#a@4lLkl2>SbjA2Q24S{)mc) z7PhVP?F^+Kqz83T3Q~bAt_R8r&yI5-93)NF?yuea$=M0D7e9-h{*y{^@z@5ygn}#Vkk7IKNM91p2_M_R0STZLB80S`=BL z1^`WYS$@Jf$~WEj?-Iu;JWf~La7*{pXVV$s4uMy`E`y%VI^c%a?~TikPf;thl?hZY z3(IL$wtCNmzSVjoBFP5NyHgym)kl6+%Kf4RrxYV9J{e`%b`^u#wIWjdq7_-qM3YWaUewmQ608OU+7^V@^;{`Bolwu}6^|kS30RIe7Pn?{+36L+7#P zE?bL{tB401xTFBbYouhX356U*}Mt}w81(SUcRDmV2LNu zBiSIZ;fnD(^W13WATCM2&k*n1(U{_pv=k5CkjDSwre-}#H8DH8B|O00hI=ROsx^j0 zMfaC2)({4#Kfe$J~D=;I+ z;{~-dgj?7BcDZnlutQSPivgMAGNZXWw!w93*nPf>YXI_{flOIP$?JC{ItF=hGb`=y z!put1`TLSs6~f-JHw{gz{&Y(NdoWixb*2J!TS#24Z58HtPE8|mFyDm_KXB(D|x zEb=R`c3-06<$Y~7GLeVSgX>9!dn~I`X%EEv);0s(KFZL-z z1MmS;VcJhWxZ4n#Y`(o2-M`#FBtH3I!NvbF-T1Yws|^HjV{6@wMrrEUB#2(`Y^(G$eb@jmlC zHghQPzrHX0VbN;-W!NKuR5iY2kazOSV1n(#;OO)A{-&zB+GYIZ+j@U|aBs|wx(hu| zZFVLZ+jN>GefzEY#OD(v7iDcL3+nZ;%`*&pi5!ZT5JBu-ex4DA)NkKfx+i>4{a1zP zI{4e*Npc@i^Y)EU>5+~F&++*$q*u=b<-p-zleyVHJ}IMFkZAmtEukJY2j_T1*Yz3T& z*scmzDMk5A=ce8+z{skj$d7 z6|ggzi`~95zmpypddrJ-KCIQ&cGv92(Fb;<17988+<;U2S9Es+_Tp^zZ04vISvI-v zex3*$$=HV#{|6FyuTcTC)4$ou<1-(AUwQ;!ZV!1QSF)F3fZ61AA)rj9w3-aGjWgNn@nGGs^aP2UP2J ze?Ocpw(GFZtNNfMyaiWI($^M_Ii&qA2q3FuXrSqIn~J6qY*Ph+PN?zj7FAwUTI=I; z=*pbezrf2=kXqXYw$tLh{dJJB+2GCZQ!6F?Yt>tV;QQp{=*~_+b-MGzc96&7W`97~ zG13{bKt$9u``1PTYrPM=7XZ*cY}O{HLZw2Cd?z)H(C>3UIakt{yM2?VO5lAOo@;nF^CE-cD73#tyZsy`)T zQNqvS>aj2(32tedGYLF(;?qkO)&qmgHVo>kL@hrmT7w*qyt~=vQWA^R%UfvyE)Ud1 zx|Jy%rNp4M6UYsU-#w-m-D_b8FYnpo-$BmI4T6{Rm^s}(lll64E>K4F7*j2M9#DPI zuD5XMeQ%O2wcnTbz*q zf&KiBcV;RYyc>7>7P04pcM8wr8q|7q48b;qg@uob3RZ{TAz`0MV-kvQ*2Vk`Pfg^M zyPqF?a7K~DeCLyB1Ob2kgFC_8L;$7xh4vNi&KrZ#UV&TkZ)Vq1NwgQes z>X7?rqajlk>7H%g^an^r<=P%D8a~ zBqA!@oS_SBTupph!!UNB9X?--6KjN6U+PL?{6qx?^Vvl^llpxTwf@LjtWrY$y}%Aq z#+liAn4r{pa{SGpTR_4V-t-T>puc;!a_i6c z?6y>ZMr{tyEl=t|q3@bz@H0bt^^D%<{*+3N3%)I93wFHgWq$<1{kr+yHhzqs;FHF* zy>b`j)0XGp3jofoQLzq~LysMI*N2{yVn-)uQ|_za*w#odqQ}6G{3InPYtVr_JO_S@ zMBcAPKVCi7YLVko%TANKkK$cH5Ax}Nc;h}w4+H&o(ISc+Qpx)Zxw*NGLK@@CQdC^3 z+Bg5a4v^#Jd{*(bZnun7xOp~lf~vStygpF~raff~8xwGJRLsdX2Br~Fkq zCnqP*)D?F#BTri=Jzw`&_ba;s6pya8HpgiaF5f8EH!jZ~Exg!oIf*Z5tQd_ilBk(64PE zOi#ld+RNr#PwZ*5_=Gc9SqDJNtYDJ#4PamcIwNy2`N8-=>NLDV#=lJ;M7WWj8D$)# zFC>-0h)4FuG+S9dEa!$JAS=Zd-dEE-vM*%9)nM@B!B)t^t!B@^!>$eG%0e+fOH{u< zU7JzE-7?(orYAd>w$>&ELc%FlcxzSF*q#v*( zfZv+P7&N|*{Iod5iWj)*k-+je<(spXYFOoK%Z1+V8aTW}h$D|9tel4-YJso#QGmsc z;FJDTHl=EU|GcnM;N+}~o*})rp=mWw|DcESHALS@3NY9qp?CBHq|^g3|j@BMwj=a19-1QM@4?_R>v#ivobi>ufLY} zE+NeA%aEZL0lFCa_4f}XIc{)@f2TuoSJvs1B`%IlwnYWzhArB zmQV+O*qC<(&F` zK>f;UUWqdqwRCO!Px-_mveuD z@tfLFg#XWZXJ^g(BVpg@kaUO__Q*gUc^c&`A6OfW+$4GkZ8Li@jylJyw!T?kIrH^iu5HbHWpV6h!grT z^Rw(W5xlIDFs>pN9a$A7E-kx9?s3-f%lg8Zj0M34tNlj-;F@kwKt5vet_2;~TaG7KLqJGArwSKc$o12#TN@bGU23(@qgZ@g4F2is>DN=-$Mcsq( z`P{d36F~YPkbb;1C>|7V52lXa)83~GcgqVH6v5xA`9vCbeha_T23(p(vb`F6yakjF zqYyN$XR|Vh%$`iqhN{92QhOIiU}$ie7((W8mSQ@BOBd$a7aFo9l$wF95nOt;DNQ`Oj@Ry!|xOW8TP4}$L?uJLCWT;=caTKg&Bx=pA#}i z^0<+f8%%@se&I6l(JAr(#}X$&DH>yiboV}mBj2Oh>3cG?(TpS;or)0i)1@M@bU5HG z+)eZD7>#`OJzMF&wvZf0uuDdgBgDyXaWF~8cJKZuM!+e+p!6W_@WC1z0kewk*ZO}x z76jPqw+=%Kxo_p3>DZmjL@zcGXT2pEH!w~&)@9(kVQK(!8JW|mYXYF__~wP|sr8N) zj)z8kSyZ(x!48mft8Yv<3MNRtNv7+9d_4S0Cd|$+OvWIt%$vX0$qg7EwT_f-N8cfj|TNS5)vnOf1*bv!JqFS#eH>8vbx2HWbxjZ zy380L401ZdyFvOHiPp(#wuLTDoIqkKDynbxoE4+}LC%lcLEhJG>HiWs(|FoXj8m6z zSF=2ZqaoSNMLe4D5vD3(euHdr&Nk%-r^k{-Y8m{gYGWd!c!CdywYXn6v?-+jw$nVV zwA+r{s79nB{kbi9)s|ZXuvJnX%Rk7Ja`LJl;Atofs{7H|M0^=_c6M}f#GQ1~TBY zlgxe`MJt}EUP#$VmXfHTnP3e9^@+)R6es!?>h4z6Tog}!Gw)^`dQ=U*XuBbl4Wi^j z7N`t5iL<|xOLyly=eSU>_UNIMeK^QTXqfi=`ZqhL8)lNUcx<WO{B( zu6U^yNpMixji3S9{y)z>8??2LDobGa;9V|&=j;9Z%Zu75sIGP_E1)$IWSpR9+!@V0 zpBZ4soLaETM7x+`3#wenhl5K_RG4AXIMU%9tvkDM2^IYQL%{_fWqVrWh)%g(u*%ll z5S6R zdQg0+z?-mjQ|hdw6;`4ErK=SXOG!$HbbR6N8O@UijpTGkin$Q@);furM*P)?D+gzh zH3T2K9Y5$N|7n@ySY^K;VEW1Q*$XO=nx>ZK?y}LyJIJEZ3tU^VQN$OE1)+*yLcdLE zihV`OyxQ|7qkcSMn8!~{yBm1ySw+_II500W4m3i)&&9cCVZi4@0Fq^T^X}<492gAz zAKX|wQUlFqOBmj^uD3gS8XJjofHf&jgy4R@w?(z_SwC)4tcJgGAlLA;D0sy9{$!&f zT|AWP{O7XaK1h-;4L>2Xg~%;XQ0AQ=&Y@3#@)>-MlIUQ0-;@eh;^g@pm#t!uBGPo! zfR~84%6_P=V~~RW!bK;{9aP-NhTV$Q}Cni5nRK%Z@h*Z!SFTKhX!dm$un(Gebf;?L_eb1oGYG5h~XK zZRNS}uo!P^pOY~c@1sbZX6KoS#AP`Q(8!HQGNs|H&WG3=drHK^b5Z>{&HNFHb2B?b z-D2E$fpf2hdyDBja|QF2kjCL0=wBMP6R*9nHMRyQXhid}<)r)z+YY0Fnn8tBz^i=x zkXSuX?B`6-J*MD`dm)1)q#<>EZKJEiuFC&{*#oG$<5|su?;Zll)Yc}1`Et>0&)t87 z*QX`wQ?!CW3Jqkk_uD(9qbV}43w?>~_j~lB{C8gQO{@QM0W8yR9dh;EiYLw@vg&yv z84Mp?7;ETfJe>C`ySN`1LNZ3E+RO$5DFFzKH1Wwo`2GldM4_7pR=mtT|HG2EDW1Dp zoMzv?c3bY$wLQ9U#1#d&U;aNPv>{-vLfo$_|EDtY ziC%%KL+ttZn;*{WJd%7NwFvPw?AX7ySfT|2sP|dX5UT9={0A4Owb6cIhmAuJH6J ze#INtKDo}H(v{rS@ zuENMhNI&DAWy;6rc%s@TBKFCLjwEK-iAI2dXjAR+{BLIl4>mjpb%0|qc)9jf$Dp#_ zB9T#j%FPciFSIZUo?azJ#c)PBfUYROoUaH@yjcEHKTej?o%61^pWAkPb<^ou(?zP% zp#HlH<@=0#ng+V8E5bji#%E_uO-;9|{#s`8^uiZCaxXI{Xe|Nq!YM#1JpAOe43eOM zrO^U>iMpc#nbcF_61B?VuHwoE6rN{~UP#CfPQc)?Wy})%BievLY+_Fo_gyj$9VVJ6 zKL%KDqYm-U0uzT5&Fg@3OR|yv3rGiTwoQ0jJjs`V(D_SSoH zO0Z@VUA*v-r=kfiT9$C*89<35{PBERNJ*T)2<2i*kPe=m#BHquDplKC>1gUq5T59a z+c@sNtItd_tZ{eSE;FS|hDkCN)c5+~fO=V~AfE@?dA2i0}m?eFQ%*T!T$LBTu3c?Y1 zpYdo%rYp2R_0u+Ocb2slpkiJyY55nxZeMvzKa~Yo{kWmF@jGem$OFYaRfVaF?duwNI<~Me)qs3^1Givo=melyrufW@ z60yhOR~Aosvg8#4rW9l~00xHiaL<0R^$p!%R3ly96U#EcuTLX7b}AO8X9@2VG-E1Y=sNr6MlQT06Bv2fVx=->!2D2JHck2ddLpBBb^ffgBN>x4SQVIhOwHze|14^V_DvqEshkN zP=~n;Pa#h3214a>gb_8>Ub3i}1xom)l&*Vd?>)>jybcI6o_v-pQ+BK}DBT_2SFAP* zM+l-Hc60UbA(0y{ehaB}buKJQlK1aaIBvo;Jin5+EW;32xAT+7BQVOhLuVhK6gJD8XCIZS7)QN{L#S(J152BlICf! z^OKX^Y@d^CvMVB)sLK4X9M zsHq2a>I}N#WF5|FXMKK!AC=ZgE)! z{OlU>&6dK-+$qI5D_VZWRi;7nseFk#f9`3q81o|SQ)Kj38?*3 z{C=6*DKbJyUa_~VgZ>lXO7jm)?=c&zE!=Qo;le#hUpkQxH#WKe|4k?Gbcfn7AeWL&=r;F}G{r^eJ--RnrbnXe zE&?qw*T9xMO%LC09;-Tx(pgQIlRKV>MTC3F0&qx{9I(oBkUiok?sO>9W)sXEPe!Z* zT2wz;V8n*H)O)*l^V3Q}_lu@uokiP0`)K1&jCijNttds$tiHML<-XSy9owS#^5RS; zbo+M_W@ruT{ zhyXv-m&c2VLKntEM0fJFx3%#T2Yw2a8zA4|i$5I!4t~05ikoNHMIM>}Y>t-)vASXI z7Cb!t)oCsqcGmxosPB%a`hEXDwnRo&R5oQMd#h|AJ-oj%{+>+#^xU&q~f-S>4rpV#wwU6*Cqf~rXwUsJLT9c6?dGBVym z+HWT@w4C>uP@0yVR8ICc5m$aAmNB`3lu!`o{X0UgO(37M1Io^FOtOXPVc09*paUcI z53MPKaY61^O*uGE+Nbc8W&{Z9s%?7Tri*Ah0thknM%0%t!@{dYf=w>Y>5)5 zTqoOZYuHVW!1*w|8W(pQb;&#ZNH)t&_Ph4sEN;QR@L=sD(m-iphWGE^$LZfpO4eP$ zIG2D48}ce&e+e~eFNGmWOVW zKQ&L~V6`k4tZlCox*Inz5c$$C;B1GeS*=05E@Dp3)5Ivtnc8<7zm{X#Bu1kNNE)=T z8DUW`O3bKOAbP=`s=Ji3=0OMf9ed+fQbA?249i427ger-Cs>zrr?DVT=Ii%_f-`?A z2~FyZxN)^fCmy);TltB^DQ4H+iuV1azxK!h_Lpe5v(85&f(hTG2zK(gbL_W++aT4B z92=wapuSjC%Nd~8_4A=51*K#0b)JZr}e{ni-(%W)@!|L98FWLVP>iqM0>JSI5 z;3xf3SHOJXhRT9r=9sLFnhr%I)Yd+rJkTSG(hKMz;LYc_(lF*@t-bq7qMBRb zB1M1IFYaog6)c~tsi?QiegqkSf7@@B2;bqOShH>hB}bGX{Fmng0h^dlAwcs_P*v@~ zpbdl9ZZwea=vwX_)vFZ#*=ui)CmmM2e0#R}$<3jBx&rv?7gCJiedCHSem z%EpxmireGq?VwV}L`S~y3NIKq>(i(0nz=>K*BvUWde86nffCr$M#gjLgzu$yGlMs9 zqCuGmcFmX@{25{jOVgRsHqn z=ajD54%g}i!V&F_WI(O7N|qdTcR6yEuxbHBUKyA-0o?ueg^I%x#TcfCC1`8yiVrF& zStEZU!TGXASu$CX$@+;?A3w4ZZ4$%zXiWTqx_AATUiIDL!~N0@V8Ny9}V>lhHY5`rYvm&Ix8B$r)b zN9J5eqgx!cR9vvU!ros`b2v^@{n*n`mzR>^U%ms^^xFP zluV++Ph3EtFoZr|lCIcY(o8R+c2NN4d1kB?0zUe>r}0FSUMA*=fD7YNF%m_7D<~(9 z;IC%LYE1cSU-;hPyRE~a^L|hX*Djx4djLuL!EHfXXzOo09wU5J!?%j+Jk1H_Tksjf zy9+MmVkSckz2b*RcYLcDLCf?a)Lr$0^+tE7@s#3e)$$=_$m1$dLE=GP!b7$#>t|1$ zVw)cxe>oVVH-Ll#W;4K~Cyxb1W?<%eFgX_&9Uc8fG)$SQ3Z5x?tL0Ye3JRW~r2G1> z*uI)pBB(U))KirxrE5j`K4f)}Bzi{|!9pvocy$7Yn&~x`nJm8e3I}0Kf16 z!%k=?p}GadTMK$Z3yQOyON}AX^q(=ibhZI+8;EOrc=@|{k5Av;l-H)QF=B86r)zyL zFa~9F3|bG+=K~o7;lyq7NEfDsxW#*=9-n98o_<{@8;!q*hSz z>^~uedRQD_o0CMXxPS=m^ zyP7xAO7;3%0YwQ>%0P32EAk7y;y)6D2!1D)&++CzqVA-rm?OeS)xZCv2!>_pW|4|e z#z#Jinqu!^W?)v3+k6%RQcYdU_@HHi7pP=1jS`PT05@YZeMm zqJN_074pP0hsJozm~O7!z{D+@0j<9jCSygWXbeVy%o)u4S5v{(0>y%&P7u>XL@ye|QT*DSg`wRb)O*X;vH_MM~63;6zNE3Y~Tj%MO8h6@tr!8JxC zGvKb>F*Qz3PPi&>p9H5W1mdT^?RWonvp|uA`Nr+npj4W}W#WyOz^g3|Vv8JRY(B-O zx&aJHy9Quz`5S^iWm>KoMXrH9d*&%Bl)=b}2?F zP(nclQ=wuyZK+n)7^}vYp5U+utt`v+NEJCuQyivsOI)tdnBvDbVrU|)w)~N(|4V8+y}~6jJt36>gL{|y zLrCmD!5mO|ooe`X{yh{U@g*nRQ{&~DLP!-)Mz=7;}QY(Sx)s~g?@fk+%4ZYB?=c))R zqh^IOgmqmI2&B4j|EIpLM`Lk0{xVvGJK-m?Iz@iGSpQu$qE2N4M^shgeMJ_%`_euy z^H-KMi0ZJfNdp=_R@v^EXBcuD-AD+aZJfS*j_w*$oK4xgk-kXVdBm%ySiq~!bxYi? zqz@?$Q;NNkh-BU*FBcgOL`D}gz8j3A!z-QNDLglFIw+y?hun)?n5)ch`$%zks90f`@VNjhak8RaP?|58sGR=; z*c_~T!hzc%o04QE;OJd28}T1NS$`9FkSB^D`zr=|df#Qr@yu|!RmyF^y9hseWBYR# zOp=+Isc%yVXMGjcB8Q&W^hziZP~yJ{rJ2V=KFV|RPq!;q(_Fc4NUjlhLin7eCZW(;TS({`z^bZ6Hh!q zqBkBGzF1;3pW3mx&N_I*S2X-2Jd}8u;d$Jk%(5>Q)sIoPK)Lei!-_HRv%5~RLm>QW zpVtr6To!;@Zm@6B8&#$^|-{DlUF7DFLcv1 ztJ!6XUVqz*gyT)fXTlFafW_{k%Kn7h{La4mK6Mc62$ig9)qgMjCxq*k?mJy%=F zklR#U;da7fvWpZ{6lEeMLO$C5Jb!ZR?VXujN_vLt_owyE$dEeQ5yZ(~Y=?>Vngwen zm_Aw)oy_wK+=avSm`Olw;%oR~L+{{u-_$@P;9?^YvD)mn{ks=mwb)xPWbG&&_;5?T zz9#*4$n0BVc_Q`_@xUEXf^8m?-r{dWoz}DFk;u~>yTFJJlg~0=S8za%ar>}jp`Q1yrWRn#7Ss{`wKbumVvO5Tk zuOyl}NkYB|*stg|KhX|c$*Q_I;gZYNByW*fKl;zqbtit1Cz5~Bzc6jI5i?sQOepD= zf=woR<>L?@YP+){muRpA)D`WtpS%2@jpfK~CQvPy;_wY~iW*LYxT5{?qZ_q}q-6xU zu#dWDWHeUJ_CL7raIu`3dpGpXo&1;G-{|WcG1wlJ7lBzp1!y!UQaduW!9=zJlsqR-^kPc;8-Io*I+pA?=-&r3azAihi(0!*#liqp!?}RD`Q{TzI zysw4x`t$G0N*ShCpHy8h-spqA-^3{9nmtOCG!M)u9o(wxC92v+j+m4oDy99@? zfQ;@fEj)~wQZ-MWRZ&!&I=VbEDPHiz+13?te4lTjZr%6W&!5}BPqFmt7CX~I9zw^& zz<94=8L<!rchg@x^EPY-+&?GjcvU90VZHEgD_AF8&}frtt?3ihS%4)$j=CGM zvyJ!kuW`oOq&mB{>xVC1(ewPpicWQ9h-Zt94@e;Oj3PsgX5_}Jt>&(&AAt5_U{Rkb zd14%=a}Wa=bXGdxPfBk)E;~`Ntjma7{R_692KApR0+MX*OzJPApnOiM8Y$=pxd0i{QvlujZCJJA{^8(_ z!aDZhSe~OgNHxKeuifYiTD0J2~@PJs(zNHuw zuJN*pXNJ-Nx41y!Tv4#awTr6d7Z?4l5eQw@*^&D+dnbBHJ~v0I67Hm-sy^JzGkxa| zy3j$Ax8HJo5fIuajVg#tRmpcEBRmAA72_N6|9f>8W2vwH-Zrd=hM?-n+nz?z`jj?M zU*?cGzR4awqH1qt`H%KlTep%&!Hv>aQKrGT6JgrWjh43q5fSS~pR=qJo^vv;nwWqa z1Z``w6B?=h4~?1hIeGZ`(SB1ZK?z1|P2CjJ{pjYMN{isJQZT+2!ak z8+13yQHhUbrm{3Wd|nEAu-7^Wk1@{o;z|n$xLS^rJ3R@lnipc-fI7CPHbQm5Nyix* z52^;c!GzSaOzoi?q^*xuCOG%?+Fe?IX zdtNF{B^b>LLRI#uh19~89v~7l_(JGezr!R|y?-yQH}zxtyW`*2amUWLEX*Bg;e-*A zfk>a*4=d*{?a>RM`7eTJw%>dC2W^{z&ZinHuH}?bS`uHA;Q??)B6rA_m|I@tLMdsJ zUMxuw#0BSV&c7FkAocg3un#?spzbH`)VBuD4~IZ!ap~ibklnqT#|!FwviOG(S}x7Z z+jc$Eo?<YPts4{EJ*JVD}`*UAMyYt$qO3M_{ z`ab2pefRBK`if;`5*qocEb}FY~=zBsE4{k}f;))7_&f0K^9Nyo$*`zT(kv2>3 zD#SoTH!hT71bSqP#T;McM))~S>c?@YK2F=@M}8mIU$X3MS*HBAQS(~9Fqoj5hsc;= zB^fWKJ{=9aW;Y;6pGP13`14X=aR+#ZXei^)dion3@xl_T2+^9?MGol*2irm8&Q{8$$U@arKD&i8%S=T?Q4<#+#K0T~<7m>#tSn z$zwfm;Rf~v9EnC$z!3vucX9C&fA_n4Ayl7_3s=Jh8CF4tc4b9oEiYQzQ{m17+wb`I zY5})vJndk2Z!0|ZHfVYxcae^A9*vYOrZOUcfxe6B@J-=GsSsbH7dB^R))y8otJQYi zxzUUgR;5?k{9)?`0fRPy$3^MG+D4-j3&tp`p{{OO2()pNK(R;Fm^v&dZ(Uqt+|$<} z+g5o$Ut7|c+H8>9*->t2_KH^C_l$!({y;h5Vt%l@sIs!u*=U9XJeb_X+xz%|!+E`38kgWuhF%Xj5Ro?W$!IOU`>Ivik5Qwf} zVuTBaqR}ujQ{_~8ZP-&BBc~S3Ei17EN5Po6BEpbkoG8nR7PTBYJ{ku>6>rbWd2wvT z5Cf=Z-Sv*V(jwD>8w`U`Y8SL!U}*8W;_0DJ?1AA`+pF&avF8)olr+^toaq{#+}(Lb z{HbL=Om|okg#EZnfJ2=Zq0Od%xbbTqGTc1)bHal!Bk8RBcNco}U6Xn_z_ZM_2geSd zt(qnm<`EiJ?3lDqw@)wYhI%J3MO#2cCw0rpwF+VgUXio--1946+^#6q$bh=g^_)Y=^*%%C;@P_F>!3 zZA3PIUZpw~A6s*4qaLt;&)f7CcJv?62A#`YS5wZu_|U?$IP13|g#X*D9T2Q)4$~Q_QyXVIuc;oR4xw0WAn7baXs( z_w&8arq>@=IixA{+yc{hi>173%n6NBP1&ap#}9F&wt6})kY%zx(?kmJ95O9GGIWrYAwr+goR-oDI>jTi6N4W$j2dV{ z@(f`r5B1~B<7%m2$n$G}tKiKI?+0<3rv6B&9ng4d>`Gm43f#E+ka18EokZ|Dhf01! zoW%^sM6ZZdU0u)#)xJXq;U5ui!n+@Sh1;&Wz5{n|Uc}B^jdWxDnWGzmNT_}JyF8oD zlS0(t?xm@#y?a0FWB+?){S_C3zCl-UOjqa}*`Pm6;ojG(^Ju!^N%VSJqP0vqJj0RZ z7cU=4B>S5nE}4Fu&bDq9vpa!{z#k~k;s}hIFHVpZkqX|Y4FtQw9pAOwX>7kom%kbZ zH*Q@lEJ|PXZ8vMS{Rf)A&t8>s_}!E{Z;-<#cJJVJ{`B5ezq&kz=U|b`SJ2~KCbE$h z3D@fT>3{dm8XOyBb0tk4&E`;#!%QLHFjAnvKjLL9YH~P>XW7jBUGiqG^FY&P4MU*0 z5-8VP_q6vBhd)zP{nvSdl&=F0)hDQLbGwj}YhEZ&;*iPi*$0U!HtK*LiFRaQhv0Ln$v< z>u%Rf!t;9n#I`<=z74dB@HQ;0`0^RU&j?8u^$z8?17~%7l{@Wny;`pJ^MI?jIA8pR zo;|)sX$H!|g76~~9JNKmkIm9zjDb1=YEV!;6tKfqBV?Y|A> zxk{%$Yom1CRBb|L;foI~PQ(sqeWlM1EgXI}a_CJLcQ9^t)KQd=VjtCmgS+S0BwID&7YjR4ej&N?FCoj?{x7gOUioXLY`h$-8y zy^xUWaVb6DpPhvqM&5gOcD*cvB_mlkG$0bY;HMghZk^!7nzpzm-(H>Bn(|vu%&fg+u zlBG{x;F4g}pY*LqrdBgl^MDi-ur~#CbX0MC7Au%GZ5$>ms{Oghs%y*_&&MiEoYRY3 z*>nj$h#Oev$L=R%@1!t@v>){nFtd;n-5#d}!c1|dDXiK(tlj$p`;WUsm_2`7;s`OA zHa(mqHxGV&*!yPxG+-+m`Z$%MNA5p1ozKDZP=D%j;0pRu#T9s2|j$p#=nqz&6MvGab| z$R7q5EutRxU-~KSrI_+`oQ(aF$v4fhk!EiyTQE9{G|*T&EM;*^C)rK0%|s^b&mFKF zBlXp4&SkMl61Z(CmTJJR!%nSVp#0;74D|_ z#me@w90SlB>F)yqvFS9ySZ3g5Dn0Ed#zFeS?>KTR#g1s_Nak}7zAI!9hX|_{*$Fw7 zD{pcQRAou44=FJ;RY7NMjgBzjP=lZQBXZ|QO;;}s3%&Qh$So=C^aCG{4vblh+MTOz zj{6${?7M%?y=;}7FpblCe_S(rkHL!KDKQR({5j2=vT?eH?~ghRpv@UOTkpN&-e*I# zL&5R14OK!-3|sfXbmQgLG~hPR0FPwIptX;mTq+#})ZXlD|6fZ8$`88T{ucxzo5iRdI^N%e|V-O;74(WbSma22jf!ev5w4Q15&b*55<^8 z`i54b<7tBH`%|nyu`GnVopE^L__@d-QWcK)%#%Lt<(9;)2ekzG>@<}$z{iC}l5dwP z=xI|E7$1bMnUo>CW#&EHJaoXbq8hqY(c~@l1DzD939rw`z+iRYX=9Rm_>PVlPcRnOVU~8Su>}ug zsL|&_Z(1?$JnMyhG~f78(?*v;=yNKH-YUN=ZFi8cOfec|Gte6{`~OSjpu10|KWn-= z!_0DG6Gm%US(|xC>PpIcbq0eS%!>@iPpRd9BUn~;6db#xip{DPZ~idt5PEt1fn61} z`JNNICyzo|$}7eoGWp4WTxx0IM5qKmh645F3RNE?%|d!X$Sqi8!X6^i}ST3D^plsU=L*L8t}N3F_GH6B^N1)5!7f- zpTU=>I(@a%eW;d$_6!6g3`yzFX*@K>rHjqi8DJ8|$7Rj~Ps*nJ3R@te`)C8u;}*l< zl5`uRbdE%4W#+Zz^6fwuYn{Q~Q26MdC?BV0i`Q#qv7sl|_aot!rWh4%f9}4ahzJOW z`Vq48yp}(OB{om1%=)m5aiB6}qJ4Obv~C4x6~k{}Ox{75w0a-tf$#pR(QMtdY*HgO zpk~iCUZr%+y58T+%yXymiljs{>n()>fh9AsV0KnkvW)}Zvp8t)&UNC9aumur9_`RX z`pQTL#ovY@|E_4Hlj>HswO9YWr*anl%6Gj?j4b1q&dQ3#c=RnXbdH%;vMmS$mz9-0 zs*sbBlJfJy&&_Ix%aL}lI^Vn?eu3jWv0s-Lk0;+tPnj0Zb#)Rq!q3k?CVWSBr(t?_ zmMM9S47dc)-%XA2f#O==@eIwZVWnC|6Z2R_slM(DB%KgrzJXus@0xcLQ!@ zJu9*p_u!f8_9xG+%*YprGZ9`0Z-frO=Ty1ar2I^SjebeQM0|k*9w>WoK0RJ1`}RUx z_x}DvIy@JFpw%_P%*CtaKJrW8`Zk%LMV7w3{=)|Kx~?zbwToocgXJVmD)DEhdKB{b z8g1$Bs4%0n!3r1*BCyf)(ecWujTR-D0lxc+?tG%TkV!_Miaeci$(zF)aHA&MDSg~{zWs|JQU+`f5D+kmmfb|D;7+^QOug<23 zR_D0$w`N@9|Gxc&QJ{Ub3%LhfLLXOLaPOtN_cSC&7&^{JUOu3PKyt@_p*^*5JJ>9B zeKspRY=q~X8uX!=cd)j0p8nNkdm9#!R4MRH0z~W>t5Mq%_T3h0Mr{v{xqJV9_P1eb zZn%wj`;GZ64x>A+iaVkO;qV4ckl^0v zWoY7QO+zJFyMvQm)_B2Lb!H4#H&>v{M*R4o*^>W@bE@gvB7#aznLQ6!S*h3+VwZPM z%3EK6BfpMMAoQ8%%S0?=;DVeQb))u~tNFE=x3V)`2b04zO>fU2b{-2XT4gMF=zoiYD%GDBnvdlBhn*M zJ%L@YOAkiY$$~QwTj=GAcwg{t8*b|Ayz-cf&>`K2Z8>YVH9am0yN}rnJ;$1+AK_T|~yT5(}C%R7o@i66-bNQIbZcowk7R&B4BcZ!v}ukZSr*?q&n z=dTDpDw;ud6R(-a8ABo^OTXyhkWf%6*hmIVV0TEjad6Fy^Csu?HM0h4SbdZ29MX+f zdx$|(6%{7lElp_{okndzM@uKbWWWG89_qXtV<>ru)-B4dd`%l|7>l8Qo>)!fMx1rg zY2iPbY=kD-%={^JU^G43S33yG)C{sS^n=n>sbRIu0=^{DEuv008g92kzkL$SsRT3R z2BC!h8B^tq(xVeygO)RxHiQ3oYhBU8k<*RPI^HvBn30l64Dxfc1 z{$>p;K_E+rj_+%3L>D=m?8JUu73GH?2{j!Vh!Ni`$RGLch&Y5Kg3tq?PNtGYvFuC)ToNbzO)hx}8n~V74sS2j| z_oWBpfBxv|YM06EGy3pC#3p-n*vJV&s*MM-=4X=KVH~^c@Sfh=Av0R-DHY*VrNXf! zRZZ|&lyqVX;DI{s3G=m)k580xlQIWJC*iW z$n?5-BhljjgcPOYIh zS=@S7_&?grWqwM1ZY*~^jr>jLv0oz?y$klu228dQ!ZoyCA25xSr8Pu89deg$VrEuA zt$X^(%!A|Z2krvE5d)-;evFyp4bk|loSbJBG-KGyisn+B5et8WqS()zoVO<9gj z@Ojk~kJ!eEjy||`L?q$w3KRrV*d3hElTFZayr)z4)Uh#MUQcI+sI>w&Or~s&I71pl z2gi5^%WIap`}=Q%2oF`;bWh@R&fpuqb8mb65lLTG6ec!m%P+T3*C_-VmS96h&D@DY z^$DvFZ5g^MxJE5DI_<5E<^u_iRC6^Gmn2|8>YD9~*jm(xi;4cBo!~9?sU@IHb+Y<; zJrjbj_ysl5KVnrZZv!W|F%mWTXUW(r#M`!&RLvsp8m?^cjW)`NVR=UOj3zT`T5Qy|liljtdX=s8!&;+H2Rz z_+|_9nMGEl9GMh2oXy3X_?mqLrx_YBWW9@?v*WD8#raMEW-WTKR|KyRguH=a@5^4C z?2m*i`UL`6OhJIV-A!IQ@->$my_SeTAiJ4CRz^^wGwYLp`7>m=0qWjp$+8h!#-KOS`)zq)XL>cQ?bGf9?w^ zN%#Bso&~qF52@^0SAWh5#J~u_gjGcV$t0s-D7lzM5}?%8#q-&d$3Q6}A9bViS6)g= zNO)b{zQwxImT-6UXGFzlUbYx?{n?$s<@}w#RQB{=4nKb=d=THrJVdtiG`j#5x-K=; zF3r_0p%}~*4@w@Pt@1n}T_J>i`RjYp?G~!RDus@LD!Ji0=Y?;Gg&I_rQ^t*V^s>Ge z`0%dtT^~E`A}EkBct30NKZ!2`LV6RA0VcgLGoL}N=vE#4xv42-#fWY=B59(aqH({n zMa^1Z)?DUAgH!$9Cynu;l67wOgj0&AG4Rp$aHD4s<aaH+f;xjlN%q+P(dL02m zG#b4OwhJ~lY0)EFG74=R2@t!1&D5sp9rBXf&b-iu=Z1 zEMJu>QE3!Z6^#nm_qrt?(&*6AE2_QZDN6QJLTGRi@`nXEEgY+)F@zF(h4Z!cIf$|b znLAjlpk^L*ppFA<{-dAoJg@9x)Dw9}fDU};Uq{y%{>&-~AD9J2U0cf9l5}NikT)Jx z*~!*x<5A0=P_V|>`o3$Z(eM&1cXaTsz1eAOfO2gKUX1Rgw~BdD;z-BT_Wdlge2Os@ zC46YF$J=!AZ1FJxC_M2Q2r`)DvF($l8W}+!J4){qBRKh-Gx3ri&HW#-ST?{yi>KTU z<-T11uNU-}H~#hntd-tx9EyIu4M?f~0k0^n_LH|D+@y6+66O&z ztFitJcC|?~y;a1gL+FG>-1NH?*%6+hRctEUhS%HOO^2P$sR96xDbHpacQ; zOOB{oUTE_Nazr=&V@yZT#<=TblZKmPVh=|o3^~KD72WS6Ht0^N9V_c2h|9)msc;5< z1N5WN*L!JJ&-bIV>`UpO0%Du{PIqi@e>9PEAWgOpz1DYWHf9{Zp4%@N5x-qx0?_(1*LOGS?d{K zJrf7-frrWZjN9@h^&&l3@ROBtnGsdKTKH~p_N!HoEayZO~mK}0o`dg+w513i=Lg4gw;`;iyia~qqIpIxdO!@men1d!z?rY1MYW`E- zFs5x=f6n-61#cQFYOoo>o1InSmRkUcysD;z<7aE61bkZ<@H$?Me#I&eUbqLSkmS4HJ;iaJi>r<)s2)!qT*u2ar{2T_X!hT5k7G}nrW#cY zEq$(oRz1!8bgo)@BMwX2Dfcob1n(faC@e1>WjI_+mD(%e2oufeZgs0l9KLEfEO&gi zZy}9svb~Zc-!R}(*X3O(ro0hYl?;M^&wF57H*+>+(VbTgxHtorE1hbZn&fy#Gki2Z zJn?>qhh*)TY=|7&I1O%Hq&YP}ep{%3UdxJWu)A%@AEvjTi(&gCCN!24@Bzs1_D5UQ z20Fqmp&;K=e*}k^dEQYxmub6z_$Ml7ZK(A@KZ3OwrMgmfZ9sN~#S1b03XLEqC62tsNvfoi`arM)B_)G&rnH*<0@1RDa;*tYjkQJ)ch!&5BSbV1 zllli0@(pl!ZCV*-=G50Y|8wun=K7B+V1S<}EegXRBr)VcK2%V=|5)W-@n}vj4>Y6| zZP#LRD@1F7W$Q1>XGIt<_=Ce(fy+Q6hVq3Ibfb-2Ry6_6Z8hO0$WOPW0tUmj47ovn zj?FLTfnDXlY9wgWHaU&T){=cXQ_lIUS4jgQ`Q(eoSP82@a(EW&J$=!LU%qAqY_S%u z-a7|t6PZac5DB z?JCy!V02tQ`p%Yk#(j;PQaELPREco|%2SR>F6-DlBA28D_YM8>@bItAP5YBp&lX_8 zod2hR9~uYDrt-K{oMD>LVNutIM_ulr+5Dwd`IEIoMoNeDA^}{I=U!YkV6_kS`YA!s zS$%LAH5i<rDLbCu`wYxujoA$>CMYq5B5U%_PLL z$kaP8Rdr=g_nair{$ARRv1uYnuB?_ZqXX+=T5$Sh`>3pO$HUa1?{u$X=fR#0fCiQ7 zf4mJ?FnDafUAHV;Q&ME4oT8mk6-$Iq6&eV_MFL0Z6VbvJBXnOnxU6$6 zO_WaZoDXzf+yMgVcHLvUD(mtEc`@aXtM51mJa$Sy%Rh^Ee7+%BFL)zX`?fHBO|S25 z3NB;3K!w8jgTtajqep}q5b~T|M8MfPFo5Tx&LYW-{C_5%GmNC96f3sfi1Y%iMn_}y6d zhPbrG4i0`W4NvCmYXsn2E=@^nT+h!gpt{_x*0jG<@#)yleMmn(<^j$v$twqh5kOKn96RvRPZ}7xKa!)3xJMx zbIQ{?i`eT}?Gk?brN`%SuD;OS-(rf?*6DOv}@TBF2&TEUs!uhaxS;A>Z3Uw zrX6y696!>~?N7pQ*g=UunJ|{1p`;~IKCiUH+&*!$JSs!0{eQJQ8ffU+=93P6%e!vq zJ!iI+v7Ckb9Z`8KD;@C%EJ9w9GuLt)x9=fR(?F)6lMx;7AI>CPGT!<~>2fD$aAA8v z0J|qpA-6iamRC_`{^!`y4@anuMHr3=G;xB)sIvR&%id%uaU^Exd6~Dna6Z}8SOG^z z^pxa}-t2DRIRIoc^Q*I}TV)50z&;b9%Y8wp@2o++>G(y0^D}vp$|l%QT-; z`0A;MlyuN)4ddE&P@pdromLD9km>X3DeWRcf)770Sd~Z`f9T z0R}UCCb#u^A>lfln-?MYMVUOeoCnCRlM43GXL|ghr?Cq2gW~Afh-7Pdp5=iYWG-sTp7<|+kX zGF|4LFn#c7vPJ}@e2T$Cil=(9xRjK{c{i36f9u$lTQzZ?br*{RD_iO1PyLS(GQf699 z(|vIGtn^|9McaM7E_nxaoglRC!X+;lt=xZUK*A|9J)pDi=s!hhXEc%tp3w)6D>$R*H-;g^_I7RD$0mIHG!JZ#$2gCBk8yoglo7^6%+Il!}q}lCGe+xD{=fc=6_?tJOd;|TYrQqnt za8_KBIIqkYs~E5em#Q%eHZQC(E9>@0@E!eAV!?Mm0>>LQW}o)JGT6;pfR+dMlh+ym zdAZVeAk6vtYcg5!q&gNb8`aR68-$PbmayQjCN9s62I-2yboFgs@I?Wbqnn6G(2>!E z!1Ou1c5Z$?OTUgHl>C##kc}jOS=o=<=@^QI8 za0>5xEg)$@3e<2o3$o)Xr2}Y&A5Zc01G8DrCpetqDXF&ATSwg~WGa3BafO20WWyF& ztFE8&e{sVf$^6zL$z|9MxF@o90_+U0FEGEF=x8IT zSDi!%0**s32-3+A0VmFsml~H8iIxo(p+F*_JjaR4XA|`Kjh_hW4+`U;O@mC#xbk3y z0l)iCooDsm0u{#1vT9TpNyh}o3smJz@KR;YA2iK8K zh0xBwz1iEjPe#NP6zbdB#V=Vk5=846p6Nl;iyRo)kWI2m8g-NYOqS0di@E;R7V(}r z-E{V@r(14ygfGuTO4P-QE_+J5vk9`Yy7BA4BfgEOQ9dlBr59AfH_CqcBHWmn0)z{q z`N1q?Es6(v0ucq}66b;L>V$bE(cXU~!T?5aCFVqp$V@t<&C zL;z6TvgdBr{Vou0eo^L7oiKFPD`ER=>DQ}Ov&KZu{2sk7aW*Brv)n=d#l~3p688LJ zf219|B}cEn4h1BTe;8;>EzP9kwnz^JPKl#L)t>gP_(lhWS`1FbO%}MLOH|r$(8WQ{ zQFI^=Et~`#TW2fjYF9d>lOg{hbz-I<+8bf5Vo4o;TE9sJZ|^NtuVAph^-xfG#sIPD zFuzoCKHsNL^8#vB2A36u+glyKp#+O4g*k4bOLHMQD4EbGV$GaPne1SRJ3E`dTOQ?6 z+6Uf7H$q3s_Q&fIyUEEn;78DnP-Lwddj%Q}O+S^!9TV1oq+p%$wz`12=X3e99KnP1 zY>4{7btInf=l&Yr@cKu<{iwPZ;XiPEc8bSK)lJ8R+kvF9jZCoL(ZOX`Xk|``<^T{& zkBJ>CZpRlndzM31wJTt^87M&>l*H^ck(94h9LL(T)IeLa@H6sT)$(NW1huWN_U1p7 zg7pZPW^GO;7E!A)N2z882d zR@v_4WM$oP3YhP)r@;V3XMa9%(tl=to@a32{BmU!a18vf4rX>JP@>GXp_E3j{~j(s zx-OiE{Ed@I(x`H8-`=i+hjHUR#d+zA5blF=X~sf;x~t(cha9Ny>9LGuv7=82LHMx~ zAZFV;JTQED*6JuYd6lRn+pp7Ds!(fqTt%|?VJsLven3~T|;#Ct-gFIUZE z`4wKqs6s`Xm~MEukIeyd_zfignCTw%(sZ~jynCP zQre7MMP74Tg{BD)?;Flu14K-+C$*Ph@FN0PM*^4+=XP z#6REngoSLsw^`s=qbjTHfX5X|mZ~$FKl@%kRJUKe;8XGx0c3CHI}Q3=0VMP1+3))%k&B{vtK#}H)?>9{f!|_$s4lls>V%+t!Xl8GL;#UUoEE= z)VFZw^PZ4(8MpWnQ8NhWd4PLOSHsso9p6V1D+E$xh$qU6yc1{8u>6JWG>HT>7`-xg zvL7ghwAF*9Yda34cx-rQXHgEGU8K0z5We`;^k@jX_{}S-ZZAMD#)~U4=i-3tT#Ber zpl^BCQyQgaNeqmh0t@aCs@ymeWv49ZN!+9F1o)$rOF}cRMxAOS&l_#YqXkAQ);BZ; zr(=>E9Gy@v%}?ep=&c^qUM`+R7l`wWkcc^Ayo}B3q30{(0-?KhD|d1Ql?Ymo2TRoR zaqUxtMjLqWl#Y&$zAZ&LpE|4#1-N=QIb^UtEU&qLIm6zUPbGM^7D`AEbedmvwJ&!C zh~w<-!FG1V{{H9P+-)>S@YE_`lnlO-y!5euG%(I#L@}EiYD%Vlfk$(yA7mw5*b4#zU++m?|n~YRmQ6EpurNjk?F*g~l zIKAetg~vK&ZogzGs(Yl!7RTRC!F=+9oh^b|HC`dbF5l6tdE$NgJwXZ)99bpBi2(y` zONhbb=aMteqtE_m2anMzGCgn!rM6;9I!*}O*|gGeDdF&$d<^B{iLe%3gKXd9hFDI} zBg-+~3LpvbK-ivITTmYi!NB45lkYmSaGFNSu4Jz@Jj&5nf1O~rV9(&W=e9rdNLK=n znZuL!saQ2;e=E9FY$fChtF9XJ=&^ycw~A8op`piS;85~t7H!s$r)|g4BEhxK>oix& zt3aS{Uti|=)HYs@s4UhH2qJm4wxW#EkK(qCTdTjqCofI17nQYd&X2NAW7N4>LzGG) zBUN9+4{WY{3A%{bXqEI}VvBgM+}(d=I=MKz%KP~PwJP5ZJ&mv7^631Jy5BRW<8im{ zn`Y)@ebbUHk)`Kil7F7gP6w-?^i9T#AVKjn7J7sfzU$>1Vumz0uP;l5Q=~VLhtnrc<=l9o&U}`yK}y~ zvu9^!cV|Xurf}uKQ>NGLpI|~*m1^`AG$7Yf#fld?<-snzb%PiV4nB5pjn#eF zgZfa`+Q|0hF>Lkq#Q&0Jjpr)9(#;N*3|ad#!_w0r(I$zpfwTcAju8XZ#^5W>y-qdm zjwJ6~ozQ;`4v?WP4FjIyQyqkhpQwg?AFk$X>2(ws)tEBhtu=ePs+TD2Y}r1?Hy%gTq&BRA1(GJj9hDkIEhP!M_Ov zLf^(_|NoJk`5aeiBQXp3{^ic)&hONghk9+)IbZhi`OyXgzF4#t6xgu=tlMzKskSmL-un&9~wW(%s zAr8(IE)1%z656HPX>@gYmGA^#%X*iN9>5QYRm*&?RHhvF$Vc82+wb|~p!WYqo%6;X zogX^=CcxFuP%jPsz|6@+*R<mOkb&^m^@1vH(^DVVTQY~OB>aLH1GL7F>y4hHy7 zI^=Kx2#kt)X`WbcnDl0)l=l)plnARp0k7Gidk;4{R{-Q?7SYNPR%atB0qj zfxB|p#I_U@%(#_V;Azbe|)Ew=qjR$9?6M@nD{6O z^K+rbg7ex}mv(K?kJZ!UjoE>HCrUm}2D??Uz$ikNMwQyNPfpHpx}SQ|)8pa`zhiP% znIp*}B);XcG1xLivd8$s>6$>t#VnH865efC8&=|e>ji>KHi_)(^(p=MMp{c{9~0#=2m!Eg7G>Q_Wq#9a#8r( zy&*dt%-&0cX)#FvYSNnwd-r|xCpz`%=-l_E8N2I=r9*;7ng6^Cz~&Q_nFvdKh5zX z9#gU2xC!XI%QGIJL^mF-Qe_ipKLfXydvE+AEkn~j1+QRGovm3YFs{#BtZ#>iV<|2u zlt@7pm~~3WL)7uoHQc7$N@|Q(KdmdbaPIop!`bL8p59BmdqHyVSf63}WVfLX3wTtr zLU-yF@A68wHgy02aokwrDv>yxFd$S@`_aCjTPg3Hev3OS2XA_v(lS&==T`&c5S;~J z;0FyQeNgs-^jO~kS4BTkIak3jr_H8g>+o1w4fXUZCRqK&&28bA`xasxe!P6&8Ewb{ zluXm##P3118BLPdvNA}SUX6!Iw`?xH zrZv8mfLl;yN5_(d0+I zr~-fDM<&xYeW@xHDmO7Q`OnNW$`lQgji3GSDV@Dxh(M2cZ5fl#VE%$VfqlYY!SdPW zb1Dk9u+3-pQ^wTtD=Nk(U#_52kMfwK^M9mE-iZu1y`_|Il5a0$Yn)mugce4pI0OQK#$kGzi&2(_{;r6ew=)A%0+@+7MoJ!m|Zy}N?8w}ohu@;HbMdfC?_ zx61K;SC#8{Njyg zrxZbRQwxh>gaFv?u6kz()Vn5tyC>s_DP|8!`x=!$TaBgeOx82b_Sy2C;>wm`O=WM> z$Fc9h#8`gzQhDLX1#+ET(sQxXah(}m1{mKqJ1 zKb(jv2`isppg9NX98hRGp1Oc7n|&17Ep}Z6uJ1>q%gEQsU8oBsWH)=*SrBaOmW+%W zEl{g`YEX6|s+0*`sGZsXkUD9*%D*#8wYF%`n#+efR-y`AtZjD>vUupGLRHd9qlvSB z6D;e<$jllM5s<4ZDk(QUU?N*)TBbiQ@{@@zTwM8n7I(3pP3&#&cX6H=m{82{M?b$j z{yQIV^aK1-l)b%GgujOPaM{H(pjP;{#JBKzvzAP6<+_#0gB3yII*;?EcC?#c;iDvy zq{lz#WU>iO+JdPIS?s!|K)7K#`e$snDn!QmdlbL8F@VQ&bkX}a?iLST1E;w`}FQyo3!Qce)HfAY;eWV+0g!l6eCrW!U}zlvr3C#gC3d zkkRA{c470de&?~v<5FMULZvjmb#__F01YpEHYIvfOM%FX$gdM>%Nn zmfRB0o_jpj%~7A4G^F%NG&-LYJNRV$X%T=MctV-{)I?b>bS z-UtS(=Ly+f*5W!GIIc3kkS|FO6t~ZOx8DYh_Jm}*(oS(#de#W4z>E!{6o)mW_93;y zO2QXfek3GWPnT6Tl>ytAqTU;mLOW}{(*$I=^p)beEn?Bc549U-bt`>n8N3tWC}=Js z8tkz&udQ&PhJguhz5bFME0MDcW&#>ZdhY%aw9dF`nDy*U(1o*xcuZcO`8u>Q@|w$k zb=EJtvYbkfHVHn(YsgBIQ(C@OxJv!75=*0du6C`JOpKmy`ll*8>XT zWlJJ2e@-5?aiB4H#rvOQo<6>2185rzZ)g+!gF69F`%6);>wNq!9IoVz>44rBS5E#ZJV@OideXi?)oh(nU@z3&U1)1sJBKfDhgNMl z_j{#uUgTH5ug=~5x;Wo>D1;f~-Y>-?096DcAgBihz{=pjLWu!(KkC`9u0Hm+=tO&j z{gQl(O=SKQ$$Ww!c_+pu1q|_TPM+LIdhF71y??tq%lfV>3pVGv88X|nB zrW4f@8F?Z%g&;34XAWnUOJPC;H8%~;*!X3TRDi~9wLCvrX`aj?1RRI{I@!1KI+Ari zsmLpNmfuJwr)aN=ST+|3XB9fJBFS3gn{o&>?uW6Fcoy{E|c&9e)^$rROR3zbqgmYXCP|g`mPCj3Fgq zl+g)tm?ea;zc$-qu|HkivLtYdUw*(sGfVF7fsJKY8m-BV)GlwwZ6oLAj*KBfhIF&J z^&n70gDc#7D9ysdx=QCDTz%Vav8$fJWJR2e55E(i@FG#>GSb$jSoGR5SH(4WBE%_O z7=?_rz*fUF9p%_2LFd}q5yA%YX*T7&o;mVxMc zQn80YaoxtlzkCUF*d?bOrHQ75xOAKJnb1+aX))O^-|Xv`iWKc_9D){0E_A!DE@S$` z+597HDry^RYnp1n3#+;9GW7ZJ-6#&26*Wk&25-~$s1_o?3dx2Gg4%7wZ}W7gY`>BZ zgfA-cEr&E#52b{Zj{uR1E{tQd+-i45nstnVDi1utE5}>arzg!{12j3lv3)asVs`l) zR?S%4BX>T|)0G);#wK%!-HCgH9!FeafnI^?2j48-@k`0>GObQ9P4ZhpIsI1VpaJ@z4$iZk}oMr6|Gavz(<=z=wGhqY%F`g8XqeW z&KbprH=J;D_bREzHp-5XL@hEn4QEXI7VrjYk0+uZy$H4QODXyy&?K|j8v7uy{B%F^ zBj-`!VaY1{BAFi1P|M?qFPRx!+77x(_l^>fi*kiM+dlY;(%^$k=x>T>)XZ+?v) z|9k!sPlLQ}#c>YgCrneyex;n`0BMGMl^8Cyp@{0HC2iI}0#8pX$<3 z%+_$jVTYvA8=EX_EzOLGEBesf7BtxcZ#ppbfgOLpC0O&znW)H zY6Fg^S{HXPx3cAS+H0^sTYj9hDtt0I{t^5f8eQ;gsKsGr&*^KtYlAQzW<>op zY2{U`a@ZLY)2aLd>P|De_pEm*9^M?rR^6Ls!K^9cDzJarmOPyx*_ zx?)+}eRlO#%HuMIJ~&tEd7GE(3yk4c=nf(jzpk?bbwTl9HM(Sq;Zm7hV_5v{1SRxk zqn!ycn7;0q*%lqOC7UG{BSWrrq^#5rrP8Y#$7>A?q90pyLdk>maP7Kx(`!9D%2t;r zDrBP$lQv_5n%9opyUn(nt&l#u2OXmB58Z?MaXe*@-xwQUCr~gK%JjqmXYz z`Qu??X+sH7HT(2rh`2s}G9s!#TrTJK^(^0Qh=S0U#jpCxk`qlSTDA8Ouj zP@H{jVX`F*RZxT&sxi+VppUy@DZj4#qlwcobM=={Gs2T12bn!mj_~y84`0Yar2Z z@!BEcdn=GhYQ=TVXB`<%S zTA2-~MoGd(TObIhKlBxgiNvwDsYyerKoU`hc6BpLQ+kW|A~5~EI`HA%DdaSe?^kw# zJ{{8fw=(XX(^E4aT5$wftq7&^Yk}#k1vtHu%;Cm_O~2nGenc$pa1!fByq_i@D%2#s z+6x9B>iLbH93wf2Zox-{sae-_;rK_X4zKe4CGXtWJqYYj?GudoA1b z4FkD~>bellv;LKPyFHCR(1HA(pLB=(R`h`Pyi8v9%ol4hvpkqag8#fFH-DX!<`_}; zIaZssB~NYFwVA9M|HMrgg z>)3()R=-NH_}2L8pE<@%sGTTnpe{(aRH;*1Cevf#aWRY0*X4v6$&5;Su+cG>8bhzY zT868ylyB<}48EJxZJGryhoL6_G>3X{t@+*iYj6QJ-cRIo2EW&dj!K`-Y0nu{BJK0- z5PDDMa!z~~+66^NQ;+rbk;Gs7e~2xo?zB0R;OT(X3aTI;=DTNH1WeVu)Ffo4yWd!~ zuLB>?k61QsD6)P|jEQRUpNk0H!MFTKP`L5AYWaY2&=mJpMbYnk00X46ubs4-1o1e9 zfDJAG5E&6qhb)Zh`Se0V)+4G6oqUG_W>;E&2Dyo?RcPIHxm4J?cXEUIOqZW(%Y-`o zo7T4-iV_gqM@0#^E+N4{1QPH0RhXCegQ%!kkgbQ$pG^>1l@nkBT zkzfv_RFVEXM%KcZbQ;3Jl)50$L$zb@^PEoT8jJC<+h$Af;kTC4eD~woel&__qlr7(}n~QD}zo*<&*XI|kqpWu#Lmn&=n2 zRZiUZ0v7K-kb;2^66#JT`aXxLJYeU1x0y?DJqwMsPfF@zS&x@1b+ivY*B7&sg(@#{ zu;|5pR!r&Pq7VfDvMcPpge_a|-7S;ReE)tq{rn-ogY&-vm$q_Q#)A}9^54hrZc+?c zN2C+((%A#YXvR&S?n_u!9OJ!)g3 zA)%DuI$B&5fS_ULCv#@OPoW0V%;iEmL?f|naNG-pRzyfDl z`$WpiB z;Sn%@01;kjb182|h)$PKn0{6eFm?TmiFJn&E2xG8C&JE^v0{dgPE6p`FkPIWPWpnEO{*({BK~;^(oRQQ(P-Wd2h^Z$9 z9n3ldGo=hFn>dD#Eq($M3YXfn_Ab&p4JU|)U6g_!C5v~459KF|h0TA|CHSxC~Wer9XBK(|mu6C08{Jt%4F)aJSpxrlxymAz!xqP5N4oK5Tsl#Wk}l`gekebGT2DW5jv_l#zD$|}y!mU9&qc`K=t`8BmVU_)J}o;vpMh`@iGm295nQuk gW)b~OVb#CHi`RlHybQT+p7(~Dk`|;)(dza81N!exKmY&$ literal 0 HcmV?d00001 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml new file mode 100644 index 0000000000000..5ba740773d5a5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_motion_velocity_out_of_lane_module + 0.1.0 + The motion_velocity_out_of_lane_module package + + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + + Apache License 2.0 + + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_motion_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + traffic_light_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml new file mode 100644 index 0000000000000..0a05163e8f1d0 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp new file mode 100644 index 0000000000000..157d545dcac0d --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -0,0 +1,85 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "calculate_slowdown_points.hpp" + +#include "footprint.hpp" + +#include +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel) +{ + // TODO(Maxime): use the library function + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0, ego_data.first_trajectory_idx); + const auto to_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory_points, 0, decision.target_trajectory_idx); + TrajectoryPoint interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || prev_slowdown_point || + can_decelerate(ego_data, interpolated_point, decision.velocity); + const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose); + const auto is_overlap_lane = boost::geometry::overlaps( + interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); + const auto is_overlap_extra_lane = + prev_slowdown_point && + boost::geometry::overlaps( + interpolated_footprint, + prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); + if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) + return interpolated_point; + } + return std::nullopt; +} + +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..19ed066548d0f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief estimate whether ego can decelerate without breaking the deceleration limit +/// @details assume ego wants to reach the target point at the target velocity +/// @param [in] ego_data ego data +/// @param [in] point target point +/// @param [in] target_vel target_velocity +bool can_decelerate( + const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel); + +/// @brief calculate the last pose along the trajectory where ego does not overlap the lane to avoid +/// @param [in] ego_data ego data +/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) +/// @param [in] footprint the ego footprint +/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits +/// @param [in] params parameters +/// @return the last pose that is not out of lane (if found) +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, + const tier4_autoware_utils::Polygon2d & footprint, + const std::optional & prev_slowdown_point, const PlannerParam & params); + +/// @brief calculate the slowdown point to insert in the trajectory +/// @param ego_data ego data (trajectory, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param prev_slowdown_point previously calculated slowdown point +/// @param params parameters +/// @return optional slowdown point to insert in the trajectory +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params); +} // namespace autoware::motion_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp new file mode 100644 index 0000000000000..585f4760290ef --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -0,0 +1,242 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "debug.hpp" + +#include "types.hpp" + +#include + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +namespace +{ + +visualization_msgs::msg::Marker get_base_marker() +{ + visualization_msgs::msg::Marker base_marker; + base_marker.header.frame_id = "map"; + base_marker.header.stamp = rclcpp::Time(0); + base_marker.id = 0; + base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + base_marker.action = visualization_msgs::msg::Marker::ADD; + base_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); + base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + return base_marker; +} +void add_footprint_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "footprints"; + for (const auto & f : footprints) { + debug_marker.points.clear(); + for (const auto & p : f) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_current_overlap_marker( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::BasicPolygon2d & current_footprint, + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "current_overlap"; + debug_marker.points.clear(); + for (const auto & p : current_footprint) + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); + if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); + if (current_overlapped_lanelets.empty()) + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); + else + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + for (const auto & ll : current_overlapped_lanelets) { + debug_marker.points.clear(); + for (const auto & p : ll.polygon3d()) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_lanelet_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, + const lanelet::ConstLanelets & lanelets, const std::string & ns, + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = ns; + debug_marker.color = color; + for (const auto & ll : lanelets) { + debug_marker.points.clear(); + + // add a small z offset to draw above the lanelet map + for (const auto & p : ll.polygon3d()) + debug_marker.points.push_back( + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); + debug_marker.points.push_back(debug_marker.points.front()); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const std::vector & trajectory_points, + const size_t first_ego_idx, const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "ranges"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + for (const auto & range : ranges) { + debug_marker.points.clear(); + debug_marker.points.push_back( + trajectory_points[first_ego_idx + range.entering_trajectory_idx].pose.position); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + for (const auto & overlap : range.debug.overlaps) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + } + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.exiting_point.x(), range.exiting_point.y(), z)); + debug_marker.points.push_back( + trajectory_points[first_ego_idx + range.exiting_trajectory_idx].pose.position); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_decision_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.action = debug_marker.ADD; + debug_marker.id = 0; + debug_marker.ns = "decisions"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.points.clear(); + for (const auto & range : ranges) { + debug_marker.type = debug_marker.LINE_STRIP; + if (range.debug.decision) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + debug_marker.points.push_back( + range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + } + debug_marker_array.markers.push_back(debug_marker); + debug_marker.points.clear(); + debug_marker.id++; + + debug_marker.type = debug_marker.TEXT_VIEW_FACING; + debug_marker.pose.position.x = range.entering_point.x(); + debug_marker.pose.position.y = range.entering_point.y(); + debug_marker.pose.position.z = z; + std::stringstream ss; + ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time + << "\n"; + if (range.debug.object) { + debug_marker.pose.position.x += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; + debug_marker.pose.position.y += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; + debug_marker.pose.position.x /= 2; + debug_marker.pose.position.y /= 2; + ss << "Obj: " << range.debug.times.object.enter_time << " - " + << range.debug.times.object.exit_time << "\n"; + } + debug_marker.scale.z = 1.0; + debug_marker.text = ss.str(); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) { + debug_marker_array.markers.push_back(debug_marker); + } +} +} // namespace +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data) +{ + constexpr auto z = 0.0; + visualization_msgs::msg::MarkerArray debug_marker_array; + + debug::add_footprint_markers( + debug_marker_array, debug_data.footprints, z, debug_data.prev_footprints); + debug::add_current_overlap_marker( + debug_marker_array, debug_data.current_footprint, debug_data.current_overlapped_lanelets, z, + debug_data.prev_current_overlapped_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.trajectory_lanelets, "trajectory_lanelets", + tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), + debug_data.prev_trajectory_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.ignored_lanelets, "ignored_lanelets", + tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data.prev_ignored_lanelets); + debug::add_lanelet_markers( + debug_marker_array, debug_data.other_lanelets, "other_lanelets", + tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data.prev_other_lanelets); + debug::add_range_markers( + debug_marker_array, debug_data.ranges, debug_data.trajectory_points, + debug_data.first_trajectory_idx, z, debug_data.prev_ranges); + debug::add_decision_markers(debug_marker_array, debug_data.ranges, z, debug_data.prev_ranges); + return debug_marker_array; +} + +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params) +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.text = "out_of_lane"; + wall.longitudinal_offset = params.front_offset; + wall.style = motion_utils::VirtualWallType::slowdown; + for (const auto & slowdown : debug_data.slowdowns) { + wall.pose = slowdown.point.pose; + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::motion_velocity_planner::out_of_lane::debug diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp new file mode 100644 index 0000000000000..4a5be9dfb07ac --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.hpp @@ -0,0 +1,31 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane::debug +{ +visualization_msgs::msg::MarkerArray create_debug_marker_array(const DebugData & debug_data); +motion_utils::VirtualWalls create_virtual_walls( + const DebugData & debug_data, const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane::debug + +#endif // DEBUG_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp new file mode 100644 index 0000000000000..56047e459bf51 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -0,0 +1,388 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "decisions.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +namespace autoware::motion_velocity_planner::out_of_lane +{ +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx) +{ + return motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.pose.position, ego_data.first_trajectory_idx + target_idx); +} + +double time_along_trajectory( + const EgoData & ego_data, const size_t target_idx, const double min_velocity) +{ + const auto dist = distance_along_trajectory(ego_data, target_idx); + const auto v = std::max( + std::max(ego_data.velocity, min_velocity), + ego_data.trajectory_points[ego_data.first_trajectory_idx + target_idx] + .longitudinal_velocity_mps * + 0.5); + return dist / v; +} + +bool object_is_incoming( + const lanelet::BasicPoint2d & object_position, + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lane) +{ + const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); + if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; + for (const auto & lls : lanelets) + for (const auto & ll : lls) + if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; + return false; +} + +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger) +{ + // skip the dynamic object if it is not in a lane preceding the overlapped lane + // lane changes are intentionally not considered + const auto object_point = lanelet::BasicPoint2d( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; + + const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; + const auto max_lon_deviation = object.shape.dimensions.x / 2.0; + auto worst_enter_time = std::optional(); + auto worst_exit_time = std::optional(); + + for (const auto & predicted_path : object.kinematics.predicted_paths) { + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + if (unique_path_points.size() < 2) continue; + const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); + const auto enter_point = + geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto enter_segment_idx = + motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); + const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( + unique_path_points, enter_segment_idx, enter_point); + const auto enter_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); + const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( + unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); + const auto enter_offset_ratio = enter_offset / enter_segment_length; + const auto enter_time = + static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; + + const auto exit_point = + geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto exit_segment_idx = + motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); + const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( + unique_path_points, exit_segment_idx, exit_point); + const auto exit_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); + const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( + unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); + const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); + const auto exit_time = + static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; + + RCLCPP_DEBUG( + logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, + enter_time, exit_time); + // predicted path is too far from the overlapping range to be relevant + const auto is_far_from_entering_point = enter_lat_dist > max_deviation; + const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; + if (is_far_from_entering_point && is_far_from_exiting_point) { + RCLCPP_DEBUG( + logger, + " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", + is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, + max_deviation); + continue; + } + const auto is_last_predicted_path_point = + (exit_segment_idx + 2 == unique_path_points.size() || + enter_segment_idx + 2 == unique_path_points.size()); + const auto does_not_reach_overlap = + is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); + if (does_not_reach_overlap) { + RCLCPP_DEBUG( + logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", + std::min(exit_offset, enter_offset), max_lon_deviation); + continue; + } + + const auto same_driving_direction_as_ego = enter_time < exit_time; + if (same_driving_direction_as_ego) { + worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; + worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; + } else { + worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; + worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; + } + } + if (worst_enter_time && worst_exit_time) { + RCLCPP_DEBUG( + logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, + *worst_exit_time); + return std::make_pair(*worst_enter_time, *worst_exit_time); + } + RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); + return {}; +} + +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const DecisionInputs & inputs, const rclcpp::Logger & logger) +{ + const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; + const auto object_point = lanelet::BasicPoint2d(p.x, p.y); + const auto half_size = object.shape.dimensions.x / 2.0; + lanelet::ConstLanelets object_lanelets; + for (const auto & ll : inputs.lanelets) + if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) + object_lanelets.push_back(ll); + + geometry_msgs::msg::Pose pose; + pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; + pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; + const auto range_size = std::abs(range_enter_length - range_exit_length); + auto worst_enter_dist = std::optional(); + auto worst_exit_dist = std::optional(); + for (const auto & lane : object_lanelets) { + const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); + RCLCPP_DEBUG( + logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); + if (path) { + lanelet::ConstLanelets lls; + for (const auto & ll : *path) lls.push_back(ll); + pose.position.set__x(object_point.x()).set__y(object_point.y()); + const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; + pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); + const auto enter_dist = + lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; + pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); + const auto exit_dist = + lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; + RCLCPP_DEBUG( + logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, + enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, + range.exiting_point.x(), range.exiting_point.y()); + const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; + if (already_entered_range) continue; + // multiple paths to the overlap -> be conservative and use the "worst" case + // "worst" = min/max arc length depending on if the lane is running opposite to the ego + // trajectory + const auto is_opposite = enter_dist > exit_dist; + if (!worst_enter_dist) + worst_enter_dist = enter_dist; + else if (is_opposite) + worst_enter_dist = std::max(*worst_enter_dist, enter_dist); + else + worst_enter_dist = std::min(*worst_enter_dist, enter_dist); + if (!worst_exit_dist) + worst_exit_dist = exit_dist; + else if (is_opposite) + worst_exit_dist = std::max(*worst_exit_dist, exit_dist); + else + worst_exit_dist = std::min(*worst_exit_dist, exit_dist); + } + } + if (worst_enter_dist && worst_exit_dist) { + const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; + return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); + } + return {}; +} + +bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) +{ + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; +} + +bool intervals_condition( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + const auto opposite_way_condition = [&]() -> bool { + const auto ego_exits_before_object_enters = + range_times.ego.exit_time + params.intervals_ego_buffer < + range_times.object.enter_time + params.intervals_obj_buffer; + RCLCPP_DEBUG( + logger, + "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " + "enter = %d\n", + range_times.ego.exit_time + params.intervals_ego_buffer, + range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); + return ego_exits_before_object_enters; + }; + const auto same_way_condition = [&]() -> bool { + const auto object_enters_during_overlap = + range_times.ego.enter_time - params.intervals_ego_buffer < + range_times.object.enter_time + params.intervals_obj_buffer && + range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < + range_times.ego.exit_time + params.intervals_ego_buffer; + const auto object_exits_during_overlap = + range_times.ego.enter_time - params.intervals_ego_buffer < + range_times.object.exit_time + params.intervals_obj_buffer && + range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < + range_times.ego.exit_time + params.intervals_ego_buffer; + RCLCPP_DEBUG( + logger, + "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " + "not " + "enter = %d\n", + object_enters_during_overlap, object_exits_during_overlap, + object_enters_during_overlap || object_exits_during_overlap); + return object_enters_during_overlap || object_exits_during_overlap; + }; + + const auto object_is_going_same_way = + range_times.object.enter_time < range_times.object.exit_time; + return (object_is_going_same_way && same_way_condition()) || + (!object_is_going_same_way && opposite_way_condition()); +} + +bool ttc_condition( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; + const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; + const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); + const auto ttc_is_bellow_threshold = + std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; + RCLCPP_DEBUG( + logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, + (collision_during_overlap || ttc_is_bellow_threshold)); + return collision_during_overlap || ttc_is_bellow_threshold; +} + +bool will_collide_on_range( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) +{ + RCLCPP_DEBUG( + logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, + range_times.object.exit_time); + return (params.mode == "threshold" && threshold_condition(range_times, params)) || + (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || + (params.mode == "ttc" && ttc_condition(range_times, params, logger)); +} + +bool should_not_enter( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger) +{ + RangeTimes range_times{}; + range_times.ego.enter_time = + time_along_trajectory(inputs.ego_data, range.entering_trajectory_idx, params.ego_min_velocity); + range_times.ego.exit_time = + time_along_trajectory(inputs.ego_data, range.exiting_trajectory_idx, params.ego_min_velocity); + RCLCPP_DEBUG( + logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", + range.entering_trajectory_idx, range.exiting_trajectory_idx, range.lane.id(), + range_times.ego.enter_time, range_times.ego.exit_time); + for (const auto & object : inputs.objects.objects) { + RCLCPP_DEBUG( + logger, "\t\t[%s] going at %2.2fm/s", + tier4_autoware_utils::toHexString(object.object_id).c_str(), + object.kinematics.initial_twist_with_covariance.twist.linear.x); + if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { + RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); + continue; // skip objects with velocity bellow a threshold + } + // skip objects that are already on the interval + const auto enter_exit_time = + params.objects_use_predicted_paths + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_dist_buffer, logger) + : object_time_to_range(object, range, inputs, logger); + if (!enter_exit_time) { + RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); + continue; // skip objects that are not driving towards the overlapping range + } + + range_times.object.enter_time = enter_exit_time->first; + range_times.object.exit_time = enter_exit_time->second; + if (will_collide_on_range(range_times, params, logger)) { + range.debug.times = range_times; + range.debug.object = object; + return true; + } + } + range.debug.times = range_times; + return false; +} + +void set_decision_velocity( + std::optional & decision, const double distance, const PlannerParam & params) +{ + if (distance < params.stop_dist_threshold) { + decision->velocity = 0.0; + } else if (distance < params.slow_dist_threshold) { + decision->velocity = params.slow_velocity; + } else { + decision.reset(); + } +} + +std::optional calculate_decision( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger) +{ + std::optional decision; + if (should_not_enter(range, inputs, params, logger)) { + decision.emplace(); + decision->target_trajectory_idx = inputs.ego_data.first_trajectory_idx + + range.entering_trajectory_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; + const auto ego_dist_to_range = + distance_along_trajectory(inputs.ego_data, range.entering_trajectory_idx); + set_decision_velocity(decision, ego_dist_to_range, params); + } + return decision; +} + +std::vector calculate_decisions( + const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) +{ + std::vector decisions; + for (const auto & range : inputs.ranges) { + if (range.entering_trajectory_idx == 0UL) continue; // skip if we already entered the range + const auto optional_decision = calculate_decision(range, inputs, params, logger); + range.debug.decision = optional_decision; + if (optional_decision) decisions.push_back(*optional_decision); + } + return decisions; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp new file mode 100644 index 0000000000000..2aa3a8b490bf6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DECISIONS_HPP_ +#define DECISIONS_HPP_ + +#include "types.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief calculate the distance along the ego trajectory between ego and some target trajectory +/// index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @return distance between ego and the target [m] +double distance_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief estimate the time when ego will reach some target trajectory index +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] target_idx target ego trajectory index +/// @param [in] min_velocity minimum ego velocity used to estimate the time +/// @return time taken by ego to reach the target [s] +double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); +/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit +/// points of an overlapping range +/// @details times when the predicted paths of the object enters/exits the range are calculated +/// but may not exist (e.g,, predicted path ends before reaching the end of the range) +/// @param [in] object dynamic object +/// @param [in] range overlapping range +/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] logger ros logger +/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in +/// the opposite direction, time at enter > time at exit +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger); +/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit +/// points of an overlapping range +/// @param [in] object dynamic object +/// @param [in] range overlapping range +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range +/// @param [in] logger ros logger +/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in +/// the opposite direction, time at enter > time at exit. +std::optional> object_time_to_range( + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const DecisionInputs & inputs, const rclcpp::Logger & logger); +/// @brief decide whether an object is coming in the range at the same time as ego +/// @details the condition depends on the mode (threshold, intervals, ttc) +/// @param [in] range_times times when ego and the object enter/exit the range +/// @param [in] params parameters +/// @param [in] logger ros logger +bool will_collide_on_range( + const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); +/// @brief check whether we should avoid entering the given range +/// @param [in] range the range to check +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return true if we should avoid entering the range +bool should_not_enter( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger); +/// @brief set the velocity of a decision (or unset it) based on the distance away from the range +/// @param [out] decision decision to update (either set its velocity or unset the decision) +/// @param [in] distance distance between ego and the range corresponding to the decision +/// @param [in] params parameters +void set_decision_velocity( + std::optional & decision, const double distance, const PlannerParam & params); +/// @brief calculate the decision to slowdown or stop before an overlapping range +/// @param [in] range the range to check +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return an optional decision to slowdown or stop +std::optional calculate_decision( + const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, + const rclcpp::Logger & logger); +/// @brief calculate decisions to slowdown or stop before some overlapping ranges +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] params parameters +/// @param [in] logger ros logger +/// @return return the calculated decisions to slowdown or stop +std::vector calculate_decisions( + const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // DECISIONS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..31631fb577b09 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,150 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + if (predicted_path.path.empty() || stop_line.size() < 2) return; + + auto stop_line_idx = 0UL; + bool found = false; + lanelet::BasicSegment2d path_segment; + path_segment.first.x() = predicted_path.path.front().position.x; + path_segment.first.y() = predicted_path.path.front().position.y; + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; + if (boost::geometry::intersects(stop_line, path_segment)) { + found = true; + break; + } + path_segment.first = path_segment.second; + } + if (found) { + auto cut_idx = stop_line_idx; + double arc_length = 0; + while (cut_idx > 0 && arc_length < object_front_overhang) { + arc_length += tier4_autoware_utils::calcDistance2d( + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); + --cut_idx; + } + predicted_path.path.resize(cut_idx); + } +} + +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr planner_data) +{ + lanelet::ConstLanelets lanelets; + lanelet::BasicLineString2d query_line; + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); + const auto query_results = lanelet::geometry::findWithin2d( + planner_data->route_handler->getLaneletMapPtr()->laneletLayer, query_line); + for (const auto & r : query_results) lanelets.push_back(r.second); + for (const auto & ll : lanelets) { + for (const auto & element : ll.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->get_traffic_signal(element->id()); + if ( + traffic_signal_stamped.has_value() && element->stopLine().has_value() && + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { + lanelet::BasicLineString2d stop_line; + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); + return stop_line; + } + } + } + return std::nullopt; +} + +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr planner_data, const double object_front_overhang) +{ + const auto stop_line = find_next_stop_line(predicted_path, planner_data); + if (stop_line) { + // we use a longer stop line to also cut predicted paths that slightly go around the stop line + auto longer_stop_line = *stop_line; + const auto diff = stop_line->back() - stop_line->front(); + longer_stop_line.front() -= diff * 0.5; + longer_stop_line.back() += diff * 0.5; + cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang); + } +} + +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = planner_data->predicted_objects->header; + for (const auto & object : planner_data->predicted_objects->objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + if (no_overlap_path.size() <= 1) return true; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, planner_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); + } + + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp new file mode 100644 index 0000000000000..0770fbc0dcff1 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -0,0 +1,60 @@ +// Copyright 2024-2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FILTER_PREDICTED_OBJECTS_HPP_ +#define FILTER_PREDICTED_OBJECTS_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief cut a predicted path beyond the given stop line +/// @param [inout] predicted_path predicted path to cut +/// @param [in] stop_line stop line used for cutting +/// @param [in] object_front_overhang extra distance to cut ahead of the stop line +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); + +/// @brief find the next red light stop line along the given path +/// @param [in] path predicted path to check for a stop line +/// @param [in] planner_data planner data with stop line information +/// @return the first red light stop line found along the path (if any) +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const std::shared_ptr planner_data); + +/// @brief cut predicted path beyond stop lines of red lights +/// @param [inout] predicted_path predicted path to cut +/// @param [in] planner_data planner data to get the map and traffic light information +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const std::shared_ptr planner_data, const double object_front_overhang); + +/// @brief filter predicted objects and their predicted paths +/// @param [in] planner_data planner data +/// @param [in] ego_data ego data +/// @param [in] params parameters +/// @return filtered predicted objects +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const std::shared_ptr planner_data, const EgoData & ego_data, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp new file mode 100644 index 0000000000000..824a847b17414 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -0,0 +1,93 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "footprint.hpp" + +#include + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +tier4_autoware_utils::Polygon2d make_base_footprint( + const PlannerParam & p, const bool ignore_offset) +{ + tier4_autoware_utils::Polygon2d base_footprint; + const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; + const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; + const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; + const auto left_offset = ignore_offset ? 0.0 : p.extra_left_offset; + base_footprint.outer() = { + {p.front_offset + front_offset, p.left_offset + left_offset}, + {p.front_offset + front_offset, p.right_offset - right_offset}, + {p.rear_offset - rear_offset, p.right_offset - right_offset}, + {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + return base_footprint; +} + +lanelet::BasicPolygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +std::vector calculate_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params) +{ + const auto base_footprint = make_base_footprint(params); + std::vector trajectory_footprints; + trajectory_footprints.reserve(ego_data.trajectory_points.size()); + double length = 0.0; + const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + + params.front_offset + params.extra_front_offset; + for (auto i = ego_data.first_trajectory_idx; + i < ego_data.trajectory_points.size() && length < range; ++i) { + const auto & trajectory_pose = ego_data.trajectory_points[i].pose; + const auto angle = tf2::getYaw(trajectory_pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back( + p.x() + trajectory_pose.position.x, p.y() + trajectory_pose.position.y); + trajectory_footprints.push_back(footprint); + if (i + 1 < ego_data.trajectory_points.size()) + length += tier4_autoware_utils::calcDistance2d( + trajectory_pose, ego_data.trajectory_points[i + 1].pose); + } + return trajectory_footprints; +} + +lanelet::BasicPolygon2d calculate_current_ego_footprint( + const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset) +{ + const auto base_footprint = make_base_footprint(params, ignore_offset); + const auto angle = tf2::getYaw(ego_data.pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + lanelet::BasicPolygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); + return footprint; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp new file mode 100644 index 0000000000000..7b80dd9618bb7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace autoware::motion_velocity_planner +{ +namespace out_of_lane +{ +/// @brief create the base footprint of ego +/// @param [in] p parameters used to create the footprint +/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the +/// footprint +/// @return base ego footprint +tier4_autoware_utils::Polygon2d make_base_footprint( + const PlannerParam & p, const bool ignore_offset = false); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +lanelet::BasicPolygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief calculate the trajectory footprints +/// @details the resulting polygon follows the format used by the lanelet library: clockwise order +/// and implicit closing edge +/// @param [in] ego_data data related to the ego vehicle (includes its trajectory) +/// @param [in] params parameters +/// @return polygon footprints for each trajectory point starting from ego's current position +std::vector calculate_trajectory_footprints( + const EgoData & ego_data, const PlannerParam & params); +/// @brief calculate the current ego footprint +/// @param [in] ego_data data related to the ego vehicle +/// @param [in] params parameters +/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the +/// footprint +lanelet::BasicPolygon2d calculate_current_ego_footprint( + const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); +} // namespace out_of_lane +} // namespace autoware::motion_velocity_planner + +#endif // FOOTPRINT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp new file mode 100644 index 0000000000000..23dbf77f08151 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -0,0 +1,126 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lanelets_selection.hpp" + +#include + +#include +#include + +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +lanelet::ConstLanelets consecutive_lanelets( + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets consecutives = route_handler->getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler->getRoutingGraphPtr()->previous(lanelet); + consecutives.insert(consecutives.end(), previous.begin(), previous.end()); + return consecutives; +} + +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler) +{ + lanelet::ConstLanelets missing_lane_change_lanelets; + const auto & routing_graph = *route_handler->getRoutingGraphPtr(); + lanelet::ConstLanelets adjacents; + lanelet::ConstLanelets consecutives; + for (const auto & ll : trajectory_lanelets) { + const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); + std::copy_if( + consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), + [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); + const auto adjacents_of_ll = routing_graph.besides(ll); + std::copy_if( + adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), + [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); + } + std::copy_if( + adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), + [&](const auto & l) { + return !contains_lanelet(missing_lane_change_lanelets, l.id()) && + !contains_lanelet(trajectory_lanelets, l.id()) && + contains_lanelet(consecutives, l.id()); + }); + return missing_lane_change_lanelets; +} + +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets trajectory_lanelets; + lanelet::BasicLineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); + for (const auto & dist_lanelet : + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, trajectory_ls)) { + if (!contains_lanelet(trajectory_lanelets, dist_lanelet.second.id())) + trajectory_lanelets.push_back(dist_lanelet.second); + } + const auto missing_lanelets = + get_missing_lane_change_lanelets(trajectory_lanelets, route_handler); + trajectory_lanelets.insert( + trajectory_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); + return trajectory_lanelets; +} + +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets ignored_lanelets; + // ignore lanelets directly behind ego + const auto behind = + tier4_autoware_utils::calcOffsetPose(ego_data.pose, params.rear_offset, 0.0, 0.0); + const lanelet::BasicPoint2d behind_point(behind.position.x, behind.position.y); + const auto behind_lanelets = lanelet::geometry::findWithin2d( + route_handler->getLaneletMapPtr()->laneletLayer, behind_point, 0.0); + for (const auto & l : behind_lanelets) { + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, l.second.id()); + if (!is_trajectory_lanelet) ignored_lanelets.push_back(l.second); + } + return ignored_lanelets; +} + +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params) +{ + lanelet::ConstLanelets other_lanelets; + const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); + const auto lanelets_within_range = lanelet::geometry::findWithin2d( + route_handler->getLaneletMapPtr()->laneletLayer, ego_point, + std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + + params.extra_front_offset); + for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; + const auto is_trajectory_lanelet = contains_lanelet(trajectory_lanelets, ll.second.id()); + const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); + if (!is_trajectory_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); + } + return other_lanelets; +} +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp new file mode 100644 index 0000000000000..c7351f0a98e89 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELETS_SELECTION_HPP_ +#define LANELETS_SELECTION_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +/// @brief checks if a lanelet is already contained in a vector of lanelets +/// @param [in] lanelets vector to check +/// @param [in] id lanelet id to check +/// @return true if the given vector contains a lanelet of the given id +inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lanelet::Id id) +{ + return std::find_if(lanelets.begin(), lanelets.end(), [&](const auto & l) { + return l.id() == id; + }) != lanelets.end(); +}; + +/// @brief calculate lanelets crossed by the ego trajectory +/// @details calculated from the ids of the trajectory msg, the lanelets containing trajectory +/// points +/// @param [in] ego_data data about the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets crossed by the ego vehicle +lanelet::ConstLanelets calculate_trajectory_lanelets( + const EgoData & ego_data, const std::shared_ptr route_handler); +/// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during +/// a lane change +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets that may be overlapped by a lane change (and are not already in +/// trajectory_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler); +/// @brief calculate lanelets that should be ignored +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @param [in] params parameters +/// @return lanelets to ignore +lanelet::ConstLanelets calculate_ignored_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +/// @brief calculate lanelets that should be checked by the module +/// @param [in] ego_data data about the ego vehicle +/// @param [in] trajectory_lanelets lanelets driven by the ego vehicle +/// @param [in] ignored_lanelets lanelets to ignore +/// @param [in] route_handler route handler +/// @param [in] params parameters +/// @return lanelets to check for overlaps +lanelet::ConstLanelets calculate_other_lanelets( + const EgoData & ego_data, const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets, + const std::shared_ptr route_handler, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp new file mode 100644 index 0000000000000..b662186049342 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -0,0 +1,305 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "out_of_lane_module.hpp" + +#include "calculate_slowdown_points.hpp" +#include "debug.hpp" +#include "decisions.hpp" +#include "filter_predicted_objects.hpp" +#include "footprint.hpp" +#include "lanelets_selection.hpp" +#include "overlapping_range.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) +{ + module_name_ = module_name; + logger_ = node.get_logger(); + clock_ = node.get_clock(); + init_parameters(node); + velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + debug_publisher_ = + node.create_publisher("~/" + ns_ + "/debug_markers", 1); + virtual_wall_publisher_ = + node.create_publisher("~/" + ns_ + "/virtual_walls", 1); +} +void OutOfLaneModule::init_parameters(rclcpp::Node & node) +{ + using tier4_autoware_utils::getOrDeclareParameter; + auto & pp = params_; + + pp.mode = getOrDeclareParameter(node, ns_ + ".mode"); + pp.skip_if_already_overlapping = + getOrDeclareParameter(node, ns_ + ".skip_if_already_overlapping"); + + pp.time_threshold = getOrDeclareParameter(node, ns_ + ".threshold.time_threshold"); + pp.intervals_ego_buffer = getOrDeclareParameter(node, ns_ + ".intervals.ego_time_buffer"); + pp.intervals_obj_buffer = + getOrDeclareParameter(node, ns_ + ".intervals.objects_time_buffer"); + pp.ttc_threshold = getOrDeclareParameter(node, ns_ + ".ttc.threshold"); + + pp.objects_min_vel = getOrDeclareParameter(node, ns_ + ".objects.minimum_velocity"); + pp.objects_use_predicted_paths = + getOrDeclareParameter(node, ns_ + ".objects.use_predicted_paths"); + pp.objects_min_confidence = + getOrDeclareParameter(node, ns_ + ".objects.predicted_path_min_confidence"); + pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); + pp.objects_cut_predicted_paths_beyond_red_lights = + getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); + + pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); + pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); + + pp.skip_if_over_max_decel = + getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); + pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); + pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); + pp.dist_buffer = getOrDeclareParameter(node, ns_ + ".action.distance_buffer"); + pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); + pp.slow_dist_threshold = + getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); + pp.stop_dist_threshold = + getOrDeclareParameter(node, ns_ + ".action.stop.distance_threshold"); + + pp.ego_min_velocity = getOrDeclareParameter(node, ns_ + ".ego.min_assumed_velocity"); + pp.extra_front_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_front_offset"); + pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); + pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); + pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.front_offset = vehicle_info.max_longitudinal_offset_m; + pp.rear_offset = vehicle_info.min_longitudinal_offset_m; + pp.left_offset = vehicle_info.max_lateral_offset_m; + pp.right_offset = vehicle_info.min_lateral_offset_m; +} + +void OutOfLaneModule::update_parameters(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + auto & pp = params_; + updateParam(parameters, ns_ + ".mode", pp.mode); + updateParam(parameters, ns_ + ".skip_if_already_overlapping", pp.skip_if_already_overlapping); + + updateParam(parameters, ns_ + ".threshold.time_threshold", pp.time_threshold); + updateParam(parameters, ns_ + ".intervals.ego_time_buffer", pp.intervals_ego_buffer); + updateParam(parameters, ns_ + ".intervals.objects_time_buffer", pp.intervals_obj_buffer); + updateParam(parameters, ns_ + ".ttc.threshold", pp.ttc_threshold); + + updateParam(parameters, ns_ + ".objects.minimum_velocity", pp.objects_min_vel); + updateParam(parameters, ns_ + ".objects.use_predicted_paths", pp.objects_use_predicted_paths); + updateParam( + parameters, ns_ + ".objects.predicted_path_min_confidence", pp.objects_min_confidence); + updateParam(parameters, ns_ + ".objects.distance_buffer", pp.objects_dist_buffer); + updateParam( + parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", + pp.objects_cut_predicted_paths_beyond_red_lights); + + updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); + updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); + + updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); + updateParam(parameters, ns_ + ".action.precision", pp.precision); + updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); + updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); + updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); + updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); + + updateParam(parameters, ns_ + ".ego.min_assumed_velocity", pp.ego_min_velocity); + updateParam(parameters, ns_ + ".ego.extra_front_offset", pp.extra_front_offset); + updateParam(parameters, ns_ + ".ego.extra_rear_offset", pp.extra_rear_offset); + updateParam(parameters, ns_ + ".ego.extra_left_offset", pp.extra_left_offset); + updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset); +} + +VelocityPlanningResult OutOfLaneModule::plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + VelocityPlanningResult result; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + out_of_lane::EgoData ego_data; + ego_data.pose = planner_data->current_odometry->pose; + ego_data.trajectory_points = ego_trajectory_points; + ego_data.first_trajectory_idx = + motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.velocity = planner_data->current_velocity->twist.linear.x; + ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); + stopwatch.tic("calculate_trajectory_footprints"); + const auto current_ego_footprint = + out_of_lane::calculate_current_ego_footprint(ego_data, params_, true); + const auto trajectory_footprints = + out_of_lane::calculate_trajectory_footprints(ego_data, params_); + const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); + // Calculate lanelets to ignore and consider + stopwatch.tic("calculate_lanelets"); + const auto trajectory_lanelets = + out_of_lane::calculate_trajectory_lanelets(ego_data, planner_data->route_handler); + const auto ignored_lanelets = out_of_lane::calculate_ignored_lanelets( + ego_data, trajectory_lanelets, planner_data->route_handler, params_); + const auto other_lanelets = out_of_lane::calculate_other_lanelets( + ego_data, trajectory_lanelets, ignored_lanelets, planner_data->route_handler, params_); + const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); + + debug_data_.reset_data(); + debug_data_.trajectory_points = ego_trajectory_points; + debug_data_.footprints = trajectory_footprints; + debug_data_.trajectory_lanelets = trajectory_lanelets; + debug_data_.ignored_lanelets = ignored_lanelets; + debug_data_.other_lanelets = other_lanelets; + debug_data_.first_trajectory_idx = ego_data.first_trajectory_idx; + + if (params_.skip_if_already_overlapping) { + debug_data_.current_footprint = current_ego_footprint; + const auto overlapped_lanelet_it = + std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { + return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); + }); + if (overlapped_lanelet_it != other_lanelets.end()) { + debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); + RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n"); + return result; + } + } + // Calculate overlapping ranges + stopwatch.tic("calculate_overlapping_ranges"); + const auto ranges = out_of_lane::calculate_overlapping_ranges( + trajectory_footprints, trajectory_lanelets, other_lanelets, params_); + const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); + // Calculate stop and slowdown points + out_of_lane::DecisionInputs inputs; + inputs.ranges = ranges; + inputs.ego_data = ego_data; + stopwatch.tic("filter_predicted_objects"); + inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); + const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); + inputs.route_handler = planner_data->route_handler; + inputs.lanelets = other_lanelets; + stopwatch.tic("calculate_decisions"); + const auto decisions = out_of_lane::calculate_decisions(inputs, params_, logger_); + const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); + stopwatch.tic("calc_slowdown_points"); + if ( // reset the previous inserted point if the timer expired + prev_inserted_point_ && + (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) + prev_inserted_point_.reset(); + auto point_to_insert = + out_of_lane::calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); + const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); + stopwatch.tic("insert_slowdown_points"); + debug_data_.slowdowns.clear(); + if ( // reset the timer if there is no previous inserted point or if we avoid the same lane + point_to_insert && + (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == + point_to_insert->slowdown.lane_to_avoid.id())) + prev_inserted_point_time_ = clock_->now(); + // reuse previous stop point if there is no new one or if its velocity is not higher than the new + // one and its arc length is lower + const auto should_use_prev_inserted_point = [&]() { + if ( + point_to_insert && prev_inserted_point_ && + prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { + const auto arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, point_to_insert->point.pose.position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + return prev_arc_length < arc_length; + } + return !point_to_insert && prev_inserted_point_; + }(); + if (should_use_prev_inserted_point) { + // if the trajectory changed the prev point is no longer on the trajectory so we project it + const auto insert_arc_length = motion_utils::calcSignedArcLength( + ego_trajectory_points, 0LU, prev_inserted_point_->point.pose.position); + prev_inserted_point_->point.pose = + motion_utils::calcInterpolatedPose(ego_trajectory_points, insert_arc_length); + point_to_insert = prev_inserted_point_; + } + if (point_to_insert) { + prev_inserted_point_ = point_to_insert; + RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); + debug_data_.slowdowns = {*point_to_insert}; + if (point_to_insert->slowdown.velocity == 0.0) + result.stop_points.push_back(point_to_insert->point.pose.position); + else + result.slowdown_intervals.emplace_back( + point_to_insert->point.pose.position, point_to_insert->point.pose.position, + point_to_insert->slowdown.velocity); + + const auto is_approaching = motion_utils::calcSignedArcLength( + ego_trajectory_points, ego_data.pose.position, + point_to_insert->point.pose.position) > 0.1 && + ego_data.velocity > 0.1; + const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING + : motion_utils::VelocityFactor::STOPPED; + velocity_factor_interface_.set( + ego_trajectory_points, ego_data.pose, point_to_insert->point.pose, status, "out_of_lane"); + result.velocity_factor = velocity_factor_interface_.get(); + } else if (!decisions.empty()) { + RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); + } + const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); + debug_data_.ranges = inputs.ranges; + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n" + "\tcalculate_lanelets = %2.0fus\n" + "\tcalculate_trajectory_footprints = %2.0fus\n" + "\tcalculate_overlapping_ranges = %2.0fus\n" + "\tfilter_pred_objects = %2.0fus\n" + "\tcalculate_decisions = %2.0fus\n" + "\tcalc_slowdown_points = %2.0fus\n" + "\tinsert_slowdown_points = %2.0fus\n", + total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); + debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); + virtual_wall_marker_creator.add_virtual_walls( + out_of_lane::debug::create_virtual_walls(debug_data_, params_)); + virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + return result; +} + +} // namespace autoware::motion_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::motion_velocity_planner::OutOfLaneModule, + autoware::motion_velocity_planner::PluginModuleInterface) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp new file mode 100644 index 0000000000000..8a20d5c850a09 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OUT_OF_LANE_MODULE_HPP_ +#define OUT_OF_LANE_MODULE_HPP_ + +#include "types.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class OutOfLaneModule : public PluginModuleInterface +{ +public: + void init(rclcpp::Node & node, const std::string & module_name) override; + void update_parameters(const std::vector & parameters) override; + VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) override; + std::string get_module_name() const override { return module_name_; } + +private: + void init_parameters(rclcpp::Node & node); + out_of_lane::PlannerParam params_; + + inline static const std::string ns_ = "out_of_lane"; + std::string module_name_; + std::optional prev_inserted_point_{}; + rclcpp::Clock::SharedPtr clock_{}; + rclcpp::Time prev_inserted_point_time_{}; + +protected: + // Debug + mutable out_of_lane::DebugData debug_data_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // OUT_OF_LANE_MODULE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp new file mode 100644 index 0000000000000..f89c620b75fb6 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -0,0 +1,127 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "overlapping_range.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet) +{ + Overlap overlap; + const auto & left_bound = lanelet.leftBound2d().basicLineString(); + const auto & right_bound = lanelet.rightBound2d().basicLineString(); + // TODO(Maxime): these intersects (for each trajectory footprint, for each lanelet) are too + // expensive + const auto overlap_left = boost::geometry::intersects(trajectory_footprint, left_bound); + const auto overlap_right = boost::geometry::intersects(trajectory_footprint, right_bound); + + lanelet::BasicPolygons2d overlapping_polygons; + if (overlap_left || overlap_right) + boost::geometry::intersection( + trajectory_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); + for (const auto & overlapping_polygon : overlapping_polygons) { + for (const auto & point : overlapping_polygon) { + if (overlap_left && overlap_right) + overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); + else if (overlap_left) + overlap.inside_distance = + std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); + else if (overlap_right) + overlap.inside_distance = + std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); + geometry_msgs::msg::Pose p; + p.position.x = point.x(); + p.position.y = point.y(); + const auto length = lanelet::utils::getArcCoordinates(trajectory_lanelets, p).length; + if (length > overlap.max_arc_length) { + overlap.max_arc_length = length; + overlap.max_overlap_point = point; + } + if (length < overlap.min_arc_length) { + overlap.min_arc_length = length; + overlap.min_overlap_point = point; + } + } + } + return overlap; +} + +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params) +{ + OverlapRanges ranges; + OtherLane other_lane(lanelet); + std::vector overlaps; + for (auto i = 0UL; i < trajectory_footprints.size(); ++i) { + const auto overlap = calculate_overlap(trajectory_footprints[i], trajectory_lanelets, lanelet); + const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; + if (has_overlap) { // open/update the range + overlaps.push_back(overlap); + if (!other_lane.range_is_open) { + other_lane.first_range_bound.index = i; + other_lane.first_range_bound.point = overlap.min_overlap_point; + other_lane.first_range_bound.arc_length = + overlap.min_arc_length - params.overlap_extra_length; + other_lane.first_range_bound.inside_distance = overlap.inside_distance; + other_lane.range_is_open = true; + } + other_lane.last_range_bound.index = i; + other_lane.last_range_bound.point = overlap.max_overlap_point; + other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; + other_lane.last_range_bound.inside_distance = overlap.inside_distance; + } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } + } + // close the range if it is still open + if (other_lane.range_is_open) { + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } + return ranges; +} + +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params) +{ + OverlapRanges ranges; + for (auto & lanelet : lanelets) { + const auto lanelet_ranges = + calculate_overlapping_ranges(trajectory_footprints, trajectory_lanelets, lanelet, params); + ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); + } + return ranges; +} + +} // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp new file mode 100644 index 0000000000000..07c8663ea21bc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OVERLAPPING_RANGE_HPP_ +#define OVERLAPPING_RANGE_HPP_ + +#include "types.hpp" + +#include + +#include + +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ + +/// @brief calculate the overlap between the given footprint and lanelet +/// @param [in] path_footprint footprint used to calculate the overlap +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelet lanelet used to calculate the overlap +/// @return the found overlap between the footprint and the lanelet +Overlap calculate_overlap( + const lanelet::BasicPolygon2d & trajectory_footprint, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet); +/// @brief calculate the overlapping ranges between the trajectory footprints and a lanelet +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelet lanelet used to calculate the overlaps +/// @param [in] params parameters +/// @return the overlapping ranges found between the footprints and the lanelet +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelet & lanelet, + const PlannerParam & params); +/// @brief calculate the overlapping ranges between the trajectory footprints and some lanelets +/// @param [in] trajectory_footprints footprints used to calculate the overlaps +/// @param [in] trajectory_lanelets trajectory lanelets used to calculate arc length along the ego +/// trajectory +/// @param [in] lanelets lanelets used to calculate the overlaps +/// @param [in] params parameters +/// @return the overlapping ranges found between the footprints and the lanelets, sorted by +/// increasing arc length along the trajectory +OverlapRanges calculate_overlapping_ranges( + const std::vector & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, const lanelet::ConstLanelets & lanelets, + const PlannerParam & params); +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp new file mode 100644 index 0000000000000..04f9f1ccac4c2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -0,0 +1,236 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner::out_of_lane +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + std::string mode; // mode used to consider a conflict with an object + bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps + // another lane + + double time_threshold; // [s](mode="threshold") objects time threshold + double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range + double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range + double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object + double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range + + bool objects_use_predicted_paths; // whether to use the objects' predicted paths + bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red + // lights' stop lines + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path + double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in + // the other lane + + double overlap_extra_length; // [m] extra length to add around an overlap range + double overlap_min_dist; // [m] min distance inside another lane to consider an overlap + // action to insert in the trajectory if an object causes a conflict at an overlap + bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel + double dist_buffer; + double slow_velocity; + double slow_dist_threshold; + double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the trajectory + double min_decision_duration; // [s] minimum duration needed a decision can be canceled + // ego dimensions used to create its polygon footprint + double front_offset; // [m] front offset (from vehicle info) + double rear_offset; // [m] rear offset (from vehicle info) + double right_offset; // [m] right offset (from vehicle info) + double left_offset; // [m] left offset (from vehicle info) + double extra_front_offset; // [m] extra front distance + double extra_rear_offset; // [m] extra rear distance + double extra_right_offset; // [m] extra right distance + double extra_left_offset; // [m] extra left distance +}; + +struct EnterExitTimes +{ + double enter_time{}; + double exit_time{}; +}; +struct RangeTimes +{ + EnterExitTimes ego{}; + EnterExitTimes object{}; +}; + +/// @brief action taken by the "out of lane" module +struct Slowdown +{ + size_t target_trajectory_idx{}; // we want to slowdown before this trajectory index + double velocity{}; // desired slow down velocity + lanelet::ConstLanelet lane_to_avoid{}; // we want to slowdown before entering this lane +}; +/// @brief slowdown to insert in a trajectory +struct SlowdownToInsert +{ + Slowdown slowdown{}; + autoware_auto_planning_msgs::msg::TrajectoryPoint point{}; +}; + +/// @brief bound of an overlap range (either the first, or last bound) +struct RangeBound +{ + size_t index{}; + lanelet::BasicPoint2d point{}; + double arc_length{}; + double inside_distance{}; +}; + +/// @brief representation of an overlap between the ego footprint and some other lane +struct Overlap +{ + double inside_distance = 0.0; ///!< distance inside the overlap + double min_arc_length = std::numeric_limits::infinity(); + double max_arc_length = 0.0; + lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length + lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length +}; + +/// @brief range along the trajectory where ego overlaps another lane +struct OverlapRange +{ + lanelet::ConstLanelet lane{}; + size_t entering_trajectory_idx{}; + size_t exiting_trajectory_idx{}; + lanelet::BasicPoint2d entering_point{}; // pose of the overlapping point closest along the lane + lanelet::BasicPoint2d exiting_point{}; // pose of the overlapping point furthest along the lane + double inside_distance{}; // [m] how much ego footprint enters the lane + mutable struct + { + std::vector overlaps{}; + std::optional decision{}; + RangeTimes times{}; + std::optional object{}; + } debug; +}; +using OverlapRanges = std::vector; +/// @brief representation of a lane and its current overlap range +struct OtherLane +{ + bool range_is_open = false; + RangeBound first_range_bound{}; + RangeBound last_range_bound{}; + lanelet::ConstLanelet lanelet{}; + lanelet::BasicPolygon2d polygon{}; + + explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) + { + polygon = lanelet.polygon2d().basicPolygon(); + } + + [[nodiscard]] OverlapRange close_range() + { + OverlapRange range; + range.lane = lanelet; + range.entering_trajectory_idx = first_range_bound.index; + range.entering_point = first_range_bound.point; + range.exiting_trajectory_idx = last_range_bound.index; + range.exiting_point = last_range_bound.point; + range.inside_distance = + std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); + range_is_open = false; + last_range_bound = {}; + return range; + } +}; + +/// @brief data related to the ego vehicle +struct EgoData +{ + std::vector trajectory_points{}; + size_t first_trajectory_idx{}; + double velocity{}; // [m/s] + double max_decel{}; // [m/s²] + geometry_msgs::msg::Pose pose{}; +}; + +/// @brief data needed to make decisions +struct DecisionInputs +{ + OverlapRanges ranges{}; + EgoData ego_data{}; + autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + std::shared_ptr route_handler{}; + lanelet::ConstLanelets lanelets{}; +}; + +/// @brief debug data +struct DebugData +{ + std::vector footprints; + std::vector slowdowns; + geometry_msgs::msg::Pose ego_pose; + OverlapRanges ranges; + lanelet::BasicPolygon2d current_footprint; + lanelet::ConstLanelets current_overlapped_lanelets; + lanelet::ConstLanelets trajectory_lanelets; + lanelet::ConstLanelets ignored_lanelets; + lanelet::ConstLanelets other_lanelets; + std::vector trajectory_points; + size_t first_trajectory_idx; + + size_t prev_footprints = 0; + size_t prev_slowdowns = 0; + size_t prev_ranges = 0; + size_t prev_current_overlapped_lanelets = 0; + size_t prev_ignored_lanelets = 0; + size_t prev_trajectory_lanelets = 0; + size_t prev_other_lanelets = 0; + void reset_data() + { + prev_footprints = footprints.size(); + footprints.clear(); + prev_slowdowns = slowdowns.size(); + slowdowns.clear(); + prev_ranges = ranges.size(); + ranges.clear(); + prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); + current_overlapped_lanelets.clear(); + prev_ignored_lanelets = ignored_lanelets.size(); + ignored_lanelets.clear(); + prev_trajectory_lanelets = trajectory_lanelets.size(); + trajectory_lanelets.clear(); + prev_other_lanelets = other_lanelets.size(); + other_lanelets.clear(); + } +}; + +} // namespace autoware::motion_velocity_planner::out_of_lane + +#endif // TYPES_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp new file mode 100644 index 0000000000000..4961cd064efaf --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/filter_predicted_objects.hpp" + +#include +#include +#include + +#include +#include + +TEST(TestCollisionDistance, CutPredictedPathBeyondLine) +{ + using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + lanelet::BasicLineString2d stop_line; + double object_front_overhang = 0.0; + const auto eps = 1e-9; + + EXPECT_NO_THROW(cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang)); + + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + predicted_path.path.push_back(pose); + pose.position.y = 1.0; + predicted_path.path.push_back(pose); + pose.position.y = 2.0; + predicted_path.path.push_back(pose); + pose.position.y = 3.0; + predicted_path.path.push_back(pose); + + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 4UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } + stop_line = {{-1.0, 2.0}, {1.0, 2.0}}; + cut_predicted_path_beyond_line(predicted_path, stop_line, object_front_overhang); + EXPECT_EQ(predicted_path.path.size(), 2UL); + for (auto i = 0UL; i < predicted_path.path.size(); ++i) { + EXPECT_EQ(predicted_path.path[i].position.x, 0.0); + EXPECT_NEAR(predicted_path.path[i].position.y, static_cast(i), eps); + } +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..f1eb7ff047fdc --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) + +autoware_package() + +# ament_auto_add_library(${PROJECT_NAME}_lib SHARED +# DIRECTORY src +# ) + +ament_auto_package(INSTALL_TO_SHARE + include +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp new file mode 100644 index 0000000000000..84591f9429a66 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -0,0 +1,106 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficSignal signal; +}; +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node) + : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + { + } + + // msgs from callbacks that are used for data-ready + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; + autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + std::shared_ptr route_handler; + + // nearest search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + // other internal data + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; + std::optional external_velocity_limit; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + + // velocity smoother + std::shared_ptr velocity_smoother_{}; + // parameters + vehicle_info_util::VehicleInfo vehicle_info_; + + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional get_traffic_signal( + const lanelet::Id id, const bool keep_last_observation = false) const + { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + return std::make_optional(traffic_light_id_map.at(id)); + } +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp new file mode 100644 index 0000000000000..9011d3fb7e079 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ + +#include "planner_data.hpp" +#include "velocity_planning_result.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ + +class PluginModuleInterface +{ +public: + virtual ~PluginModuleInterface() = default; + virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; + virtual void update_parameters(const std::vector & parameters) = 0; + virtual VelocityPlanningResult plan( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) = 0; + virtual std::string get_module_name() const = 0; + motion_utils::VelocityFactorInterface velocity_factor_interface_; + rclcpp::Logger logger_ = rclcpp::get_logger(""); + rclcpp::Publisher::SharedPtr debug_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; +}; + +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp new file mode 100644 index 0000000000000..9b011b74503f2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/velocity_planning_result.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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 AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ +#define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +struct SlowdownInterval +{ + SlowdownInterval( + const geometry_msgs::msg::Point & from_, const geometry_msgs::msg::Point & to_, + const double vel) + : from{from_}, to{to_}, velocity{vel} + { + } + geometry_msgs::msg::Point from{}; + geometry_msgs::msg::Point to{}; + double velocity{}; +}; +struct VelocityPlanningResult +{ + std::vector stop_points{}; + std::vector slowdown_intervals{}; + std::optional velocity_factor{}; +}; +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml new file mode 100644 index 0000000000000..ad791d247ee11 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -0,0 +1,40 @@ + + + + autoware_motion_velocity_planner_common + 0.1.0 + Common functions and interfaces for motion_velocity_planner modules + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + motion_velocity_smoother + rclcpp + route_handler + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt new file mode 100644 index 0000000000000..0af4da6fd4426 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_velocity_planner_node) + +find_package(autoware_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "srv/LoadPlugin.srv" + "srv/UnloadPlugin.srv" + DEPENDENCIES +) + +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + DIRECTORY src +) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "autoware::motion_velocity_planner::MotionVelocityPlannerNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_lib + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") +endif() + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_node_interface.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME}_lib + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md new file mode 100644 index 0000000000000..f8c5eb172abd2 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -0,0 +1,58 @@ +# Motion Velocity Planner + +## Overview + +`motion_velocity_planner` is a planner to adjust the trajectory velocity based on the obstacles around the vehicle. +It loads modules as plugins. Please refer to the links listed below for detail on each module. + +![Architecture](./docs/MotionVelocityPlanner-InternalInterface.drawio.svg) + +- [Out of Lane](../autoware_motion_velocity_out_of_lane_module/README.md) + +Each module calculates stop and slow down points to be inserted in the ego trajectory. +These points are assumed to correspond to the `base_link` frame of the ego vehicle as it follows the trajectory. +This means that to stop before a wall, a stop point is inserted in the trajectory at a distance ahead of the wall equal to the vehicle front offset (wheelbase + front overhang, see the [vehicle dimensions](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/). + +![set_stop_velocity](./docs/set_stop_velocity.drawio.svg) + +## Input topics + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | +| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | + +## Output topics + +| Name | Type | Description | +| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | + +## Services + +| Name | Type | Description | +| ------------------------- | -------------------------------------------------------- | ---------------------------- | +| `~/service/load_plugin` | autoware_motion_velocity_planner_node::srv::LoadPlugin | To request loading a plugin | +| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin | + +## Node parameters + +| Parameter | Type | Description | +| ---------------- | ---------------- | ---------------------- | +| `launch_modules` | vector\ | module names to launch | + +In addition, the following parameters should be provided to the node: + +- [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml); +- [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml); +- [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml); +- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/motion_velocity_smoother/#parameters) +- Parameters of each plugin that will be loaded. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml new file mode 100644 index 0000000000000..5b2fea537d4f7 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg new file mode 100644 index 0000000000000..ea3b7ad99d4e5 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg @@ -0,0 +1,538 @@ + + + + + + + + + + + + + + + +
+
+
+ ~/input/dynamic_objects +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/vehicle_odometry +
+
+
+ + + + + + + + + + + + + +
+
+
+ ~/input/trajectory +
+
+
+ + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + Motion Velocity Planner + + + + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + + + + +
+
+
+ + Updated +
+ Trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+
Stop/Slowdown
+
+ points +
+
+
+
+
+ + + + + + + + + + + + + +
+
+
+ Data +
+
+
+ + + + + + + + + + + + +
+
+
+ Planner Data +
+
+
+ + + + + + + + + + + + + Loaded Modules + + + + + + + + + + out_of_lane + + + + + + + + + + obstacle_velocity_limiter + + + + + + + + + + dynamic_obstacle_stop + + + + + + + + + +
+
+
...
+
+
+ + + + + + + + + + + + +
+
+
+ Smooth Velocity +
+
+
+ + + + + + + + + + + + + obstacle_stop + + + + + + + + + + +
+
+
+ ~/input/vector_map +
+
+
+ + + + + + + + + + + + +
+
+
+ + ~/output/trajectory + +
+
+
+ + + + + + + + + + + + + +
+
+
+ ... +
+
+
+ + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg new file mode 100644 index 0000000000000..5ae7d7dbb5461 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg @@ -0,0 +1,197 @@ + + + + + + + + + + + +
+
+
+ +

+ + stop point + +

+ +
+
+
+ + stop point + + + + + + + + + + + +
+
+
+ +

+ + v + +

+ +
+
+
+ + v + + + + + + +
+
+
+ +

+ + x + +

+ +
+
+
+ + x + + + + + + +
+
+
+ +

+ + baselink to front + +

+ +
+
+
+ + baselink to front + + + + + + + + + + + + + + +
+
+
+ +

+ + output velocity + +

+ +
+
+
+ + output velocity + + + + + + + +
+
+
+ +

+ + reference velocity + +

+ +
+
+
+ + reference velocity + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml new file mode 100644 index 0000000000000..3732d86ef380a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml new file mode 100644 index 0000000000000..2b52610c0276a --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -0,0 +1,57 @@ + + + + autoware_motion_velocity_planner_node + 0.1.0 + Node of the motion_velocity_planner + + Maxime Clement + + Apache License 2.0 + + Maxime Clement + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + rosidl_default_generators + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_motion_velocity_planner_common + diagnostic_msgs + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + motion_velocity_smoother + pcl_conversions + pluginlib + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + rosidl_default_runtime + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_motion_velocity_out_of_lane_module + autoware_planning_test_manager + + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json new file mode 100644 index 0000000000000..7db22e5e39d17 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for the Motion Velocity Planner", + "type": "object", + "definitions": { + "motion_velocity_planner": { + "type": "object", + "properties": { + "smooth_velocity_before_planning": { + "type": "boolean", + "default": true, + "description": "if true, smooth the velocity profile of the input trajectory before planning" + } + }, + "required": ["smooth_velocity_before_planning"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/motion_velocity_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp new file mode 100644 index 0000000000000..c0373678270dd --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -0,0 +1,458 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "node.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace +{ +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} +} // namespace + +namespace autoware::motion_velocity_planner +{ +MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("motion_velocity_planner_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + planner_data_(*this) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Subscribers + sub_trajectory_ = this->create_subscription( + "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), + create_subscription_options(this)); + sub_predicted_objects_ = + this->create_subscription( + "~/input/dynamic_objects", 1, + std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), + create_subscription_options(this)); + sub_no_ground_pointcloud_ = this->create_subscription( + "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1), + create_subscription_options(this)); + sub_vehicle_odometry_ = this->create_subscription( + "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1), + create_subscription_options(this)); + sub_acceleration_ = this->create_subscription( + "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), + create_subscription_options(this)); + sub_lanelet_map_ = this->create_subscription( + "~/input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), + create_subscription_options(this)); + sub_traffic_signals_ = + this->create_subscription( + "~/input/traffic_signals", 1, + std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), + create_subscription_options(this)); + sub_virtual_traffic_light_states_ = + this->create_subscription( + "~/input/virtual_traffic_light_states", 1, + std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), + create_subscription_options(this)); + sub_occupancy_grid_ = this->create_subscription( + "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1), + create_subscription_options(this)); + + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&MotionVelocityPlannerNode::on_unload_plugin, this, _1, _2)); + + // Publishers + trajectory_pub_ = + this->create_publisher("~/output/trajectory", 1); + velocity_factor_publisher_ = + this->create_publisher( + "~/output/velocity_factors", 1); + + // Parameters + smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); + // nearest search + planner_data_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + // set velocity smoother param + set_velocity_smoother_params(); + + // Initialize PlannerManager + for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } + planner_manager_.load_module_plugin(*this, name); + } + + set_param_callback_ = this->add_on_set_parameters_callback( + std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); +} + +void MotionVelocityPlannerNode::on_load_plugin( + const LoadPlugin::Request::SharedPtr request, + [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.load_module_plugin(*this, request->plugin_name); +} + +void MotionVelocityPlannerNode::on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.unload_module_plugin(*this, request->plugin_name); +} + +// NOTE: argument planner_data must not be referenced for multithreading +bool MotionVelocityPlannerNode::is_data_ready() const +{ + const auto & d = planner_data_; + auto clock = *get_clock(); + const auto check_with_msg = [&](const auto ptr, const auto & msg) { + constexpr auto throttle_duration = 3000; // [ms] + if (!ptr) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg); + return false; + } + return true; + }; + + return check_with_msg(d.current_odometry, "Waiting for current odometry") && + check_with_msg(d.current_velocity, "Waiting for current velocity") && + check_with_msg(d.current_acceleration, "Waiting for current acceleration") && + check_with_msg(d.predicted_objects, "Waiting for predicted objects") && + check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") && + check_with_msg(map_ptr_, "Waiting for the map") && + check_with_msg( + d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") && + check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid"); +} + +void MotionVelocityPlannerNode::on_occupancy_grid( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.occupancy_grid = msg; +} + +void MotionVelocityPlannerNode::on_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.predicted_objects = msg; +} + +void MotionVelocityPlannerNode::on_no_ground_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); + return; + } + + pcl::PointCloud pc; + pcl::fromROSMsg(*msg, pc); + + Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); + pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); + if (!pc.empty()) { + tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + } + + { + std::lock_guard lock(mutex_); + planner_data_.no_ground_pointcloud = pc_transformed; + } +} + +void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + auto current_odometry = std::make_shared(); + current_odometry->header = msg->header; + current_odometry->pose = msg->pose.pose; + planner_data_.current_odometry = current_odometry; + + auto current_velocity = std::make_shared(); + current_velocity->header = msg->header; + current_velocity->twist = msg->twist.twist; + planner_data_.current_velocity = current_velocity; +} + +void MotionVelocityPlannerNode::on_acceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.current_acceleration = msg; +} + +void MotionVelocityPlannerNode::set_velocity_smoother_params() +{ + planner_data_.velocity_smoother_ = + std::make_shared(*this); +} + +void MotionVelocityPlannerNode::on_lanelet_map( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + map_ptr_ = msg; + has_received_map_ = true; +} + +void MotionVelocityPlannerNode::on_traffic_signals( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); + for (const auto & signal : msg->signals) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } + } +} + +void MotionVelocityPlannerNode::on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.virtual_traffic_light_states = msg; +} + +void MotionVelocityPlannerNode::on_trajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) +{ + std::unique_lock lk(mutex_); + + if (!is_data_ready()) { + return; + } + + if (input_trajectory_msg->points.empty()) { + RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); + return; + } + + if (has_received_map_) { + planner_data_.route_handler = std::make_shared(*map_ptr_); + has_received_map_ = false; + } + + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ + input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()}; + + auto output_trajectory_msg = generate_trajectory(input_trajectory_points); + output_trajectory_msg.header = input_trajectory_msg->header; + + lk.unlock(); + + trajectory_pub_->publish(output_trajectory_msg); + published_time_publisher_->publish_if_subscribed( + trajectory_pub_, output_trajectory_msg.header.stamp); +} + +void MotionVelocityPlannerNode::insert_stop( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const +{ + const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); + const auto insert_idx = motion_utils::insertTargetPoint(seg_idx, stop_point, trajectory.points); + if (insert_idx) { + for (auto idx = *insert_idx; idx < trajectory.points.size(); ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert stop point"); + } +} + +void MotionVelocityPlannerNode::insert_slowdown( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const +{ + const auto from_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.from); + const auto from_insert_idx = + motion_utils::insertTargetPoint(from_seg_idx, slowdown_interval.from, trajectory.points); + const auto to_seg_idx = + motion_utils::findNearestSegmentIndex(trajectory.points, slowdown_interval.to); + const auto to_insert_idx = + motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); + if (from_insert_idx && to_insert_idx) { + for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) + trajectory.points[idx].longitudinal_velocity_mps = 0.0; + } else { + RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); + } +} + +autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const +{ + const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose; + const double v0 = planner_data.current_velocity->twist.linear.x; + const double a0 = planner_data.current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data.external_velocity_limit; + const auto & smoother = planner_data.velocity_smoother_; + + const auto traj_lateral_acc_filtered = + smoother->applyLateralAccelerationFilter(trajectory_points); + + const auto traj_steering_rate_limited = + smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false); + + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = smoother->resampleTrajectory( + traj_steering_rate_limited, v0, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_resampled, current_pose, planner_data.ego_nearest_dist_threshold, + planner_data.ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + autoware::motion_velocity_planner::TrajectoryPoints clipped; + autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + RCLCPP_ERROR(get_logger(), "failed to smooth"); + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); + } + return traj_smoothed; +} + +autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) +{ + autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg; + output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; + if (smooth_velocity_before_planning_) + input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); + + const auto planning_results = planner_manager_.plan_velocities( + input_trajectory_points, std::make_shared(planner_data_)); + + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; + velocity_factors.header.frame_id = "map"; + velocity_factors.header.stamp = get_clock()->now(); + + for (const auto & planning_result : planning_results) { + for (const auto & stop_point : planning_result.stop_points) + insert_stop(output_trajectory_msg, stop_point); + for (const auto & slowdown_interval : planning_result.slowdown_intervals) + insert_slowdown(output_trajectory_msg, slowdown_interval); + if (planning_result.velocity_factor) + velocity_factors.factors.push_back(*planning_result.velocity_factor); + } + + velocity_factor_publisher_->publish(velocity_factors); + return output_trajectory_msg; +} + +rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + { + std::unique_lock lk(mutex_); // for planner_manager_ + planner_manager_.update_module_parameters(parameters); + } + + updateParam(parameters, "smooth_velocity_before_planning", smooth_velocity_before_planning_); + updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); + updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); + + set_velocity_smoother_params(); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} +} // namespace autoware::motion_velocity_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion_velocity_planner::MotionVelocityPlannerNode) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp new file mode 100644 index 0000000000000..ecf128bf9a012 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -0,0 +1,140 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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 NODE_HPP_ +#define NODE_HPP_ + +#include "planner_manager.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_motion_velocity_planner_node::srv::LoadPlugin; +using autoware_motion_velocity_planner_node::srv::UnloadPlugin; +using TrajectoryPoints = std::vector; + +class MotionVelocityPlannerNode : public rclcpp::Node +{ +public: + explicit MotionVelocityPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // subscriber + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_objects_; + rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; + rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + rclcpp::Subscription::SharedPtr sub_acceleration_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_virtual_traffic_light_states_; + rclcpp::Subscription::SharedPtr sub_occupancy_grid_; + + void on_trajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); + void on_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_traffic_signals( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void on_virtual_traffic_light_states( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); + void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + + // publishers + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr + velocity_factor_publisher_; + + // parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; + bool smooth_velocity_before_planning_{}; + /// @brief set parameters of the velocity smoother + void set_velocity_smoother_params(); + + // members + PlannerData planner_data_; + MotionVelocityPlannerManager planner_manager_; + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + bool has_received_map_ = false; + + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void on_unload_plugin( + const UnloadPlugin::Request::SharedPtr request, + const UnloadPlugin::Response::SharedPtr response); + void on_load_plugin( + const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + rcl_interfaces::msg::SetParametersResult on_set_param( + const std::vector & parameters); + + // mutex for planner_data_ + std::mutex mutex_; + + // function + bool is_data_ready() const; + void insert_stop( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & stop_point) const; + void insert_slowdown( + autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const; + autoware::motion_velocity_planner::TrajectoryPoints smooth_trajectory( + const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, + const autoware::motion_velocity_planner::PlannerData & planner_data) const; + autoware_auto_planning_msgs::msg::Trajectory generate_trajectory( + autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // NODE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp new file mode 100644 index 0000000000000..fa3a3b1ae5dcb --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planner_manager.hpp" + +#include + +#include +#include + +namespace autoware::motion_velocity_planner +{ + +MotionVelocityPlannerManager::MotionVelocityPlannerManager() +: plugin_loader_( + "autoware_motion_velocity_planner_node", + "autoware::motion_velocity_planner::PluginModuleInterface") +{ +} + +void MotionVelocityPlannerManager::load_module_plugin(rclcpp::Node & node, const std::string & name) +{ + // Check if the plugin is already loaded. + if (plugin_loader_.isClassLoaded(name)) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node, name); + + // register + loaded_plugins_.push_back(plugin); + RCLCPP_DEBUG_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } +} + +void MotionVelocityPlannerManager::unload_module_plugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if(loaded_plugins_.begin(), loaded_plugins_.end(), [&](const auto plugin) { + return plugin->get_module_name() == name; + }); + + if (it == loaded_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + loaded_plugins_.erase(it, loaded_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + +void MotionVelocityPlannerManager::update_module_parameters( + const std::vector & parameters) +{ + for (auto & plugin : loaded_plugins_) plugin->update_parameters(parameters); +} + +std::vector MotionVelocityPlannerManager::plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data) +{ + std::vector results; + for (auto & plugin : loaded_plugins_) + results.push_back(plugin->plan(ego_trajectory_points, planner_data)); + return results; +} +} // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp new file mode 100644 index 0000000000000..a5e330535fc73 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// 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 PLANNER_MANAGER_HPP_ +#define PLANNER_MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::motion_velocity_planner +{ +class MotionVelocityPlannerManager +{ +public: + MotionVelocityPlannerManager(); + void load_module_plugin(rclcpp::Node & node, const std::string & name); + void unload_module_plugin(rclcpp::Node & node, const std::string & name); + void update_module_parameters(const std::vector & parameters); + std::vector plan_velocities( + const std::vector & ego_trajectory_points, + const std::shared_ptr planner_data); + +private: + pluginlib::ClassLoader plugin_loader_; + std::vector> loaded_plugins_; +}; +} // namespace autoware::motion_velocity_planner + +#endif // PLANNER_MANAGER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv new file mode 100644 index 0000000000000..7b9f08ab0ded8 --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp new file mode 100644 index 0000000000000..3ad5bab8e070b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "node.hpp" + +#include +#include +#include + +#include + +#include + +using autoware::motion_velocity_planner::MotionVelocityPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: motion_velocity_planner → test_node_ + test_manager->setTrajectorySubscriber("motion_velocity_planner_node/output/trajectory"); + + // set motion_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("motion_velocity_planner_node/input/trajectory"); + + test_manager->setInitialPoseTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("motion_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto motion_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); + const auto motion_velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + + const auto get_motion_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_motion_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector module_names; + module_names.emplace_back("autoware::motion_velocity_planner::OutOfLaneModule"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", + motion_velocity_smoother_dir + "/config/Analytical.param.yaml", + motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", + get_motion_velocity_module_config("out_of_lane")}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "motion_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "motion_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "motion_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "motion_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "motion_velocity_planner_node/input/traffic_signals"); + test_manager->publishVirtualTrafficLightState( + test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); + test_manager->publishOccupancyGrid( + test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); + + // make sure motion_velocity_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromTrajectory(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 887e1167a7c3e..c40bfac40a135 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -85,7 +85,8 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/velocity_factors/surround_obstacle", "/planning/velocity_factors/traffic_light", "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway"}; + "/planning/velocity_factors/walkway", + "/planning/velocity_factors/motion_velocity_planner"}; std::vector steering_factor_topics = { "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", From 0e97466fe6b4d389658ef64a7c4334890075f58c Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Fri, 31 May 2024 01:40:53 +0900 Subject: [PATCH 058/120] refactor(ci): rename docker image from `autoware-openadk` to `autoware` (#7179) Signed-off-by: Yutaka Kondo --- .github/workflows/build-and-test-arm64.yaml | 2 +- .github/workflows/build-and-test-differential-arm64.yaml | 2 +- .github/workflows/build-and-test-differential.yaml | 4 ++-- .github/workflows/build-and-test.yaml | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-and-test-arm64.yaml b/.github/workflows/build-and-test-arm64.yaml index a5e00496cc50a..c9a4b46874e18 100644 --- a/.github/workflows/build-and-test-arm64.yaml +++ b/.github/workflows/build-and-test-arm64.yaml @@ -19,7 +19,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/build-and-test-differential-arm64.yaml b/.github/workflows/build-and-test-differential-arm64.yaml index bd422af46bc4d..d6e49eaabc5bd 100644 --- a/.github/workflows/build-and-test-differential-arm64.yaml +++ b/.github/workflows/build-and-test-differential-arm64.yaml @@ -29,7 +29,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 3ad7b6d434f33..fb98d321fde88 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -28,7 +28,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository @@ -77,7 +77,7 @@ jobs: clang-tidy-differential: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt-cuda + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda steps: - name: Check out repository uses: actions/checkout@v4 diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 520c2cb9983b0..a469e7a33d190 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -21,7 +21,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository From 365b5a85e1f3ac82fbdb694b342cd7e702ab793d Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Fri, 31 May 2024 08:45:56 +0900 Subject: [PATCH 059/120] fix(localization_error_monitor): fixed header stamp (#7174) Fixed header stamp at localization_error_monitor Signed-off-by: Shintaro Sakoda --- .../src/localization_error_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 67cdb78c942fb..7a1cd0b213c39 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -129,7 +129,7 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in diag_merged_status.hardware_id = this->get_name(); diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); + diag_msg.header.stamp = input_msg->header.stamp; diag_msg.status.push_back(diag_merged_status); diag_pub_->publish(diag_msg); } From a4a3b82d429888a7d947ea61e8f67f8312e1b61c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 31 May 2024 11:10:43 +0900 Subject: [PATCH 060/120] fix(multi_object_tracker): prevent too large object tracking (#7159) * fix: set minimum and maximum object for size update Signed-off-by: Taekjin LEE * fix: bug of convertConvexHullToBoundingBox Signed-off-by: Taekjin LEE * fix: return false when footprint is less than 3 points Signed-off-by: Taekjin LEE * fix: size filter bug Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * fix: try to convert polygon to bbox Signed-off-by: Taekjin LEE * chore: clean-up Signed-off-by: Taekjin LEE * fix: bicycle tracker to try bbox convert Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * chore: clean-up Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../multi_object_tracker/utils/utils.hpp | 41 +++++++------ .../src/tracker/model/bicycle_tracker.cpp | 50 +++++++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 56 +++++++++++++----- .../tracker/model/normal_vehicle_tracker.cpp | 58 ++++++++++++++----- .../src/tracker/model/pedestrian_tracker.cpp | 54 ++++++++++++----- 5 files changed, 187 insertions(+), 72 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index b19b0ba7ee10c..5842246bc1313 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -295,40 +295,43 @@ inline void calcAnchorPointOffset( * @param input_object: input convex hull objects * @param output_object: output bounding box objects */ -inline void convertConvexHullToBoundingBox( +inline bool convertConvexHullToBoundingBox( const autoware_auto_perception_msgs::msg::DetectedObject & input_object, autoware_auto_perception_msgs::msg::DetectedObject & output_object) { - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + // look for bounding box boundary double max_x = 0; double max_y = 0; double min_x = 0; double min_y = 0; double max_z = 0; - - // look for bounding box boundary for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { - Eigen::Vector2d vertex{ - input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; - - const Eigen::Vector2d local_vertex = R_inv * (vertex - center); - max_x = std::max(max_x, local_vertex.x()); - max_y = std::max(max_y, local_vertex.y()); - min_x = std::min(min_x, local_vertex.x()); - min_y = std::min(min_y, local_vertex.y()); - - max_z = std::max(max_z, static_cast(input_object.shape.footprint.points.at(i).z)); + const double foot_x = input_object.shape.footprint.points.at(i).x; + const double foot_y = input_object.shape.footprint.points.at(i).y; + const double foot_z = input_object.shape.footprint.points.at(i).z; + max_x = std::max(max_x, foot_x); + max_y = std::max(max_y, foot_y); + min_x = std::min(min_x, foot_x); + min_y = std::min(min_y, foot_y); + max_z = std::max(max_z, foot_z); } // calc bounding box state const double length = max_x - min_x; const double width = max_y - min_y; const double height = max_z; + + // calc new center + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; @@ -341,6 +344,8 @@ inline void convertConvexHullToBoundingBox( output_object.shape.dimensions.x = length; output_object.shape.dimensions.y = width; output_object.shape.dimensions.z = height; + + return true; } inline bool getMeasurementYaw( diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 0698e4651b6bf..4e5f14fde6834 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -71,10 +71,12 @@ BicycleTracker::BicycleTracker( } else { bounding_box_ = {1.0, 0.5, 1.7}; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // Set motion model parameters { @@ -172,7 +174,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, updating_object); + if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { + updating_object = object; + } } else { updating_object = object; } @@ -226,22 +230,38 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box - return true; + return false; } - constexpr double gain = 0.1; - constexpr double gain_inv = 1.0 - gain; + // check bound box size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 0.1; // [m] + if ( + bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max || + bbox_object.shape.dimensions.z > size_max) { + return false; + } else if ( + bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min || + bbox_object.shape.dimensions.z < size_min) { + return false; + } // update object size - bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; + bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z; + + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index e8ca8a47bbc78..15c6b2e722ed5 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -79,16 +79,24 @@ BigVehicleTracker::BigVehicleTracker( last_input_bounding_box_ = bounding_box_; } else { autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - utils::convertConvexHullToBoundingBox(object, bbox_object); - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box."); + bounding_box_ = {6.0, 2.0, 2.0}; // default value + } else { + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; + } last_input_bounding_box_ = bounding_box_; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 30.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // Set motion model parameters { @@ -195,7 +203,12 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + bbox_object = object; + } } else { bbox_object = object; } @@ -306,6 +319,20 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return false; + } + + // check object size abnormality + constexpr double size_max = 40.0; // [m] + constexpr double size_min = 1.0; // [m] + if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { + return false; + } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + return false; + } + constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -315,10 +342,13 @@ bool BigVehicleTracker::measureWithShape( bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + + // set maximum and minimum size + constexpr double max_size = 30.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 54d6e84dd6656..bd04cc613ef12 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -79,16 +79,25 @@ NormalVehicleTracker::NormalVehicleTracker( last_input_bounding_box_ = bounding_box_; } else { autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - utils::convertConvexHullToBoundingBox(object, bbox_object); - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding " + "box."); + bounding_box_ = {3.0, 2.0, 1.8}; // default value + } else { + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; + } last_input_bounding_box_ = bounding_box_; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 20.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // Set motion model parameters { @@ -195,7 +204,13 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + bbox_object = object; + } + } else { bbox_object = object; } @@ -306,6 +321,20 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return false; + } + + // check object size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 1.0; // [m] + if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { + return false; + } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + return false; + } + constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -315,10 +344,13 @@ bool NormalVehicleTracker::measureWithShape( bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + + // set maximum and minimum size + constexpr double max_size = 20.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 0d4411b5266ad..fd9ef82dfdca3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -66,19 +66,23 @@ PedestrianTracker::PedestrianTracker( // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.3, 1.7}; + cylinder_ = {0.5, 1.7}; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // do not update polygon shape } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); + cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); // Set motion model parameters { @@ -209,22 +213,46 @@ bool PedestrianTracker::measureWithShape( constexpr double gain_inv = 1.0 - gain; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // check bound box size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 0.1; // [m] + if ( + object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || + object.shape.dimensions.z > size_max) { + return false; + } else if ( + object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || + object.shape.dimensions.z < size_min) { + return false; + } + // update bounding box size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // check cylinder size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 0.1; // [m] + if (object.shape.dimensions.x > size_max || object.shape.dimensions.z > size_max) { + return false; + } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.z < size_min) { + return false; + } cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; } else { + // do not update polygon shape return false; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); + cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); return true; } From badf1f7bb8c90a70c61f63c470ec7ac75023a03d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 31 May 2024 12:39:14 +0900 Subject: [PATCH 061/120] chore(static_obstacle_avoidance, dynamic_obstacle_avoidance): rename avoidance package (#7168) * chore(autoware_behavior_path_static_obstacle_avoidance_module): rename package and namespace Signed-off-by: satoshi-ota * chore(autoware_behavior_path_dynamic_obstacle_avoidance_module): rename package and namespace Signed-off-by: satoshi-ota * chore(tier4_planning_launch): update module name Signed-off-by: satoshi-ota * chore(rtc_interface): update module name Signed-off-by: satoshi-ota * chore(avoidance): update module param file name Signed-off-by: satoshi-ota * chore(avoidance): update schema file name Signed-off-by: satoshi-ota * fix(AbLC): fix file name Signed-off-by: satoshi-ota * docs: update module name Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_planning.launch.xml | 16 +- .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../dynamic_obstacle_avoidance.param.yaml} | 0 .../image/2024-04-18_15-13-01.png | Bin .../image/2024-04-18_15-32-03.png | Bin .../drivable_area_extraction_width.drawio.svg | 0 .../image/image-20230807-151945.png | Bin .../image/image-20230807-152835.png | Bin .../image/image-20230808-095936.png | Bin .../image/image-20230808-152853.png | Bin .../image/opposite_directional_object.svg | 0 .../image/same_directional_object.svg | 0 .../manager.hpp | 17 +- .../scene.hpp | 14 +- .../package.xml | 4 +- .../plugins.xml | 3 + .../src/manager.cpp | 10 +- .../src/scene.cpp | 70 ++++--- ...t_behavior_path_planner_node_interface.cpp | 7 +- .../CMakeLists.txt | 2 +- .../README.md | 6 +- .../static_obstacle_avoidance.param.yaml} | 0 .../images/avoidance-debug-marker.png | Bin .../images/avoidance_debug_message_array.png | Bin .../images/avoidance_design.fig1.drawio.svg | 0 ...shift_point_and_its_constraints.drawio.png | Bin .../images/dynamic_lateral_margin.svg | 0 .../images/envelope_polygon.svg | 0 ...ape_multi_object_both_direction.drawio.svg | 0 ...hape_multi_object_one_direction.drawio.svg | 0 ...to_decide_path_shape_one_object.drawio.svg | 0 .../images/intersection_problem.drawio.svg | 0 ...tacle_to_road_shoulder_distance.drawio.svg | 0 .../images/parked-car-detection.svg | 0 .../images/safety_check_flowchart.svg | 0 .../images/safety_check_margin.svg | 0 .../images/safety_check_step_1.svg | 0 .../images/safety_check_step_2.svg | 0 .../images/shift_length_parameters.drawio.svg | 0 .../images/shift_line_generation.svg | 0 .../images/target_vehicle_selection.svg | 0 .../images/todo.png | Bin .../images/use_adjacent_lane.svg | 0 .../images/use_hatched_road_markings.svg | 0 .../images/use_intersection_areas.svg | 0 .../images/use_opposite_lane.svg | 0 .../images/yield_limitation.svg | 0 .../images/yield_maneuver.svg | 0 .../images/yield_maneuver_flowchart.svg | 0 .../data_structs.hpp | 8 +- .../debug.hpp | 14 +- .../helper.hpp | 20 +- .../manager.hpp | 18 +- .../parameter_helper.hpp | 8 +- .../scene.hpp | 22 +- .../shift_line_generator.hpp | 18 +- .../type_alias.hpp | 6 +- .../utils.hpp | 12 +- .../package.xml | 4 +- .../plugins.xml | 3 + .../static_obstacle_avoidance.schema.json} | 0 .../src/debug.cpp | 6 +- .../src/manager.cpp | 12 +- .../src/scene.cpp | 189 ++++++++++-------- .../src/shift_line_generator.cpp | 79 ++++---- .../src/utils.cpp | 11 +- ...t_behavior_path_planner_node_interface.cpp | 20 +- .../test/test_utils.cpp | 10 +- .../data_structs.hpp | 2 +- .../scene.hpp | 4 +- .../package.xml | 2 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 26 +-- ...t_behavior_path_planner_node_interface.cpp | 5 +- .../plugins.xml | 3 - .../plugins.xml | 3 - planning/behavior_path_planner/README.md | 38 ++-- .../config/scene_module_manager.param.yaml | 4 +- .../interface/scene_module_visitor.hpp | 4 +- planning/rtc_interface/src/rtc_interface.cpp | 4 +- 81 files changed, 375 insertions(+), 335 deletions(-) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/CMakeLists.txt (89%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/README.md (99%) rename planning/{behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml => autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml} (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/2024-04-18_15-13-01.png (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/2024-04-18_15-32-03.png (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/drivable_area_extraction_width.drawio.svg (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/image-20230807-151945.png (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/image-20230807-152835.png (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/image-20230808-095936.png (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/image-20230808-152853.png (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/opposite_directional_object.svg (100%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/image/same_directional_object.svg (100%) rename planning/{behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module}/manager.hpp (69%) rename planning/{behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module}/scene.hpp (97%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/package.xml (89%) create mode 100644 planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/src/manager.cpp (97%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/src/scene.cpp (96%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_dynamic_obstacle_avoidance_module}/test/test_behavior_path_planner_node_interface.cpp (95%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/CMakeLists.txt (91%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/README.md (98%) rename planning/{behavior_path_avoidance_module/config/avoidance.param.yaml => autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml} (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/avoidance-debug-marker.png (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/avoidance_debug_message_array.png (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/avoidance_design.fig1.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/avoidance_module-shift_point_and_its_constraints.drawio.png (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/dynamic_lateral_margin.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/envelope_polygon.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/how_to_decide_path_shape_one_object.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/intersection_problem.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/obstacle_to_road_shoulder_distance.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/parked-car-detection.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/safety_check_flowchart.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/safety_check_margin.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/safety_check_step_1.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/safety_check_step_2.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/shift_length_parameters.drawio.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/shift_line_generation.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/target_vehicle_selection.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/todo.png (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/use_adjacent_lane.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/use_hatched_road_markings.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/use_intersection_areas.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/use_opposite_lane.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/yield_limitation.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/yield_maneuver.svg (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/images/yield_maneuver_flowchart.svg (100%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/data_structs.hpp (98%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/debug.hpp (79%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/helper.hpp (95%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/manager.hpp (66%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/parameter_helper.hpp (98%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/scene.hpp (94%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/shift_line_generator.hpp (91%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/type_alias.hpp (91%) rename planning/{behavior_path_avoidance_module/include/behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module}/utils.hpp (94%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/package.xml (92%) create mode 100644 planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml rename planning/{behavior_path_avoidance_module/schema/avoidance.schema.json => autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json} (100%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/src/debug.cpp (99%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/src/manager.cpp (96%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/src/scene.cpp (89%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/src/shift_line_generator.cpp (94%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/src/utils.cpp (99%) rename planning/{behavior_path_dynamic_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/test/test_behavior_path_planner_node_interface.cpp (84%) rename planning/{behavior_path_avoidance_module => autoware_behavior_path_static_obstacle_avoidance_module}/test/test_utils.cpp (83%) delete mode 100644 planning/behavior_path_avoidance_module/plugins.xml delete mode 100644 planning/behavior_path_dynamic_avoidance_module/plugins.xml diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 57d0280330641..51f0a32284626 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -7,9 +7,9 @@ - + - + @@ -42,13 +42,13 @@ - + - + diff --git a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt similarity index 89% rename from planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt index 98874b8923324..e7df28307e754 100644 --- a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_dynamic_avoidance_module) +project(autoware_behavior_path_dynamic_obstacle_avoidance_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_dynamic_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md similarity index 99% rename from planning/behavior_path_dynamic_avoidance_module/README.md rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index b8af767afd9a5..e0fec11d166a6 100644 --- a/planning/behavior_path_dynamic_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -1,4 +1,4 @@ -# Dynamic avoidance design +# Avoidance module for dynamic objects This module is under development. diff --git a/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml diff --git a/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230807-151945.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230807-151945.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230807-152835.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230807-152835.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230808-095936.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230808-095936.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230808-152853.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230808-152853.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp similarity index 69% rename from planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index c2c17c4e55c1e..caf0544b0852e 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_dynamic_avoidance_module/scene.hpp" +#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -28,16 +28,19 @@ namespace behavior_path_planner { -class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface +class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { public: - DynamicAvoidanceModuleManager() : SceneModuleManagerInterface{"dynamic_avoidance"} {} + DynamicObstacleAvoidanceModuleManager() + : SceneModuleManagerInterface{"dynamic_obstacle_avoidance"} + { + } void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_); } @@ -52,4 +55,4 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp similarity index 97% rename from planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 353a5fab92032..22152911b46b2 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -164,7 +164,7 @@ struct LatFeasiblePaths std::vector left_path; std::vector right_path; }; -class DynamicAvoidanceModule : public SceneModuleInterface +class DynamicObstacleAvoidanceModule : public SceneModuleInterface { public: struct DynamicAvoidanceObject @@ -336,7 +336,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string reason{""}; }; - DynamicAvoidanceModule( + DynamicObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, @@ -444,8 +444,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface getLogger(), parameters_->enable_debug_info, reason_text.c_str(), obj_uuid.c_str()); } - std::vector target_objects_; - // std::vector prev_target_objects_; + std::vector target_objects_; + // std::vector prev_target_objects_; std::optional> prev_input_ref_path_points_{std::nullopt}; std::optional> ref_path_before_lane_change_{std::nullopt}; std::shared_ptr parameters_; @@ -458,4 +458,4 @@ class DynamicAvoidanceModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/package.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml similarity index 89% rename from planning/behavior_path_dynamic_avoidance_module/package.xml rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 11792a15b2cd2..9b64069eddafd 100644 --- a/planning/behavior_path_dynamic_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_dynamic_avoidance_module + autoware_behavior_path_dynamic_obstacle_avoidance_module 0.1.0 - The behavior_path_dynamic_avoidance_module package + The autoware_behavior_path_dynamic_obstacle_avoidance_module package Takayuki Murooka Yuki Takagi diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..3827012b12e22 --- /dev/null +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp similarity index 97% rename from planning/behavior_path_dynamic_avoidance_module/src/manager.cpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index baa1c631df8da..de524a3b63de9 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_dynamic_avoidance_module/manager.hpp" +#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -37,7 +37,7 @@ PolygonGenerationMethod convertToPolygonGenerationMethod(const std::string & str } } // namespace -void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) +void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { // init manager interface initInterface(node, {""}); @@ -149,7 +149,7 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) parameters_ = std::make_shared(p); } -void DynamicAvoidanceModuleManager::updateModuleParams( +void DynamicObstacleAvoidanceModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -287,7 +287,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( }); } -bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const +bool DynamicObstacleAvoidanceModuleManager::isAlwaysExecutableModule() const { return true; } @@ -295,5 +295,5 @@ bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::DynamicAvoidanceModuleManager, + behavior_path_planner::DynamicObstacleAvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp similarity index 96% rename from planning/behavior_path_dynamic_avoidance_module/src/scene.cpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index a5368732ef3bb..efc56c4d61f5f 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_dynamic_avoidance_module/scene.hpp" +#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -301,7 +301,7 @@ std::vector convertToPoints( } } // namespace -DynamicAvoidanceModule::DynamicAvoidanceModule( +DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, @@ -315,7 +315,7 @@ DynamicAvoidanceModule::DynamicAvoidanceModule( { } -bool DynamicAvoidanceModule::isExecutionRequested() const +bool DynamicObstacleAvoidanceModule::isExecutionRequested() const { RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); @@ -339,13 +339,13 @@ bool DynamicAvoidanceModule::isExecutionRequested() const return !target_objects_.empty(); } -bool DynamicAvoidanceModule::isExecutionReady() const +bool DynamicObstacleAvoidanceModule::isExecutionReady() const { RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionReady."); return true; } -void DynamicAvoidanceModule::updateData() +void DynamicObstacleAvoidanceModule::updateData() { // stop_watch_.tic(std::string(__func__)); @@ -380,12 +380,12 @@ void DynamicAvoidanceModule::updateData() // [ms]"); } -bool DynamicAvoidanceModule::canTransitSuccessState() +bool DynamicObstacleAvoidanceModule::canTransitSuccessState() { return target_objects_.empty(); } -BehaviorModuleOutput DynamicAvoidanceModule::plan() +BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan() { // stop_watch_.tic(std::string(__func__)); @@ -444,19 +444,19 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() return output; } -CandidateOutput DynamicAvoidanceModule::planCandidate() const +CandidateOutput DynamicObstacleAvoidanceModule::planCandidate() const { auto candidate_path = utils::generateCenterLinePath(planner_data_); return CandidateOutput(*candidate_path); } -BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval() +BehaviorModuleOutput DynamicObstacleAvoidanceModule::planWaitingApproval() { BehaviorModuleOutput out = plan(); return out; } -ObjectType DynamicAvoidanceModule::getObjectType(const uint8_t label) const +ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) const { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -487,7 +487,7 @@ ObjectType DynamicAvoidanceModule::getObjectType(const uint8_t label) const return ObjectType::OUT_OF_SCOPE; } -void DynamicAvoidanceModule::registerRegulatedObjects( +void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const std::vector & prev_objects) { const auto input_path = getPreviousModuleOutput().path; @@ -575,7 +575,7 @@ void DynamicAvoidanceModule::registerRegulatedObjects( } } -void DynamicAvoidanceModule::registerUnregulatedObjects( +void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const std::vector & prev_objects) { const auto input_path = getPreviousModuleOutput().path; @@ -649,7 +649,7 @@ void DynamicAvoidanceModule::registerUnregulatedObjects( } } -void DynamicAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( +void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( const std::vector & prev_objects) { const auto & input_path = getPreviousModuleOutput().path; @@ -809,7 +809,7 @@ void DynamicAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( // prev_input_ref_path_points_ = input_ref_path_points; } -void DynamicAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( +void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( const std::vector & prev_objects) { const auto & input_path = getPreviousModuleOutput().path; @@ -870,7 +870,7 @@ void DynamicAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( } } -LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( +LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { const double lat_acc = parameters_->max_ego_lat_acc; @@ -907,7 +907,7 @@ LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( return ego_lat_feasible_paths; } -[[maybe_unused]] void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( +[[maybe_unused]] void DynamicObstacleAvoidanceModule::updateRefPathBeforeLaneChange( const std::vector & ego_ref_path_points) { if (ref_path_before_lane_change_) { @@ -934,7 +934,7 @@ LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( } [[maybe_unused]] std::optional> -DynamicAvoidanceModule::calcCollisionSection( +DynamicObstacleAvoidanceModule::calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const { const size_t ego_idx = planner_data_->findEgoIndex(ego_path); @@ -971,7 +971,7 @@ DynamicAvoidanceModule::calcCollisionSection( return std::make_pair(*collision_start_idx, ego_path.size() - 1); } -TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( +TimeWhileCollision DynamicObstacleAvoidanceModule::calcTimeWhileCollision( const std::vector & ego_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { @@ -1027,7 +1027,7 @@ TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( return TimeWhileCollision{time_to_start_collision, time_to_end_collision}; } -bool DynamicAvoidanceModule::isObjectFarFromPath( +bool DynamicObstacleAvoidanceModule::isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const { const double obj_max_length = calcObstacleMaxLength(predicted_object.shape); @@ -1037,7 +1037,7 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( return parameters_->max_obj_lat_offset_to_ego_path < min_obj_dist_to_path; } -bool DynamicAvoidanceModule::willObjectCutIn( +bool DynamicObstacleAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { @@ -1079,7 +1079,7 @@ bool DynamicAvoidanceModule::willObjectCutIn( return true; } -DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( +DynamicObstacleAvoidanceModule::DecisionWithReason DynamicObstacleAvoidanceModule::willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const { @@ -1120,7 +1120,7 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return DecisionWithReason{false}; } -[[maybe_unused]] bool DynamicAvoidanceModule::willObjectBeOutsideEgoChangingPath( +[[maybe_unused]] bool DynamicObstacleAvoidanceModule::willObjectBeOutsideEgoChangingPath( const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const { @@ -1139,7 +1139,8 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return true; } -std::pair DynamicAvoidanceModule::getAdjacentLanes( +std::pair +DynamicObstacleAvoidanceModule::getAdjacentLanes( const double forward_distance, const double backward_distance) const { const auto & rh = planner_data_->route_handler; @@ -1179,7 +1180,8 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } -DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( +DynamicObstacleAvoidanceModule::LatLonOffset +DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const { @@ -1213,7 +1215,7 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi obj_lon_min_max_offset.max_value, obj_lon_min_max_offset.min_value}; } -MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( +MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, @@ -1286,7 +1288,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( obj_lon_offset.max_value + end_length_to_avoid}; } -double DynamicAvoidanceModule::calcValidLengthToAvoid( +double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const @@ -1379,7 +1381,8 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( } // min value denotes near side, max value denotes far side -std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( +std::optional +DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( const std::vector & ref_path_points_for_obj_poly, const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, const bool is_collision_left, const double obj_normal_vel, @@ -1473,7 +1476,8 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi } // min value denotes near side, max value denotes far side -std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( +std::optional +DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( const std::vector & ref_path_points_for_obj_poly, const std::optional & prev_object, const DynamicAvoidanceObject & object) const @@ -1557,7 +1561,7 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi // NOTE: object does not have const only to update min_bound_lat_offset. std::optional -DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( +DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { @@ -1663,7 +1667,7 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( // should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi) std::optional -DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( +DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { const auto obj_path = *std::max_element( @@ -1722,7 +1726,7 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // except for the area required for ego safety. // input: an object, the minimum area required for ego safety, and some global params std::optional -DynamicAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( +DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const { std::vector obj_poses; @@ -1783,8 +1787,8 @@ DynamicAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( // Calculate the driving area required to ensure the safety of the own vehicle. // It is assumed that this area will not be clipped. // input: ego's reference path, ego's pose, ego's speed, and some global params -DynamicAvoidanceModule::EgoPathReservePoly DynamicAvoidanceModule::calcEgoPathReservePoly( - const PathWithLaneId & ego_path) const +DynamicObstacleAvoidanceModule::EgoPathReservePoly +DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & ego_path) const { // This function require almost 0.5 ms. Should be refactored in the future // double calculation_time; diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp similarity index 95% rename from planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 4ae77249b5d0f..bd9e6fd483c7c 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -47,7 +47,7 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); + module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -60,8 +60,9 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + - "/config/avoidance.param.yaml"}); + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_dynamic_obstacle_avoidance_module") + + "/config/dynamic_obstacle_avoidance.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_avoidance_module/CMakeLists.txt b/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt similarity index 91% rename from planning/behavior_path_avoidance_module/CMakeLists.txt rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt index c94591f9bbd30..e646ad29ed42a 100644 --- a/planning/behavior_path_avoidance_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_avoidance_module) +project(autoware_behavior_path_static_obstacle_avoidance_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md similarity index 98% rename from planning/behavior_path_avoidance_module/README.md rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md index aa7a3a42efdb4..93b57ac3a4831 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -758,9 +758,9 @@ WIP ## Parameters -The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml`. +The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml`. -{{ json_to_markdown("planning/behavior_path_avoidance_module/schema/avoidance.schema.json") }} +{{ json_to_markdown("planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json") }} ## Future extensions / Unimplemented parts @@ -797,7 +797,7 @@ Developers can see what is going on in each process by visualizing all the avoid ![fig1](./images/avoidance-debug-marker.png) -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. +To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `static_obstacle_avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. ### Echoing debug message to find out why the objects were ignored diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml similarity index 100% rename from planning/behavior_path_avoidance_module/config/avoidance.param.yaml rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml diff --git a/planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png diff --git a/planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png diff --git a/planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig1.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig1.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png diff --git a/planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg diff --git a/planning/behavior_path_avoidance_module/images/envelope_polygon.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/envelope_polygon.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg diff --git a/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/parked-car-detection.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/parked-car-detection.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_margin.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_margin.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_step_1.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_step_1.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_step_2.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_step_2.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg diff --git a/planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/shift_line_generation.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/shift_line_generation.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg diff --git a/planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg diff --git a/planning/behavior_path_avoidance_module/images/todo.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/todo.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png diff --git a/planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg diff --git a/planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg diff --git a/planning/behavior_path_avoidance_module/images/use_intersection_areas.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_intersection_areas.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg diff --git a/planning/behavior_path_avoidance_module/images/use_opposite_lane.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_opposite_lane.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg diff --git a/planning/behavior_path_avoidance_module/images/yield_limitation.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/yield_limitation.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg diff --git a/planning/behavior_path_avoidance_module/images/yield_maneuver.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/yield_maneuver.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg diff --git a/planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp similarity index 98% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 2e432e093583f..a63c283704001 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -670,4 +670,4 @@ struct DebugData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp similarity index 79% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp index 6a4f206ddf309..85b0e1aad0ae3 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using behavior_path_planner::AvoidanceParameters; @@ -50,7 +50,7 @@ MarkerArray createOtherObjectsMarkerArray( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); @@ -60,4 +60,4 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp similarity index 95% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp index 55d14e424b7f6..5da56a320ad3b 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_path_planner::helper::avoidance +namespace behavior_path_planner::helper::static_obstacle_avoidance { using behavior_path_planner::PathShifter; @@ -200,7 +200,7 @@ class AvoidanceHelper double getShiftLength( const ObjectData & object, const bool & is_on_right, const double & margin) const { - using utils::avoidance::calcShiftLength; + using utils::static_obstacle_avoidance::calcShiftLength; const auto shift_length = calcShiftLength(is_on_right, object.overhang_points.front().first, margin); @@ -333,7 +333,7 @@ class AvoidanceHelper return true; } - const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(object); const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); @@ -513,6 +513,6 @@ class AvoidanceHelper std::optional> max_v_point_; }; -} // namespace behavior_path_planner::helper::avoidance +} // namespace behavior_path_planner::helper::static_obstacle_avoidance -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp similarity index 66% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp index e23a8a96ee7da..1616339840ba4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/scene.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -28,16 +28,18 @@ namespace behavior_path_planner { -class AvoidanceModuleManager : public SceneModuleManagerInterface +class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { public: - AvoidanceModuleManager() : SceneModuleManagerInterface{"avoidance"} {} + StaticObstacleAvoidanceModuleManager() : SceneModuleManagerInterface{"static_obstacle_avoidance"} + { + } void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_); } @@ -50,4 +52,4 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp similarity index 98% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 461084c085ca7..56b9bad535e7d 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -11,12 +11,12 @@ // 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 BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ #include "tier4_autoware_utils/ros/parameter.hpp" -#include +#include #include #include @@ -403,4 +403,4 @@ AvoidanceParameters getParameter(rclcpp::Node * node) } } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp similarity index 94% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp index da5e29ef4d5e9..57090e9f551cf 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_avoidance_module/shift_line_generator.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" @@ -36,13 +36,13 @@ namespace behavior_path_planner { -using helper::avoidance::AvoidanceHelper; +using helper::static_obstacle_avoidance::AvoidanceHelper; using tier4_planning_msgs::msg::AvoidanceDebugMsg; -class AvoidanceModule : public SceneModuleInterface +class StaticObstacleAvoidanceModule : public SceneModuleInterface { public: - AvoidanceModule( + StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & @@ -424,7 +424,7 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr parameters_; - utils::avoidance::ShiftLineGenerator generator_; + utils::static_obstacle_avoidance::ShiftLineGenerator generator_; AvoidancePlanningData avoid_data_; @@ -458,4 +458,4 @@ class AvoidanceModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp similarity index 91% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp index e721401207830..21eaef884b5e6 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using behavior_path_planner::PathShifter; -using behavior_path_planner::helper::avoidance::AvoidanceHelper; +using behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; class ShiftLineGenerator { @@ -242,6 +242,6 @@ class ShiftLineGenerator double base_offset_{0.0}; }; -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp similarity index 91% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 67bca3099df6f..afdfba30ce267 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ #include #include @@ -71,4 +71,4 @@ using tier4_autoware_utils::pose2transform; using tier4_autoware_utils::toHexString; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp similarity index 94% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp index cf68f577ed7c0..420edd17ca372 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -23,7 +23,7 @@ #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using behavior_path_planner::PlannerData; @@ -174,6 +174,6 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml similarity index 92% rename from planning/behavior_path_avoidance_module/package.xml rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 5f0a658735b2f..3643257d243b4 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_avoidance_module + autoware_behavior_path_static_obstacle_avoidance_module 0.1.0 - The behavior_path_avoidance_module package + The autoware_behavior_path_static_obstacle_avoidance_module package Takamasa Horibe Zulfaqar Azmi diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..a4e1da9d5dccb --- /dev/null +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json similarity index 100% rename from planning/behavior_path_avoidance_module/schema/avoidance.schema.json rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp similarity index 99% rename from planning/behavior_path_avoidance_module/src/debug.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 4d3b4605ac956..b8e6bb99fbebb 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/debug.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -23,7 +23,7 @@ #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { namespace { @@ -653,7 +653,7 @@ MarkerArray createDebugMarkerArray( return msg; } -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr) { diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp similarity index 96% rename from planning/behavior_path_avoidance_module/src/manager.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index bdf6f4e822f7b..bfc95e0730cd6 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/manager.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp" -#include "behavior_path_avoidance_module/parameter_helper.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -26,7 +26,7 @@ namespace behavior_path_planner { -void AvoidanceModuleManager::init(rclcpp::Node * node) +void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; @@ -39,7 +39,8 @@ void AvoidanceModuleManager::init(rclcpp::Node * node) parameters_ = std::make_shared(p); } -void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) +void StaticObstacleAvoidanceModuleManager::updateModuleParams( + const std::vector & parameters) { using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; @@ -282,4 +283,5 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) + behavior_path_planner::StaticObstacleAvoidanceModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp similarity index 89% rename from planning/behavior_path_avoidance_module/src/scene.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 9247d9b3d200a..0a6d490f7fbf1 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/scene.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_avoidance_module/debug.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -71,7 +71,7 @@ lanelet::BasicLineString3d toLineString3d(const std::vector & bound) } } // namespace -AvoidanceModule::AvoidanceModule( +StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & @@ -83,7 +83,7 @@ AvoidanceModule::AvoidanceModule( { } -bool AvoidanceModule::isExecutionRequested() const +bool StaticObstacleAvoidanceModule::isExecutionRequested() const { RCLCPP_DEBUG(getLogger(), "AVOIDANCE isExecutionRequested"); @@ -105,7 +105,7 @@ bool AvoidanceModule::isExecutionRequested() const [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); }); } -bool AvoidanceModule::isExecutionReady() const +bool StaticObstacleAvoidanceModule::isExecutionReady() const { RCLCPP_DEBUG_STREAM(getLogger(), "---Avoidance GO/NO-GO status---"); RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "SAFE:" << avoid_data_.safe); @@ -115,7 +115,8 @@ bool AvoidanceModule::isExecutionReady() const return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } -AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningData & data) const +AvoidanceState StaticObstacleAvoidanceModule::getCurrentModuleState( + const AvoidancePlanningData & data) const { const bool has_avoidance_target = std::any_of( data.target_objects.begin(), data.target_objects.end(), @@ -159,7 +160,7 @@ AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningDat return AvoidanceState::RUNNING; } -bool AvoidanceModule::canTransitSuccessState() +bool StaticObstacleAvoidanceModule::canTransitSuccessState() { const auto & data = avoid_data_; @@ -190,22 +191,23 @@ bool AvoidanceModule::canTransitSuccessState() return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED; } -void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) +void StaticObstacleAvoidanceModule::fillFundamentalData( + AvoidancePlanningData & data, DebugData & debug) { // reference pose data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath()); // lanelet info - data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( + data.current_lanelets = utils::static_obstacle_avoidance::getCurrentLanesFromPath( getPreviousModuleOutput().reference_path, planner_data_); - data.extend_lanelets = - utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); + data.extend_lanelets = utils::static_obstacle_avoidance::getExtendLanes( + data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes const auto is_within_current_lane = - utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_); + utils::static_obstacle_avoidance::isWithinLanes(data.current_lanelets, planner_data_); const auto red_signal_lane_itr = std::find_if( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); @@ -218,12 +220,15 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { if (!not_use_adjacent_lane) { data.drivable_lanes.push_back( - utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + utils::static_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); } else if (red_signal_lane_itr->id() != lanelet.id()) { data.drivable_lanes.push_back( - utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + utils::static_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); } else { - data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet)); + data.drivable_lanes.push_back( + utils::static_obstacle_avoidance::generateNotExpandedDrivableLanes(lanelet)); } }); @@ -271,18 +276,19 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.reference_path, 0, data.reference_path.points.size(), motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); - data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine( + data.to_return_point = utils::static_obstacle_avoidance::calcDistanceToReturnDeadLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); - data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine( + data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); // target objects for avoidance fillAvoidanceTargetObjects(data, debug); // lost object compensation - utils::avoidance::updateRegisteredObject(registered_objects_, data.target_objects, parameters_); - utils::avoidance::compensateDetectionLost( + utils::static_obstacle_avoidance::updateRegisteredObject( + registered_objects_, data.target_objects, parameters_); + utils::static_obstacle_avoidance::compensateDetectionLost( registered_objects_, data.target_objects, data.other_objects); // sort object order by longitudinal distance @@ -294,14 +300,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat path_shifter_.setPath(data.reference_path); } -void AvoidanceModule::fillAvoidanceTargetObjects( +void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::avoidance::fillAvoidanceNecessity; - using utils::avoidance::fillObjectStoppableJudge; - using utils::avoidance::filterTargetObjects; - using utils::avoidance::separateObjectsByPath; - using utils::avoidance::updateRoadShoulderDistance; + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + using utils::static_obstacle_avoidance::filterTargetObjects; + using utils::static_obstacle_avoidance::separateObjectsByPath; + using utils::static_obstacle_avoidance::updateRoadShoulderDistance; using utils::traffic_light::calcDistanceToRedTrafficLight; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. @@ -360,7 +366,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } } -ObjectData AvoidanceModule::createObjectData( +ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { using boost::geometry::return_centroid; @@ -384,14 +390,15 @@ ObjectData AvoidanceModule::createObjectData( object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( + utils::static_obstacle_avoidance::fillObjectEnvelopePolygon( object_data, registered_objects_, object_closest_pose, parameters_); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); + utils::static_obstacle_avoidance::fillObjectMovingTime( + object_data, stopped_objects_, parameters_); // Calc lateral deviation from path to target object. object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 @@ -401,7 +408,7 @@ ObjectData AvoidanceModule::createObjectData( return object_data; } -bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const +bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_->isShifted()) { @@ -453,7 +460,8 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const return true; } -void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const +void StaticObstacleAvoidanceModule::fillShiftLine( + AvoidancePlanningData & data, DebugData & debug) const { auto path_shifter = path_shifter_; @@ -485,12 +493,13 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de /** * STEP4: Generate avoidance path. */ - ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); + ShiftedPath spline_shift_path = + utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = path_shifter.generate(&spline_shift_path, true, SHIFT_TYPE::SPLINE); data.candidate_path = success_spline_path_generation ? spline_shift_path - : utils::avoidance::toShiftedPath(data.reference_path); + : utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); /** * STEP5: Check avoidance path safety. @@ -503,7 +512,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de helper_->isReady(data.target_objects); } -void AvoidanceModule::fillEgoStatus( +void StaticObstacleAvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { data.state = getCurrentModuleState(data); @@ -597,7 +606,7 @@ void AvoidanceModule::fillEgoStatus( } } -void AvoidanceModule::fillDebugData( +void StaticObstacleAvoidanceModule::fillDebugData( const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { if (!data.stop_target_object) { @@ -624,8 +633,8 @@ void AvoidanceModule::fillDebugData( const auto max_avoid_margin = lateral_hard_margin * o_front.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto avoidance_distance = helper_->getSharpAvoidanceDistance( - helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); + const auto avoidance_distance = helper_->getSharpAvoidanceDistance(helper_->getShiftLength( + o_front, utils::static_obstacle_avoidance::isOnRight(o_front), max_avoid_margin)); const auto prepare_distance = helper_->getNominalPrepareDistance(); const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; @@ -637,7 +646,8 @@ void AvoidanceModule::fillDebugData( } } -void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) +void StaticObstacleAvoidanceModule::updateEgoBehavior( + const AvoidancePlanningData & data, ShiftedPath & path) { if (parameters_->disable_path_update) { return; @@ -676,7 +686,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif setStopReason(StopReason::AVOIDANCE, path.path); } -bool AvoidanceModule::isSafePath( +bool StaticObstacleAvoidanceModule::isSafePath( ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { const auto & p = planner_data_->parameters; @@ -717,8 +727,9 @@ bool AvoidanceModule::isSafePath( const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; - const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); + const auto safety_check_target_objects = + utils::static_obstacle_avoidance::getSafetyCheckTargetObjects( + avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); if (safety_check_target_objects.empty()) { return true; @@ -782,7 +793,8 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const +PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( + const PathWithLaneId & original_path) const { const auto previous_path = helper_->getPreviousReferencePath(); @@ -844,7 +856,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig return extended_path; } -BehaviorModuleOutput AvoidanceModule::plan() +BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { const auto & data = avoid_data_; @@ -866,8 +878,10 @@ BehaviorModuleOutput AvoidanceModule::plan() } // generate path with shift points that have been inserted. - ShiftedPath linear_shift_path = utils::avoidance::toShiftedPath(data.reference_path); - ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); + ShiftedPath linear_shift_path = + utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); + ShiftedPath spline_shift_path = + utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = path_shifter_.generate(&spline_shift_path, true, SHIFT_TYPE::SPLINE); const auto success_linear_path_generation = @@ -956,7 +970,8 @@ BehaviorModuleOutput AvoidanceModule::plan() std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { current_drivable_area_info.drivable_lanes.push_back( - utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + utils::static_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); }); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = @@ -976,7 +991,7 @@ BehaviorModuleOutput AvoidanceModule::plan() if (!object.is_avoidable) clip_objects.push_back(object); }); current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( + utils::static_obstacle_avoidance::generateObstaclePolygonsForDrivableArea( clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); } else { current_drivable_area_info.obstacles.clear(); @@ -991,7 +1006,7 @@ BehaviorModuleOutput AvoidanceModule::plan() return output; } -CandidateOutput AvoidanceModule::planCandidate() const +CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const { const auto & data = avoid_data_; @@ -1030,7 +1045,7 @@ CandidateOutput AvoidanceModule::planCandidate() const return output; } -BehaviorModuleOutput AvoidanceModule::planWaitingApproval() +BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() { BehaviorModuleOutput out = plan(); @@ -1044,7 +1059,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() return out; } -void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) +void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) { if (parameters_->disable_path_update) { return; @@ -1085,10 +1100,10 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) /** * set new shift points. remove old shift points if it has a conflict. */ -void AvoidanceModule::addNewShiftLines( +void StaticObstacleAvoidanceModule::addNewShiftLines( PathShifter & path_shifter, const AvoidLineArray & new_shift_lines) const { - ShiftLineArray future = utils::avoidance::toShiftLineArray(new_shift_lines); + ShiftLineArray future = utils::static_obstacle_avoidance::toShiftLineArray(new_shift_lines); size_t min_start_idx = std::numeric_limits::max(); for (const auto & sl : new_shift_lines) { @@ -1151,7 +1166,7 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setLateralAccelerationLimit(helper_->getLateralMaxAccelLimit()); } -bool AvoidanceModule::isValidShiftLine( +bool StaticObstacleAvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { if (shift_lines.empty()) { @@ -1214,9 +1229,9 @@ bool AvoidanceModule::isValidShiftLine( return true; // valid shift line. } -void AvoidanceModule::updateData() +void StaticObstacleAvoidanceModule::updateData() { - using utils::avoidance::toShiftedPath; + using utils::static_obstacle_avoidance::toShiftedPath; helper_->setData(planner_data_); @@ -1224,7 +1239,7 @@ void AvoidanceModule::updateData() helper_->setPreviousSplineShiftPath(toShiftedPath(getPreviousModuleOutput().path)); helper_->setPreviousLinearShiftPath(toShiftedPath(getPreviousModuleOutput().path)); helper_->setPreviousReferencePath(getPreviousModuleOutput().path); - helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( + helper_->setPreviousDrivingLanes(utils::static_obstacle_avoidance::getCurrentLanesFromPath( getPreviousModuleOutput().reference_path, planner_data_)); } @@ -1266,18 +1281,18 @@ void AvoidanceModule::updateData() safe_ = avoid_data_.safe; } -void AvoidanceModule::processOnEntry() +void StaticObstacleAvoidanceModule::processOnEntry() { initVariables(); } -void AvoidanceModule::processOnExit() +void StaticObstacleAvoidanceModule::processOnExit() { initVariables(); initRTCStatus(); } -void AvoidanceModule::initVariables() +void StaticObstacleAvoidanceModule::initVariables() { helper_->reset(); generator_.reset(); @@ -1289,7 +1304,7 @@ void AvoidanceModule::initVariables() arrived_path_end_ = false; } -void AvoidanceModule::initRTCStatus() +void StaticObstacleAvoidanceModule::initRTCStatus() { left_shift_array_.clear(); right_shift_array_.clear(); @@ -1298,7 +1313,7 @@ void AvoidanceModule::initRTCStatus() candidate_uuid_ = generateUUID(); } -void AvoidanceModule::updateRTCData() +void StaticObstacleAvoidanceModule::updateRTCData() { const auto & data = avoid_data_; @@ -1333,23 +1348,24 @@ void AvoidanceModule::updateRTCData() updateCandidateRTCStatus(output); } -void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const +void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { - using utils::avoidance::createTargetObjectsMarkerArray; + using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); } -void AvoidanceModule::updateDebugMarker( +void StaticObstacleAvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { debug_marker_.markers.clear(); - debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); + debug_marker_ = + utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); } -void AvoidanceModule::updateAvoidanceDebugData( +void StaticObstacleAvoidanceModule::updateAvoidanceDebugData( std::vector & avoidance_debug_msg_array) const { debug_data_.avoidance_debug_msg_array.avoidance_info.clear(); @@ -1366,7 +1382,7 @@ void AvoidanceModule::updateAvoidanceDebugData( } } -double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const +double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { const auto & p = parameters_; const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -1395,8 +1411,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const : object_parameter.lateral_hard_margin; const auto avoid_margin = lateral_hard_margin * object.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto avoidance_distance = helper_->getMinAvoidanceDistance( - helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); + const auto avoidance_distance = helper_->getMinAvoidanceDistance(helper_->getShiftLength( + object, utils::static_obstacle_avoidance::isOnRight(object), avoid_margin)); const auto constant_distance = helper_->getFrontConstantDistance(object); const auto prepare_distance = helper_->getNominalPrepareDistance(0.0); @@ -1406,7 +1422,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const p->stop_max_distance); } -void AvoidanceModule::insertReturnDeadLine( +void StaticObstacleAvoidanceModule::insertReturnDeadLine( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1437,7 +1453,7 @@ void AvoidanceModule::insertReturnDeadLine( // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); return; @@ -1450,7 +1466,7 @@ void AvoidanceModule::insertReturnDeadLine( return; } - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); // insert slow down speed. @@ -1483,7 +1499,7 @@ void AvoidanceModule::insertReturnDeadLine( } } -void AvoidanceModule::insertWaitPoint( +void StaticObstacleAvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1504,7 +1520,7 @@ void AvoidanceModule::insertWaitPoint( // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_); return; } @@ -1519,7 +1535,7 @@ void AvoidanceModule::insertWaitPoint( // If target object can be stopped for, insert a deceleration point and return if (data.stop_target_object.value().is_stoppable) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_); return; } @@ -1527,11 +1543,11 @@ void AvoidanceModule::insertWaitPoint( // If the object cannot be stopped for, calculate a "mild" deceleration distance // and insert a deceleration point at that distance const auto stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertStopPoint( +void StaticObstacleAvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1563,7 +1579,7 @@ void AvoidanceModule::insertStopPoint( // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_); return; } @@ -1575,11 +1591,11 @@ void AvoidanceModule::insertStopPoint( } constexpr double MARGIN = 1.0; - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const +void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1643,7 +1659,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto avoid_margin = lateral_hard_margin * object.value().distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength( - object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin); + object.value(), utils::static_obstacle_avoidance::isOnRight(object.value()), avoid_margin); // check slow down feasibility const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length); @@ -1661,7 +1677,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, slow_pose_); return; @@ -1691,7 +1707,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_object); } -void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const +void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1738,20 +1754,21 @@ void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_accel_end_point); } -std::shared_ptr AvoidanceModule::get_debug_msg_array() const +std::shared_ptr StaticObstacleAvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now(); return std::make_shared(debug_data_.avoidance_debug_msg_array); } -void AvoidanceModule::acceptVisitor(const std::shared_ptr & visitor) const +void StaticObstacleAvoidanceModule::acceptVisitor( + const std::shared_ptr & visitor) const { if (visitor) { visitor->visitAvoidanceModule(this); } } -void SceneModuleVisitor::visitAvoidanceModule(const AvoidanceModule * module) const +void SceneModuleVisitor::visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const { avoidance_visitor_ = module->get_debug_msg_array(); } diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp similarity index 94% rename from planning/behavior_path_avoidance_module/src/shift_line_generator.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index fc3b9e24ff9f6..f9e35ebd4c1b2 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/shift_line_generator.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { namespace @@ -123,7 +123,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // use each object param const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(object); // use absolute dist for return-to-center, relative dist from current for avoiding. const auto avoiding_shift = desire_shift_length - current_ego_shift; @@ -254,10 +254,11 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } } - const auto is_object_on_right = utils::avoidance::isOnRight(o); + const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o); const auto desire_shift_length = helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); - if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { + if (utils::static_obstacle_avoidance::isSameDirectionShift( + is_object_on_right, desire_shift_length)) { o.info = ObjectInfo::SAME_DIRECTION_SHIFT; if (o.avoid_required && is_forward_object(o)) { break; @@ -302,7 +303,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); }(); - al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( + al_avoid.start_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( data.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); al_avoid.start = data.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position); @@ -314,7 +315,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // misc al_avoid.id = generateUUID(); al_avoid.object = o; - al_avoid.object_on_right = utils::avoidance::isOnRight(o); + al_avoid.object_on_right = utils::static_obstacle_avoidance::isOnRight(o); } AvoidLine al_return; @@ -340,7 +341,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // misc al_return.id = generateUUID(); al_return.object = o; - al_return.object_on_right = utils::avoidance::isOnRight(o); + al_return.object_on_right = utils::static_obstacle_avoidance::isOnRight(o); } const bool skip_return_shift = [&]() { @@ -376,7 +377,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( o.is_avoidable = true; } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); debug.step1_current_shift_line = toArray(outlines); @@ -479,7 +480,7 @@ void ShiftLineGenerator::generateTotalShiftLine( const auto & al = avoid_lines.at(j); for (size_t i = 0; i < N; ++i) { // calc current interpolated shift - const auto i_shift = utils::avoidance::lerpShiftLengthOnArc(arcs.at(i), al); + const auto i_shift = utils::static_obstacle_avoidance::lerpShiftLengthOnArc(arcs.at(i), al); // update maximum shift for positive direction if (i_shift > sl.pos_shift_line.at(i)) { @@ -548,8 +549,8 @@ void ShiftLineGenerator::generateTotalShiftLine( AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine( const AvoidancePlanningData & data, ShiftLineData & shift_line_data) const { - using utils::avoidance::setEndData; - using utils::avoidance::setStartData; + using utils::static_obstacle_avoidance::setEndData; + using utils::static_obstacle_avoidance::setStartData; const auto & path = data.reference_path; const auto & arcs = data.arclength_from_ego; @@ -706,8 +707,9 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( } } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_merged_shift_line); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( + data, debug.step1_merged_shift_line); return ret; } @@ -749,8 +751,9 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_filled_shift_line); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( + data, debug.step1_filled_shift_line); return ret; } @@ -771,7 +774,7 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( // fill gap between ego and nearest shift line. if (sorted.front().start_longitudinal > 0.0) { AvoidLine ego_line{}; - utils::avoidance::setEndData( + utils::static_obstacle_avoidance::setEndData( ego_line, helper_->getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); @@ -795,8 +798,9 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(ret, false); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_front_shift_line); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( + data, debug.step1_front_shift_line); return ret; } @@ -806,7 +810,8 @@ AvoidLineArray ShiftLineGenerator::applyCombineProcess( DebugData & debug) const { debug.step1_registered_shift_line = registered_lines; - return utils::avoidance::combineRawShiftLinesWithUniqueCheck(registered_lines, shift_lines); + return utils::static_obstacle_avoidance::combineRawShiftLinesWithUniqueCheck( + registered_lines, shift_lines); } AvoidLineArray ShiftLineGenerator::applyMergeProcess( @@ -821,7 +826,7 @@ AvoidLineArray ShiftLineGenerator::applyMergeProcess( // set parent id for (auto & al : merged_shift_lines) { - al.parent_ids = utils::avoidance::calcParentIds(shift_lines, al); + al.parent_ids = utils::static_obstacle_avoidance::calcParentIds(shift_lines, al); } // sort by distance from ego. @@ -963,15 +968,15 @@ void ShiftLineGenerator::applySimilarGradFilter( for (size_t i = 1; i < input.size(); ++i) { AvoidLine combine{}; - utils::avoidance::setStartData( + utils::static_obstacle_avoidance::setStartData( combine, base_line.start_shift_length, base_line.start, base_line.start_idx, base_line.start_longitudinal); - utils::avoidance::setEndData( + utils::static_obstacle_avoidance::setEndData( combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx, input.at(i).end_longitudinal); - combine.parent_ids = - utils::avoidance::concatParentIds(base_line.parent_ids, input.at(i).parent_ids); + combine.parent_ids = utils::static_obstacle_avoidance::concatParentIds( + base_line.parent_ids, input.at(i).parent_ids); combine_buffer.push_back(input.at(i)); @@ -1060,14 +1065,16 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // avoidance points: No, shift points: Yes -> select last shift point. if (!has_candidate_point && has_registered_point) { - last_sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); + last_sl = + utils::static_obstacle_avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); } // avoidance points: Yes, shift points: Yes -> select the last one from both. if (has_candidate_point && has_registered_point) { helper_->alignShiftLinesOrder(ret, false); const auto & al = ret.back(); - const auto & sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); + const auto & sl = + utils::static_obstacle_avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; } @@ -1190,8 +1197,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.start_idx = last_sl.end_idx; al.start = last_sl.end; al.start_longitudinal = arclength_from_ego.at(al.start_idx); - al.end_idx = - utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.end_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( + arclength_from_ego, prepare_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = prepare_distance_scaled; al.end_shift_length = last_sl.end_shift_length; @@ -1204,11 +1211,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( { AvoidLine al; al.id = generateUUID(); - al.start_idx = - utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.start_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( + arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = prepare_distance_scaled; - al.end_idx = utils::avoidance::findPathIndexFromArclength( + al.end_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); @@ -1315,7 +1322,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( void ShiftLineGenerator::updateRegisteredRawShiftLines(const AvoidancePlanningData & data) { - utils::avoidance::fillAdditionalInfoFromPoint(data, raw_registered_); + utils::static_obstacle_avoidance::fillAdditionalInfoFromPoint(data, raw_registered_); AvoidLineArray avoid_lines; @@ -1364,7 +1371,7 @@ void ShiftLineGenerator::setRawRegisteredShiftLine( } auto future_with_info = shift_lines; - utils::avoidance::fillAdditionalInfoFromPoint(data, future_with_info); + utils::static_obstacle_avoidance::fillAdditionalInfoFromPoint(data, future_with_info); // sort by longitudinal std::sort(future_with_info.begin(), future_with_info.end(), [](auto a, auto b) { @@ -1405,4 +1412,4 @@ void ShiftLineGenerator::setRawRegisteredShiftLine( } } } -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp similarity index 99% rename from planning/behavior_path_avoidance_module/src/utils.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 089571bc8392e..1533a7dd49edf 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -14,8 +14,8 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" @@ -37,7 +37,7 @@ #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using autoware_perception_msgs::msg::TrafficSignalElement; @@ -1681,7 +1681,8 @@ void filterTargetObjects( } // Find the footprint point closest to the path, set to object_data.overhang_distance. - o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); + o.overhang_points = + utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); // TODO(Satoshi Ota) parametrize stop time threshold if need. @@ -2303,4 +2304,4 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp similarity index 84% rename from planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index cf38a0b4993fe..c70b78265a17b 100644 --- a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -47,22 +47,22 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + module_names.emplace_back("behavior_path_planner::StaticObstacleAvoidanceModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_dynamic_avoidance_module") + - "/config/dynamic_avoidance.param.yaml"}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_avoidance_module/test/test_utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp similarity index 83% rename from planning/behavior_path_avoidance_module/test/test_utils.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp index 1a114eef9f7f7..da681e72533b0 100644 --- a/planning/behavior_path_avoidance_module/test/test_utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include #include using behavior_path_planner::ObjectData; -using behavior_path_planner::utils::avoidance::isOnRight; -using behavior_path_planner::utils::avoidance::isSameDirectionShift; -using behavior_path_planner::utils::avoidance::isShiftNecessary; +using behavior_path_planner::utils::static_obstacle_avoidance::isOnRight; +using behavior_path_planner::utils::static_obstacle_avoidance::isSameDirectionShift; +using behavior_path_planner::utils::static_obstacle_avoidance::isShiftNecessary; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp index 8e7d1f67d3157..d7fd8b82bd71e 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -14,7 +14,7 @@ #ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" namespace behavior_path_planner { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 54a4a0c70486d..4ef4c0673c0c5 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_lane_change_module/scene.hpp" #include @@ -24,7 +24,7 @@ namespace behavior_path_planner { using AvoidanceDebugData = DebugData; -using helper::avoidance::AvoidanceHelper; +using helper::static_obstacle_avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index 0f9f3f6dc9d42..84540618e152d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - behavior_path_avoidance_module + autoware_behavior_path_static_obstacle_avoidance_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index a3b28b4d63d06..c7c00d38956c1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index d7d7fa8b60513..80ae361ee3ea1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -14,13 +14,13 @@ #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" -#include +#include #include #include #include @@ -103,13 +103,14 @@ void AvoidanceByLaneChange::updateSpecialData() if (avoidance_data_.target_objects.empty()) { direction_ = Direction::NONE; } else { - direction_ = utils::avoidance::isOnRight(avoidance_data_.target_objects.front()) + direction_ = utils::static_obstacle_avoidance::isOnRight(avoidance_data_.target_objects.front()) ? Direction::LEFT : Direction::RIGHT; } - utils::avoidance::updateRegisteredObject(registered_objects_, avoidance_data_.target_objects, p); - utils::avoidance::compensateDetectionLost( + utils::static_obstacle_avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, p); + utils::static_obstacle_avoidance::compensateDetectionLost( registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); std::sort( @@ -222,28 +223,29 @@ std::optional AvoidanceByLaneChange::createObjectData( object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( + utils::static_obstacle_avoidance::fillObjectEnvelopePolygon( object_data, registered_objects_, object_closest_pose, p); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); + utils::static_obstacle_avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_points = - utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path); + object_data.overhang_points = utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance( + object_data, data.reference_path); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity(object_data, registered_objects_, vehicle_width, p); + utils::static_obstacle_avoidance::fillAvoidanceNecessity( + object_data, registered_objects_, vehicle_width, p); - utils::avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( + utils::static_obstacle_avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( data.reference_path_rough, getEgoPosition(), object_data); return object_data; } @@ -262,7 +264,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( - nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + nearest_object, utils::static_obstacle_avoidance::isOnRight(nearest_object), avoid_margin); return avoidance_helper_->getMinAvoidanceDistance(shift_length); } diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index f66e944cb7cbc..69ce87aabc7b4 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -67,8 +67,9 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + - "/config/avoidance.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + "/config/avoidance_by_lane_change.param.yaml"}); diff --git a/planning/behavior_path_avoidance_module/plugins.xml b/planning/behavior_path_avoidance_module/plugins.xml deleted file mode 100644 index f25677dad1e9a..0000000000000 --- a/planning/behavior_path_avoidance_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_dynamic_avoidance_module/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml deleted file mode 100644 index fd2e1bc4137b7..0000000000000 --- a/planning/behavior_path_dynamic_avoidance_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index beaa1ae6c5263..a799fad0a1c36 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -24,17 +24,17 @@ Essentially, the module has three primary responsibilities: Behavior Path Planner has following scene modules -| Name | Description | Details | -| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | -| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | -| External Lane Change | WIP | LINK | -| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | -| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | +| Name | Description | Details | +| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | +| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | !!! Note @@ -138,19 +138,19 @@ Enabling and disabling the modules in the behavior path planner is primarily man The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: -- `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. +- `launch_static_obstacle_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. !!! note Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. -The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `avoidance.enable_module` in +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in ```xml - + ``` -corresponds to launch_avoidance_module from `default_preset.yaml`. +corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`. Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. @@ -245,12 +245,12 @@ behavior_path_planner ├── behavior_path_planner.param.yaml ├── drivable_area_expansion.param.yaml ├── scene_module_manager.param.yaml -├── avoidance -│ └── avoidance.param.yaml +├── static_obstacle_avoidance +│ └── static_obstacle_avoidance.param.yaml ├── avoidance_by_lc │ └── avoidance_by_lc.param.yaml -├── dynamic_avoidance -│ └── dynamic_avoidance.param.yaml +├── dynamic_obstacle_avoidance +│ └── dynamic_obstacle_avoidance.param.yaml ├── goal_planner │ └── goal_planner.param.yaml ├── lane_change diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index c2b6babc402a2..6cf8719b2ef83 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -59,7 +59,7 @@ priority: 1 max_module_size: 1 - avoidance: + static_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false @@ -75,7 +75,7 @@ priority: 3 max_module_size: 1 - dynamic_avoidance: + dynamic_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp index ffc5daa7aa2ff..1bce30b18edd7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -21,7 +21,7 @@ namespace behavior_path_planner { // Forward Declaration -class AvoidanceModule; +class StaticObstacleAvoidanceModule; class AvoidanceByLCModule; class ExternalRequestLaneChangeModule; class LaneChangeInterface; @@ -35,7 +35,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; class SceneModuleVisitor { public: - void visitAvoidanceModule(const AvoidanceModule * module) const; + void visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 5e4b202071e49..ea80dc9bb9d1f 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -62,9 +62,9 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_BY_LC_LEFT; } else if (module_name == "avoidance_by_lane_change_right") { module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { + } else if (module_name == "static_obstacle_avoidance_left") { module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { + } else if (module_name == "static_obstacle_avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; } else if (module_name == "goal_planner") { module.type = Module::GOAL_PLANNER; From 0f1ae3e6b01e7e3f4b79b3b9c97e327309d9a25b Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 13:47:10 +0900 Subject: [PATCH 062/120] fix(ndt_scan_matcher): remove unused dependency (#7185) rm unused dependency Signed-off-by: a-maumau --- localization/ndt_scan_matcher/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index a37e7c370adf5..b62e810926331 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -22,7 +22,6 @@ diagnostic_msgs fmt geometry_msgs - glog libpcl-all-dev localization_util nav_msgs From 88c50b42a3266c221808a42a95c68a704ff68e33 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 14:24:41 +0900 Subject: [PATCH 063/120] feat(pose_initializer): componentize PoseInitializer (#7134) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * add log output both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- localization/pose_initializer/CMakeLists.txt | 12 ++++++-- .../launch/pose_initializer.launch.xml | 2 +- localization/pose_initializer/package.xml | 1 + .../pose_initializer_core.cpp | 6 +++- .../pose_initializer_core.hpp | 2 +- .../pose_initializer_node.cpp | 29 ------------------- 6 files changed, 17 insertions(+), 35 deletions(-) delete mode 100644 localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index 81a60a1d6b717..08d5bb47fca09 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -4,8 +4,7 @@ project(pose_initializer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_initializer_node - src/pose_initializer/pose_initializer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_initializer/pose_initializer_core.cpp src/pose_initializer/gnss_module.cpp src/pose_initializer/ndt_module.cpp @@ -15,6 +14,12 @@ ament_auto_add_executable(pose_initializer_node src/pose_initializer/ndt_localization_trigger_module.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInitializer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) @@ -30,7 +35,8 @@ if(BUILD_TESTING) add_testcase(test/test_copy_vector_to_array.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 3e4911e2c2936..2ffdebf39c474 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 85c9c26bd6c8c..58968828bc2fa 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -25,6 +25,7 @@ map_height_fitter motion_utils rclcpp + rclcpp_components std_srvs tier4_autoware_utils tier4_localization_msgs diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index dd368960d33db..2fc7d61890837 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -26,7 +26,8 @@ #include #include -PoseInitializer::PoseInitializer() : Node("pose_initializer") +PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_initializer", options) { const auto node = component_interface_utils::NodeAdaptor(this); group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -209,3 +210,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializer::get_gnss_pose() throw ServiceException( Initialize::Service::Response::ERROR_GNSS_SUPPORT, "GNSS is not supported."); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInitializer) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index c7f1c5eec6db0..eeba71b90129b 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -34,7 +34,7 @@ class NdtLocalizationTriggerModule; class PoseInitializer : public rclcpp::Node { public: - PoseInitializer(); + explicit PoseInitializer(const rclcpp::NodeOptions & options); ~PoseInitializer(); private: diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp deleted file mode 100644 index c5b31c4e37ccd..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2020 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 "pose_initializer_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); - return 0; -} From d5a6c54cc57a98911fc199680746b779fdcd5aee Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 31 May 2024 15:36:22 +0900 Subject: [PATCH 064/120] feat(lane_change): apply new RTC state (#7152) * feat(lane_change): support for new RTC state transition Signed-off-by: Fumiya Watanabe * fix: distance at aborting Signed-off-by: Fumiya Watanabe * fix(lane_change): empty check Signed-off-by: Fumiya Watanabe * fix(rtc_interface):update activation depends on RTC state Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../interface.hpp | 12 ++++++++++ .../src/interface.cpp | 22 +++++++++++++++---- .../src/utils/utils.cpp | 10 +++++++-- planning/rtc_interface/src/rtc_interface.cpp | 22 +++++++++++++++---- 4 files changed, 56 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 6e40f880467fd..4c3d129ac687e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -125,6 +125,18 @@ class LaneChangeInterface : public SceneModuleInterface ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; + void updateRTCStatus( + const double start_distance, const double finish_distance, const bool safe, + const uint8_t & state) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), safe, state, start_distance, finish_distance, clock_->now()); + } + } + } + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 2567787a3f2e4..1669842117f9f 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -117,13 +117,18 @@ BehaviorModuleOutput LaneChangeInterface::plan() updateSteeringFactorPtr(output); if (module_type_->isAbortState()) { waitApproval(); - removeRTCStatus(); const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::ABORTING); } else { clearWaitingApproval(); + const auto path = + assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); } return output; @@ -147,7 +152,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { - removeRTCStatus(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); path_candidate_ = std::make_shared(); return out; } @@ -156,7 +163,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus( - candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; return out; @@ -239,6 +247,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } @@ -249,6 +260,9 @@ bool LaneChangeInterface::canTransitFailureState() if (module_type_->isAbleToReturnCurrentLane()) { log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); return true; } } diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index ab9a0b7176fdb..4eccd00d78c09 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -646,8 +646,14 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path) { - const auto start_idx = path.info.shift_line.start_idx; - const auto end_idx = path.info.shift_line.end_idx; + if (path.shifted_path.shift_length.empty()) { + return 0.0; + } + + const auto start_idx = + std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1); + const auto end_idx = + std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1); return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index ea80dc9bb9d1f..77fe29b41d893 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -153,7 +153,16 @@ std::vector RTCInterface::validateCooperateCommands( registered_status_.statuses.begin(), registered_status_.statuses.end(), [command](auto & s) { return s.uuid == command.uuid; }); if (itr != registered_status_.statuses.end()) { - response.success = true; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + response.success = true; + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[validateCooperateCommands] uuid : " + << to_string(command.uuid) + << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " + << itr->state.type << std::endl); + response.success = false; + } } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) @@ -175,8 +184,10 @@ void RTCInterface::updateCooperateCommandStatus(const std::vectorcommand_status = command.command; - itr->auto_mode = false; + if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { + itr->command_status = command.command; + itr->auto_mode = false; + } } } } @@ -219,7 +230,7 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; - status.state.type = State::WAITING_FOR_EXECUTION; + status.state.type = state; status.start_distance = start_distance; status.finish_distance = finish_distance; status.auto_mode = is_auto_mode_enabled_; @@ -291,6 +302,9 @@ bool RTCInterface::isActivated(const UUID & uuid) const [uuid](auto & s) { return s.uuid == uuid; }); if (itr != registered_status_.statuses.end()) { + if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) { + return false; + } if (itr->auto_mode) { return itr->safe; } else { From c92185ae76acbe22a505efaf83533c6377a7f5a6 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 31 May 2024 16:00:16 +0900 Subject: [PATCH 065/120] chore(motion_velocity_smoother): remove string stream creation (#7131) * chore(motion_velocity_smoother): remove string stream creation Signed-off-by: Fumiya Watanabe * fix: use format string Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../velocity_planning_utils.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 2bcb31ff10969..9fa4d697f06cb 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -199,13 +199,12 @@ bool calcStopVelocityWithConstantJerkAccLimit( } // for debug - std::stringstream ss; + RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity."); for (unsigned int i = 0; i < ts.size(); ++i) { - ss << "t: " << ts.at(i) << ", x: " << xs.at(i) << ", v: " << vs.at(i) << ", a: " << as.at(i) - << ", j: " << js.at(i) << std::endl; + RCLCPP_DEBUG( + rclcpp::get_logger("velocity_planning_utils"), "--- t: %f, x: %f, v: %f, a: %f, j: %f", + ts.at(i), xs.at(i), vs.at(i), as.at(i), js.at(i)); } - RCLCPP_DEBUG( - rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity. %s", ss.str().c_str()); const double a_target = 0.0; const double v_margin = 0.3; From fea4ed982b6b36b0ad37d803262af6c22520e8c0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 31 May 2024 16:05:34 +0900 Subject: [PATCH 066/120] fix(avoidance): fix bugs in parked vehicle filtering logic (#7072) * fix(avoidance): check prev/next lane Signed-off-by: satoshi-ota * fix(avoidance): fix parked vehicle filtering Signed-off-by: satoshi-ota * fix(avoidance): check intersection location Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/utils.cpp | 95 ++++++++++++++++--- 1 file changed, 80 insertions(+), 15 deletions(-) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 1533a7dd49edf..660b8d228b6f2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -318,25 +318,55 @@ bool isWithinCrosswalk( bool isWithinIntersection( const ObjectData & object, const std::shared_ptr & route_handler) { - const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else"); - if (id == "else") { + const std::string area_id = object.overhang_lanelet.attributeOr("intersection_area", "else"); + if (area_id == "else") { + return false; + } + + const std::string location = object.overhang_lanelet.attributeOr("location", "else"); + if (location == "private") { return false; } const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); - const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); return boost::geometry::within( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } -bool isOnEgoLane(const ObjectData & object) +bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - return boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), - object.overhang_lanelet.polygon2d().basicPolygon()); + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon())) { + return true; + } + + // push previous lanelet + lanelet::ConstLanelets prev_lanelet; + if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + prev_lanelet.front().polygon2d().basicPolygon())) { + return true; + } + } + + // push next lanelet + lanelet::ConstLanelet next_lanelet; + if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + next_lanelet.polygon2d().basicPolygon())) { + return true; + } + } + + return false; } bool isParallelToEgoLane(const ObjectData & object, const double threshold) @@ -604,14 +634,49 @@ bool isNeverAvoidanceTarget( if (object.is_on_ego_lane) { const auto right_lane = - planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true); + if (right_lane.has_value() && isOnRight(object)) { + const lanelet::Attribute & right_lane_sub_type = + right_lane.value().attribute(lanelet::AttributeName::Subtype); + if (right_lane_sub_type != "road_shoulder") { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto is_disjoint_right_lane = + boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon()); + if (is_disjoint_right_lane) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + } + const auto left_lane = - planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); - if (right_lane.has_value() && left_lane.has_value()) { - object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; - RCLCPP_DEBUG( - rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); - return true; + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); + if (left_lane.has_value() && !isOnRight(object)) { + const lanelet::Attribute & left_lane_sub_type = + left_lane.value().attribute(lanelet::AttributeName::Subtype); + if (left_lane_sub_type != "road_shoulder") { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + const auto is_disjoint_left_lane = + boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon()); + if (is_disjoint_left_lane) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } } } @@ -754,7 +819,7 @@ bool isSatisfiedWithVehicleCondition( const std::shared_ptr & parameters) { object.behavior = getObjectBehavior(object, parameters); - object.is_on_ego_lane = isOnEgoLane(object); + object.is_on_ego_lane = isOnEgoLane(object, planner_data->route_handler); if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { return false; From 693244601f3e67c88bb4eca52ee38fb323c59934 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 16:10:14 +0900 Subject: [PATCH 067/120] feat(ar_tag_based_localizer): componentize ArTagBasedLocalizer (#7187) * remove unusing main func Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change exec name and change log output from log to both Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ar_tag_based_localizer/CMakeLists.txt | 20 ++++--- .../launch/ar_tag_based_localizer.launch.xml | 2 +- .../ar_tag_based_localizer/package.xml | 1 + .../src/ar_tag_based_localizer.cpp | 5 +- .../ar_tag_based_localizer/src/main.cpp | 53 ------------------- 5 files changed, 19 insertions(+), 62 deletions(-) delete mode 100644 localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index d625064b8f6cb..a8435fa056847 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -14,27 +14,33 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -ament_auto_add_executable(ar_tag_based_localizer - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/ar_tag_based_localizer.cpp ) -target_include_directories(ar_tag_based_localizer + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) -target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ArTagBasedLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_ar_tag_based_localizer + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/ar_tag_based_localizer.cpp ) - target_include_directories(test_ar_tag_based_localizer + target_include_directories(test_${PROJECT_NAME} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS} ) - target_link_libraries(test_ar_tag_based_localizer ${OpenCV_LIBRARIES}) + target_link_libraries(test_${PROJECT_NAME} ${OpenCV_LIBRARIES}) endif() ament_auto_package( diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 272338905c3f0..34602ca70daf4 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 072479cc7aaf5..90fdd2fee31f4 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -24,6 +24,7 @@ lanelet2_extension localization_util rclcpp + rclcpp_components tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 43ac1e1098453..9a6823e330acd 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -65,7 +65,7 @@ #include ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) -: Node("ar_tag_based_localizer", options), cam_info_received_(false) +: rclcpp::Node("ar_tag_based_localizer", options), cam_info_received_(false) { /* Declare node parameters @@ -346,3 +346,6 @@ std::vector ArTagBasedLocalizer::detect_landmarks( return landmarks; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ArTagBasedLocalizer) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp deleted file mode 100644 index 8ef1dd6195580..0000000000000 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2023 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr ptr = std::make_shared(); - rclcpp::spin(ptr); - rclcpp::shutdown(); -} From 05e307118df971027992570576cd2fb8c709517e Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 31 May 2024 16:35:02 +0900 Subject: [PATCH 068/120] feat(obstacle_cruise_planner)!: ignore to garze against unknwon objects (#7177) Signed-off-by: Yuki Takagi Co-authored-by: Takayuki Murooka --- .../config/obstacle_cruise_planner.param.yaml | 3 ++- .../include/obstacle_cruise_planner/node.hpp | 1 + planning/obstacle_cruise_planner/src/node.cpp | 23 ++++++++++++++----- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index bd9c5750af78f..2ccd000657948 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -87,7 +87,8 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint + max_lat_margin_against_unknown: -0.3 # lateral margin between the unknown obstacles and ego's footprint crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 3f18c6e1728c6..fd65446408db1 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -200,6 +200,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double prediction_resampling_time_horizon; // max lateral margin double max_lat_margin_for_stop; + double max_lat_margin_for_stop_against_unknown; double max_lat_margin_for_cruise; double max_lat_margin_for_slow_down; double lat_hysteresis_margin_for_slow_down; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 21e3ecb7bf758..c5f0c73a13b78 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -247,6 +247,8 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara max_lat_margin_for_stop = node.declare_parameter("behavior_determination.stop.max_lat_margin"); + max_lat_margin_for_stop_against_unknown = + node.declare_parameter("behavior_determination.stop.max_lat_margin_against_unknown"); max_lat_margin_for_cruise = node.declare_parameter("behavior_determination.cruise.max_lat_margin"); enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); @@ -311,6 +313,9 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.stop.max_lat_margin_against_unknown", + max_lat_margin_for_stop_against_unknown); tier4_autoware_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); tier4_autoware_utils::updateParam( @@ -682,8 +687,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( }(); const double max_lat_margin = std::max( - std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise), - p.max_lat_margin_for_slow_down); + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); if (max_lat_margin < min_lat_dist_to_traj_poly) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1174,7 +1179,13 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( if (!isStopObstacle(obstacle.classification.label)) { return std::nullopt; } - if (p.max_lat_margin_for_stop < precise_lat_dist) { + + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { return std::nullopt; } @@ -1207,7 +1218,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + std::hypot( vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop)); + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); if (collision_points.empty()) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1232,8 +1243,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = + createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); From b3c83d50e1eb0d1849e7ef54fefaddb9e78ab1ca Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 16:36:08 +0900 Subject: [PATCH 069/120] feat(pose_estimator_arbiter): componentize PoseEstimatorArbiter (#7183) * mod to componentize and use glog Signed-off-by: a-maumau * change log output from screen to both Signed-off-by: a-maumau * style(pre-commit): autofix * Update localization/pose_estimator_arbiter/CMakeLists.txt add namespace Co-authored-by: Kento Yabuuchi * remove unusing main func Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kento Yabuuchi --- .../pose_estimator_arbiter/CMakeLists.txt | 25 ++++++-------- .../example_rule/CMakeLists.txt | 4 +-- .../launch/pose_estimator_arbiter.launch.xml | 2 +- .../pose_estimator_arbiter/package.xml | 5 +-- .../pose_estimator_arbiter.hpp | 2 +- .../pose_estimator_arbiter_core.cpp | 7 ++-- .../pose_estimator_arbiter_node.cpp | 33 ------------------- 7 files changed, 20 insertions(+), 58 deletions(-) delete mode 100644 localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt index 9a47b654a6ab4..eefb7fc9a6879 100644 --- a/localization/pose_estimator_arbiter/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -4,29 +4,23 @@ project(pose_estimator_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(glog REQUIRED) - find_package(PCL REQUIRED COMPONENTS common) include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) -# ============================== -# switch rule library -ament_auto_add_library(switch_rule - SHARED - src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp -) -target_include_directories(switch_rule PUBLIC src) - # ============================== # pose estimator arbiter node -ament_auto_add_executable(${PROJECT_NAME} +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp - src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC src) -target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) -# ============================== +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -53,6 +47,7 @@ endif() add_subdirectory(example_rule) # ============================== -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch ) diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt index 333f92842b860..b2b5a828e42e7 100644 --- a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -7,7 +7,7 @@ ament_auto_add_library(example_rule src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp ) target_include_directories(example_rule PUBLIC src example_rule/src) -target_link_libraries(example_rule switch_rule) +target_link_libraries(example_rule pose_estimator_arbiter) # ============================== # define test definition macro @@ -16,7 +16,7 @@ function(add_testcase filepath) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" switch_rule example_rule) + target_link_libraries("${test_name}" pose_estimator_arbiter example_rule) target_include_directories(${test_name} PUBLIC src) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml index 0a708e3f48988..b5be96fc3ce44 100644 --- a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 480b323f3031d..d164086ada87e 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -15,12 +15,12 @@ autoware_auto_mapping_msgs diagnostic_msgs geometry_msgs - glog lanelet2_extension magic_enum pcl_conversions pcl_ros pluginlib + rclcpp_components sensor_msgs std_msgs std_srvs @@ -29,9 +29,6 @@ visualization_msgs yabloc_particle_filter - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index 9e67dfc063964..54dac6ac254c1 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -47,7 +47,7 @@ class PoseEstimatorArbiter : public rclcpp::Node using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: - PoseEstimatorArbiter(); + explicit PoseEstimatorArbiter(const rclcpp::NodeOptions & options); private: // Set of running pose estimators specified by ros param `pose_sources` diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp index 4fc3fd9b914a6..67c555227976d 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -42,8 +42,8 @@ static std::unordered_set parse_estimator_name_args( return running_estimator_list; } -PoseEstimatorArbiter::PoseEstimatorArbiter() -: Node("pose_estimator_arbiter"), +PoseEstimatorArbiter::PoseEstimatorArbiter(const rclcpp::NodeOptions & options) +: rclcpp::Node("pose_estimator_arbiter", options), running_estimator_list_(parse_estimator_name_args( declare_parameter>("pose_sources"), get_logger())), logger_configure_(std::make_unique(this)) @@ -211,3 +211,6 @@ void PoseEstimatorArbiter::on_timer() } } // namespace pose_estimator_arbiter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pose_estimator_arbiter::PoseEstimatorArbiter) diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp deleted file mode 100644 index 20aaaf10abaab..0000000000000 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 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 "pose_estimator_arbiter/pose_estimator_arbiter.hpp" - -#include - -int main(int argc, char * argv[]) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} From 633f9b26f012dd5b9f1a5479da50477aa03e3272 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 16:49:42 +0900 Subject: [PATCH 070/120] feat(map_height_fitter): componentize MapHeightFitter (#7192) * rename file name and mod to componentize and use glog Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change exec name and add log output both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- map/map_height_fitter/CMakeLists.txt | 12 ++++++++---- .../launch/map_height_fitter.launch.xml | 2 +- map/map_height_fitter/package.xml | 1 + .../src/{node.cpp => map_height_fitter_node.cpp} | 15 ++++----------- 4 files changed, 14 insertions(+), 16 deletions(-) rename map/map_height_fitter/src/{node.cpp => map_height_fitter_node.cpp} (83%) diff --git a/map/map_height_fitter/CMakeLists.txt b/map/map_height_fitter/CMakeLists.txt index 8821158c54757..0dec2f6a1663a 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/map_height_fitter/CMakeLists.txt @@ -5,8 +5,9 @@ find_package(autoware_cmake REQUIRED) find_package(PCL REQUIRED COMPONENTS common) autoware_package() -ament_auto_add_library(map_height_fitter SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_height_fitter.cpp + src/map_height_fitter_node.cpp ) target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) @@ -14,11 +15,14 @@ target_link_libraries(map_height_fitter ${PCL_LIBRARIES}) # These are treated as errors in compile, so pedantic warnings are disabled for this package. target_compile_options(map_height_fitter PRIVATE -Wno-pedantic) -ament_auto_add_executable(node - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapHeightFitterNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor ) ament_auto_package( INSTALL_TO_SHARE launch - config) + config +) diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/map_height_fitter/launch/map_height_fitter.launch.xml index 353f2151ee172..3e01a35a8e519 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/map_height_fitter/launch/map_height_fitter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 7f384aa43ec7b..568c77f2509c6 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -25,6 +25,7 @@ libpcl-common pcl_conversions rclcpp + rclcpp_components sensor_msgs tf2_geometry_msgs tf2_ros diff --git a/map/map_height_fitter/src/node.cpp b/map/map_height_fitter/src/map_height_fitter_node.cpp similarity index 83% rename from map/map_height_fitter/src/node.cpp rename to map/map_height_fitter/src/map_height_fitter_node.cpp index 55702dc08450d..fdc7604b68855 100644 --- a/map/map_height_fitter/src/node.cpp +++ b/map/map_height_fitter/src/map_height_fitter_node.cpp @@ -23,7 +23,8 @@ using tier4_localization_msgs::srv::PoseWithCovarianceStamped; class MapHeightFitterNode : public rclcpp::Node { public: - MapHeightFitterNode() : Node("map_height_fitter"), fitter_(this) + explicit MapHeightFitterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("map_height_fitter", options), fitter_(this) { const auto on_service = [this]( const PoseWithCovarianceStamped::Request::SharedPtr req, @@ -46,13 +47,5 @@ class MapHeightFitterNode : public rclcpp::Node rclcpp::Service::SharedPtr srv_; }; -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapHeightFitterNode) From 38ccce7a81183ab9962159442004496e963c8ece Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 31 May 2024 18:24:40 +0900 Subject: [PATCH 071/120] feat(diagnostic_graph_utils): componentize node (#7189) Signed-off-by: Takagi, Isamu --- system/diagnostic_graph_utils/CMakeLists.txt | 13 ++++++++++--- .../diagnostic_graph_utils/doc/node/converter.md | 4 ++-- system/diagnostic_graph_utils/doc/node/dump.md | 4 ++-- system/diagnostic_graph_utils/package.xml | 1 + .../diagnostic_graph_utils/src/node/converter.cpp | 15 +++------------ .../diagnostic_graph_utils/src/node/converter.hpp | 2 +- system/diagnostic_graph_utils/src/node/dump.cpp | 15 +++------------ system/diagnostic_graph_utils/src/node/dump.hpp | 2 +- 8 files changed, 23 insertions(+), 33 deletions(-) diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt index 48af5e8fc304f..b68e7bedb57ed 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/diagnostic_graph_utils/CMakeLists.txt @@ -9,12 +9,19 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/lib/subscription.cpp ) -ament_auto_add_executable(dump +ament_auto_add_library(${PROJECT_NAME}_tools SHARED src/node/dump.cpp + src/node/converter.cpp ) -ament_auto_add_executable(converter - src/node/converter.cpp +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::DumpNode" + EXECUTABLE dump_node +) + +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::ConverterNode" + EXECUTABLE converter_node ) ament_auto_package() diff --git a/system/diagnostic_graph_utils/doc/node/converter.md b/system/diagnostic_graph_utils/doc/node/converter.md index c3db547167474..407a99c87f73e 100644 --- a/system/diagnostic_graph_utils/doc/node/converter.md +++ b/system/diagnostic_graph_utils/doc/node/converter.md @@ -5,7 +5,7 @@ This tool converts `/diagnostics_graph` to `/diagnostics_array` so it can be rea ## Usage ```bash -ros2 run diagnostic_graph_utils converter +ros2 run diagnostic_graph_utils converter_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils converter +ros2 run diagnostic_graph_utils converter_node ``` Terminal 3: diff --git a/system/diagnostic_graph_utils/doc/node/dump.md b/system/diagnostic_graph_utils/doc/node/dump.md index 090e9679676b6..c76bb85ed75cb 100644 --- a/system/diagnostic_graph_utils/doc/node/dump.md +++ b/system/diagnostic_graph_utils/doc/node/dump.md @@ -5,7 +5,7 @@ This tool displays `/diagnostics_graph` in table format. ## Usage ```bash -ros2 run diagnostic_graph_utils dump +ros2 run diagnostic_graph_utils dump_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils dump +ros2 run diagnostic_graph_utils dump_node ``` Output: diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml index 42d44efb6697e..c5a70363bfecb 100644 --- a/system/diagnostic_graph_utils/package.xml +++ b/system/diagnostic_graph_utils/package.xml @@ -12,6 +12,7 @@ diagnostic_msgs rclcpp + rclcpp_components tier4_system_msgs ament_lint_auto diff --git a/system/diagnostic_graph_utils/src/node/converter.cpp b/system/diagnostic_graph_utils/src/node/converter.cpp index e9e40fa0a0756..159cc6e0c3cab 100644 --- a/system/diagnostic_graph_utils/src/node/converter.cpp +++ b/system/diagnostic_graph_utils/src/node/converter.cpp @@ -19,7 +19,7 @@ namespace diagnostic_graph_utils { -ConverterNode::ConverterNode() : Node("converter") +ConverterNode::ConverterNode(const rclcpp::NodeOptions & options) : Node("converter", options) { using std::placeholders::_1; pub_array_ = create_publisher("/diagnostics_array", rclcpp::QoS(1)); @@ -40,14 +40,5 @@ void ConverterNode::on_update(DiagGraph::ConstSharedPtr graph) } // namespace diagnostic_graph_utils -int main(int argc, char ** argv) -{ - using diagnostic_graph_utils::ConverterNode; - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::ConverterNode) diff --git a/system/diagnostic_graph_utils/src/node/converter.hpp b/system/diagnostic_graph_utils/src/node/converter.hpp index 09ce62d7293ec..19364a8ff8240 100644 --- a/system/diagnostic_graph_utils/src/node/converter.hpp +++ b/system/diagnostic_graph_utils/src/node/converter.hpp @@ -28,7 +28,7 @@ namespace diagnostic_graph_utils class ConverterNode : public rclcpp::Node { public: - ConverterNode(); + explicit ConverterNode(const rclcpp::NodeOptions & options); private: using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; diff --git a/system/diagnostic_graph_utils/src/node/dump.cpp b/system/diagnostic_graph_utils/src/node/dump.cpp index 5fa4922dec1a5..8456a92cbaa9b 100644 --- a/system/diagnostic_graph_utils/src/node/dump.cpp +++ b/system/diagnostic_graph_utils/src/node/dump.cpp @@ -24,7 +24,7 @@ namespace diagnostic_graph_utils { -DumpNode::DumpNode() : Node("dump") +DumpNode::DumpNode(const rclcpp::NodeOptions & options) : Node("dump", options) { using std::placeholders::_1; sub_graph_.register_create_callback(std::bind(&DumpNode::on_create, this, _1)); @@ -132,14 +132,5 @@ void DumpNode::on_update(DiagGraph::ConstSharedPtr graph) } // namespace diagnostic_graph_utils -int main(int argc, char ** argv) -{ - using diagnostic_graph_utils::DumpNode; - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::DumpNode) diff --git a/system/diagnostic_graph_utils/src/node/dump.hpp b/system/diagnostic_graph_utils/src/node/dump.hpp index e37a1ea971bf5..c990fb77a53db 100644 --- a/system/diagnostic_graph_utils/src/node/dump.hpp +++ b/system/diagnostic_graph_utils/src/node/dump.hpp @@ -28,7 +28,7 @@ namespace diagnostic_graph_utils class DumpNode : public rclcpp::Node { public: - DumpNode(); + explicit DumpNode(const rclcpp::NodeOptions & options); private: void on_create(DiagGraph::ConstSharedPtr graph); From 9ae42389e978fe4743e2a1e53b478a70236be53c Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 31 May 2024 18:25:11 +0900 Subject: [PATCH 072/120] feat(dummy_diag_publisher): componentize node (#7190) Signed-off-by: Takagi, Isamu --- system/dummy_diag_publisher/CMakeLists.txt | 8 ++++-- .../dummy_diag_publisher_core.hpp | 2 +- .../dummy_diag_publisher_node.launch.xml | 2 +- .../src/dummy_diag_publisher_core.cpp | 17 +++++++----- .../src/dummy_diag_publisher_node.cpp | 26 ------------------- 5 files changed, 19 insertions(+), 36 deletions(-) delete mode 100644 system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/dummy_diag_publisher/CMakeLists.txt index cecb317bf9c34..794e7d35e1194 100644 --- a/system/dummy_diag_publisher/CMakeLists.txt +++ b/system/dummy_diag_publisher/CMakeLists.txt @@ -4,11 +4,15 @@ project(dummy_diag_publisher) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/dummy_diag_publisher_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/dummy_diag_publisher_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "DummyDiagPublisher" + EXECUTABLE ${PROJECT_NAME}_node +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp index 8398c15b8e8f6..071e665ece6ec 100644 --- a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp +++ b/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp @@ -35,7 +35,7 @@ struct DiagConfig class DummyDiagPublisher : public rclcpp::Node { public: - DummyDiagPublisher(); + explicit DummyDiagPublisher(const rclcpp::NodeOptions & options); private: enum Status { diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml index 8e71ce37543c2..9d9193fb5f7a7 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml +++ b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index cfe1692c91df9..9abf325e62833 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -245,12 +245,14 @@ void DummyDiagPublisher::onTimer() updater_.force_update(); } -DummyDiagPublisher::DummyDiagPublisher() -: Node( - "dummy_diag_publisher", rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)), - updater_(this) +rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) +{ + return options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( + true); +} + +DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) +: Node("dummy_diag_publisher", override_options(options)), updater_(this) { // Parameter @@ -277,3 +279,6 @@ DummyDiagPublisher::DummyDiagPublisher() timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&DummyDiagPublisher::onTimer, this)); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(DummyDiagPublisher) diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp deleted file mode 100644 index a532e8d1c6d01..0000000000000 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2020 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 "dummy_diag_publisher/dummy_diag_publisher_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} From ba292be31fcb069081ae0cb56a265989a8e59969 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 31 May 2024 18:37:39 +0900 Subject: [PATCH 073/120] refactor(start_planner): improve readability of end condition (#7181) * refactor(start_planner): clarify condition of transition to success Signed-off-by: kyoichi-sugahara * modify flowchart Signed-off-by: kyoichi-sugahara * add debug print Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../README.md | 76 +++++++++++++------ .../start_planner_module.hpp | 3 +- .../src/start_planner_module.cpp | 48 +++++++++--- 3 files changed, 92 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ff0550e4d4867..dd3dddef9d863 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -91,36 +91,66 @@ The `StartPlannerModule` is designed to initiate its execution based on specific ### **End Conditions** -The `StartPlannerModule` terminates if the pull out / freespace maneuver has been completed. The `canTransitSuccessState` function assesses these conditions to decide if the module should terminate its execution. +The `StartPlannerModule` terminates when specific conditions are met, depending on the type of planner being used. The `canTransitSuccessState` function determines whether the module should transition to the success state based on the following criteria: -```plantuml -@startuml -start -:Start hasFinishedPullOut(); +#### When the Freespace Planner is active -if (status_.driving_forward && status_.found_pull_out_path) then (yes) +- If the end point of the freespace path is reached, the module transitions to the success state. - if (status_.planner_type == FREESPACE) then (yes) - :Calculate distance\nto pull_out_path.end_pose; - if (distance < th_arrived_distance) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - else (no) - :Calculate arclength for\ncurrent_pose and pull_out_path.end_pose; - if (arclength_current - arclength_pull_out_end > offset) then (yes) - :return true;\n(Terminate module); - else (no) - :return false;\n(Continue running); - endif - endif +#### When any other type of planner is active + +The transition to the success state is determined by the following conditions: + +- If a reverse path is being generated or the search for a pull out path fails: + - The module does not transition to the success state. +- If the end point of the pull out path's shift section is reached: + - The module transitions to the success state. + +The flowchart below illustrates the decision-making process in the `canTransitSuccessState` function: +```plantuml +@startuml +@startuml +skinparam ActivityBackgroundColor #white +skinparam ActivityBorderColor #black +skinparam ActivityBorderThickness 1 +skinparam ActivityArrowColor #black +skinparam ActivityArrowThickness 1 +skinparam ActivityStartColor #black +skinparam ActivityEndColor #black +skinparam ActivityDiamondBackgroundColor #white +skinparam ActivityDiamondBorderColor #black +skinparam ActivityDiamondFontColor #black +partition canTransitSuccessState() { +start +if (planner type is FREESPACE?) then (yes) +if (Has reached freespace end?) then (yes) +#FF006C:true; +stop else (no) - :return false;\n(Continue running); +:false; +stop endif - +else (no) +if (driving is forward?) then (yes) +if (pull out path is found?) then (yes) +if (Has reached pull out end?) then (yes) +#FF006C:true; stop +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +endif +} @enduml ``` diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 591d8adb60819..ea5afb133d57f 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -306,7 +306,8 @@ class StartPlannerModule : public SceneModuleInterface const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; - bool hasFinishedPullOut() const; + bool hasReachedFreespaceEnd() const; + bool hasReachedPullOutEnd() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; bool isStopped(); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 280e1112ef7fb..d3afdb2aefae5 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -579,7 +579,35 @@ bool StartPlannerModule::isExecutionReady() const bool StartPlannerModule::canTransitSuccessState() { - return hasFinishedPullOut(); + // Freespace Planner: + // - Can transit to success if the goal position is reached. + // - Cannot transit to success if the goal position is not reached. + if (status_.planner_type == PlannerType::FREESPACE) { + if (hasReachedFreespaceEnd()) { + RCLCPP_DEBUG( + getLogger(), "Transit to success: Freespace planner reached the end point of the path."); + return true; + } + return false; + } + + // Other Planners: + // - Cannot transit to success if the vehicle is driving in reverse. + // - Cannot transit to success if a safe path cannot be found due to: + // - Insufficient margin against static objects. + // - No path found that stays within the lane. + // In such cases, a stop point needs to be embedded and keep running start_planner. + // - Can transit to success if the end point of the pullout path is reached. + if (!status_.driving_forward || !status_.found_pull_out_path) { + return false; + } + + if (hasReachedPullOutEnd()) { + RCLCPP_DEBUG(getLogger(), "Transit to success: Reached the end point of the pullout path."); + return true; + } + + return false; } BehaviorModuleOutput StartPlannerModule::plan() @@ -1180,17 +1208,16 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( return stop_objects_in_pull_out_lanes; } -bool StartPlannerModule::hasFinishedPullOut() const +bool StartPlannerModule::hasReachedFreespaceEnd() const { - if (!status_.driving_forward || !status_.found_pull_out_path) { - return false; - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; +} +bool StartPlannerModule::hasReachedPullOutEnd() const +{ const auto current_pose = planner_data_->self_odometry->pose.pose; - if (status_.planner_type == PlannerType::FREESPACE) { - return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < - parameters_->th_arrived_distance; - } // check that ego has passed pull out end point const double backward_path_length = @@ -1205,9 +1232,8 @@ bool StartPlannerModule::hasFinishedPullOut() const // offset to not finish the module before engage constexpr double offset = 0.1; - const bool has_finished = arclength_current.length - arclength_pull_out_end.length > offset; - return has_finished; + return arclength_current.length - arclength_pull_out_end.length > offset; } bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const From 76ae2cdd8b7f6325d7ca361c638cbec5626af30b Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 31 May 2024 19:34:00 +0900 Subject: [PATCH 074/120] fix(ndt_scan_matcher): change diag time_stamp (#7195) * change diag time_stamp Signed-off-by: Yamato Ando * fix time now Signed-off-by: Yamato Ando * style(pre-commit): autofix * rename variable Signed-off-by: Yamato Ando --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ndt_scan_matcher/diagnostics_module.hpp | 5 ++-- .../src/diagnostics_module.cpp | 9 ++++--- .../src/map_update_module.cpp | 2 -- .../src/ndt_scan_matcher_core.cpp | 26 ++++++++++++------- 4 files changed, 25 insertions(+), 17 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp index 828eb6bed346b..4f7b5eff6521b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -31,10 +31,11 @@ class DiagnosticsModule template void addKeyValue(const std::string & key, const T & value); void updateLevelAndMessage(const int8_t level, const std::string & message); - void publish(); + void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; rclcpp::Publisher::SharedPtr diagnostics_pub_; diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp index 9d7eed46414ea..1e08ebb89f51e 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -85,15 +85,16 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str } } -void DiagnosticsModule::publish() +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray()); + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; - diagnostics_msg.header.stamp = clock_->now(); + diagnostics_msg.header.stamp = publish_time_stamp; diagnostics_msg.status.push_back(diagnostics_status_msg_); if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index ae68dfc44c2c9..40f0b1f4465fa 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -52,8 +52,6 @@ void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().nanoseconds()); - // check is_activated diagnostics_ptr->addKeyValue("is_activated", is_activated); if (!is_activated) { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 4da1c6865dbb7..8acfe3bd5c1ca 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -193,11 +193,15 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) void NDTScanMatcher::callback_timer() { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_map_update_->clear(); + diagnostics_map_update_->addKeyValue("timer_callback_time_stamp", ros_time_now.nanoseconds()); + map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); - diagnostics_map_update_->publish(); + diagnostics_map_update_->publish(ros_time_now); } void NDTScanMatcher::callback_initial_pose( @@ -207,7 +211,7 @@ void NDTScanMatcher::callback_initial_pose( callback_initial_pose_main(initial_pose_msg_ptr); - diagnostics_initial_pose_->publish(); + diagnostics_initial_pose_->publish(initial_pose_msg_ptr->header.stamp); } void NDTScanMatcher::callback_initial_pose_main( @@ -260,7 +264,7 @@ void NDTScanMatcher::callback_regularization_pose( regularization_pose_buffer_->push_back(pose_conv_msg_ptr); - diagnostics_regularization_pose_->publish(); + diagnostics_regularization_pose_->publish(pose_conv_msg_ptr->header.stamp); } void NDTScanMatcher::callback_sensor_points( @@ -286,7 +290,7 @@ void NDTScanMatcher::callback_sensor_points( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_scan_points_->publish(); + diagnostics_scan_points_->publish(sensor_points_msg_in_sensor_frame->header.stamp); } bool NDTScanMatcher::callback_sensor_points_main( @@ -867,8 +871,10 @@ void NDTScanMatcher::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -878,15 +884,19 @@ void NDTScanMatcher::service_trigger_node( diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); - diagnostics_trigger_node_->publish(); + diagnostics_trigger_node_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { + const rclcpp::Time ros_time_now = this->now(); + diagnostics_ndt_align_->clear(); + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); + service_ndt_align_main(req, res); // check is_succeed_service @@ -899,15 +909,13 @@ void NDTScanMatcher::service_ndt_align( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - diagnostics_ndt_align_->publish(); + diagnostics_ndt_align_->publish(ros_time_now); } void NDTScanMatcher::service_ndt_align_main( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().nanoseconds()); - // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; From ecdbb651e1786baf770dae1e0f5ce957b9ae0c95 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 20:39:15 +0900 Subject: [PATCH 075/120] fix(map_tf_generator): add log output (#7197) add log output Signed-off-by: a-maumau --- map/map_tf_generator/launch/map_tf_generator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml index 43d487c2bc982..197085f31d9c4 100644 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -3,7 +3,7 @@ - + From 7970b4f2339d868c3ca67c334e944bd674605665 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 31 May 2024 21:35:03 +0900 Subject: [PATCH 076/120] fix(gnss_poser): change log output from screen to both (#7200) change log output from screen to both Signed-off-by: a-maumau --- sensing/gnss_poser/launch/gnss_poser.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 90c3553312dc4..c33f9a7589812 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -7,7 +7,7 @@ - + From 833509668ccff949dc61d32665d91fa89d7f9ae7 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 31 May 2024 21:48:06 +0900 Subject: [PATCH 077/120] refactor(image_projection_based_fusion): rework param (#6289) --- ...roi_sync.param.yaml => fusion_common.param.yaml} | 0 .../segmentation_pointcloud_fusion.param.yaml | 13 ++++++------- .../launch/pointpainting_fusion.launch.xml | 2 +- .../launch/roi_cluster_fusion.launch.xml | 2 +- .../launch/roi_detected_object_fusion.launch.xml | 2 +- .../launch/roi_pointcloud_fusion.launch.xml | 2 +- .../segmentation_pointcloud_fusion.launch.xml | 2 +- ...i_sync.schema.json => fusion_common.schema.json} | 4 ++-- 8 files changed, 13 insertions(+), 14 deletions(-) rename perception/image_projection_based_fusion/config/{roi_sync.param.yaml => fusion_common.param.yaml} (100%) rename perception/image_projection_based_fusion/schema/{roi_sync.schema.json => fusion_common.schema.json} (97%) diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/fusion_common.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/roi_sync.param.yaml rename to perception/image_projection_based_fusion/config/fusion_common.param.yaml diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 79a8b860ebdd3..2120a909cd672 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -31,10 +31,9 @@ # debug debug_mode: false - filter_scope_min_x: -100 - filter_scope_max_x: 100 - filter_scope_min_y: -100 - filter_scope_max_y: 100 - filter_scope_min_z: -100 - filter_scope_max_z: 100 - image_buffer_size: 15 + filter_scope_min_x: -100.0 + filter_scope_max_x: 100.0 + filter_scope_min_y: -100.0 + filter_scope_max_y: 100.0 + filter_scope_min_z: -100.0 + filter_scope_max_z: 100.0 diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index a22b2c2a13bda..d2f803f13d376 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -24,7 +24,7 @@ - + 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 8df1a374b00b6..f624b099fccb3 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 @@ -19,7 +19,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index c9da81af9ddb0..f11280b7f7c67 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -19,7 +19,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index 046d88d06e2a1..cde06744aca58 100644 --- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -21,7 +21,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml index 1db2bb20793ac..cf4d104b9e05a 100644 --- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -18,7 +18,7 @@ - + diff --git a/perception/image_projection_based_fusion/schema/roi_sync.schema.json b/perception/image_projection_based_fusion/schema/fusion_common.schema.json similarity index 97% rename from perception/image_projection_based_fusion/schema/roi_sync.schema.json rename to perception/image_projection_based_fusion/schema/fusion_common.schema.json index 411fb678a49a7..73ee1661adaea 100644 --- a/perception/image_projection_based_fusion/schema/roi_sync.schema.json +++ b/perception/image_projection_based_fusion/schema/fusion_common.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Synchronization of RoI Fusion Nodes", "type": "object", "definitions": { - "roi_sync": { + "fusion_common": { "type": "object", "properties": { "input_offset_ms": { @@ -74,7 +74,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/roi_sync" + "$ref": "#/definitions/fusion_common" } }, "required": ["ros__parameters"] From 6fe9fc648eaa3c1c36dc75fb8800212bd075bbfe Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Sat, 1 Jun 2024 08:06:08 +0900 Subject: [PATCH 078/120] feat(start_planner,lane_departure_checker): add margin param for lane departure check (#7193) * add param for lane departure margin Signed-off-by: Daniel Sanchez * json thing Signed-off-by: Daniel Sanchez * docs Signed-off-by: Daniel Sanchez * make separate param for lane departure margin expansion Signed-off-by: Daniel Sanchez * update docs Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- control/lane_departure_checker/README.md | 1 + .../config/lane_departure_checker.param.yaml | 1 + .../lane_departure_checker.hpp | 1 + .../schema/lane_departure_checker.json | 6 ++++++ .../lane_departure_checker.cpp | 3 ++- .../lane_departure_checker_node.cpp | 2 ++ .../README.md | 17 +++++++++-------- .../config/start_planner.param.yaml | 1 + .../data_structs.hpp | 1 + .../src/manager.cpp | 5 +++++ .../src/start_planner_module.cpp | 5 +++++ 11 files changed, 34 insertions(+), 9 deletions(-) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 67b770aefb3d2..4a6592a103f2f 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -93,6 +93,7 @@ This package includes the following features: | Name | Type | Description | Default value | | :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | | footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | +| footprint_extra_margin | double | Coefficient for expanding footprint margin. When checking for lane departure | 0.0 | | resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | | max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | | delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | 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 f0a8df21c425b..2724c28e2536a 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -17,6 +17,7 @@ # Core footprint_margin_scale: 1.0 + footprint_extra_margin: 0.0 resample_interval: 0.3 max_deceleration: 2.8 delay_time: 1.3 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 c658cf4497973..b95c0a4b26e5c 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 @@ -60,6 +60,7 @@ typedef boost::geometry::index::rtree LaneDepartureChecker::createVehicleFootprints( const PathWithLaneId & path) const { // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(); + const auto local_vehicle_footprint = + vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); // Create vehicle footprint on each Path point std::vector vehicle_footprints; 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 06d11133920f4..4d6c86990c399 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 @@ -150,6 +150,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Core Parameter param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); + param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); param_.resample_interval = declare_parameter("resample_interval"); param_.max_deceleration = declare_parameter("max_deceleration"); param_.delay_time = declare_parameter("delay_time"); @@ -403,6 +404,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); + update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin); update_param(parameters, "resample_interval", param_.resample_interval); update_param(parameters, "max_deceleration", param_.max_deceleration); update_param(parameters, "delay_time", param_.delay_time); diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index dd3dddef9d863..ab2f0460b3fcb 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -494,14 +494,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 46469de68149e..7de76c28cef5c 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -40,6 +40,7 @@ geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 + lane_departure_check_expansion_margin: 0.0 backward_velocity: -1.0 pull_out_max_steer_angle: 0.26 # 15deg # search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 0e85c1d0e76b3..04248ee7bd5fb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -71,6 +71,7 @@ struct StartPlannerParameters behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck object_types_to_check_for_path_generation{}; double center_line_path_interval{0.0}; + double lane_departure_check_expansion_margin{0.0}; // shift pull out bool enable_shift_pull_out{false}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 794577d7cc7aa..d5731f54224b6 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -101,6 +101,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "arc_path_interval"); p.parallel_parking_parameters.pull_out_lane_departure_margin = node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg p.parallel_parking_parameters.center_line_path_interval = @@ -435,6 +437,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "lane_departure_margin", p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); updateParam( parameters, ns + "pull_out_max_steer_angle", p->parallel_parking_parameters.pull_out_max_steer_angle); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index d3afdb2aefae5..7e1f38f30c378 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -65,6 +65,11 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters->lane_departure_check_expansion_margin; + + lane_departure_checker_->setParam(lane_departure_checker_params); // set enabled planner if (parameters_->enable_shift_pull_out) { From d70c8c97fb0120af2dcc08ab96866449e7ce07fa Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 2 Jun 2024 10:52:21 +0900 Subject: [PATCH 079/120] feat(obstacle_avoidance_planner): use polling subscriber (#7213) Signed-off-by: Takayuki Murooka --- .../obstacle_avoidance_planner/node.hpp | 12 ++++---- .../obstacle_avoidance_planner/src/node.cpp | 30 ++++++++++--------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 3677e6c5fb075..db17496288766 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -22,6 +22,7 @@ #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -83,16 +84,14 @@ class ObstacleAvoidancePlanner : public rclcpp::Node TrajectoryParam traj_param_{}; EgoNearestParam ego_nearest_param_{}; - // variables for subscribers - Odometry::ConstSharedPtr ego_state_ptr_; - // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr virtual_wall_pub_; // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber ego_odom_sub_{ + this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -113,8 +112,9 @@ class ObstacleAvoidancePlanner : public rclcpp::Node void resetPreviousData(); // main functions - bool isDataReady(const Path & path, rclcpp::Clock clock) const; - PlannerData createPlannerData(const Path & path) const; + bool checkInputPath(const Path & path, rclcpp::Clock clock) const; + PlannerData createPlannerData( + const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const; std::vector generateOptimizedTrajectory(const PlannerData & planner_data); std::vector extendTrajectory( const std::vector & traj_points, diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index c1da988eb5c25..b05d3f9da0c57 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -95,8 +95,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -224,8 +222,16 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); - // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + // check if input path is valid + if (!checkInputPath(*path_ptr, *get_clock())) { + return; + } + + // check if ego's odometry is valid + const auto ego_odom_ptr = ego_odom_sub_.takeData(); + if (!ego_odom_ptr) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "Waiting for ego pose and twist."); return; } @@ -245,7 +251,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) } // 1. create planner data - const auto planner_data = createPlannerData(*path_ptr); + const auto planner_data = createPlannerData(*path_ptr, ego_odom_ptr); // 2. generate optimized trajectory const auto optimized_traj_points = generateOptimizedTrajectory(planner_data); @@ -276,13 +282,8 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } -bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const +bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); - return false; - } - if (path.points.size() < 2) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); return false; @@ -297,7 +298,8 @@ bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock cloc return true; } -PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const +PlannerData ObstacleAvoidancePlanner::createPlannerData( + const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const { // create planner data PlannerData planner_data; @@ -305,8 +307,8 @@ PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points); planner_data.left_bound = path.left_bound; planner_data.right_bound = path.right_bound; - planner_data.ego_pose = ego_state_ptr_->pose.pose; - planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x; + planner_data.ego_pose = ego_odom_ptr->pose.pose; + planner_data.ego_vel = ego_odom_ptr->twist.twist.linear.x; debug_data_ptr_->ego_pose = planner_data.ego_pose; return planner_data; From a553c6db3dca9fbf7b2227fbd1d09a3dd1da65fb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 2 Jun 2024 10:53:38 +0900 Subject: [PATCH 080/120] feat(path_smoother): use polling subscriber (#7214) Signed-off-by: Takayuki Murooka --- .../path_smoother/elastic_band_smoother.hpp | 9 ++++----- .../path_smoother/src/elastic_band_smoother.cpp | 16 ++++++++-------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index e0bdb326adb33..bc566fdfb96bf 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -22,6 +22,7 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include @@ -72,9 +73,6 @@ class ElasticBandSmoother : public rclcpp::Node CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; - // variables for subscribers - Odometry::ConstSharedPtr ego_state_ptr_; - // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -84,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -104,7 +102,8 @@ class ElasticBandSmoother : public rclcpp::Node void resetPreviousData(); // main functions - bool isDataReady(const Path & path, rclcpp::Clock clock) const; + bool isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const; void applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 6fb732309da4b..75300286ac9dc 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -79,8 +79,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -156,7 +154,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->tic(__func__); // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + const auto ego_state_ptr = odom_sub_.takeData(); + if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) { return; } @@ -181,7 +180,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) // 1. calculate trajectory with Elastic Band // 1.a check if replan (= optimization) is required PlannerData planner_data( - input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + input_traj_points, ego_state_ptr->pose.pose, ego_state_ptr->twist.twist.linear.x); const bool is_replan_required = [&]() { if (replan_checker_ptr_->isResetRequired(planner_data)) { // NOTE: always replan when resetting previous optimization @@ -195,7 +194,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( - input_traj_points, ego_state_ptr_->pose.pose) + input_traj_points, ego_state_ptr->pose.pose) : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); @@ -203,7 +202,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) std::make_shared>(smoothed_traj_points); // 2. update velocity - applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); + applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr->pose.pose); // 3. extend trajectory to connect the optimized trajectory and the following path smoothly auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points); @@ -230,9 +229,10 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); } -bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const +bool ElasticBandSmoother::isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { + if (!ego_state_ptr) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); return false; } From 85fef81ebb1c836cecd4588f301b7ca1a3607437 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 2 Jun 2024 14:45:17 +0900 Subject: [PATCH 081/120] fix(yabloc): fix bug in capturing in lambda function (#7208) * fix(yabloc): fix bug in capturing in lambda function Signed-off-by: Ryuta Kambe * style(pre-commit): autofix --------- Signed-off-by: Ryuta Kambe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../yabloc/yabloc_common/src/extract_line_segments.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 9987c4fbb5e72..98c70b44c63fc 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -25,7 +25,8 @@ pcl::PointCloud extract_near_line_segments( // All line segments contained in a square with max_range on one side must be taken out, // so pick up those that are closer than the **diagonals** of the square. - auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool { + auto check_intersection = [sqrt_two, max_range, + pose_vector](const pcl::PointNormal & pn) -> bool { const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector; const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector; From 573d816853fdd5228924ca2934a803c6986db7f4 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Sun, 2 Jun 2024 14:45:35 +0900 Subject: [PATCH 082/120] refactor(yabloc): use constexpr properly (#7207) Signed-off-by: Ryuta Kambe --- localization/yabloc/yabloc_common/src/extract_line_segments.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 98c70b44c63fc..9940dca1fd62b 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -20,7 +20,7 @@ pcl::PointCloud extract_near_line_segments( const pcl::PointCloud & line_segments, const Sophus::SE3f & transform, const float max_range) { - constexpr double sqrt_two = std::sqrt(2); + constexpr double sqrt_two = 1.41421356237309504880; const Eigen::Vector3f pose_vector = transform.translation(); // All line segments contained in a square with max_range on one side must be taken out, From 8416d2c26a3099cffea3105239449b023e8db06f Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Sun, 2 Jun 2024 14:47:44 +0900 Subject: [PATCH 083/120] fix(autoware_auto_common): nullptr_t (#7212) Signed-off-by: Shumpei Wakabayashi --- .../helper_functions/message_adapters.hpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index d3bda57b3374f..cc2fb0e41c372 100644 --- a/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -36,7 +36,7 @@ using TimeStamp = builtin_interfaces::msg::Time; /// \brief Helper class to check existence of header file in compile time: /// https://stackoverflow.com/a/16000226/2325407 -template +template struct HasHeader : std::false_type { }; @@ -48,60 +48,60 @@ struct HasHeader : std::true_type /////////// Template declarations -/// Get frame id from message. nullptr_t is used to prevent template ambiguity on +/// Get frame id from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const std::string & get_frame_id(const T & msg) noexcept; -/// Get a reference to the frame id from message. nullptr_t is used to prevent +/// Get a reference to the frame id from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template std::string & get_frame_id(T & msg) noexcept; -/// Get stamp from message. nullptr_t is used to prevent template ambiguity on +/// Get stamp from message. std::nullptr_t is used to prevent template ambiguity on /// SFINAE specializations. Provide a default value on specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template const TimeStamp & get_stamp(const T & msg) noexcept; -/// Get a reference to the stamp from message. nullptr_t is used to prevent +/// Get a reference to the stamp from message. std::nullptr_t is used to prevent /// template ambiguity on SFINAE specializations. Provide a default value on /// specializations for a friendly API. /// \tparam T Message type. /// \param msg Message. /// \return Frame id of the message. -template +template TimeStamp & get_stamp(T & msg) noexcept; /////////////// Default specializations for message types that contain a header. -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> const std::string & get_frame_id(const T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> std::string & get_frame_id(T & msg) noexcept { return msg.header.frame_id; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp & get_stamp(T & msg) noexcept { return msg.header.stamp; } -template ::value, nullptr_t>::type = nullptr> +template ::value, std::nullptr_t>::type = nullptr> TimeStamp get_stamp(const T & msg) noexcept { return msg.header.stamp; From e132ad75408ac62191d707b961cffea0f7762745 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Mon, 3 Jun 2024 09:42:23 +0900 Subject: [PATCH 084/120] test(image_projection_based_fusion): add unit test code for geometry (#7096) * add geometry utility test code * style(pre-commit): autofix * fix scope and declare * change declare name * style(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 3 + .../test/test_geometry.cpp | 206 ++++++++++++++++++ 2 files changed, 209 insertions(+) create mode 100644 perception/image_projection_based_fusion/test/test_geometry.cpp diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 825c4888ac7ea..0f4cb7112f74a 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -168,6 +168,9 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_utils test/test_utils.cpp ) + ament_auto_add_gtest(test_geometry + test/test_geometry.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/image_projection_based_fusion/test/test_geometry.cpp b/perception/image_projection_based_fusion/test/test_geometry.cpp new file mode 100644 index 0000000000000..ac8160fe89124 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_geometry.cpp @@ -0,0 +1,206 @@ +// Copyright 2024 TIER IV, inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/utils/geometry.hpp" + +#include + +TEST(objectToVertices, test_objectToVertices) +{ + // Test `boundingBoxToVertices()` and `cylinderToVertices()` simultaneously + // Test at Shape::BOUNDING_BOX + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = 2.0; + pose.position.z = 3.0; + const double angle = M_PI / 12; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = std::sin(angle); + pose.orientation.w = std::cos(angle); + { + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 0; + shape.dimensions.x = 4.0; + shape.dimensions.y = 6.0; + shape.dimensions.z = 8.0; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_FALSE(vertices.empty()); + EXPECT_NEAR(vertices.at(0).x(), 1.2320508075688772935274, 1e-6); + EXPECT_NEAR(vertices.at(0).y(), 5.598076211353315940291, 1e-6); + EXPECT_NEAR(vertices.at(0).z(), -1.0, 1e-6); + EXPECT_NEAR(vertices.at(7).x(), -2.232050807568877293527, 1e-6); + EXPECT_NEAR(vertices.at(7).y(), 3.598076211353315940291, 1e-6); + EXPECT_NEAR(vertices.at(7).z(), 7.0, 1e-6); + } + + { + // Test at Shape::CYLINDER + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 1; + shape.dimensions.x = 4.0; + shape.dimensions.y = 6.0; + shape.dimensions.z = 8.0; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_FALSE(vertices.empty()); + EXPECT_NEAR(vertices.at(0).x(), 2.732050807568877293528, 1e-6); + EXPECT_NEAR(vertices.at(0).y(), 3.0, 1e-6); + EXPECT_NEAR(vertices.at(0).z(), 7.0, 1e-6); + EXPECT_NEAR(vertices.at(11).x(), 2.732050807568877293528, 1e-6); + EXPECT_NEAR(vertices.at(11).y(), 1.0, 1e-6); + EXPECT_NEAR(vertices.at(11).z(), -1.0, 1e-6); + } + + { + // Test at Shape::POLYGON (Nothing to do) + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 2; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_TRUE(vertices.empty()); + } +} + +TEST(transformPoints, test_transformPoints) +{ + std::vector input_points; + Eigen::Vector3d point(0.0, 0.0, 0.0); + input_points.push_back(point); + Eigen::Translation translation(1.0, 1.0, 1.0); + Eigen::Matrix3d rotation = (Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())) + .toRotationMatrix(); + Eigen::Affine3d affine_transform = rotation * translation; + std::vector output_points; + + image_projection_based_fusion::transformPoints(input_points, affine_transform, output_points); + + EXPECT_FALSE(output_points.empty()); + EXPECT_NEAR(output_points.at(0).x(), 0.7071067811865475244008, 1e-6); + EXPECT_NEAR(output_points.at(0).y(), 0.5, 1e-6); + EXPECT_NEAR(output_points.at(0).z(), 1.5, 1e-6); +} + +TEST(is_inside, test_is_inside) +{ + // Test default pattern + sensor_msgs::msg::RegionOfInterest outer; + outer.x_offset = 30; + outer.y_offset = 40; + outer.height = 400; + outer.width = 300; + const double outer_offset_scale = 1.0; + { + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 31; + inner.y_offset = 41; + inner.height = 399; + inner.width = 299; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_TRUE(inside_flag); + } + + { + // Test left-top outside pattern + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 29; + inner.y_offset = 39; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_FALSE(inside_flag); + } + + { + // Test right-bottom outside pattern + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 31; + inner.y_offset = 41; + inner.height = 401; + inner.width = 301; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_FALSE(inside_flag); + } +} + +TEST(sanitizeROI, test_sanitizeROI) +{ + { + // Test default pattern + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 10; + roi.y_offset = 20; + roi.height = 200; + roi.width = 100; + int height = 400; // image height + int width = 300; // image width + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 200); + EXPECT_EQ(roi.width, 100); + } + + { + // Test pattern that x_offset or y_offset is not in image + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 100; + roi.y_offset = 200; + int height = 100; + int width = 50; + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 0); + EXPECT_EQ(roi.width, 0); + } + + { + // Test patten that roi does not fit within image + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 10; + roi.y_offset = 20; + roi.height = 500; + roi.width = 400; + int height = 100; + int width = 50; + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 80); + EXPECT_EQ(roi.width, 40); + } +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From cb739f17f6960bbb3e3529a29f3c7ce2c8b1d07b Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 3 Jun 2024 10:09:22 +0900 Subject: [PATCH 085/120] feat(detected_object_validation): filter unknown objects with its footprint (#7186) feat: filter unknown objects with its footprint Signed-off-by: yoshiri --- .../object_lanelet_filter.hpp | 3 ++ .../utils/utils.hpp | 18 +++++++- .../src/object_lanelet_filter.cpp | 45 ++++++++++++++----- 3 files changed, 55 insertions(+), 11 deletions(-) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 00826de68157f..6c652d0ad093e 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -80,6 +80,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); + bool isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); bool isSameDirectionWithLanelets( const lanelet::ConstLanelets & lanelets, diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp index 2c46e1b9dd110..853733dc0ee5f 100644 --- a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp @@ -15,8 +15,9 @@ #ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ -#include +#include +#include namespace utils { struct FilterTargetLabel @@ -31,6 +32,21 @@ struct FilterTargetLabel bool PEDESTRIAN; bool isTarget(const uint8_t label) const; }; // struct FilterTargetLabel + +inline bool hasBoundingBox(const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return true; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return true; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + return false; + } else { + // unknown shape type. + return false; + } +} + } // namespace utils #endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index fe304a3ea22bb..80a2ce4b6563f 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -139,17 +139,9 @@ bool ObjectLaneletFilterNode::filterObject( bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { - Polygon2d polygon; - const auto footprint = setFootprint(transformed_object); - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint( - point, transformed_object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); const bool is_polygon_overlap = - isPolygonOverlapLanelets(polygon, intersected_road_lanelets) || - isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets); + isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || + isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); filter_pass = filter_pass && is_polygon_overlap; } @@ -234,6 +226,39 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( return intersected_lanelets; } +bool ObjectLaneletFilterNode::isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets) +{ + // if has bounding box, use polygon overlap + if (utils::hasBoundingBox(object)) { + Polygon2d polygon; + const auto footprint = setFootprint(object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + return isPolygonOverlapLanelets(polygon, intersected_lanelets); + } else { + // if object do not have bounding box, check each footprint is inside polygon + for (const auto & point : object.shape.footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + geometry_msgs::msg::Pose point2d; + point2d.position.x = point_transformed.x; + point2d.position.y = point_transformed.y; + for (const auto & lanelet : intersected_lanelets) { + if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) { + return true; + } + } + } + return false; + } +} + bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) { From 1b7c600e82cacdeaf540f7ff5bce1523c6c2af9a Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 10:38:55 +0900 Subject: [PATCH 086/120] feat: remove autoware_auto_geometry package (#7217) * feat!: remove autoware_auto_geometry package Signed-off-by: Ryohsuke Mitsudome * docs: remove autoware_auto_geometry package from docs Signed-off-by: Ryohsuke Mitsudome --------- Signed-off-by: Ryohsuke Mitsudome --- .github/CODEOWNERS | 1 - common/.pages | 4 - common/autoware_auto_geometry/CMakeLists.txt | 39 -- .../autoware_auto_geometry/design/interval.md | 111 ---- .../design/polygon_intersection_2d-design.md | 51 -- .../design/spatial-hash-design.md | 99 --- .../bounding_box/bounding_box_common.hpp | 188 ------ .../bounding_box/eigenbox_2d.hpp | 247 -------- .../bounding_box/lfit.hpp | 281 --------- .../bounding_box/rotating_calipers.hpp | 280 --------- .../bounding_box_2d.hpp | 34 - .../autoware_auto_geometry/common_2d.hpp | 587 ------------------ .../autoware_auto_geometry/common_3d.hpp | 77 --- .../autoware_auto_geometry/convex_hull.hpp | 195 ------ .../autoware_auto_geometry/hull_pockets.hpp | 111 ---- .../autoware_auto_geometry/intersection.hpp | 312 ---------- .../autoware_auto_geometry/interval.hpp | 358 ----------- .../autoware_auto_geometry/lookup_table.hpp | 179 ------ .../autoware_auto_geometry/spatial_hash.hpp | 332 ---------- .../spatial_hash_config.hpp | 450 -------------- .../visibility_control.hpp | 41 -- common/autoware_auto_geometry/package.xml | 30 - .../src/bounding_box.cpp | 157 ----- .../src/spatial_hash.cpp | 104 ---- .../test/include/test_bounding_box.hpp | 550 ---------------- .../test/include/test_spatial_hash.hpp | 260 -------- .../test/src/lookup_table.cpp | 166 ----- .../test/src/test_area.cpp | 132 ---- .../test/src/test_common_2d.cpp | 197 ------ .../test/src/test_convex_hull.cpp | 372 ----------- .../test/src/test_geometry.cpp | 25 - .../test/src/test_hull_pockets.cpp | 182 ------ .../test/src/test_intersection.cpp | 155 ----- .../test/src/test_interval.cpp | 259 -------- .../package.xml | 1 - 35 files changed, 6567 deletions(-) delete mode 100644 common/autoware_auto_geometry/CMakeLists.txt delete mode 100644 common/autoware_auto_geometry/design/interval.md delete mode 100644 common/autoware_auto_geometry/design/polygon_intersection_2d-design.md delete mode 100644 common/autoware_auto_geometry/design/spatial-hash-design.md delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp delete mode 100644 common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp delete mode 100644 common/autoware_auto_geometry/package.xml delete mode 100644 common/autoware_auto_geometry/src/bounding_box.cpp delete mode 100644 common/autoware_auto_geometry/src/spatial_hash.cpp delete mode 100644 common/autoware_auto_geometry/test/include/test_bounding_box.hpp delete mode 100644 common/autoware_auto_geometry/test/include/test_spatial_hash.hpp delete mode 100644 common/autoware_auto_geometry/test/src/lookup_table.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_area.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_common_2d.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_convex_hull.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_geometry.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_hull_pockets.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_intersection.cpp delete mode 100644 common/autoware_auto_geometry/test/src/test_interval.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5f5d5008c0f1c..e60173068f028 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,6 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai diff --git a/common/.pages b/common/.pages index f1ecfc71f96b6..5ce88e0561c34 100644 --- a/common/.pages +++ b/common/.pages @@ -6,10 +6,6 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons - - 'autoware_auto_geometry': - - 'interval': common/autoware_auto_geometry/design/interval - - 'polygon intersection 2d': common/autoware_auto_geometry/design/polygon_intersection_2d-design - - 'spatial hash': common/autoware_auto_geometry/design/spatial-hash-design - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt deleted file mode 100644 index ee819b7cd797c..0000000000000 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_geometry) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - include/autoware_auto_geometry/spatial_hash.hpp - include/autoware_auto_geometry/intersection.hpp - include/autoware_auto_geometry/spatial_hash_config.hpp - src/spatial_hash.cpp - src/bounding_box.cpp -) - -if(BUILD_TESTING) - set(GEOMETRY_GTEST geometry_gtest) - set(GEOMETRY_SRC test/src/test_geometry.cpp - test/src/test_convex_hull.cpp - test/src/test_hull_pockets.cpp - test/src/test_interval.cpp - test/src/lookup_table.cpp - test/src/test_area.cpp - test/src/test_common_2d.cpp - test/src/test_intersection.cpp - ) - ament_add_ros_isolated_gtest(${GEOMETRY_GTEST} ${GEOMETRY_SRC}) - target_compile_options(${GEOMETRY_GTEST} PRIVATE -Wno-conversion -Wno-sign-conversion) - target_include_directories(${GEOMETRY_GTEST} PRIVATE "test/include" "include") - ament_target_dependencies(${GEOMETRY_GTEST} - "autoware_auto_common" - "autoware_auto_geometry_msgs" - "autoware_auto_planning_msgs" - "autoware_auto_vehicle_msgs" - "geometry_msgs" - "osrf_testing_tools_cpp") - target_link_libraries(${GEOMETRY_GTEST} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md deleted file mode 100644 index 4fe65ff8e0310..0000000000000 --- a/common/autoware_auto_geometry/design/interval.md +++ /dev/null @@ -1,111 +0,0 @@ -# Interval - -The interval is a standard 1D real-valued interval. -The class implements a representation and operations on the interval type and guarantees interval validity on construction. -Basic operations and accessors are implemented, as well as other common operations. -See 'Example Usage' below. - -## Target use cases - -- Range or containment checks. - The interval class simplifies code that involves checking membership of a value to a range, or intersecting two ranges. - It also provides consistent behavior and consistent handling of edge cases. - -## Properties - -- **empty**: An empty interval is equivalent to an empty set. - It contains no elements. - It is a valid interval, but because it is empty, the notion of measure (length) is undefined; the measure of an empty interval is _not_ zero. - The implementation represents the measure of an empty interval with `NaN`. -- **zero measure**: An interval with zero measure is an interval whose bounds are exactly equal. - The measure is zero because the interval contains only a single point, and points have zero measure. - However, because it does contain a single element, the interval is _not_ empty. -- **valid**: A valid interval is either empty or has min/max bounds such that (min <= max). On construction, interval objects are guaranteed to be valid. - An attempt to construct an invalid interval results in a runtime_error exception being thrown. -- **pseudo-immutable**: Once constructed the only way to change the value of an interval is to overwrite it with a new one; an existing object cannot be modified. - -## Conventions - -- All operations on interval objects are defined as static class methods on the interval class. - This is a functional-style of programming that basically turns the class into a namespace that grants functions access to private member variables of the object they operate on. - -## Assumptions - -- The interval is only intended for floating point types. - This is enforced via static assertion. -- The constructor for non-empty intervals takes two arguments 'min' and 'max', and they must be ordered (i.e., min <= max). - If this assumption is violated, an exception is emitted and construction fails. - -## Example Usage - -```c++ -#include "autoware_auto_geometry/interval.hpp" - -#include - -// using-directive is just for illustration; don't do this in practice -using namespace autoware::common::geometry; - -// bounds for example interval -constexpr auto MIN = 0.0; -constexpr auto MAX = 1.0; - -// -// Try to construct an invalid interval. This will give the following error: -// 'Attempted to construct an invalid interval: {"min": 1.0, "max": 0.0}' -// - -try { - const auto i = Interval_d(MAX, MIN); -} catch (const std::runtime_error& e) { - std::cerr << e.what(); -} - -// -// Construct a double precision interval from 0 to 1 -// - -const auto i = Interval_d(MIN, MAX); - -// -// Test accessors and properties -// - -std::cout << Interval_d::min(i) << " " << Interval_d::max(i) << "\n"; -// Prints: 0.0 1.0 - -std::cout << Interval_d::empty(i) << " " << Interval_d::length(i) << "\n"; -// Prints: false 1.0 - -std::cout << Interval_d::contains(i, 0.3) << "\n"; -// Prints: true - -std::cout << Interval_d::is_subset_eq(Interval_d(0.2, 0.4), i) << "\n"; -// Prints: true - -// -// Test operations. -// - -std::cout << Interval_d::intersect(i, Interval(-1.0, 0.3)) << "\n"; -// Prints: {"min": 0.0, "max": 0.3} - -std::cout << Interval_d::project_to_interval(i, 0.5) << " " - << Interval_d::project_to_interval(i, -1.3) << "\n"; -// Prints: 0.5 0.0 - -// -// Distinguish empty/zero measure -// - -const auto i_empty = Interval(); -const auto i_zero_length = Interval(0.0, 0.0); - -std::cout << Interval_d::empty(i_empty) << " " - << Interval_d::empty(i_zero_length) << "\n"; -// Prints: true false - -std::cout << Interval_d::zero_measure(i_empty) << " " - << Interval_d::zero_measure(i_zero_length) << "\n"; -// Prints: false false -``` diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md deleted file mode 100644 index f651c218bc80d..0000000000000 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ /dev/null @@ -1,51 +0,0 @@ -# 2D Convex Polygon Intersection - -Two convex polygon's intersection can be visualized on the image below as the blue area: - - - -## Purpose / Use cases - -Computing the intersection between two polygons can be useful in many applications of scene -understanding. It can be used to estimate collision detection, shape alignment, shape -association and in any application that deals with the objects around the perceiving agent. - -## Design - -[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following -observations about convex polygon intersection: - -- Intersection of two convex polygons is a convex polygon -- A vertex from a polygon that is contained in the other polygon is a vertex of the intersection - shape. (Vertices A, C, D in the shape above) -- An edge from a polygon that is contained in the other polygon is an edge in the intersection - shape. (edge C-D in the shape above) -- Edge intersections between two polygons are vertices in the intersection shape. (Vertices B, - E in the shape above.) - -### Inner-workings / Algorithms - -With the observation mentioned above, the current algorithm operates in the following way: - -- Compute and find the vertices from each polygon that is contained in the other polygon - (Vertices A, C, D) -- Compute and find the intersection points between each polygon (Vertices B, E) -- Compute the convex hull shaped by these vertices by ordering them CCW. - -### Inputs / Outputs / API - -Inputs: - -- Two iterables that contain vertices of the convex polygons ordered in the CCW direction. - -Outputs: - -- A list of vertices of the intersection shape ordered in the CCW direction. - -## Future Work - -- #1230: Applying efficient algorithms. - -## Related issues - -- #983: Integrate vision detections in object tracker diff --git a/common/autoware_auto_geometry/design/spatial-hash-design.md b/common/autoware_auto_geometry/design/spatial-hash-design.md deleted file mode 100644 index 58eecf3ee841b..0000000000000 --- a/common/autoware_auto_geometry/design/spatial-hash-design.md +++ /dev/null @@ -1,99 +0,0 @@ -# Spatial Hash - -The spatial hash is a data structure designed for efficient fixed-radius near-neighbor queries in -low dimensions. - -The fixed-radius near-neighbors problem is defined as follows: - -`For point p, find all points p' s.t. d(p, p') < r` - -Where in this case `d(p, p')` is euclidean distance, and `r` is the fixed -radius. - -For `n` points with an average of `k` neighbors each, this data structure can -perform `m` near-neighbor queries (to generate lists of near-neighbors for `m` -different points) in `O(mk)` time. - -By contrast, using a k-d tree for successive nearest-neighbor queries results in -a running time of `O(m log n)`. - -The spatial hash works as follows: - -- Each point is assigned to a bin in the predefined bounding area defined by - `x_min/x_max` and `y_min/y_max` -- This can be done by converting x and y position into x and y index - respectively - - For example with the bin containing `x_min` and `y_min` as index `(0, 0)` - - The two (or more) indices can then be converted into a single index -- Once every point of interest has been inserted into the hash, near-neighbor - queries can begin: - - The bin of the reference point is first computed - - For each point in each adjacent bin, perform an explicit distance computation - between said point and the reference point. If the distance is below the given - radius, said point is considered to be a near-neighbor - -Under the hood, an `std::unordered_multimap` is used, where the key is a bin/voxel index. -The bin size was computed to be the same as the lookup distance. - - - -In addition, this data structure can support 2D or 3D queries. This is determined during -configuration, and baked into the data structure via the configuration class. The purpose of -this was to avoid if statements in tight loops. The configuration class specializations themselves -use CRTP (Curiously Recurring Template Patterns) to do "static polymorphism", and avoid -a dispatching call. - -## Performance characterization - -### Time - -Insertion is `O(n)` because lookup time for the underlying hashmap is `O(n)` for -hashmaps. In practice, lookup time for hashmaps and thus insertion time should -be `O(1)`. - -Removing a point is `O(1)` because the current API only supports removal via -direct reference to a node. - -Finding `k` near-neighbors is worst case `O(n)` in the case of an adversarial -example, but in practice `O(k)`. - -### Space - -The module consists of the following components: - -- The internal hashmap is `O(n + n + A * n)`, where `A` is an arbitrary - constant (load factor) -- The other components of the spatial hash are `O(n + n)` - -This results in `O(n)` space complexity. - -## States - -The spatial hash's state is dictated by the status of the underlying unordered_multimap. - -The data structure is wholly configured by a -[config](@ref autoware::common::geometry::spatial_hash::Config) class. The constructor -of the class determines in the data structure accepts strictly 2D or strictly 3D queries. - -## Inputs - -The primary method of introducing data into the data structure is via the -[insert](@ref autoware::common::geometry::spatial_hash::SpatialHashBase::insert) method. - -## Outputs - -The primary method of retrieving data from the data structure is via the -[near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near)\(2D -configuration\) -or [near](@ref autoware::common::geometry::spatial_hash::SpatialHash::near) -\(3D configuration\) method. - -The whole data structure can also be traversed using standard constant iterators. - -## Future Work - -- Performance tuning and optimization - -## Related issues - -- #28: Port to autoware.Auto diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp deleted file mode 100644 index 0f3a68e14d442..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief Common functionality for bounding box computation algorithms - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Functions and types for generating enclosing bounding boxes around a set of points -namespace bounding_box -{ -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin The start of the list of points -/// \param[in] end One past the end of the list of points -/// \param[out] box A box for which the z component of centroid, corners, and size gets filled -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, BoundingBox & box) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - box.centroid.z = (max_z + min_z) * 0.5F; - for (auto & corner : box.corners) { - corner.z = box.centroid.z; - } - box.size.z = (max_z - min_z) * 0.5F; -} - -/// \brief Computes height of bounding box given a full list of points -/// \param[in] begin Iterator pointing to the start of the range of points -/// \param[in] end Iterator pointing the the end of the range of points -/// \param[out] shape A shape in which vertices z values and height field will be set -/// \tparam IT An iterator type, must dereference into a point type with float member z, or -/// appropriate point adapter defined -template -void compute_height(const IT begin, const IT end, autoware_auto_perception_msgs::msg::Shape & shape) -{ - float32_t max_z = -std::numeric_limits::max(); - float32_t min_z = std::numeric_limits::max(); - for (auto it = begin; it != end; ++it) { - const float32_t z = point_adapter::z_(*it); - if (z <= min_z) { - min_z = z; - } - if (z >= max_z) { - max_z = z; - } - } - for (auto & corner : shape.footprint.points) { - corner.z = min_z; - } - shape.dimensions.z = max_z - min_z; -} - -namespace details -{ -/// Templated alias for getting a type without CV or reference qualification -template -using base_type = typename std::remove_cv::type>::type; - -/// Templated alias for an array of 4 objects of type PointT -template -using Point4 = std::array; - -/// \brief Helper struct for compile time introspection of array size from -/// Ref: https://stackoverflow.com/questions/16866033 -template -struct array_size; -template -struct array_size> -{ - static std::size_t const size = N; -}; -static_assert( - array_size>::size == 4U, - "Bounding box does not have the right number of corners"); - -/// \brief Compute length, width, area of bounding box -/// \param[in] corners Corners of the current bounding box -/// \param[out] ret A point struct used to hold size of box defined by corners -void GEOMETRY_PUBLIC -size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret); -/// \brief Computes centroid and orientation of a box given corners -/// \param[in] corners Array of final corners of bounding box -/// \param[out] box Message to have corners, orientation, and centroid updated -void GEOMETRY_PUBLIC -finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box); - -/// \brief given support points and two orthogonal directions, compute corners of bounding box -/// \tparam PointT Type of a point, must have float members x and y` -/// \tparam IT An iterator dereferenceable into PointT -/// \param[out] corners Gets filled with corner points of bounding box -/// \param[in] supports Iterators referring to support points of current bounding box -/// e.g. bounding box is touching these points -/// \param[in] directions Directions of each edge of the bounding box from each support point -template -void compute_corners( - decltype(BoundingBox::corners) & corners, const Point4 & supports, - const Point4 & directions) -{ - for (uint32_t idx = 0U; idx < corners.size(); ++idx) { - const uint32_t jdx = (idx != 3U) ? (idx + 1U) : 0U; - const auto pt = - intersection_2d(*supports[idx], directions[idx], *supports[jdx], directions[jdx]); - // TODO(c.ho) handle error - point_adapter::xr_(corners[idx]) = point_adapter::x_(pt); - point_adapter::yr_(corners[idx]) = point_adapter::y_(pt); - } -} -// TODO(c.ho) type trait enum base - -/// \brief Copy vertices of the given box into a Shape type -/// \param box Box to be converted -/// \return Shape type filled with box vertices -autoware_auto_perception_msgs::msg::Shape GEOMETRY_PUBLIC make_shape(const BoundingBox & box); - -/// \brief Copy centroid and orientation info of the box into Pose type -/// \param box BoundingBox to be converted -/// \return Pose type filled with centroid and orientation from box -geometry_msgs::msg::Pose GEOMETRY_PUBLIC make_pose(const BoundingBox & box); - -/// \brief Fill DetectedObject type with contents from a BoundingBox type -/// \param box BoundingBox to be converted -/// \return Filled DetectedObject type -autoware_auto_perception_msgs::msg::DetectedObject GEOMETRY_PUBLIC -make_detected_object(const BoundingBox & box); - -/// \brief Transform corners from object-local coordinates using the given centroid and orientation -/// \param shape_msg Msg containing corners in object-local coordinates -/// \param centroid Centroid of the polygon whose corners are defined in shape_msg -/// \param orientation Orientation of the polygon -/// \return corners transformed such that their centroid and orientation correspond to the -/// given inputs -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation); - -} // namespace details -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp deleted file mode 100644 index e0f2e66e87ee5..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore eigenbox, EIGENBOX -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief Simplified 2d covariance matrix -struct Covariance2d -{ - /// \brief Variance in the x direction - float32_t xx; - /// \brief Variance in the y direction - float32_t yy; - /// \brief x-y covariance - float32_t xy; - /// \brief Number of points - std::size_t num_points; -}; // struct Covariance2d - -// cspell: ignore Welford -/// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return A 2d covariance matrix for all points in the list -template -Covariance2d covariance_2d(const IT begin, const IT end) -{ - Covariance2d ret{0.0F, 0.0F, 0.0F, 0U}; - float32_t ux = 0.0F; - float32_t uy = 0.0F; - float32_t num_points = 0.0F; - using point_adapter::x_; - using point_adapter::y_; - for (auto it = begin; it != end; ++it) { - ++ret.num_points; - num_points = static_cast(ret.num_points); - const auto & pt = *it; - // update mean x - const float32_t dx = x_(pt) - ux; - ux = ux + (dx / num_points); - // update cov - const float32_t dy = y_(pt) - uy; - ret.xy += (x_(pt) - ux) * (dy); - // update mean y - uy = uy + (dy / num_points); - // update M2 - ret.xx += dx * (x_(pt) - ux); - ret.yy += dy * (y_(pt) - uy); - } - // finalize sample (co-)variance - if (ret.num_points > 1U) { - num_points = num_points - 1.0F; - } - ret.xx /= num_points; - ret.yy /= num_points; - ret.xy /= num_points; - - return ret; -} - -/// \brief Compute eigenvectors and eigenvalues -/// \param[in] cov 2d Covariance matrix -/// \param[out] eig_vec1 First eigenvector -/// \param[out] eig_vec2 Second eigenvector -/// \tparam PointT Point type that has at least float members x and y -/// \return A pair of eigenvalues: The first is the larger eigenvalue -/// \throw std::runtime error if you would get degenerate covariance -template -std::pair eig_2d( - const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2) -{ - const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // Add a small fudge to alleviate floating point errors - float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits::epsilon(); - if (disc < 0.0F) { - throw std::runtime_error( - "pca_2d: negative discriminant! Should never happen for well formed " - "covariance matrix"); - } - disc = sqrtf(disc); - // compute eigenvalues - const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc); - // compute eigenvectors - using point_adapter::xr_; - using point_adapter::yr_; - // We compare squared value against floating epsilon to make sure that eigen vectors - // are persistent against further calculations. - // (e.g. taking cross product of two eigen vectors) - if (fabsf(cov.xy * cov.xy) > std::numeric_limits::epsilon()) { - xr_(eig_vec1) = cov.xy; - yr_(eig_vec1) = ret.first - cov.xx; - xr_(eig_vec2) = cov.xy; - yr_(eig_vec2) = ret.second - cov.xx; - } else { - if (cov.xx > cov.yy) { - xr_(eig_vec1) = 1.0F; - yr_(eig_vec1) = 0.0F; - xr_(eig_vec2) = 0.0F; - yr_(eig_vec2) = 1.0F; - } else { - xr_(eig_vec1) = 0.0F; - yr_(eig_vec1) = 1.0F; - xr_(eig_vec2) = 1.0F; - yr_(eig_vec2) = 0.0F; - } - } - return ret; -} - -/// \brief Given eigenvectors, compute support (furthest) point in each direction -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT type of a point with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] eig1 First principal component of cluster -/// \param[in] eig2 Second principal component of cluster -/// \param[out] support Array to get filled with extreme points in each of the principal -/// components -/// \return whether or not eig2 is ccw wrt eig1 -template -bool8_t compute_supports( - const IT begin, const IT end, const PointT & eig1, const PointT & eig2, Point4 & support) -{ - const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F; - std::array metrics{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max(), std::numeric_limits::max()}}; - for (auto it = begin; it != end; ++it) { - const PointT & pt = *it; - float32_t val = dot_2d(ret ? eig1 : eig2, pt); - if (val > metrics[0U]) { - metrics[0U] = val; - support[0U] = it; - } - if (val < metrics[2U]) { - metrics[2U] = val; - support[2U] = it; - } - val = dot_2d(ret ? eig2 : eig1, pt); - if (val > metrics[1U]) { - metrics[1U] = val; - support[1U] = it; - } - if (val < metrics[3U]) { - metrics[3U] = val; - support[3U] = it; - } - } - return ret; -} - -/// \brief Compute bounding box given a pair of basis directions -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \tparam PointT Point type of the lists, must have float members x and y -/// \param[in] ax1 First basis direction, assumed to be normal to ax2 -/// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1 -/// \param[in] supports Array of iterators referring to points that are most extreme in each basis -/// direction. Should be result of compute_supports function -/// \return A bounding box based on the basis axes and the support points -template -BoundingBox compute_bounding_box( - const PointT & ax1, const PointT & ax2, const Point4 & supports) -{ - decltype(BoundingBox::corners) corners; - const Point4 directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}}; - compute_corners(corners, supports, directions); - - // build box - BoundingBox bbox; - finalize_box(corners, bbox); - size_2d(corners, bbox.size); - return bbox; -} -} // namespace details - -/// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not -/// modify the list. The resulting bounding box is not necessarily minimum in any way -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \return An oriented bounding box in x-y. This bounding box has no height information -template -BoundingBox eigenbox_2d(const IT begin, const IT end) -{ - // compute covariance - const details::Covariance2d cov = details::covariance_2d(begin, end); - - // compute eigenvectors - using PointT = details::base_type; - PointT eig1; - PointT eig2; - const auto eig_v = details::eig_2d(cov, eig1, eig2); - - // find extreme points - details::Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports); - // build box - if (is_ccw) { - std::swap(eig1, eig2); - } - BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports); - bbox.value = eig_v.first; - - return bbox; -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp deleted file mode 100644 index 07fd6c989cedc..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp +++ /dev/null @@ -1,281 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements 2D pca on a linked list of points to estimate an oriented -/// bounding box - -// cspell: ignore LFIT, lfit -// LFIT means "L-Shape Fitting" -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ - -/// \brief A representation of the M matrix for the L-fit algorithm -struct LFitWs -{ - /// \brief Number of points in the first partition - std::size_t p; - /// \brief Number of points in the second partition - std::size_t q; - // assume matrix of form: [a b; c d] - /// \brief Sum of x values in first partition - float32_t m12a; - /// \brief Sum of y values in first partition - float32_t m12b; - /// \brief Sum of y values in second partition - float32_t m12c; - /// \brief Negative sum of x values in second partition - float32_t m12d; - // m22 is a symmetric matrix - /// \brief Sum_p x_2 + sum_q y_2 - float32_t m22a; - /// \brief Sum_p x*y - sum_q x*y - float32_t m22b; - /// \brief Sum_p y_2 + sum_x y_2 - float32_t m22d; -}; // struct LFitWs - -/// \brief Initialize M matrix for L-fit algorithm -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[out] ws A representation of the M matrix to get initialized -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -template -void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws) -{ - ws.p = 1UL; - ws.q = size - 1UL; - // init P terms (first partition) - using point_adapter::x_; - using point_adapter::y_; - const auto & pt = *begin; - const float32_t px = x_(pt); - const float32_t py = y_(pt); - // assume matrix of form: [a b; c d] - ws.m12a = px; - ws.m12b = py; - ws.m12c = 0.0F; - ws.m12d = 0.0F; - // m22 is a symmetric matrix - ws.m22a = px * px; - ws.m22b = px * py; - ws.m22d = py * py; - auto it = begin; - ++it; - for (; it != end; ++it) { - const auto & qt = *it; - const float32_t qx = x_(qt); - const float32_t qy = y_(qt); - ws.m12c += qy; - ws.m12d -= qx; - ws.m22a += qy * qy; - ws.m22b -= qx * qy; - ws.m22d += qx * qx; - } -} - -/// \brief Solves the L fit problem for a given M matrix -/// \tparam PointT The point type of the cluster being L-fitted -/// \param[in] ws A representation of the M Matrix -/// \param[out] dir The best fit direction for this partition/M matrix -/// \return The L2 residual for this fit (the score, lower is better) -template -float32_t solve_lfit(const LFitWs & ws, PointT & dir) -{ - const float32_t pi = 1.0F / static_cast(ws.p); - const float32_t qi = 1.0F / static_cast(ws.q); - const Covariance2d M{// matrix of form [x, z; z y] - ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)), - ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)), - ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)), 0UL}; - PointT eig1; - const auto eig_v = eig_2d(M, eig1, dir); - return eig_v.second; -} - -/// \brief Increments L fit M matrix with information in the point -/// \tparam PointT The point type -/// \param[in] pt The point to increment the M matrix with -/// \param[inout] ws A representation of the M matrix -template -void increment_lfit_ws(const PointT & pt, LFitWs & ws) -{ - const float32_t px = point_adapter::x_(pt); - const float32_t py = point_adapter::y_(pt); - ws.m12a += px; - ws.m12b += py; - ws.m12c -= py; - ws.m12d += px; - ws.m22b += 2.0F * px * py; - const float32_t px2y2 = (px - py) * (px + py); - ws.m22a += px2y2; - ws.m22d -= px2y2; -} - -/// \tparam IT An iterator type that should dereference into a point type with float members x and y -template -class LFitCompare -{ -public: - /// \brief Constructor, initializes normal direction - /// \param[in] hint A 2d vector with the direction that will be used to order the - /// point list - explicit LFitCompare(const PointT & hint) - : m_nx(point_adapter::x_(hint)), m_ny(point_adapter::y_(hint)) - { - } - - /// \brief Comparator operation, returns true if the projection of a the tangent line - /// is smaller than the projection of b - /// \param[in] p The first point for comparison - /// \param[in] q The second point for comparison - /// \return True if a has a smaller projection than b on the tangent line - bool8_t operator()(const PointT & p, const PointT & q) const - { - using point_adapter::x_; - using point_adapter::y_; - return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny)); - } - -private: - const float32_t m_nx; - const float32_t m_ny; -}; // class LFitCompare - -/// \brief The main implementation of L-fitting a bounding box to a list of points. -/// Assumes sufficiently valid, large enough, and appropriately ordered point list -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \return A bounding box that minimizes the LFit residual -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size) -{ - // initialize M - LFitWs ws{}; - init_lfit_ws(begin, end, size, ws); - // solve initial problem - details::base_type best_normal; - float32_t min_eig = solve_lfit(ws, best_normal); - // solve subsequent problems - auto it = begin; - ++it; - for (; it != end; ++it) { - // update M - ws.p += 1U; - ws.q -= 1U; - if (ws.q == 0U) { // checks for q = 0 case - break; - } - increment_lfit_ws(*it, ws); - // solve incremented problem - decltype(best_normal) dir; - const float32_t score = solve_lfit(ws, dir); - // update optima - if (score < min_eig) { - min_eig = score; - best_normal = dir; - } - } - // can recover best corner point, but don't care, need to cover all points - const auto inv_norm = 1.0F / norm_2d(best_normal); - if (!std::isnormal(inv_norm)) { - throw std::runtime_error{"LFit: Abnormal norm"}; - } - best_normal = times_2d(best_normal, inv_norm); - auto best_tangent = get_normal(best_normal); - // find extreme points - Point4 supports; - const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports); - if (is_ccw) { - std::swap(best_normal, best_tangent); - } - BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports); - bbox.value = min_eig; - - return bbox; -} -} // namespace details - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation" -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] size The number of points in the point list -/// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list -/// \return A pair where the first element is an iterator pointing to the nearest point, and the -/// second element is the size of the list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -/// \throw std::domain_error If the number of points is too few -template -BoundingBox lfit_bounding_box_2d( - const IT begin, const IT end, const PointT hint, const std::size_t size) -{ - if (size <= 1U) { - throw std::domain_error("LFit requires >= 2 points!"); - } - // NOTE: assumes points are in base_link/sensor frame! - // sort along tangent wrt sensor origin - // lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified - std::partial_sort(begin, end, end, details::LFitCompare{hint}); - - return details::lfit_bounding_box_2d_impl(begin, end, size); -} - -/// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed -/// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation". -/// This implementation sorts the list using std::sort -/// \return An oriented bounding box in x-y. This bounding box has no height information -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \tparam IT An iterator type dereferenceable into a point with float members x and y -template -BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) -{ - // use the principal component as the sorting direction - const auto cov = details::covariance_2d(begin, end); - using PointT = details::base_type; - PointT eig1; - PointT eig2; - (void)details::eig_2d(cov, eig1, eig2); - (void)eig2; - return lfit_bounding_box_2d(begin, end, eig1, cov.num_points); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp deleted file mode 100644 index fb75384eb07cb..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes - -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -/// \brief Find which (next) edge has smallest angle delta to directions, rotate directions -/// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order). -/// E.g. if anchor point = p_i, edge = P[i+1] - P[i] -/// \param[inout] directions Array of directions of current bounding box (in ccw order) -/// \tparam PointT Point type of the lists, must have float members x and y -/// \return index of array to advance, e.g. the one with the smallest angle between edge and dir -template -uint32_t update_angles(const Point4 & edges, Point4 & directions) -{ - // find smallest angle to next - float32_t best_cos_th = -std::numeric_limits::max(); - float32_t best_edge_dir_mag = 0.0F; - uint32_t advance_idx = 0U; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - const float32_t edge_dir_mag = std::max( - norm_2d(edges[idx]) * norm_2d(directions[idx]), std::numeric_limits::epsilon()); - const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag; - if (cos_th > best_cos_th) { - best_cos_th = cos_th; - best_edge_dir_mag = edge_dir_mag; - advance_idx = idx; - } - } - - // update directions by smallest angle - const float32_t sin_th = - cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag; - for (uint32_t idx = 0U; idx < edges.size(); ++idx) { - rotate_2d(directions[idx], best_cos_th, sin_th); - } - - return advance_idx; -} - -/// \brief Given support points i, find direction of edge: e = P[i+1] - P[i] -/// \tparam PointT Point type of the lists, must have float members x and y -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[in] support Array of points that are most extreme in 4 directions for current OBB -/// \param[out] edges An array to be filled with the polygon edges next from support points -template -void init_edges(const IT begin, const IT end, const Point4 & support, Point4 & edges) -{ - for (uint32_t idx = 0U; idx < support.size(); ++idx) { - auto tmp_it = support[idx]; - ++tmp_it; - const PointT & q = (tmp_it == end) ? *begin : *tmp_it; - edges[idx] = minus_2d(q, *support[idx]); - } -} - -/// \brief Scan through list to find support points for bounding box oriented in direction of -/// u = P[1] - P[0] -/// \tparam IT The iterator type, should dereference to a point type with float members x and y -/// \param[in] begin An iterator pointing to the first point in a point list -/// \param[in] end An iterator pointing to one past the last point in the point list -/// \param[out] support Array that gets filled with pointers to points that are most extreme in -/// each direction aligned with and orthogonal to the first polygon edge. -/// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense) -template -void init_bbox(const IT begin, const IT end, Point4 & support) -{ - // compute initial orientation using first two points - auto pt_it = begin; - ++pt_it; - const auto u = minus_2d(*pt_it, *begin); - support[0U] = begin; - std::array metric{ - {-std::numeric_limits::max(), -std::numeric_limits::max(), - std::numeric_limits::max()}}; - // track u * p, fabsf(u x p), and -u * p - for (pt_it = begin; pt_it != end; ++pt_it) { - // check points against orientation for run_ptr - const auto & pt = *pt_it; - // u * p - const float32_t up = dot_2d(u, pt); - if (up > metric[0U]) { - metric[0U] = up; - support[1U] = pt_it; - } - // -u * p - if (up < metric[2U]) { - metric[2U] = up; - support[3U] = pt_it; - } - // u x p - const float32_t uxp = cross_2d(u, pt); - if (uxp > metric[1U]) { - metric[1U] = uxp; - support[2U] = pt_it; - } - } -} -/// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method. -/// This function may possibly also be used for computing the width of a convex hull, as it uses the -/// external supports of a single convex hull. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect -/// to -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -/// \tparam MetricF A functor that computes a float measure from the x and y size (width and length) -/// of a bounding box, assumed to be packed in a Point32 message. -/// \throw std::domain_error if the list of points is too small to reasonable generate a bounding -/// box -template -BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn) -{ - using PointT = base_type; - // sanity check TODO(c.ho) more checks (up to n = 2?) - if (begin == end) { - throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end))); - } - // initial scan to get anchor points - Point4 support; - init_bbox(begin, end, support); - // initialize directions accordingly - Point4 directions; - { // I just don't want the temporary variable floating around - auto tmp = support[0U]; - ++tmp; - directions[0U] = minus_2d(*tmp, *support[0U]); - directions[1U] = get_normal(directions[0U]); - directions[2U] = minus_2d(directions[0U]); - directions[3U] = minus_2d(directions[1U]); - } - // initialize edges - Point4 edges; - init_edges(begin, end, support, edges); - // size of initial guess - BoundingBox bbox; - decltype(BoundingBox::corners) best_corners; - compute_corners(best_corners, support, directions); - size_2d(best_corners, bbox.size); - bbox.value = metric_fn(bbox.size); - // rotating calipers step: incrementally advance, update angles, points, compute area - for (auto it = begin; it != end; ++it) { - // find smallest angle to next, update directions - const uint32_t advance_idx = update_angles(edges, directions); - // recompute area - decltype(BoundingBox::corners) corners; - compute_corners(corners, support, directions); - decltype(BoundingBox::size) tmp_size; - size_2d(corners, tmp_size); - const float32_t tmp_value = metric_fn(tmp_size); - // update best if necessary - if (tmp_value < bbox.value) { - // corners: memcpy is fine since I know corners and best_corners are distinct - (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners)); - // size - bbox.size = tmp_size; - bbox.value = tmp_value; - } - // Step to next iteration of calipers - { - // update smallest support - auto next_it = support[advance_idx]; - ++next_it; - const auto run_it = (end == next_it) ? begin : next_it; - support[advance_idx] = run_it; - // update edges - next_it = run_it; - ++next_it; - const PointT & next = (end == next_it) ? (*begin) : (*next_it); - edges[advance_idx] = minus_2d(next, *run_it); - } - } - - finalize_box(best_corners, bbox); - - // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2 - // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did, - // it would probably get smoothed out by prediction. But, this could cause issues with the - // collision detection algorithms (i.e GJK), but probably not. - - return bbox; -} -} // namespace details - -/// \brief Compute the minimum area bounding box given a convex hull of points. -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum area bounding box, value field is the area -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_area_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x * pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum perimeter bounding box given a convex hull of points -/// This function is exposed in case a user might already have a convex hull via other methods. -/// \param[in] begin An iterator to the first point on a convex hull -/// \param[in] end An iterator to one past the last point on a convex hull -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam IT An iterator type dereferenceable into a point type with float members x and y -template -BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end) -{ - const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t { - return pt.x + pt.y; - }; - return details::rotating_calipers_impl(begin, end, metric_fn); -} - -/// \brief Compute the minimum area bounding box given an unstructured list of points. -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum area bounding box, value field is the area -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_area_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_area_bounding_box(list.cbegin(), last); -} - -/// \brief Compute the minimum perimeter bounding box given an unstructured list of points -/// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and -/// without memory allocation. -/// \param[inout] list A list of points to form a hull around, gets reordered -/// \return A minimum perimeter bounding box, value field is half the perimeter -/// \tparam PointT Point type of the lists, must have float members x and y -template -BoundingBox minimum_perimeter_bounding_box(std::list & list) -{ - const auto last = convex_hull(list); - return minimum_perimeter_bounding_box(list.cbegin(), last); -} -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp deleted file mode 100644 index c4c52928ac19a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2017-2020 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ - -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -} // namespace geometry -} // namespace common -} // namespace autoware -#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp deleted file mode 100644 index e3c2e58c9f80e..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ /dev/null @@ -1,587 +0,0 @@ -// Copyright 2017-2021 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 2D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace comparison = autoware::common::helper_functions::comparisons; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Temporary namespace for point adapter methods, for use with nonstandard point types -namespace point_adapter -{ -/// \brief Gets the x value for a point -/// \return The x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto x_(const PointT & pt) -{ - return pt.x; -} -/// \brief Gets the x value for a TrajectoryPoint message -/// \return The x value of the point -/// \param[in] pt The point -inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets the y value for a point -/// \return The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto y_(const PointT & pt) -{ - return pt.y; -} -/// \brief Gets the y value for a TrajectoryPoint message -/// \return The y value of the point -/// \param[in] pt The point -inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets the z value for a point -/// \return The z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto z_(const PointT & pt) -{ - return pt.z; -} -/// \brief Gets the z value for a TrajectoryPoint message -/// \return The z value of the point -/// \param[in] pt The point -inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -/// \brief Gets a reference to the x value for a point -/// \return A reference to the x value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & xr_(PointT & pt) -{ - return pt.x; -} -/// \brief Gets a reference to the x value for a TrajectoryPoint -/// \return A reference to the x value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.x; -} -/// \brief Gets a reference to the y value for a point -/// \return A reference to The y value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & yr_(PointT & pt) -{ - return pt.y; -} -/// \brief Gets a reference to the y value for a TrajectoryPoint -/// \return A reference to the y value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.y; -} -/// \brief Gets a reference to the z value for a point -/// \return A reference to the z value of the point -/// \param[in] pt The point -/// \tparam PointT The point type -template -inline auto & zr_(PointT & pt) -{ - return pt.z; -} -/// \brief Gets a reference to the z value for a TrajectoryPoint -/// \return A reference to the z value of the TrajectoryPoint -/// \param[in] pt The TrajectoryPoint -inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt) -{ - return pt.pose.position.z; -} -} // namespace point_adapter - -namespace details -{ -// Return the next iterator, cycling back to the beginning of the list if you hit the end -template -IT circular_next(const IT begin, const IT end, const IT current) noexcept -{ - auto next = std::next(current); - if (end == next) { - next = begin; - } - return next; -} - -} // namespace details - -/// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y -/// \brief compute whether line segment rp is counter clockwise relative to line segment qp -/// \param[in] pt shared point for both line segments -/// \param[in] r point to check if it forms a ccw angle -/// \param[in] q reference point -/// \return whether angle formed is ccw. Three collinear points is considered ccw -template -inline auto ccw(const T1 & pt, const T2 & q, const T3 & r) -{ - using point_adapter::x_; - using point_adapter::y_; - return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F; -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p x q = p1 * q2 - p2 * q1 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d cross product -template -inline auto cross_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * y_(q)) - (y_(pt) * x_(q)); -} - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y -/// \brief compute p * q = p1 * q1 + p2 * q2 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 2d scalar dot product -template -inline auto dot_2d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the 2d difference between two points, p - q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the difference in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) - x_(q); - point_adapter::yr_(r) = y_(p) - y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The unary minus or negation operator applied to a single point's 2d fields -/// \param[in] p The left hand side -/// \return A point with the negation in the x and y fields, all other fields are default -/// initialized -template -T minus_2d(const T & p) -{ - T r; - point_adapter::xr_(r) = -point_adapter::x_(p); - point_adapter::yr_(r) = -point_adapter::y_(p); - return r; -} -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The 2d addition operation, p + q -/// \param[in] p The left hand side -/// \param[in] q The right hand side -/// \return A point with the sum in the x and y fields, all other fields are default -/// initialized -template -T plus_2d(const T & p, const T & q) -{ - T r; - using point_adapter::x_; - using point_adapter::y_; - point_adapter::xr_(r) = x_(p) + x_(q); - point_adapter::yr_(r) = y_(p) + y_(q); - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief The scalar multiplication operation, p * a -/// \param[in] p The point value -/// \param[in] a The scalar value -/// \return A point with the scaled x and y fields, all other fields are default -/// initialized -template -T times_2d(const T & p, const float32_t a) -{ - T r; - point_adapter::xr_(r) = static_cast(point_adapter::x_(p)) * a; - point_adapter::yr_(r) = static_cast(point_adapter::y_(p)) * a; - return r; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief solve p + t * u = q + s * v -/// Ref: https://stackoverflow.com/questions/563198/ -/// \param[in] pt anchor point of first line -/// \param[in] u direction of first line -/// \param[in] q anchor point of second line -/// \param[in] v direction of second line -/// \return intersection point -/// \throw std::runtime_error if lines are (nearly) collinear or parallel -template -inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v) -{ - const float32_t num = cross_2d(minus_2d(pt, q), u); - float32_t den = cross_2d(v, u); - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - if (fabsf(den) < FEPS) { - if (fabsf(num) < FEPS) { - // collinear case, anything is ok - den = 1.0F; - } else { - // parallel case, no valid output - throw std::runtime_error( - "intersection_2d: no unique solution (either collinear or parallel)"); - } - } - return plus_2d(q, times_2d(v, num / den)); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate point given precomputed sin and cos -/// \param[inout] pt point to rotate -/// \param[in] cos_th precomputed cosine value -/// \param[in] sin_th precomputed sine value -template -inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th) -{ - const float32_t x = point_adapter::x_(pt); - const float32_t y = point_adapter::y_(pt); - point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y); - point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief rotate by radian angle th in z direction with ccw positive -/// \param[in] pt reference point to rotate -/// \param[in] th_rad angle by which to rotate point -/// \return rotated point -template -inline T rotate_2d(const T & pt, const float32_t th_rad) -{ - T q(pt); // It's reasonable to expect a copy constructor - const float32_t s = sinf(th_rad); - const float32_t c = cosf(th_rad); - rotate_2d(q, c, s); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief compute q s.t. p T q, or p * q = 0 -/// This is the equivalent of a 90 degree ccw rotation -/// \param[in] pt point to get normal point of -/// \return point normal to p (un-normalized) -template -inline T get_normal(const T & pt) -{ - T q(pt); - point_adapter::xr_(q) = -point_adapter::y_(pt); - point_adapter::yr_(q) = point_adapter::x_(pt); - return q; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief get magnitude of x and y components: -/// \param[in] pt point to get magnitude of -/// \return magnitude of x and y components together -template -inline auto norm_2d(const T & pt) -{ - return sqrtf(static_cast(dot_2d(pt, pt))); -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on line segment p-q to point r -/// Based on equations from https://stackoverflow.com/a/1501725 and -/// http://paulbourke.net/geometry/pointlineplane/ -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line segment p-q to point r -template -inline T closest_segment_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = static_cast(dot_2d(qp, qp)); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const Interval_f unit_interval(0.0f, 1.0f); - const float32_t val = static_cast(dot_2d(minus_2d(r, p), qp)) / len2; - const float32_t t = Interval_f::clamp_to(unit_interval, val); - ret = plus_2d(p, times_2d(qp, t)); - } - return ret; -} -// -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the closest point on the line going through p-q to point r -// Obtained by simplifying closest_segment_point_2d. -/// \param[in] p First point defining the line -/// \param[in] q Second point defining the line -/// \param[in] r Reference point to find the closest point to -/// \return Closest point on line p-q to point r -/// \throw std::runtime_error if the two points coincide and hence don't uniquely -// define a line -template -inline T closest_line_point_2d(const T & p, const T & q, const T & r) -{ - const T qp = minus_2d(q, p); - const float32_t len2 = dot_2d(qp, qp); - T ret = p; - if (len2 > std::numeric_limits::epsilon()) { - const float32_t t = dot_2d(minus_2d(r, p), qp) / len2; - ret = plus_2d(p, times_2d(qp, t)); - } else { - throw std::runtime_error( - "closet_line_point_2d: line ill-defined because given points coincide"); - } - return ret; -} - -/// \tparam T point type. Must have point adapters defined or have float members x and y -/// \brief Compute the distance from line segment p-q to point r -/// \param[in] p First point defining the line segment -/// \param[in] q Second point defining the line segment -/// \param[in] r Reference point to find the distance from the line segment to -/// \return Distance from point r to line segment p-q -template -inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r) -{ - const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r); - return norm_2d(pq_r); -} - -/// \brief Make a 2D unit vector given an angle. -/// \tparam T Point type. Must have point adapters defined or have float members x and y -/// \param th Angle in radians -/// \return Unit vector in the direction of the given angle. -template -inline T make_unit_vector2d(float th) -{ - T r; - point_adapter::xr_(r) = std::cos(th); - point_adapter::yr_(r) = std::sin(th); - return r; -} - -/// \brief Compute squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return squared euclidean distance -template -inline OUT squared_distance_2d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - return (x * x) + (y * y); -} - -/// \brief Compute euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x and y -/// \tparam T2 point type. Must have point adapters defined or have float members x and y -/// \param a point 1 -/// \param b point 2 -/// \return euclidean distance -template -inline OUT distance_2d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_2d(a, b)); -} - -/// \brief Check the given point's position relative the infinite line passing -/// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft() -/// \tparam T T point type. Must have point adapters defined or have float members x and y -/// \param p1 point 1 laying on the infinite line -/// \param p2 point 2 laying on the infinite line -/// \param q point to be checked against the line -/// \return > 0 for point q left of the line through p1 to p2 -/// = 0 for point q on the line through p1 to p2 -/// < 0 for point q right of the line through p1 to p2 -template -inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q) -{ - return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)); -} - -/// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise -/// direction): This function does not check for convexity -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Beginning of point sequence -/// \param[in] end One past the last of the point sequence -/// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order. -/// Returns true for collinear points as well -template -bool all_ordered(const IT begin, const IT end) noexcept -{ - // Short circuit: a line or point is always CCW or otherwise ill-defined - if (std::distance(begin, end) <= 2U) { - return true; - } - bool is_first_point_cw = false; - // Can't use std::all_of because that works directly on the values - for (auto line_start = begin; line_start != end; ++line_start) { - const auto line_end = details::circular_next(begin, end, line_start); - const auto query_point = details::circular_next(begin, end, line_end); - // Check if 3 points starting from current point are in clockwise direction - const bool is_cw = comparison::abs_lte( - check_point_position_to_line_2d(*line_start, *line_end, *query_point), 0.0F, 1e-3F); - if (is_cw) { - if (line_start == begin) { - is_first_point_cw = true; - } else { - if (!is_first_point_cw) { - return false; - } - } - } else if (is_first_point_cw) { - return false; - } - } - return true; -} - -/// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW) -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_2d(const IT begin, const IT end) noexcept -{ - using point_adapter::x_; - using point_adapter::y_; - using T = decltype(x_(*begin)); - auto area = T{}; // zero initialization - // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm - for (auto it = begin; it != end; ++it) { - const auto next = details::circular_next(begin, end, it); - area += x_(*it) * y_(*next); - area -= x_(*next) * y_(*it); - } - return std::abs(T{0.5} * area); -} - -/// Compute area of convex hull, throw if points are not ordered (convexity check is not -/// implemented) -/// \throw std::domain_error if points are not ordered either CW or CCW -/// \tparam IT Iterator type pointing to a point containing float x and float y -/// \param[in] begin Iterator pointing to the beginning of the polygon points -/// \param[in] end Iterator pointing to one past the last of the polygon points -/// \return The area of the polygon, in squared of whatever units your points are in -template -auto area_checked_2d(const IT begin, const IT end) -{ - if (!all_ordered(begin, end)) { - throw std::domain_error{"Cannot compute area: points are not ordered"}; - } - return area_2d(begin, end); -} - -/// \brief Check if the given point is inside or on the edge of the given polygon -/// \tparam IteratorType iterator type. The value pointed to by this must have point adapters -/// defined or have float members x and y -/// \tparam PointType point type. Must have point adapters defined or have float members x and y -/// \param start_it iterator pointing to the first vertex of the polygon -/// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in -/// order. -/// \param p point to be searched -/// \return True if the point is inside or on the edge of the polygon. False otherwise -template -bool is_point_inside_polygon_2d( - const IteratorType & start_it, const IteratorType & end_it, const PointType & p) -{ - std::int32_t winding_num = 0; - - for (IteratorType it = start_it; it != end_it; ++it) { - auto next_it = std::next(it); - if (next_it == end_it) { - next_it = start_it; - } - if (point_adapter::y_(*it) <= point_adapter::y_(p)) { - // Upward crossing edge - if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) { - if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - ++winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } else { - // Downward crossing edge - if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) { - if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) { - --winding_num; - } else { - if (comparison::abs_eq_zero(check_point_position_to_line_2d(*it, *next_it, p), 1e-3F)) { - return true; - } - } - } - } - } - return winding_num != 0; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp deleted file mode 100644 index 74cd210dec586..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file includes common functionality for 3D geometry, such as dot products - -#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \tparam T1, T2 point type. Must have point adapters defined or have float members x, y and z -/// \brief compute p * q = p1 * q1 + p2 * q2 + p3 * 13 -/// \param[in] pt first point -/// \param[in] q second point -/// \return 3d scalar dot product -template -inline auto dot_3d(const T1 & pt, const T2 & q) -{ - using point_adapter::x_; - using point_adapter::y_; - using point_adapter::z_; - return (x_(pt) * x_(q)) + (y_(pt) * y_(q) + z_(pt) * z_(q)); -} - -/// \brief Compute 3D squared euclidean distance between two points -/// \tparam OUT return type. Type of the returned distance. -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return squared 3D euclidean distance -template -inline OUT squared_distance_3d(const T1 & a, const T2 & b) -{ - const auto x = static_cast(point_adapter::x_(a)) - static_cast(point_adapter::x_(b)); - const auto y = static_cast(point_adapter::y_(a)) - static_cast(point_adapter::y_(b)); - const auto z = static_cast(point_adapter::z_(a)) - static_cast(point_adapter::z_(b)); - return (x * x) + (y * y) + (z * z); -} - -/// \brief Compute 3D euclidean distance between two points -/// \tparam T1 point type. Must have point adapters defined or have float members x y and z -/// \tparam T2 point type. Must have point adapters defined or have float members x y and z -/// \param a point 1 -/// \param b point 2 -/// \return 3D euclidean distance -template -inline OUT distance_3d(const T1 & a, const T2 & b) -{ - return std::sqrt(squared_distance_3d(a, b)); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp deleted file mode 100644 index ae81c55ad6b55..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked -/// lists of points - -#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -// lint -e537 NOLINT pclint vs cpplint -#include -// lint -e537 NOLINT pclint vs cpplint -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief Contains computation geometry functions not intended for the end user to directly use -namespace details -{ - -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order -/// \param[inout] hull An empty list of points, assumed to have same allocator as points -/// (for splice) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_lower_hull(std::list & points, std::list & hull) -{ - auto hull_it = hull.cbegin(); - auto point_it = points.cbegin(); - // This ensures that the points we splice to back to the end of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to tail of list until point from head of list satisfies ccw - bool8_t is_ccw = true; - while ((hull.cbegin() != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - // return this node to list for consideration in upper hull - points.splice(points.end(), hull, current_hull_it); - } - const auto last_point_it = point_it; - ++point_it; - // Splice head of list to tail of (lower) hull - hull.splice(hull.end(), points, last_point_it); - // point_it has been advanced, hull_it has been advanced (to where point_it was previously) - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} -/// \brief Moves points comprising the lower convex hull from points to hull. -/// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain -/// the leftmost point -/// \param[inout] hull A list of points, assumed to have same allocator as points (for splice), -/// and to contain the lower hull (minus the left-most point) -/// \tparam PointT The point type for the points list -/// \tparam HullT the point type for the hull list -template -void form_upper_hull(std::list & points, std::list & hull) -{ - // TODO(c.ho) consider reverse iterators, not sure if they work with splice() - auto hull_it = hull.cend(); - --hull_it; - const auto lower_hull_end = hull_it; - auto point_it = points.cend(); - --point_it; - // This ensures that the points we splice to back to the head of list are not processed - const auto iters = points.size(); - for (auto idx = decltype(iters){0}; idx < iters; ++idx) { - // splice points from tail of hull to head of list until ccw is satisfied with tail of list - bool8_t is_ccw = true; - while ((lower_hull_end != hull_it) && is_ccw) { - const auto current_hull_it = hull_it; - --hull_it; - is_ccw = ccw(*hull_it, *current_hull_it, *point_it); - if (!is_ccw) { - hull_it = current_hull_it; - break; - } - points.splice(points.begin(), hull, current_hull_it); - } - const auto last_point_it = point_it; - --point_it; - // Splice points from tail of lexically ordered point list to tail of hull - hull.splice(hull.end(), points, last_point_it); - hull_it = last_point_it; - } - // loop is guaranteed not to underflow because a node can be removed from points AT MOST once - // per loop iteration. The loop is upper bounded to the prior size of the point list -} - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z direction) -/// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull_impl(std::list & list) -{ - // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied - const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t { - using point_adapter::x_; - using point_adapter::y_; - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - return (fabsf(x_(a) - x_(b)) > FEPS) ? (x_(a) < x_(b)) : (y_(a) < y_(b)); - }; - list.sort(lexical_comparator); - - // Temporary list to store points - std::list tmp_hull_list{list.get_allocator()}; - - // Shuffle lower hull points over to tmp_hull_list - form_lower_hull(list, tmp_hull_list); - - // Resort list since we can't guarantee the order TODO(c.ho) is this true? - list.sort(lexical_comparator); - // splice first hull point to beginning of list to ensure upper hull is properly closed - // This is done after sorting because we know exactly where it should go, and keeping it out of - // sorting reduces complexity somewhat - list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin()); - - // build upper hull - form_upper_hull(list, tmp_hull_list); - // Move hull to beginning of list, return iterator pointing to one after the convex hull - const auto ret = list.begin(); - // first move left-most point to ensure it is first - auto tmp_it = tmp_hull_list.end(); - --tmp_it; - list.splice(list.begin(), tmp_hull_list, tmp_it); - // Then move remainder of hull points - list.splice(ret, tmp_hull_list); - return ret; -} -} // namespace details - -/// \brief A static memory implementation of convex hull computation. Shuffles points around the -/// deque such that the points of the convex hull of the deque of points are first in the -/// deque, with the internal points following in an unspecified order. -/// -/// The head of the deque will be the point with the smallest x value, with the other -/// points following in a counter-clockwise manner (from a top down view/facing -z -/// direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result -/// as previously stated does not hold). -/// \param[inout] list A list of nodes that will be reordered into a ccw convex hull -/// \return An iterator pointing to one after the last point contained in the hull -/// \tparam PointT Type of a point, must have x and y float members -template -typename std::list::const_iterator convex_hull(std::list & list) -{ - return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list); -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp deleted file mode 100644 index cd9fd49f71635..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file implements an algorithm for getting a list of "pockets" in the convex -/// hull of a non-convex simple polygon. - -#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/// \brief Compute a list of "pockets" of a simple polygon -/// (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make -/// up the difference between the polygon and its convex hull. -/// This currently has a bug: -// * "Rollover" is not properly handled - if a pocket contains the segment from -// the last point in the list to the first point (which is still part of the -// polygon), that does not get added -/// -/// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull) -/// \param[in] polygon_end End iterator for the simple polygon -/// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon -/// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon -/// \return A vector of vectors of the iterator value type. Each inner vector contains the points -/// for one pocket. We return by value instead of as iterator pairs, because it is possible -/// that the edge connecting the final point in the list and the first point in the list is -/// part of a pocket as well, and this is awkward to represent using iterators into the -/// original collection. -/// -/// \tparam Iter1 Iterator to a point type -/// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be) -template -typename std::vector::value_type>> hull_pockets( - const Iter1 polygon_start, const Iter1 polygon_end, const Iter2 convex_hull_start, - const Iter2 convex_hull_end) -{ - auto pockets = std::vector::value_type>>{}; - if (std::distance(polygon_start, polygon_end) <= 3) { - return pockets; - } - - // Function to check whether a point is in the convex hull. - const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) { - return std::any_of(convex_hull_start, convex_hull_end, [p](auto hull_entry) { - return norm_2d(minus_2d(hull_entry, *p)) < std::numeric_limits::epsilon(); - }); - }; - - // We go through the points of the polygon only once, adding pockets to the list of pockets - // as we detect them. - std::vector::value_type> current_pocket{}; - for (auto it = polygon_start; it != polygon_end; it = std::next(it)) { - if (in_convex_hull(it)) { - if (current_pocket.size() > 1) { - current_pocket.emplace_back(*it); - pockets.push_back(current_pocket); - } - current_pocket.clear(); - current_pocket.emplace_back(*it); - } else { - current_pocket.emplace_back(*it); - } - } - - // At this point, we have reached the end of the polygon, but have not considered the connection - // of the final point back to the first. In case the first point is part of a pocket as well, we - // have some points left in current_pocket, and we add one final pocket that is made up by those - // points plus the first point in the collection. - if (current_pocket.size() > 1) { - current_pocket.push_back(*polygon_start); - pockets.push_back(current_pocket); - } - - return pockets; -} -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp deleted file mode 100644 index 08c70c3a5a6be..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/convex_hull.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -using autoware::common::geometry::closest_line_point_2d; -using autoware::common::geometry::convex_hull; -using autoware::common::geometry::dot_2d; -using autoware::common::geometry::get_normal; -using autoware::common::geometry::minus_2d; -using autoware::common::geometry::norm_2d; -using autoware::common::geometry::times_2d; -using autoware_auto_perception_msgs::msg::BoundingBox; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; - -using Point = geometry_msgs::msg::Point32; - -namespace details -{ - -/// Alias for a std::pair of two points -using Line = std::pair; - -/// \tparam Iter1 Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Compute a sorted list of faces of a polyhedron given a list of points -/// \param[in] start Start iterator of the list of points -/// \param[in] end End iterator of the list of points -/// \return The list of faces -template -std::vector get_sorted_face_list(const Iter start, const Iter end) -{ - // First get a sorted list of points - convex_hull does that by modifying its argument - auto corner_list = std::list(start, end); - const auto first_interior_point = convex_hull(corner_list); - - std::vector face_list{}; - auto itLast = corner_list.begin(); - auto itNext = std::next(itLast, 1); - do { - face_list.emplace_back(Line{*itLast, *itNext}); - itLast = itNext; - itNext = std::next(itNext, 1); - } while ((itNext != first_interior_point) && (itNext != corner_list.end())); - - face_list.emplace_back(Line{*itLast, corner_list.front()}); - - return face_list; -} - -/// \brief Append points of the polygon `internal` that are contained in the polygon `external`. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_contained_points( - const Iterable1T & external, const Iterable2T & internal, - std::list & result) -{ - std::copy_if( - internal.begin(), internal.end(), std::back_inserter(result), [&external](const auto & pt) { - return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt); - }); -} - -/// \brief Append the intersecting points between two polygons into the output list. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -void append_intersection_points( - const Iterable1T & polygon1, const Iterable2T & polygon2, - std::list & result) -{ - using FloatT = decltype(point_adapter::x_(std::declval())); - using Interval = common::geometry::Interval; - - auto get_edge = [](const auto & list, const auto & iterator) { - const auto next_it = std::next(iterator); - const auto & next_pt = (next_it != list.end()) ? *next_it : list.front(); - return std::make_pair(*iterator, next_pt); - }; - - // Get the max absolute value out of two intervals and a scalar. - auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) { - auto get_abs_max = [](const auto & interval) { - return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval))); - }; - return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2))); - }; - - // Compare each edge from polygon1 to each edge from polygon2 - for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) { - const auto & edge1 = get_edge(polygon1, corner1_it); - - Interval edge1_x_interval{ - std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)), - std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))}; - - Interval edge1_y_interval{ - std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)), - std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))}; - - for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) { - try { - const auto & edge2 = get_edge(polygon2, corner2_it); - const auto & intersection = common::geometry::intersection_2d( - edge1.first, minus_2d(edge1.second, edge1.first), edge2.first, - minus_2d(edge2.second, edge2.first)); - - Interval edge2_x_interval{ - std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)), - std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))}; - - Interval edge2_y_interval{ - std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)), - std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))}; - - // cspell: ignore feps - // feps means "Float EPSilon" - - // The accumulated floating point error depends on the magnitudes of each end of the - // intervals. Hence the upper bound of the absolute magnitude should be taken into account - // while computing the epsilon. - const auto max_feps_scale = std::max( - compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)), - compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))); - const auto feps = max_feps_scale * std::numeric_limits::epsilon(); - // Only accept intersections that lie on both of the line segments (edges) - if ( - Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) && - Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) && - Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps)) { - result.push_back(intersection); - } - } catch (const std::runtime_error &) { - // Parallel lines. TODO(yunus.caliskan): #1229 - continue; - } - } - } -} - -} // namespace details - -// TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion -/// \tparam Iter Iterator over point-types that must have point adapters -// defined or have float members x and y -/// \brief Check if polyhedra defined by two given sets of points intersect -// This uses SAT for doing the actual checking -// (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection) -/// \param[in] begin1 Start iterator to first list of point types -/// \param[in] end1 End iterator to first list of point types -/// \param[in] begin2 Start iterator to first list of point types -/// \param[in] end2 End iterator to first list of point types -/// \return true if the boxes collide, false otherwise. -template -bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2) -{ - // Obtain sorted lists of faces of both boxes, merge them into one big list of faces - auto faces = details::get_sorted_face_list(begin1, end1); - const auto faces_2 = details::get_sorted_face_list(begin2, end2); - faces.reserve(faces.size() + faces_2.size()); - faces.insert(faces.end(), faces_2.begin(), faces_2.end()); - - // Also look at last line - for (const auto & face : faces) { - // Compute normal vector to the face and define a closure to get progress along it - const auto normal = get_normal(minus_2d(face.second, face.first)); - auto get_position_along_line = [&normal](auto point) { - return dot_2d(normal, minus_2d(point, Point{})); - }; - - // Define a function to get the minimum and maximum projected position of the corners - // of a given bounding box along the normal line of the face - auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end) { - const auto zero_point = Point{}; - auto min_corners = get_position_along_line(closest_line_point_2d(normal, zero_point, *begin)); - auto max_corners = min_corners; - - for (auto & point = begin; point != end; ++point) { - const auto point_projected = closest_line_point_2d(normal, zero_point, *point); - const auto position_along_line = get_position_along_line(point_projected); - min_corners = std::min(min_corners, position_along_line); - max_corners = std::max(max_corners, position_along_line); - } - return std::pair{min_corners, max_corners}; - }; - - // Perform the actual computations for the extent computation - auto minmax_1 = get_projected_min_max(begin1, end1); - auto minmax_2 = get_projected_min_max(begin2, end2); - - // Check for any intersections - const auto eps = std::numeric_limits::epsilon(); - if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) { - // Found separating hyperplane, stop - return false; - } - } - - // No separating hyperplane found, boxes collide - return true; -} - -/// \brief Get the intersection between two polygons. The polygons should be provided in an -/// identical format to the output of `convex_hull` function as in the corners should be ordered -/// in a CCW fashion. -/// The computation is done by: -/// * Find the corners of each polygon that are contained by the other polygon. -/// * Find the intersection points between two polygons -/// * Combine these points and order CCW to get the final polygon. -/// The criteria for intersection is better explained in: -/// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977) -/// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B -/// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient -/// algorithm: #1230 -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam PointT Point type that have the adapters for the x and y fields. -/// set to `Point1T` -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return The resulting conv -template < - template class Iterable1T, template class Iterable2T, typename PointT> -std::list convex_polygon_intersection2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - std::list result; - details::append_contained_points(polygon1, polygon2, result); - details::append_contained_points(polygon2, polygon1, result); - details::append_intersection_points(polygon1, polygon2, result); - const auto end_it = common::geometry::convex_hull(result); - result.resize(static_cast(std::distance(result.cbegin(), end_it))); - return result; -} - -/// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons -/// span a zero area, the result is 0.0. -/// \tparam Iterable1T A container class that has stl style iterators defined. -/// \tparam Iterable2T A container class that has stl style iterators defined. -/// \tparam Point1T Point type that have the adapters for the x and y fields. -/// \tparam Point2T Point type that have the adapters for the x and y fields. -/// \param polygon1 A convex polygon -/// \param polygon2 A convex polygon -/// \return (Intersection / Union) between two given polygons. -/// \throws std::domain_error If there is any inconsistency on the underlying geometrical -/// computation. -template < - template class Iterable1T, template class Iterable2T, typename PointT> -common::types::float32_t convex_intersection_over_union_2d( - const Iterable1T & polygon1, const Iterable2T & polygon2) -{ - constexpr auto eps = std::numeric_limits::epsilon(); - const auto intersection = convex_polygon_intersection2d(polygon1, polygon2); - - const auto intersection_area = - common::geometry::area_2d(intersection.begin(), intersection.end()); - - if (intersection_area < eps) { - return 0.0F; // There's either no intersection or the points are collinear - } - - const auto polygon1_area = common::geometry::area_2d(polygon1.begin(), polygon1.end()); - const auto polygon2_area = common::geometry::area_2d(polygon2.begin(), polygon2.end()); - - const auto union_area = polygon1_area + polygon2_area - intersection_area; - if (union_area < eps) { - throw std::domain_error("IoU is undefined for polygons with a zero union area"); - } - - return intersection_area / union_area; -} - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp deleted file mode 100644 index 54be2c32b1d05..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ - -/** - * @brief Data structure to contain scalar interval bounds. - * - * @post The interval is guaranteed to be valid upon successful construction. An - * interval [min, max] is "valid" if it is empty (min/max = NaN) or its bounds - * are ordered (min <= max). - */ -template -class Interval -{ - static_assert(std::is_floating_point::value, "Intervals only support floating point types."); - -public: - /** - * @brief Compute equality. - * - * Two intervals compare equal iff they are both valid and they are both - * either empty or have equal bounds. - * - * @note This is defined inline because the class is templated. - * - * @return True iff the intervals compare equal. - */ - friend bool operator==(const Interval & i1, const Interval & i2) - { - const auto min_eq = (Interval::min(i1) == Interval::min(i2)); - const auto max_eq = (Interval::max(i1) == Interval::max(i2)); - const auto bounds_equal = (min_eq && max_eq); - const auto both_empty = (Interval::empty(i1) && Interval::empty(i2)); - return both_empty || bounds_equal; - } - - /** - * @brief Compute inequality and the logical negation of equality. - * @note This is defined inline because the class is templated. - */ - friend bool operator!=(const Interval & i1, const Interval & i2) { return !(i1 == i2); } - - /** - * @brief Stream overload for Interval types. - * - * @note Output precision is fixed inside the function definition, and the - * serialization is JSON compatible. - * - * @note The serialization is lossy. It is used for debugging and for - * generating exception strings. - */ - friend std::ostream & operator<<(std::ostream & os, const Interval & i) - { - constexpr auto PRECISION = 5; - - // Internal helper to format the output. - const auto readable_extremum = [](const T & val) { - if (val == std::numeric_limits::lowest()) { - return std::string("REAL_MIN"); - } - if (val == std::numeric_limits::max()) { - return std::string("REAL_MAX"); - } - - std::stringstream ss; - ss.setf(std::ios::fixed, std::ios::floatfield); - ss.precision(PRECISION); - ss << val; - return ss.str(); - }; - - // Do stream output. - if (Interval::empty(i)) { - return os << "{}"; - } - return os << "{\"min\": " << readable_extremum(Interval::min(i)) - << ", \"max\": " << readable_extremum(Interval::max(i)) << "}"; - } - - /** - * @brief Test whether the two intervals have bounds within epsilon of each - * other. - * - * @note If both intervals are empty, this returns true. If only one is empty, - * this returns false. - */ - static bool abs_eq(const Interval & i1, const Interval & i2, const T & eps); - - /** @brief The minimum bound of the interval. */ - static T min(const Interval & i) { return i.min_; } - - /** @brief The maximum bound of the interval. */ - static T max(const Interval & i) { return i.max_; } - - /** - * @brief Return the measure (length) of the interval. - * - * @note For empty or invalid intervals, NaN is returned. See Interval::empty - * for note on distinction between measure zero and the emptiness property. - */ - static T measure(const Interval & i); - - /** - * @brief Utility for checking whether an interval has zero measure. - * - * @note For empty or invalid intervals, false is returned. See - * Interval::empty for note on distinction between measure zero and the - * emptiness property. - * - * @return True iff the interval has zero measure. - */ - static bool zero_measure(const Interval & i); - - /** - * @brief Whether the interval is empty or not. - * - * @note Emptiness refers to whether the interval contains any points and is - * thus a distinct property from measure: an interval is non-empty if contains - * only a single point even though its measure in that case is zero. - * - * @return True iff the interval is empty. - */ - static bool empty(const Interval & i); - - /** - * @brief Test for whether a given interval contains a given value within the given epsilon - * @return True iff 'interval' contains 'value'. - */ - static bool contains( - const Interval & i, const T & value, const T & eps = std::numeric_limits::epsilon()); - - /** - * @brief Test for whether 'i1' subseteq 'i2' - * @return True iff i1 subseteq i2. - */ - static bool is_subset_eq(const Interval & i1, const Interval & i2); - - /** - * @brief Compute the intersection of two intervals as a new interval. - */ - static Interval intersect(const Interval & i1, const Interval & i2); - - /** - * @brief Clamp a scalar 'val' onto 'interval'. - * @return If 'val' in 'interval', return 'val'; otherwise return the nearer - * interval bound. - */ - static T clamp_to(const Interval & i, T val); - - /** - * @brief Constructor: initialize an empty interval with members set to NaN. - */ - Interval(); - - /** - * @brief Constructor: specify exact interval bounds. - * - * @note An empty interval is specified by setting both bounds to NaN. - * @note An exception is thrown if the specified bounds are invalid. - * - * @post Construction is successful iff the interval is valid. - */ - Interval(const T & min, const T & max); - -private: - static constexpr T NaN = std::numeric_limits::quiet_NaN(); - - T min_; - T max_; - - /** - * @brief Verify that the bounds are valid in an interval. - * @note This method is private because it can only be used in the - * constructor. Once an interval has been constructed, its bounds are - * guaranteed to be valid. - */ - static bool bounds_valid(const Interval & i); -}; // class Interval - -//------------------------------------------------------------------------------ - -typedef Interval Interval_d; -typedef Interval Interval_f; - -//------------------------------------------------------------------------------ - -template -constexpr T Interval::NaN; - -//------------------------------------------------------------------------------ - -template -Interval::Interval() : min_(Interval::NaN), max_(Interval::NaN) -{ -} - -//------------------------------------------------------------------------------ - -template -Interval::Interval(const T & min, const T & max) : min_(min), max_(max) -{ - if (!Interval::bounds_valid(*this)) { - std::stringstream ss; - ss << "Attempted to construct an invalid interval: " << *this; - throw std::runtime_error(ss.str()); - } -} - -//------------------------------------------------------------------------------ - -template -bool Interval::abs_eq(const Interval & i1, const Interval & i2, const T & eps) -{ - namespace comp = helper_functions::comparisons; - - const auto both_empty = Interval::empty(i1) && Interval::empty(i2); - const auto both_non_empty = !Interval::empty(i1) && !Interval::empty(i2); - - const auto mins_equal = comp::abs_eq(Interval::min(i1), Interval::min(i2), eps); - const auto maxes_equal = comp::abs_eq(Interval::max(i1), Interval::max(i2), eps); - const auto both_non_empty_equal = both_non_empty && mins_equal && maxes_equal; - - return both_empty || both_non_empty_equal; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::zero_measure(const Interval & i) -{ - // An empty interval will return false because (NaN == NaN) is false. - return Interval::min(i) == Interval::max(i); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::empty(const Interval & i) -{ - return std::isnan(Interval::min(i)) && std::isnan(Interval::max(i)); -} - -//------------------------------------------------------------------------------ - -template -bool Interval::bounds_valid(const Interval & i) -{ - const auto is_ordered = (Interval::min(i) <= Interval::max(i)); - - // Check for emptiness explicitly because it requires both bounds to be NaN - return Interval::empty(i) || is_ordered; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::is_subset_eq(const Interval & i1, const Interval & i2) -{ - const auto lower_contained = (Interval::min(i1) >= Interval::min(i2)); - const auto upper_contained = (Interval::max(i1) <= Interval::max(i2)); - return lower_contained && upper_contained; -} - -//------------------------------------------------------------------------------ - -template -bool Interval::contains(const Interval & i, const T & value, const T & eps) -{ - return common::helper_functions::comparisons::abs_gte(value, Interval::min(i), eps) && - common::helper_functions::comparisons::abs_lte(value, Interval::max(i), eps); -} - -//------------------------------------------------------------------------------ - -template -T Interval::measure(const Interval & i) -{ - return Interval::max(i) - Interval::min(i); -} - -//------------------------------------------------------------------------------ - -template -Interval Interval::intersect(const Interval & i1, const Interval & i2) -{ - // Construct intersection assuming an intersection exists. - try { - const auto either_empty = Interval::empty(i1) || Interval::empty(i2); - const auto min = std::max(Interval::min(i1), Interval::min(i2)); - const auto max = std::min(Interval::max(i1), Interval::max(i2)); - return either_empty ? Interval() : Interval(min, max); - } catch (...) { - } - - // Otherwise, the intersection is empty. - return Interval(); -} - -//------------------------------------------------------------------------------ - -template -T Interval::clamp_to(const Interval & i, T val) -{ - // clamp val to min - val = std::max(val, Interval::min(i)); - - // clamp val to max - val = std::min(val, Interval::max(i)); - - return Interval::empty(i) ? Interval::NaN : val; -} - -//------------------------------------------------------------------------------ - -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp deleted file mode 100644 index 7b8867ca096ae..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2017-2020 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -/// \file -/// \brief This file contains a 1D linear lookup table implementation - -#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace helper_functions -{ - -namespace -{ - -/** - * @brief Compute a scaling between 'a' and 'b' - * @note scaling is clamped to be in the interval [0, 1] - */ -template -T interpolate(const T & a, const T & b, U scaling) -{ - static const geometry::Interval UNIT_INTERVAL(static_cast(0), static_cast(1)); - scaling = geometry::Interval::clamp_to(UNIT_INTERVAL, scaling); - - const auto m = (b - a); - const auto offset = static_cast(m) * scaling; - return a + static_cast(offset); -} - -// TODO(c.ho) support more forms of interpolation as template functor -// Actual lookup logic, assuming all invariants hold: -// Throw if value is not finite -template -T lookup_impl_1d(const std::vector & domain, const std::vector & range, const T value) -{ - if (!std::isfinite(value)) { - throw std::domain_error{"Query value is not finite (NAN or INF)"}; - } - if (value <= domain.front()) { - return range.front(); - } else if (value >= domain.back()) { - return range.back(); - } else { - // Fall through to normal case - } - - auto second_idx{0U}; - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (value < domain[idx]) { - second_idx = idx; - break; - } - } - // T must be a floating point between 0 and 1 - const auto num = static_cast(value - domain[second_idx - 1U]); - const auto den = static_cast(domain[second_idx] - domain[second_idx - 1U]); - const auto t = num / den; - const auto val = interpolate(range[second_idx - 1U], range[second_idx], t); - return static_cast(val); -} - -// Check invariants for table lookup: -template -void check_table_lookup_invariants(const std::vector & domain, const std::vector & range) -{ - if (domain.size() != range.size()) { - throw std::domain_error{"Domain's size does not match range's"}; - } - if (domain.empty()) { - throw std::domain_error{"Empty domain or range"}; - } - // Check if sorted: Can start at 1 because not empty - for (auto idx = 1U; idx < domain.size(); ++idx) { - if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1 - throw std::domain_error{"Domain is not sorted"}; - } - } -} -} // namespace - -/// Do a 1D table lookup: Does some semi-expensive O(N) error checking first. -/// If query value fall out of the domain, then the value at the corresponding edge of the domain is -/// returned. -/// \param[in] domain The domain, or set of x values -/// \param[in] range The range, or set of y values -/// \param[in] value The point in the domain to query, x -/// \return A linearly interpolated value y, corresponding to the query, x -/// \throw std::domain_error If domain or range is empty -/// \throw std::domain_error If range is not the same size as domain -/// \throw std::domain_error If domain is not sorted -/// \throw std::domain_error If value is not finite (NAN or INF) -/// \tparam T The type of the function, must be interpolatable -template -T lookup_1d(const std::vector & domain, const std::vector & range, const T value) -{ - check_table_lookup_invariants(domain, range); - - return lookup_impl_1d(domain, range, value); -} - -/// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed -/// into the constructor and not done in the lookup function call -/// \tparam T The type of the function, must be interpolatable -template -class LookupTable1D -{ -public: - /// Constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(const std::vector & domain, const std::vector & range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Move constructor - /// \param[in] domain The domain, or set of x values - /// \param[in] range The range, or set of y values - /// \throw std::domain_error If domain or range is empty - /// \throw std::domain_error If range is not the same size as domain - /// \throw std::domain_error If domain is not sorted - LookupTable1D(std::vector && domain, std::vector && range) - : m_domain{domain}, m_range{range} - { - check_table_lookup_invariants(m_domain, m_range); - } - - /// Do a 1D table lookup - /// If query value fall out of the domain, then the value at the corresponding edge of the domain - /// is returned. - /// \param[in] value The point in the domain to query, x - /// \return A linearly interpolated value y, corresponding to the query, x - /// \throw std::domain_error If value is not finite - T lookup(const T value) const { return lookup_impl_1d(m_domain, m_range, value); } - /// Get the domain table - const std::vector & domain() const noexcept { return m_domain; } - /// Get the range table - const std::vector & range() const noexcept { return m_range; } - -private: - std::vector m_domain; - std::vector m_range; -}; // class LookupTable1D - -} // namespace helper_functions -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp deleted file mode 100644 index 8936e2c17d779..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ /dev/null @@ -1,332 +0,0 @@ -// Copyright 2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash_config.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include - -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ - -/// \brief An implementation of the spatial hash or integer lattice data structure for efficient -/// (O(1)) near neighbor queries. -/// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z -/// -/// This implementation can support both 2D and 3D queries -/// (though only one type per data structure), and can support queries of varying radius. This data -/// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions. -template -class GEOMETRY_PUBLIC SpatialHashBase -{ - using Index3 = details::Index3; - // lint -e{9131} NOLINT There's no other way to make this work in a static assert - static_assert( - std::is_same::value || std::is_same::value, - "SpatialHash only works with Config2d or Config3d"); - -public: - using Hash = std::unordered_multimap; - using IT = typename Hash::const_iterator; - /// \brief Wrapper around an iterator and a distance (from some query point) - class Output - { - public: - /// \brief Constructor - /// \param[in] iterator An iterator pointing to some point - /// \param[in] distance The euclidean distance (2d or 3d) to a reference point - Output(const IT iterator, const float32_t distance) : m_iterator(iterator), m_distance(distance) - { - } - /// \brief Get stored point - /// \return A const reference to the stored point - const PointT & get_point() const { return m_iterator->second; } - /// \brief Get underlying iterator - /// \return A copy of the underlying iterator - IT get_iterator() const { return m_iterator; } - /// \brief Convert to underlying point - /// \return A reference to the underlying point - operator const PointT &() const { return get_point(); } - /// \brief Convert to underlying iterator - /// \return A copy of the iterator - operator IT() const { return get_iterator(); } - /// \brief Get distance to reference point - /// \return The distance - float32_t get_distance() const { return m_distance; } - - private: - IT m_iterator; - float32_t m_distance; - }; // class Output - using OutputVector = typename std::vector; - - /// \brief Constructor - /// \param[in] cfg The configuration object for this class - explicit SpatialHashBase(const ConfigT & cfg) - : m_config{cfg}, - m_hash(), - m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output - m_bins_hit{}, // zero initialization (and below) - m_neighbors_found{} - { - } - - /// \brief Inserts point - /// \param[in] pt The Point to insert - /// \return Iterator pointing to the inserted point - /// \throw std::length_error If the data structure is at capacity - IT insert(const PointT & pt) - { - if (size() >= capacity()) { - throw std::length_error{"SpatialHash: Cannot insert past capacity"}; - } - return insert_impl(pt); - } - - /// \brief Inserts a range of points - /// \param[in] begin The start of the range of points to insert - /// \param[in] end The end of the range of points to insert - /// \tparam IteratorT The iterator type - /// \throw std::length_error If the range of points to insert exceeds the data structure's - /// capacity - template - void insert(IteratorT begin, IteratorT end) - { - // This check is here for strong exception safety - if ((size() + std::distance(begin, end)) > capacity()) { - throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"}; - } - for (IteratorT it = begin; it != end; ++it) { - (void)insert_impl(*it); - } - } - - /// \brief Removes the specified element from the data structure - /// \param[in] point An iterator pointing to a point to be removed - /// \return An iterator pointing to the element after the erased element - /// \throw std::domain_error If pt is invalid or does not belong to this data structure - /// - /// \note There is no reliable way to check if an iterator is invalid. The checks here are - /// based on a heuristic and is not guaranteed to find all invalid iterators. This method - /// should be used with care and only on valid iterators - IT erase(const IT point) - { - if (end() == m_hash.find(point->first)) { - throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"}; - } - return m_hash.erase(point); - } - - /// \brief Reset the state of the data structure - void clear() { m_hash.clear(); } - /// \brief Get current number of element stored in this data structure - /// \return Number of stored elements - Index size() const { return m_hash.size(); } - /// \brief Get the maximum capacity of the data structure - /// \return The capacity of the data structure - Index capacity() const { return m_config.get_capacity(); } - /// \brief Whether the hash is empty - /// \return True if data structure is empty - bool8_t empty() const { return m_hash.empty(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT begin() const { return m_hash.begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT end() const { return m_hash.end(); } - /// \brief Get iterator to beginning of data structure - /// \return Iterator - IT cbegin() const { return begin(); } - /// \brief Get iterator to end of data structure - /// \return Iterator - IT cend() const { return end(); } - - /// \brief Get the number of bins touched during the lifetime of this object, for debugging and - /// size tuning - /// \return The total number of bins touched during near() queries - Index bins_hit() const { return m_bins_hit; } - - /// \brief Get number of near neighbors found during the lifetime of this object, for debugging - /// and size tuning - /// \return The total number of neighbors found during near() queries - Index neighbors_found() const { return m_neighbors_found; } - -protected: - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near_impl( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - // reset output - m_neighbors.clear(); - // Compute bin, bin range - const Index3 ref_idx = m_config.index3(x, y, z); - const float32_t radius2 = radius * radius; - const details::BinRange idx_range = m_config.bin_range(ref_idx, radius); - Index3 idx = idx_range.first; - // For bins in radius - do { // guaranteed to have at least the bin ref_idx is in - // update book-keeping - ++m_bins_hit; - // Iterating in a square/cube pattern is easier than constructing sphere pattern - if (m_config.is_candidate_bin(ref_idx, idx, radius2)) { - // For point in bin - const Index jdx = m_config.index(idx); - const auto range = m_hash.equal_range(jdx); - for (auto it = range.first; it != range.second; ++it) { - const auto & pt = it->second; - const float32_t dist2 = m_config.distance_squared(x, y, z, pt); - if (dist2 <= radius2) { - // Only compute true distance if necessary - m_neighbors.emplace_back(it, sqrtf(dist2)); - } - } - } - } while (m_config.next_bin(idx_range, idx)); - // update book-keeping - m_neighbors_found += m_neighbors.size(); - return m_neighbors; - } - -private: - /// \brief Internal insert method with no error checking - /// \param[in] pt The Point to insert - GEOMETRY_LOCAL IT insert_impl(const PointT & pt) - { - // Compute bin - const Index idx = - m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt)); - // Insert into bin - return m_hash.insert(std::make_pair(idx, pt)); - } - - const ConfigT m_config; - Hash m_hash; - OutputVector m_neighbors; - Index m_bins_hit; - Index m_neighbors_found; -}; // class SpatialHashBase - -/// \brief The class to be used for specializing on -/// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function -/// signatures on 2D and 3D configurations -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash; - -/// \brief Explicit specialization of SpatialHash for 2D configuration -/// \tparam PointT The point type stored in this data structure. -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config2d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius) - { - return this->near_impl(x, y, 0.0F, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. Only the x and y members are respected. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), radius); - } -}; - -/// \brief Explicit specialization of SpatialHash for 3D configuration -/// \tparam PointT The point type stored in this data structure. Must have float members x, y and z -template -class GEOMETRY_PUBLIC SpatialHash : public SpatialHashBase -{ -public: - using OutputVector = typename SpatialHashBase::OutputVector; - - explicit SpatialHash(const Config3d & cfg) : SpatialHashBase(cfg) {} - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] x The x component of the reference point - /// \param[in] y The y component of the reference point - /// \param[in] z The z component of the reference point, respected only if the spatial hash is not - /// 2D. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near( - const float32_t x, const float32_t y, const float32_t z, const float32_t radius) - { - return this->near_impl(x, y, z, radius); - } - - /// \brief Finds all points within a fixed radius of a reference point - /// \param[in] pt The reference point. - /// \param[in] radius The radius within which to find all near points - /// \return A const reference to a vector containing iterators pointing to - /// all points within the radius, and the actual distance to the reference point - const OutputVector & near(const PointT & pt, const float32_t radius) - { - return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt), radius); - } -}; - -template -using SpatialHash2d = SpatialHash; -template -using SpatialHash3d = SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp deleted file mode 100644 index 24c4d6e879d4a..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ /dev/null @@ -1,450 +0,0 @@ -// Copyright 2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. -/// \file -/// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in -/// 2D - -#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ - -#include "autoware_auto_geometry/common_2d.hpp" -#include "autoware_auto_geometry/visibility_control.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -/// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup -namespace spatial_hash -{ -/// \brief Index type for identifying bins of the hash/lattice -using Index = std::size_t; -/// \brief Spatial hash functionality not intended to be used by an external user -namespace details -{ -/// \brief Internal struct for packing three indices together -/// -/// The use of this struct publicly is a violation of our coding standards, but I claim it's -/// fine because (a) it's details, (b) it is literally three unrelated members packaged together. -/// This type is needed for conceptual convenience so I don't have massive function parameter -/// lists -struct GEOMETRY_PUBLIC Index3 -{ - Index x; - Index y; - Index z; -}; // struct Index3 - -using BinRange = std::pair; -} // namespace details - -/// \brief The base class for the configuration object for the SpatialHash class -/// \tparam Derived The type of the derived class to support static polymorphism/CRTP -template -class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp -{ -public: - /// \brief Constructor for spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The look up radius - /// \param[in] capacity The maximum number of points the spatial hash can store - Config( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) - : m_min_x{min_x}, - m_min_y{min_y}, - m_min_z{min_z}, - m_max_x{max_x}, - m_max_y{max_y}, - m_max_z{max_z}, - m_side_length{radius}, - m_side_length2{radius * radius}, - m_side_length_inv{1.0F / radius}, - m_capacity{capacity}, - m_max_x_idx{check_basis_direction(min_x, max_x)}, - m_max_y_idx{check_basis_direction(min_y, max_y)}, - m_max_z_idx{check_basis_direction(min_z, max_z)}, - m_y_stride{m_max_x_idx + 1U}, - m_z_stride{m_y_stride * (m_max_y_idx + 1U)} - { - if (radius <= 0.0F) { - throw std::domain_error("Error constructing SpatialHash: must have positive side length"); - } - - if ((m_max_y_idx + m_y_stride) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // small fudging to prevent weird boundary effects - // (e.g (x=x_max, y) rolls index over to (x=0, y+1) - // cspell: ignore FEPS - // FEPS means "Float EPSilon" - constexpr auto FEPS = std::numeric_limits::epsilon(); - // lint -e{1938} read only access is fine NOLINT - m_max_x -= FEPS; - m_max_y -= FEPS; - m_max_z -= FEPS; - if ((m_z_stride + m_max_z_idx) > std::numeric_limits::max() / 2U) { - // TODO(c.ho) properly check for multiplication overflow - throw std::domain_error("SpatialHash::Config: voxel index may overflow!"); - } - // I don't know if this is even possible given other checks - if (std::numeric_limits::max() == m_max_z_idx) { - throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value"); - } - } - - /// \brief Given a reference index triple, compute the first and last bin - /// \param[in] ref The reference index triple - /// \param[in] radius The allowable radius for any point in the reference bin to any point in the - /// range - /// \return A pair where the first element is an index triple of the first bin, and the second - /// element is an index triple for the last bin - details::BinRange bin_range(const details::Index3 & ref, const float radius) const - { - // Compute distance in units of voxels - const Index i_radius = static_cast(std::ceil(radius / m_side_length)); - // Dumb ternary because potentially unsigned Index type - const Index x_min = (ref.x > i_radius) ? (ref.x - i_radius) : 0U; - const Index y_min = (ref.y > i_radius) ? (ref.y - i_radius) : 0U; - const Index z_min = (ref.z > i_radius) ? (ref.z - i_radius) : 0U; - // In 2D mode, m_max_z should be 0, same with ref.z - const Index x_max = std::min(ref.x + i_radius, m_max_x_idx); - const Index y_max = std::min(ref.y + i_radius, m_max_y_idx); - const Index z_max = std::min(ref.z + i_radius, m_max_z_idx); - // return bottom-left portion of cube and top-right portion of cube - return {{x_min, y_min, z_min}, {x_max, y_max, z_max}}; - } - - /// \brief Get next index within a given range - /// \return True if idx is valid and still in range - /// \param[in] range The max and min bin indices - /// \param[inout] idx The index to be incremented, updated even if a negative result occurs - bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const - { - // TODO(c.ho) is there any way to make this neater without triple nested if? - bool8_t ret = true; - ++idx.x; - if (idx.x > range.second.x) { - idx.x = range.first.x; - ++idx.y; - if (idx.y > range.second.y) { - idx.y = range.first.y; - ++idx.z; - if (idx.z > range.second.z) { - ret = false; - } - } - } - return ret; - } - /// \brief Get the maximum capacity of the spatial hash - /// \return The capacity - Index get_capacity() const { return m_capacity; } - - /// \brief Getter for the side length, equivalently the lookup radius - float32_t radius2() const { return m_side_length2; } - - //////////////////////////////////////////////////////////////////////////////////////////// - // "Polymorphic" API - /// \brief Compute the single index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The combined index of the bin for a given point - Index bin(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().bin_(x, y, z); - } - /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points - /// such that their distance is within a certain threshold - /// \param[in] ref The index triple of the bin containing the reference point - /// \param[in] query The index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if query and ref could possibly hold points within reference distance to one - /// another - bool is_candidate_bin( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const - { - return this->impl().valid(ref, query, ref_distance2); - } - /// \brief Compute the decomposed index given a point - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const - { - return this->impl().index3_(x, y, z); - } - /// \brief Compute the composed single index given a decomposed index - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index(const details::Index3 & idx) const { return this->impl().index_(idx); } - /// \brief Compute the squared distance between the two points - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d or 3d) - template - float32_t distance_squared( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - return this->impl().distance_squared_(x, y, z, pt); - } - -protected: - /// \brief Computes the index in the x basis direction - /// \param[in] x The x value of a point - /// \return The x offset of the index - Index x_index(const float32_t x) const - { - return static_cast( - std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv)); - } - /// \brief Computes the index in the y basis direction - /// \param[in] y The y value of a point - /// \return The x offset of the index - Index y_index(const float32_t y) const - { - return static_cast( - std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv)); - } - /// \brief Computes the index in the z basis direction - /// \param[in] z The z value of a point - /// \return The x offset of the index - Index z_index(const float32_t z) const - { - return static_cast( - std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv)); - } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx) const { return xdx + (ydx * m_y_stride); } - /// \brief Compose the provided index offsets - Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const - { - return bin_impl(xdx, ydx) + (zdx * m_z_stride); - } - - /// \brief The index offset of a point given it's x and y values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash - Index bin_impl(const float32_t x, const float32_t y) const - { - return bin_impl(x_index(x), y_index(y)); - } - /// \brief The index of a point given it's x, y and z values - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const - { - return bin_impl(x, y) + (z_index(z) * m_z_stride); - } - /// \brief The distance between two indices as a float, where adjacent indices have zero - /// distance (e.g. dist(0, 1) = 0) - float32_t idx_distance(const Index ref_idx, const Index query_idx) const - { - /// Not using fabs because Index is (possibly) unsigned - const Index i_dist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx); - float32_t dist = static_cast(i_dist) - 1.0F; - return std::max(dist, 0.0F); - } - - /// \brief Get side length squared - float side_length2() const { return m_side_length2; } - -private: - /// \brief Sanity check a range in a basis direction - /// \return The number of voxels in the given basis direction - Index check_basis_direction(const float32_t min, const float32_t max) const - { - if (min >= max) { - throw std::domain_error("SpatialHash::Config: must have min < max"); - } - // This family of checks is to ensure that you don't get weird casting effects due to huge - // floating point values - const float64_t dmax = static_cast(max); - const float64_t dmin = static_cast(min); - const float64_t width = (dmax - dmin) * static_cast(m_side_length_inv); - constexpr float64_t flt_max = static_cast(std::numeric_limits::max()); - if (flt_max <= width) { - throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit"); - } - return static_cast(width); - } - float32_t m_min_x; - float32_t m_min_y; - float32_t m_min_z; - float32_t m_max_x; - float32_t m_max_y; - float32_t m_max_z; - float32_t m_side_length; - float32_t m_side_length2; - float32_t m_side_length_inv; - Index m_capacity; - Index m_max_x_idx; - Index m_max_y_idx; - Index m_max_z_idx; - Index m_y_stride; - Index m_z_stride; -}; // class Config - -/// \brief Configuration class for a 2d spatial hash -class GEOMETRY_PUBLIC Config2d : public Config -{ -public: - /// \brief Config constructor for 2D spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 2d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 2D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 2d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 2d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 2d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (2d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - (void)z; - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - return (dx * dx) + (dy * dy); - } -}; // class Config2d - -/// \brief Configuration class for a 3d spatial hash -class GEOMETRY_PUBLIC Config3d : public Config -{ -public: - /// \brief Config constructor for a 3d spatial hash - /// \param[in] min_x The minimum x value for the spatial hash - /// \param[in] max_x The maximum x value for the spatial hash - /// \param[in] min_y The minimum y value for the spatial hash - /// \param[in] max_y The maximum y value for the spatial hash - /// \param[in] min_z The minimum z value for the spatial hash - /// \param[in] max_z The maximum z value for the spatial hash - /// \param[in] radius The lookup distance - /// \param[in] capacity The maximum number of points the spatial hash can store - Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity); - /// \brief The index of a point given it's x, y and z values, 3d implementation - /// \param[in] x The x value of a point - /// \param[in] y the y value of a point - /// \param[in] z the z value of a point - /// \return The index of the bin for the specified point - Index bin_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Determine if a bin could possibly hold a point within a distance to any point in a - /// reference bin for the 3D case - /// \param[in] ref The decomposed index triple of the reference bin - /// \param[in] query The decomposed index triple of the bin being queried - /// \param[in] ref_distance2 The squared threshold distance - /// \return True if the reference bin and query bin could possibly hold a point within the - /// reference distance - bool valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const; - /// \brief Compute the decomposed index given a point, 3d implementation - /// \param[in] x The x component of the point - /// \param[in] y The y component of the point - /// \param[in] z The z component of the point - /// \return The decomposed index triple of the bin for the given point - details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const; - /// \brief Compute the composed single index given a decomposed index, 3d implementation - /// \param[in] idx A decomposed index triple for a bin - /// \return The composed bin index - Index index_(const details::Index3 & idx) const; - /// \brief Compute the squared distance between the two points, 3d implementation - /// \tparam PointT A point type with float members x, y and z, or point adapters defined - /// \param[in] x The x component of the first point - /// \param[in] y The y component of the first point - /// \param[in] z The z component of the first point - /// \param[in] pt The other point being compared - /// \return The squared distance between the points (3d) - template - float32_t distance_squared_( - const float32_t x, const float32_t y, const float32_t z, const PointT & pt) const - { - const float32_t dx = x - point_adapter::x_(pt); - const float32_t dy = y - point_adapter::y_(pt); - const float32_t dz = z - point_adapter::z_(pt); - return (dx * dx) + (dy * dy) + (dz * dz); - } -}; // class Config3d -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware - -#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp deleted file mode 100644 index 8972246997997..0000000000000 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllexport) -#define GEOMETRY_LOCAL -#else // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#define GEOMETRY_PUBLIC __declspec(dllimport) -#define GEOMETRY_LOCAL -#endif // defined(GEOMETRY_BUILDING_DLL) || defined(GEOMETRY_EXPORTS) -#elif defined(__linux__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#elif defined(QNX) -#define GEOMETRY_PUBLIC __attribute__((visibility("default"))) -#define GEOMETRY_LOCAL __attribute__((visibility("hidden"))) -#else // defined(__linux__) -#error "Unsupported Build Configuration" -#endif // defined(__WIN32) -#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml deleted file mode 100644 index f53412298a485..0000000000000 --- a/common/autoware_auto_geometry/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_auto_geometry - 1.0.0 - Geometry related algorithms - Satoshi Ota - Apache License 2.0 - - Apex.AI, Inc. - - ament_cmake_auto - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_planning_msgs - autoware_auto_tf2 - autoware_auto_vehicle_msgs - geometry_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - osrf_testing_tools_cpp - - - ament_cmake - - diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp deleted file mode 100644 index 3a4ea96a151a2..0000000000000 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" -#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" - -#include -// cspell: ignore eigenbox -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace bounding_box -{ -namespace details -{ -//////////////////////////////////////////////////////////////////////////////// -void size_2d(const decltype(BoundingBox::corners) & corners, geometry_msgs::msg::Point32 & ret) -{ - ret.x = std::max( - norm_2d(minus_2d(corners[1U], corners[0U])), std::numeric_limits::epsilon()); - ret.y = std::max( - norm_2d(minus_2d(corners[2U], corners[1U])), std::numeric_limits::epsilon()); -} -//////////////////////////////////////////////////////////////////////////////// -void finalize_box(const decltype(BoundingBox::corners) & corners, BoundingBox & box) -{ - (void)std::copy(&corners[0U], &corners[corners.size()], &box.corners[0U]); - // orientation: this is robust to lines - const float32_t yaw_rad_2 = - atan2f(box.corners[2U].y - box.corners[1U].y, box.corners[2U].x - box.corners[1U].x) * 0.5F; - box.orientation.w = cosf(yaw_rad_2); - box.orientation.z = sinf(yaw_rad_2); - // centroid - box.centroid = times_2d(plus_2d(corners[0U], corners[2U]), 0.5F); -} - -autoware_auto_perception_msgs::msg::Shape make_shape(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::Shape retval; - // Polygon is 2D rectangle - geometry_msgs::msg::Polygon polygon; - auto & points = polygon.points; - points.resize(4); - - // Note that the x and y of size field in BoundingBox should be swapped when being used to - // compute corners. - // origin of reference system: centroid of bbox - points[0].x = -0.5F * box.size.y; - points[0].y = -0.5F * box.size.x; - points[0].z = -box.size.z; - - points[1] = points[0]; - points[1].y += box.size.x; - - points[2] = points[1]; - points[2].x += box.size.y; - - points[3] = points[2]; - points[3].y -= box.size.x; - - retval.footprint = polygon; - retval.dimensions.z = 2 * box.size.z; - - return retval; -} - -autoware_auto_perception_msgs::msg::DetectedObject make_detected_object(const BoundingBox & box) -{ - autoware_auto_perception_msgs::msg::DetectedObject ret; - - ret.kinematics.pose_with_covariance.pose.position.x = static_cast(box.centroid.x); - ret.kinematics.pose_with_covariance.pose.position.y = static_cast(box.centroid.y); - ret.kinematics.pose_with_covariance.pose.position.z = static_cast(box.centroid.z); - ret.kinematics.pose_with_covariance.pose.orientation.x = static_cast(box.orientation.x); - ret.kinematics.pose_with_covariance.pose.orientation.y = static_cast(box.orientation.y); - ret.kinematics.pose_with_covariance.pose.orientation.z = static_cast(box.orientation.z); - ret.kinematics.pose_with_covariance.pose.orientation.w = static_cast(box.orientation.w); - ret.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - - ret.shape = make_shape(box); - - ret.existence_probability = 1.0F; - - autoware_auto_perception_msgs::msg::ObjectClassification label; - label.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - label.probability = 1.0F; - ret.classification.emplace_back(std::move(label)); - - return ret; -} - -std::vector GEOMETRY_PUBLIC get_transformed_corners( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, - const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation) -{ - std::vector retval(shape_msg.footprint.points.size()); - geometry_msgs::msg::TransformStamped tf; - tf.transform.rotation = orientation; - tf.transform.translation.x = centroid.x; - tf.transform.translation.y = centroid.y; - tf.transform.translation.z = centroid.z; - - for (size_t i = 0U; i < shape_msg.footprint.points.size(); ++i) { - tf2::doTransform(shape_msg.footprint.points[i], retval[i], tf); - } - return retval; -} - -} // namespace details -/////////////////////////////////////////////////////////////////////////////// -// pre-compilation -using autoware::common::types::PointXYZIF; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using PointXYZIFVIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const PointXYZIFVIT begin, const PointXYZIFVIT end); -template BoundingBox lfit_bounding_box_2d( - const PointXYZIFVIT begin, const PointXYZIFVIT end); -using geometry_msgs::msg::Point32; -template BoundingBox minimum_area_bounding_box(std::list & list); -template BoundingBox minimum_perimeter_bounding_box(std::list & list); -using Point32VIT = std::vector::iterator; -template BoundingBox eigenbox_2d(const Point32VIT begin, const Point32VIT end); -template BoundingBox lfit_bounding_box_2d(const Point32VIT begin, const Point32VIT end); -} // namespace bounding_box -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp deleted file mode 100644 index ffd91aaa08b3a..0000000000000 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include -// lint -e537 NOLINT repeated include file due to cpplint rule -#include - -namespace autoware -{ -namespace common -{ -namespace geometry -{ -namespace spatial_hash -{ -//////////////////////////////////////////////////////////////////////////////// -Config2d::Config2d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, {}, std::numeric_limits::min(), radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return bin_impl(x, y); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config2d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config2d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - (void)z; - return {x_index(x), y_index(y), Index{}}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config2d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y); -} -//////////////////////////////////////////////////////////////////////////////// -Config3d::Config3d( - const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, - const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity) -: Config(min_x, max_x, min_y, max_y, min_z, max_z, radius, capacity) -{ -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::bin_(const float32_t x, const float32_t y, const float32_t z) const -{ - return bin_impl(x, y, z); -} -//////////////////////////////////////////////////////////////////////////////// -bool Config3d::valid( - const details::Index3 & ref, const details::Index3 & query, const float ref_distance2) const -{ - const float dx = idx_distance(ref.x, query.x); - const float dy = idx_distance(ref.y, query.y); - const float dz = idx_distance(ref.z, query.z); - // minor algebraic manipulation happening below - return (((dx * dx) + (dy * dy) + (dz * dz)) * side_length2()) <= ref_distance2; -} -//////////////////////////////////////////////////////////////////////////////// -details::Index3 Config3d::index3_(const float32_t x, const float32_t y, const float32_t z) const -{ - return {x_index(x), y_index(y), z_index(z)}; // zero initialization -} -//////////////////////////////////////////////////////////////////////////////// -Index Config3d::index_(const details::Index3 & idx) const -{ - return bin_impl(idx.x, idx.y, idx.z); -} -//////////////////////////////////////////////////////////////////////////////// -template class SpatialHash; -template class SpatialHash; -} // namespace spatial_hash -} // namespace geometry -} // namespace common -} // namespace autoware diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp deleted file mode 100644 index a179fbde5f151..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ /dev/null @@ -1,550 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_BOUNDING_BOX_HPP_ -#define TEST_BOUNDING_BOX_HPP_ - -#include "autoware_auto_geometry/bounding_box/lfit.hpp" -// cspell: ignore lfit -#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::geometry::point_adapter::yr_; -using autoware_auto_perception_msgs::msg::BoundingBox; - -template -class BoxTest : public ::testing::Test -{ -protected: - std::list points; - BoundingBox box; - - void minimum_area_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_area_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - void minimum_perimeter_bounding_box() - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::minimum_perimeter_bounding_box(points); - // apex_test_tools::memory_test::stop(); - } - - // cspell: ignore eigenbox - template - void eigenbox(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::eigenbox_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - template - void lfit_bounding_box_2d(const IT begin, const IT end) - { - // apex_test_tools::memory_test::start(); - box = autoware::common::geometry::bounding_box::lfit_bounding_box_2d(begin, end); - // apex_test_tools::memory_test::stop(); - } - - PointT make(const float x, const float y) - { - PointT ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; - } - - void check(const float cx, const float cy, const float sx, const float sy, const float val) const - { - constexpr float32_t TOL = 1.0E-4F; - ASSERT_LT(fabsf(box.size.x - sx), TOL); - ASSERT_LT(fabsf(box.size.y - sy), TOL); - ASSERT_LT(fabsf(box.value - val), TOL) << box.value; - - ASSERT_LT(fabsf(box.centroid.x - cx), TOL); - ASSERT_LT(fabsf(box.centroid.y - cy), TOL); - } - - void test_corners(const std::vector & expect, const float TOL = 1.0E-6F) const - { - for (uint32_t idx = 0U; idx < 4U; ++idx) { - bool found = false; - for (auto & p : expect) { - if (fabsf(x_(p) - box.corners[idx].x) < TOL && fabsf(y_(p) - box.corners[idx].y) < TOL) { - found = true; - break; - } - } - ASSERT_TRUE(found) << idx << ": " << box.corners[idx].x << ", " << box.corners[idx].y; - } - } - - /// \brief th_deg - phi_deg, normalized to +/- 180 deg - float32_t angle_distance_deg(const float th_deg, const float phi_deg) const - { - return fmodf((th_deg - phi_deg) + 540.0F, 360.0F) - 180.0F; - } - - /// \brief converts a radian value to a degree value - float32_t rad2deg(const float rad_val) const { return rad_val * 57.2958F; } - - void test_orientation(const float ref_angle_deg, const float TOL = 1.0E-4F) const - { - bool found = false; - const float angle_deg = rad2deg(2.0F * asinf(box.orientation.z)); - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 90.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 180.0F)) < TOL; - found |= fabsf(angle_distance_deg(angle_deg, ref_angle_deg + 270.0F)) < TOL; - ASSERT_TRUE(found) << angle_deg; - } -}; // BoxTest - -// Instantiate tests for given types, add more types here as they are used -using PointTypesBoundingBox = - ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BoxTest, PointTypesBoundingBox, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// TODO(c.ho) consider typed and parameterized tests: -// https://stackoverflow.com/questions/3258230/passing-a-typename-and-string-to-parameterized-test-using-google-test - -/////////////////////////////////////////// -TYPED_TEST(BoxTest, PointSegmentDistance) -{ - using autoware::common::geometry::closest_segment_point_2d; - using autoware::common::geometry::point_line_segment_distance_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 2.0F); - // boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -2.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 3.0F); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - t = closest_segment_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), 1.0F); - ASSERT_FLOAT_EQ(y_(t), 5.0F); - ASSERT_FLOAT_EQ(point_line_segment_distance_2d(p, q, r), 4.0F); -} - -TYPED_TEST(BoxTest, ClosestPointOnLine) -{ - using autoware::common::geometry::closest_line_point_2d; - // normal case - TypeParam p = this->make(-1.0F, 0.0F); - TypeParam q = this->make(-1.0F, 2.0F); - TypeParam r = this->make(1.0F, 1.0F); - TypeParam t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -1.0F); - ASSERT_FLOAT_EQ(y_(t), 1.0F); - // out-of-boundary case - p = this->make(1.0F, 0.0F); - q = this->make(-2.0F, 0.0F); - r = this->make(-5.0F, 0.0F); - t = closest_line_point_2d(p, q, r); - ASSERT_FLOAT_EQ(x_(t), -5.0F); - ASSERT_NEAR(y_(t), 0.0F, std::numeric_limits::epsilon()); - // singular case - p = this->make(1.0F, 5.0F); - q = this->make(1.0F, 5.0F); - r = this->make(1.0F, 1.0F); - EXPECT_THROW(t = closest_line_point_2d(p, q, r), std::runtime_error); -} - -TYPED_TEST(BoxTest, Basic) -{ - const std::vector data{ - {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}}; - this->points.insert(this->points.begin(), data.begin(), data.end()); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_corners(data); - this->test_orientation(0.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 2.0F); - this->test_corners(data); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, OrientedTriangle) -{ - this->points.insert(this->points.begin(), {this->make(5, 5), this->make(7, 7), this->make(5, 7)}); - - this->minimum_area_bounding_box(); - - this->check(5.5F, 6.5F, sqrtf(2.0F), 2.0F * sqrtf(2.0F), 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(4, 6), this->make(6, 8)}); - this->test_orientation(45.0F); - - this->minimum_perimeter_bounding_box(); - - this->check(6.0F, 6.0F, 2.0F, 2.0F, 4.0F); - this->test_corners({this->make(5, 5), this->make(7, 7), this->make(5, 7), this->make(7, 5)}); - this->test_orientation(0.0F); -} -// -TYPED_TEST(BoxTest, Hull) -{ - const uint32_t FUZZ_SIZE = 1024U; - const float dx = 9.0F; - const float dy = 15.0F; - const float rx = 10.0F; - const float ry = 5.0F; - const float dth = 0.0F; - - ASSERT_EQ(FUZZ_SIZE % 4U, 0U); - - // fuzz part 1 - for (uint32_t idx = 0U; idx < FUZZ_SIZE; ++idx) { - const float th = ((idx * autoware::common::types::TAU) / FUZZ_SIZE) + dth; - this->points.push_back(this->make(rx * cosf(th) + dx, ry * sinf(th) + dy)); - } - - this->minimum_area_bounding_box(); - - // allow 10cm = 2% of size for tolerance - const float TOL_M = 0.1F; - ASSERT_LT(fabsf(this->box.centroid.x - dx), TOL_M); - ASSERT_LT(fabsf(this->box.centroid.y - dy), TOL_M); - - this->test_corners( - {this->make(dx + rx, dy + ry), this->make(dx - rx, dy + ry), this->make(dx - rx, dy - ry), - this->make(dx + rx, dy - ry)}, - TOL_M); - - this->test_orientation(this->rad2deg(dth), 1.0F); - // allow 1 degree of tolerance - - ASSERT_LT(fabsf(this->box.size.y - 2.0F * rx), TOL_M); - ASSERT_LT(fabsf(this->box.size.x - 2.0F * ry), TOL_M); - ASSERT_FLOAT_EQ(this->box.value, this->box.size.x * this->box.size.y); -} - -// -TYPED_TEST(BoxTest, Collinear) -{ - this->points.insert( - this->points.begin(), {this->make(-2, -2), this->make(-3, -2), this->make(-4, -2), - this->make(-2, -4), this->make(-3, -4), this->make(-4, -4), - this->make(-2, -3), this->make(-2, -3), this->make(-2, -4)}); - - this->minimum_area_bounding_box(); - - this->check(-3.0F, -3.0F, 2.0F, 2.0F, 4.0F); - this->test_corners( - {this->make(-2, -2), this->make(-2, -4), this->make(-4, -4), this->make(-4, -2)}); - this->test_orientation(0.0F); -} - -// -TYPED_TEST(BoxTest, Line1) -{ - this->points.insert( - this->points.begin(), {this->make(-4, 3), this->make(-8, 6), this->make(-12, 9), - this->make(-16, 12), this->make(-20, 15), this->make(-24, 18), - this->make(-28, 21), this->make(-32, 24), this->make(-36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); - - this->minimum_perimeter_bounding_box(); - - this->check(-20.0F, 15.0F, 1.0E-6F, 40.0F, 40.00001F); - this->test_orientation(this->rad2deg(atan2f(3, -4))); - this->test_corners( - {this->make(-4, 3), this->make(-30, 27), this->make(-4, 3), this->make(-36, 27)}); -} - -// -TYPED_TEST(BoxTest, Line2) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 0), this->make(8, 0), this->make(12, 0), this->make(16, 0), this->make(20, 0), - this->make(24, 0), this->make(28, 0), this->make(32, 0), this->make(36, 0)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 0.0F, 1.0E-6F, 32.0F, 3.2E-5F); - this->test_orientation(0.0F); - this->test_corners({this->make(4, 0), this->make(36, 0), this->make(4, 0), this->make(36, 0)}); -} - -// -TYPED_TEST(BoxTest, Line3) -{ - this->points.insert( - this->points.begin(), - {this->make(4, 3), this->make(8, 6), this->make(12, 9), this->make(16, 12), this->make(20, 15), - this->make(24, 18), this->make(28, 21), this->make(32, 24), this->make(36, 27)}); - - this->minimum_area_bounding_box(); - - this->check(20.0F, 15.0F, 1.0E-6F, 40.0F, 4.0E-5F); - this->test_orientation(this->rad2deg(atan2f(3, 4))); - this->test_corners({this->make(4, 3), this->make(36, 27), this->make(4, 3), this->make(36, 27)}); -} - -/* _ - /_/ <-- first guess is suboptimal - -*/ -TYPED_TEST(BoxTest, SuboptimalInit) -{ - this->points.insert( - this->points.begin(), - {this->make(8, 15), this->make(17, 0), this->make(0, 0), this->make(25, 15)}); - - this->minimum_area_bounding_box(); - - this->check(12.5F, 7.5F, 15.0F, 25.0F, 375.0F); - this->test_orientation(this->rad2deg(atan2f(15, 8))); - // these are approximate. - this->test_corners( - {this->make(0, 0), this->make(25, 15), this->make(11.7647F, 22.0588F), - this->make(13.2353F, -7.05882F)}, - 0.1F); -} - -// -TYPED_TEST(BoxTest, Centered) -{ - this->points.insert( - this->points.begin(), - {this->make(-1, 0), this->make(1, 0), this->make(0, -1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.0F, 0.0F, sqrtf(2.0F), sqrtf(2.0F), 2.0F); - this->test_orientation(45.0F); - this->test_corners({this->make(-1, 0), this->make(1, 0), this->make(0, 1), this->make(0, -1)}); -} - -// convex_hull is imperfect in this case, check if this can save the day -TYPED_TEST(BoxTest, OverlappingPoints) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1), - this->make(0, 0), this->make(1, 0), this->make(1, 1), this->make(0, 1)}); - - this->minimum_area_bounding_box(); - - this->check(0.5F, 0.5F, 1.0F, 1.0F, 1.0F); - this->test_orientation(0.0F); - this->test_corners({this->make(0, 0), this->make(1, 0), this->make(0, 1), this->make(1, 1)}); -} - -// check that minimum perimeter box is different from minimum area box -TYPED_TEST(BoxTest, Perimeter) -{ - this->points.insert( - this->points.begin(), {this->make(0, 0), this->make(0, 1), this->make(0, 2), this->make(0, 3), - this->make(0, 4), this->make(1, -0.1), // small fudge to force diagonal - this->make(2, 0), this->make(3, 0)}); - - this->minimum_area_bounding_box(); - - this->check(0.54F, 1.28F, 5.0F, 12.0F / 5.0F, 12.0F); - this->test_orientation(-53.13F, 0.001F); - this->test_corners( - {this->make(3, 0), this->make(0, 4), this->make(-1.92, 2.56), this->make(1.08, -1.44)}); - - // eigenbox should produce AABB TODO(c.ho) - // compute minimum perimeter box - this->minimum_perimeter_bounding_box(); - this->check(1.5F, 1.95F, 4.1F, 3.0F, 7.1F); - // perimeter for first box would be 14.8 - this->test_orientation(0.0F, 0.001F); - this->test_corners( - {this->make(3, -0.1), this->make(0, 4), this->make(3, 4), this->make(0, -0.1)}); -} - -// bounding box is diagonal on an L -TYPED_TEST(BoxTest, Eigenbox1) -{ - std::vector v{this->make(0, 0), this->make(0, 1), - this->make(-1, 2), // small fudge to force diagonal - this->make(0, 3), this->make(0, 4), - this->make(1, 0), this->make(2, -1), // small fudge to force diagonal - this->make(3, 0), this->make(4, 0)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - // rotating calipers should produce a diagonal box - this->minimum_area_bounding_box(); - const float r = 4.0F; - - this->check(1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * r); - const std::vector diag_corners{ - this->make(4, 0), this->make(0, 4), this->make(-2, 2), this->make(2, -2)}; - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - - //// perimeter should also produce diagonal //// - this->minimum_perimeter_bounding_box(); - this->check( - 1.0F, 1.0F, r / sqrtf(2.0F), sqrtf(2.0F) * r, r * (sqrtf(2.0F) + (1.0F / sqrtf(2.0F)))); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// eigenbox should also produce aabb //// - this->eigenbox(v.begin(), v.end()); - // TODO(c.ho) don't care about value.. - this->check(1.0F, 1.0F, r * sqrtf(2.0F), r / sqrtf(2.0F), 4.375F); - this->test_corners(diag_corners); - this->test_orientation(45.0F, 0.001F); - //// Lfit should produce aabb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - - this->test_corners( - {this->make(4, -1), this->make(-1, 4), this->make(4, 4), this->make(-1, -1)}, 0.25F); - this->test_orientation(0.0F, 3.0F); -} - -// same as above test, just rotated -TYPED_TEST(BoxTest, Eigenbox2) -{ - std::vector v{this->make(0, 0), this->make(1, 1), - this->make(1, 2), // small fudge to force diagonal - this->make(3, 3), this->make(4, 4), - this->make(1, -1), this->make(1, -2), // small fudge to force diagonal - this->make(3, -3), this->make(4, -4)}; - this->points.insert(this->points.begin(), v.begin(), v.end()); - - const std::vector diag_corners{ - this->make(4, 4), this->make(0, 4), this->make(0, -4), this->make(4, -4)}; - // rotating calipers should produce a aabb - this->minimum_area_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 32); - this->test_corners(diag_corners); - this->test_orientation(0.0F, 0.001F); - - //// perimeter should also produce aabb //// - this->minimum_perimeter_bounding_box(); - this->check(2.0F, 0.0F, 8, 4, 12); - this->test_corners(diag_corners); - - //// eigenbox should also produce obb //// - this->eigenbox(v.begin(), v.end()); - this->test_orientation(0.0F, 0.001F); - this->test_corners(diag_corners); - //// Lfit should produce obb //// - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(3.5, 4.5), this->make(-1, 0), this->make(3.5, -4.5), this->make(8, 0)}, 0.2F); - this->test_orientation(45.0F, 2.0F); -} -// line case for eigenbox -TYPED_TEST(BoxTest, Eigenbox3) -{ - // horizontal line with some noise - std::vector v{this->make(0, 0.00), this->make(1, -0.01), this->make(3, 0.02), - this->make(3, 0.03), this->make(4, -0.04), this->make(-1, 0.01), - this->make(-2, -0.02), this->make(-3, -0.03), this->make(-4, 0.04)}; - this->lfit_bounding_box_2d(v.begin(), v.end()); - this->test_corners( - {this->make(-4, -0.04), this->make(-4, 0.04), this->make(4, -0.04), this->make(4, 0.04)}, 0.2F); - this->test_orientation(0.0F, 1.0F); -} - -// bad case: causes intersection2d to fail -// See https://gitlab.apex.ai/ApexAI/grand_central/issues/2862#note_156875 for more failure cases -TYPED_TEST(BoxTest, IntersectFail) -{ - std::vector vals{ - this->make(-13.9, 0.100006), this->make(-14.1, 0.100006), this->make(-13.9, 0.300003), - this->make(-14.1, 0.300003), this->make(-13.9, 0.5), this->make(-14.1, 0.5), - this->make(-13.9, 0.699997), this->make(-14.1, 0.699997), this->make(-14.3, 0.699997)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(-2.1, 10.1), this->make(-1.89999, 10.1), this->make(-1.89999, 10.3), - this->make(-1.7, 10.3), this->make(-1.5, 10.3), this->make(-1.3, 10.3), - this->make(-0.5, 10.3), this->make(-0.300003, 10.3), this->make(-0.0999908, 10.3), - this->make(0.699997, 10.3), this->make(0.900009, 10.3), this->make(1.3, 10.3), - this->make(1.7, 10.3)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); - vals = {this->make(2.7, -5.5), this->make(2.7, -5.3), this->make(2.7, -5.1), - this->make(2.7, -4.9), this->make(2.7, -4.7), this->make(2.7, -4.5), - this->make(2.7, -4.3), this->make(2.7, -4.1), this->make(2.7, -3.9)}; - EXPECT_NO_THROW(this->lfit_bounding_box_2d(vals.begin(), vals.end())); -} -/// Handle slight floating point underflow case -// Note: raw discriminant checks are disabled because they don't work. I suspect this is due to -// tiny differences in floating point math when using our compile flags -TYPED_TEST(BoxTest, EigUnderflow) -{ - using autoware::common::geometry::bounding_box::details::Covariance2d; - // auto discriminant = [](const Covariance2d cov) -> float32_t { - // // duplicated raw math - // const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F; - // const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy); - // return (tr_2 * tr_2) - det; - // }; - TypeParam u, v; - const Covariance2d c1{0.0300002, 0.0300002, 5.46677e-08, 0U}; - // EXPECT_LT(discriminant(c1), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c1, u, v)); - const Covariance2d c2{0.034286, 0.0342847, 2.12874e-09, 0U}; - // EXPECT_LT(discriminant(c2), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c2, u, v)); - const Covariance2d c3{0.0300002, 0.0300002, -2.84987e-08, 0U}; - // EXPECT_LT(discriminant(c3), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c3, u, v)); - const Covariance2d c4{0.0300004, 0.0300002, 3.84156e-08, 0U}; - // EXPECT_LT(discriminant(c4), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c4, u, v)); - const Covariance2d c5{0.0300014, 0.0300014, -7.45058e-09, 0U}; - // EXPECT_LT(discriminant(c5), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c5, u, v)); - const Covariance2d c6{0.0400004, 0.0400002, 3.28503e-08, 0U}; - // EXPECT_LT(discriminant(c6), 0.0F); - EXPECT_NO_THROW(autoware::common::geometry::bounding_box::details::eig_2d(c6, u, v)); -} - -//////////////////////////////////////////////// - -#endif // TEST_BOUNDING_BOX_HPP_ diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp deleted file mode 100644 index fc2d97c257b95..0000000000000 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ /dev/null @@ -1,260 +0,0 @@ -// Copyright 2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef TEST_SPATIAL_HASH_HPP_ -#define TEST_SPATIAL_HASH_HPP_ - -#include "autoware_auto_geometry/spatial_hash.hpp" - -#include - -#include -#include - -using autoware::common::geometry::spatial_hash::Config2d; -using autoware::common::geometry::spatial_hash::Config3d; -using autoware::common::geometry::spatial_hash::SpatialHash; -using autoware::common::geometry::spatial_hash::SpatialHash2d; -using autoware::common::geometry::spatial_hash::SpatialHash3d; -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedSpatialHashTest : public ::testing::Test -{ -public: - TypedSpatialHashTest() : ref(), EPS(0.001F) - { - ref.x = 0.0F; - ref.y = 0.0F; - ref.z = 0.0F; - } - -protected: - template - void add_points( - SpatialHash & hash, const uint32_t points_per_ring, const uint32_t num_rings, - const float32_t dr, const float32_t dx = 0.0F, const float32_t dy = 0.0F) - { - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - - // insert - float32_t r = dr; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) + dx; - pt.y = r * sinf(th) + dy; - pt.z = 0.0F; - hash.insert(pt); - th += dth; - } - r += dr; - } - } - PointT ref; - const float32_t EPS; -}; // SpatialHash -// test struct - -// Instantiate tests for given types, add more types here as they are used -using PointTypesSpatialHash = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedSpatialHashTest, PointTypesSpatialHash, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -/////////////////////////////////////////////////////////////// -// TODO(christopher.ho) helper functions to simplify this stuff -/// all points in one bin -TYPED_TEST(TypedSpatialHashTest, OneBin) -{ - using PointT = TypeParam; - const float32_t dr = 1.0F; - Config2d cfg{-10.0F, 10.0F, -10.0F, 10.0F, 3.0F, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 10U; - const uint32_t NUM_RINGS = 1U; - this->add_points(hash, PTS_PER_RING, NUM_RINGS, dr); - - // loop through all points - float r = dr - this->EPS; - for (uint32_t rdx = 0U; rdx < NUM_RINGS + 1U; ++rdx) { - const uint32_t n_pts = rdx * PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - ASSERT_EQ(points_seen, n_pts); - r += dr; - // Make sure statistics are consistent - EXPECT_EQ(hash.bins_hit(), 9U * (1U + rdx)); - EXPECT_EQ(hash.neighbors_found(), rdx * PTS_PER_RING); - } - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(PTS_PER_RING, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} -/// test out of bounds points -TYPED_TEST(TypedSpatialHashTest, Oob) -{ - using PointT = TypeParam; - const float32_t dr = 20.0F; - Config2d cfg{-2.0F, 2.0F, -2.0F, 2.0F, dr + this->EPS, 1024U}; - SpatialHash2d hash{cfg}; - - // build concentric rings around origin - const uint32_t PTS_PER_RING = 12U; - this->add_points(hash, PTS_PER_RING, 1U, dr); - - // loop through all points - float32_t r = dr + this->EPS; - const uint32_t n_pts = PTS_PER_RING; - const auto & neighbors = hash.near(this->ref, r); - uint32_t points_seen = 0U; - for (const auto itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); - ASSERT_LT(dist, r); - ASSERT_GT(dist, 10.0F * sqrtf(2.0F)); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); -} -// 3d test case -TYPED_TEST(TypedSpatialHashTest, 3d) -{ - using PointT = TypeParam; - Config3d cfg{-30.0F, 30.0F, -30.0F, 30.0F, -30.0F, 30.0F, 30.0F, 1024U}; - SpatialHash3d hash{cfg}; - EXPECT_TRUE(hash.empty()); - - // build concentric rings around origin - const uint32_t points_per_ring = 32U; - const uint32_t num_rings = 5U; - const float32_t dth = 2.0F * 3.14159F / points_per_ring; - std::vector pts{}; - - // insert - const float32_t r = 10.0F; - float32_t phi = 0.0f; - for (uint32_t rdx = 0U; rdx < num_rings; ++rdx) { - float32_t th = 0.0F; - for (uint32_t pdx = 0U; pdx < points_per_ring; ++pdx) { - PointT pt; - pt.x = r * cosf(th) * cosf(phi); - pt.y = r * sinf(th) * cosf(phi); - pt.z = r * sinf(phi); - pts.push_back(pt); - th += dth; - } - hash.insert(pts.begin(), pts.end()); - pts.clear(); - phi += 1.0f; - } - EXPECT_FALSE(hash.empty()); - EXPECT_EQ(hash.size(), num_rings * points_per_ring); - - // loop through all points - const uint32_t n_pts = num_rings * points_per_ring; - const auto & neighbors = hash.near(this->ref, r + this->EPS); - uint32_t points_seen = 0U; - for (const auto & itd : neighbors) { - const PointT & pt = itd; - const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); - ASSERT_LT(dist, r + this->EPS); - ASSERT_FLOAT_EQ(dist, itd.get_distance()); - ++points_seen; - } - - ASSERT_EQ(points_seen, n_pts); - - // check iterators etc. - uint32_t count = 0U; - for (auto iter = hash.cbegin(); iter != hash.cend(); ++iter) { - // TODO(c.ho) check uniqueness of stuff - ++count; - } - EXPECT_EQ(n_pts, count); - hash.clear(); - EXPECT_EQ(hash.size(), 0U); - EXPECT_TRUE(hash.empty()); - count = 0U; - for (auto it : hash) { - // TODO(c.ho) check uniqueness of stuff - (void)it; - ++count; - } - EXPECT_EQ(count, 0U); -} - -/// edge cases -TEST(SpatialHashConfig, BadCases) -{ - // negative side length - EXPECT_THROW(Config2d({-30.0F, 30.0F, -30.0F, 30.0F, -1.0F, 1024U}), std::domain_error); - // min_x >= max_x - EXPECT_THROW(Config2d({31.0F, 30.0F, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_y >= max_y - EXPECT_THROW(Config2d({-30.0F, 30.0F, 30.1F, 30.0F, 1.0F, 1024U}), std::domain_error); - // min_z >= max_z - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, 31.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - // floating point limit - constexpr float32_t max_float = std::numeric_limits::max(); - EXPECT_THROW(Config2d({-max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -max_float, max_float, -30.0F, 30.0F, 1.0F, 1024U}), - std::domain_error); - EXPECT_THROW( - Config3d({-30.0F, 30.0F, -30.0F, 30.0F, -max_float, max_float, 1.0F, 1024U}), - std::domain_error); - // y would overflow - // constexpr float32_t big_float = - // static_cast(std::numeric_limits::max() / 4UL); - // EXPECT_THROW(Config({-big_float, big_float, -big_float, big_float, 0.001F, 1024U}), - // std::domain_error); - // z would overflow - // EXPECT_THROW( - // Config3d({-30.0F, 30.0F, -99999.0F, 99999.0F, -99999.0F, 99999.0F, 0.001F, 1024U}), - // std::domain_error); - // TODO(c.ho) re-enable test when we can actually check unsigned integer multiplication overflow -} -#endif // TEST_SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp deleted file mode 100644 index 81865656c55b5..0000000000000 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2017-2020 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/lookup_table.hpp" - -#include - -#include - -#include -#include - -using autoware::common::helper_functions::interpolate; -using autoware::common::helper_functions::lookup_1d; -using autoware::common::helper_functions::LookupTable1D; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class BadCase : public ::testing::Test -{ -}; - -using BadTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(BadCase, BadTypes, ); - -// Empty domain/range -TYPED_TEST(BadCase, Empty) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({}, {}, T{}), std::domain_error); -} - -// Unequal domain/range -TYPED_TEST(BadCase, UnequalDomain) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{1}, T{2}}, {T{1}}, T{}), std::domain_error); -} - -// Not sorted domain -TYPED_TEST(BadCase, DomainNotSorted) -{ - using T = TypeParam; - EXPECT_THROW(lookup_1d({T{2}, T{1}}, {T{1}, T{2}}, T{}), std::domain_error); -} - -//////////////////////////////////////////////////////////////////////////////// - -template -class SanityCheck : public ::testing::Test -{ -public: - using T = Type; - - void SetUp() override - { - domain_ = std::vector{T{1}, T{3}, T{5}}; - range_ = std::vector{T{2}, T{4}, T{0}}; - ASSERT_NO_THROW(table_ = std::make_unique>(domain_, range_)); - } - - bool not_in_domain(const T bad_value) const noexcept - { - return std::find(domain_.begin(), domain_.end(), bad_value) == domain_.end(); - } - - void check(const T expected, const T actual) const noexcept - { - if (std::is_floating_point::value) { - EXPECT_FLOAT_EQ(actual, expected); - } else { - EXPECT_EQ(actual, expected); - } - } - -protected: - std::vector domain_{}; - std::vector range_{}; - std::unique_ptr> table_{}; -}; - -using NormalTypes = ::testing::Types; -TYPED_TEST_SUITE(SanityCheck, NormalTypes, ); - -TYPED_TEST(SanityCheck, Exact) -{ - const auto x = this->domain_[1U]; - const auto result = this->table_->lookup(x); - ASSERT_FALSE(this->not_in_domain(x)); - this->check(result, this->range_[1U]); -} - -TYPED_TEST(SanityCheck, Interpolation) -{ - const auto x = TypeParam{2}; - // Assert x is not identically in domain_ - ASSERT_TRUE(this->not_in_domain(x)); - const auto result = this->table_->lookup(x); - this->check(result, TypeParam{3}); -} - -TYPED_TEST(SanityCheck, AboveRange) -{ - const auto x = TypeParam{999999}; - ASSERT_GT(x, this->domain_.back()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.back()); -} - -TYPED_TEST(SanityCheck, BelowRange) -{ - const auto x = TypeParam{0}; - ASSERT_LT(x, this->domain_.front()); // domain is known to be sorted - const auto result = this->table_->lookup(x); - this->check(result, this->range_.front()); -} - -TEST(LookupTableHelpers, Interpolate) -{ - { - const auto scaling = 0.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = -1.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 2.0f); - } - - { - const auto scaling = 2.0f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 1.0f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.5f); - } - - { - const auto scaling = 0.75f; - EXPECT_EQ(interpolate(0.0f, 1.0f, scaling), 0.75f); - EXPECT_EQ(interpolate(2.0f, 3.5f, scaling), 3.125f); - } -} - -// TODO(c.ho) check with more interesting functions diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp deleted file mode 100644 index bc9c28682ed44..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 Apex.AI, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include - -#include - -#include -#include -#include - -template -class AreaTest : public ::testing::Test -{ -protected: - DataStructure data_{}; - using Point = typename DataStructure::value_type; - using Real = decltype(autoware::common::geometry::point_adapter::x_(std::declval())); - - auto area() { return autoware::common::geometry::area_checked_2d(data_.begin(), data_.end()); } - - void add_point(Real x, Real y) - { - namespace pa = autoware::common::geometry::point_adapter; - Point p{}; - pa::xr_(p) = x; - pa::yr_(p) = y; - (void)data_.insert(data_.end(), p); - } -}; - -// Data structures to test... -template -using TestTypes_ = ::testing::Types..., std::list...>; -// ... and point types to test -using TestTypes = TestTypes_; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(AreaTest, TestTypes, ); - -// The empty set has zero area -TYPED_TEST(AreaTest, DegenerateZero) -{ - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An individual point has zero area -TYPED_TEST(AreaTest, DegenerateOne) -{ - this->add_point(0.0, 0.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// An line segment has zero area -TYPED_TEST(AreaTest, DegenerateTwo) -{ - this->add_point(1.0, -1.0); - this->add_point(-3.0, 2.0); - EXPECT_FLOAT_EQ(0.0, this->area()); -} - -// Simple triangle -TYPED_TEST(AreaTest, Triangle) -{ - this->add_point(1.0, 0.0); - this->add_point(3.0, 0.0); // 2.0 wide - this->add_point(2.0, 2.0); // 2.0 tall - EXPECT_FLOAT_EQ(2.0, this->area()); // A = (1/2) * b * h -} - -// Rectangle is easy to do computational -TYPED_TEST(AreaTest, Rectangle) -{ - this->add_point(-5.0, -5.0); - this->add_point(-2.0, -5.0); // L = 3 - this->add_point(-2.0, -1.0); // H = 4 - this->add_point(-5.0, -1.0); - EXPECT_FLOAT_EQ(12.0, this->area()); // A = b * h -} - -// Parallelogram is slightly less trivial than a rectangle -TYPED_TEST(AreaTest, Parallelogram) -{ - this->add_point(-5.0, 1.0); - this->add_point(-2.0, 1.0); // L = 3 - this->add_point(-1.0, 3.0); // H = 2 - this->add_point(-4.0, 3.0); - EXPECT_FLOAT_EQ(6.0, this->area()); // A = b * h -} - -// Octagon is analytical and reasonably easy to build -TYPED_TEST(AreaTest, Octagon) -{ - const auto sq2 = std::sqrt(2.0); - const auto a = 1.0; - const auto a2 = a / 2.0; - const auto b = (a + sq2) / 2.0; - this->add_point(-a2, -b); - this->add_point(a2, -b); - this->add_point(b, -a2); - this->add_point(b, a2); - this->add_point(a2, b); - this->add_point(-a2, b); - this->add_point(-b, a2); - this->add_point(-b, -a2); - const auto expect = (2.0 * (1.0 + sq2)) * (a * a); - EXPECT_FLOAT_EQ(expect, this->area()); // A = b * h -} - -// Bad case -TYPED_TEST(AreaTest, NotCcw) -{ - this->add_point(0.0, 0.0); - this->add_point(1.0, 1.0); - this->add_point(1.0, 0.0); - this->add_point(2.0, 1.0); - EXPECT_THROW(this->area(), std::domain_error); -} diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp deleted file mode 100644 index baf6967edd47e..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// Copyright 2021 The 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/common_2d.hpp" - -#include -#include - -#include - -#include -#include - -using autoware::common::geometry::point_adapter::xr_; -using autoware::common::geometry::point_adapter::yr_; - -// Helper function for adding new points -template -T make_points(const float x, const float y) -{ - T ret; - xr_(ret) = x; - yr_(ret) = y; - return ret; -} - -// PointTypes to be tested -using PointTypes = - ::testing::Types; - -// Wrapper function for stubbing output of -// autoware::common::geometry::check_point_position_to_line_2d -template -int point_position_checker(const T & p1, const T & p2, const T & q) -{ - auto result = autoware::common::geometry::check_point_position_to_line_2d(p1, p2, q); - if (result > 0.0F) { - return 1; - } else if (result < 0.0F) { - return -1; - } - return result; -} - -// Templated struct defining the function parameters for -// autoware::common::geometry::check_point_position_to_line_2d and input_output vector for -// assertion of return values from the function -template -struct PointPositionToLine : public ::testing::Test -{ - struct Parameters - { - T p1; - T p2; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(PointPositionToLine); - -template -std::vector::Parameters, int>> - PointPositionToLine::input_output{ - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(1.0F, 5.0F)}, -1}, - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-1.0F, 0.5F)}, 1}, - // Check point on the line - {{make_points(0.0F, 0.0F), make_points(-1.0F, 1.0F), make_points(-2.0F, 2.0F)}, 0}, - }; - -TYPED_TEST_P(PointPositionToLine, PointPositionToLineTest) -{ - for (size_t i = 0; i < PointPositionToLine::input_output.size(); ++i) { - const auto & input = PointPositionToLine::input_output[i].first; - EXPECT_EQ( - point_position_checker(input.p1, input.p2, input.q), - PointPositionToLine::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(PointPositionToLine, PointPositionToLineTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, PointPositionToLine, PointTypes, ); - -///////////////////////////////////////////////////////////////////////////////////// - -// Templated struct defining the function parameters for -// autoware::common::geometry::is_point_inside_polygon_2d and input_output vector for -// assertion of return values from the function -template -struct InsidePolygon : public ::testing::Test -{ - struct Parameters - { - std::vector polygon; - T q; - }; - static std::vector> input_output; -}; - -TYPED_TEST_SUITE_P(InsidePolygon); - -template -std::vector::Parameters, bool>> InsidePolygon::input_output{ - // point inside the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.F, 0.5F)}, - true}, - // point below the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.F)}, - false}, - // point above the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 1.75F)}, - false}, - // point on the rectangle - {{{make_points(0.0F, 0.0F), make_points(1.0F, 1.0F), make_points(0.5F, 1.5F), - make_points(-0.5F, 0.5F)}, - make_points(0.5F, 0.5F)}, - true}, -}; - -TYPED_TEST_P(InsidePolygon, InsidePolygonTest) -{ - for (size_t i = 0; i < InsidePolygon::input_output.size(); ++i) { - const auto & input = InsidePolygon::input_output[i].first; - EXPECT_EQ( - autoware::common::geometry::is_point_inside_polygon_2d( - input.polygon.begin(), input.polygon.end(), input.q), - InsidePolygon::input_output[i].second) - << "Index " << i; - } -} - -REGISTER_TYPED_TEST_SUITE_P(InsidePolygon, InsidePolygonTest); -// cppcheck-suppress syntaxError -INSTANTIATE_TYPED_TEST_SUITE_P(Test, InsidePolygon, PointTypes, ); - -TEST(ordered_check, basic) -{ - // CW - std::vector points_list = { - make_points(8.0, 4.0), - make_points(9.0, 1.0), - make_points(3.0, 2.0), - make_points(2.0, 5.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // CCW - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(9.0, 1.0), - make_points(8.0, 4.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(2.0, 5.0), - make_points(3.0, 2.0), - make_points(8.0, 4.0), - make_points(9.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Unordered - points_list = { - make_points(0.0, 0.0), - make_points(1.0, 1.0), - make_points(1.0, 0.0), - make_points(2.0, 1.0)}; - EXPECT_FALSE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); - - // Collinearity - points_list = { - make_points(2.0, 2.0), - make_points(4.0, 4.0), - make_points(6.0, 6.0)}; - EXPECT_TRUE(autoware::common::geometry::all_ordered(points_list.begin(), points_list.end())); -} diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp deleted file mode 100644 index 8b9bbce36c428..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ /dev/null @@ -1,372 +0,0 @@ -// Copyright 2017-2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" - -#include - -#include - -#include -#include - -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedConvexHullTest : public ::testing::Test -{ -protected: - std::list list; - - typename std::list::const_iterator convex_hull() - { - const auto ret = autoware::common::geometry::convex_hull(list); - return ret; - } - - void check_hull( - const typename std::list::const_iterator last, const std::vector & expect, - const bool8_t strict = true) - { - uint32_t items = 0U; - for (auto & pt : expect) { - bool8_t found = false; - auto it = list.begin(); - while (it != last) { - constexpr float32_t TOL = 1.0E-6F; - if ( - fabsf(pt.x - it->x) <= TOL && fabsf(pt.y - it->y) <= TOL && - (fabsf(pt.z - it->z) <= TOL || !strict)) // TODO(@estive): z if only strict - { - found = true; - break; - } - ++it; - } - ASSERT_TRUE(found) << items; - ++items; - } - if (strict) { - ASSERT_EQ(items, expect.size()); - } - } - - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class convex_hull_test - -// Instantiate tests for given types, add more types here as they are used -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedConvexHullTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -////////////////////////////////////////// - -/* - 3 - 2 -1 -*/ -TYPED_TEST(TypedConvexHullTest, Triangle) -{ - std::vector expect({this->make(1, 0, 0), this->make(3, 1, 0), this->make(2, 2, 0)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 3U); - // check order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 3); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} -/* - 2 1 - -4 - 3 -*/ -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadrilateral) -{ - std::vector expect( - {this->make(-1, -1, 1), this->make(-5, -1, 2), this->make(-2, -6, 3), this->make(-6, -5, 4)}); - this->list.insert(this->list.begin(), expect.begin(), expect.end()); - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(this->list.size(), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, -6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, -2); - ++it; // node 3 - ASSERT_FLOAT_EQ(it->x, -1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, -5); - ++it; // node 2 - ASSERT_EQ(it, last); -} - -// test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, QuadHull) -{ - std::vector data( - {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), - this->make(6, 5, 5)}); - std::vector expect{{data[0], data[1], data[2], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->x, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->x, 5); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->x, 6); - ++it; // node 4 - ASSERT_FLOAT_EQ(it->x, 2); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// a ring plus a bunch of random stuff in the middle -TYPED_TEST(TypedConvexHullTest, Hull) -{ - const uint32_t HULL_SIZE = 13U; - const uint32_t FUZZ_SIZE = 50U; - const float32_t dth = 1.133729384F; // some weird irrational(ish) number - const float32_t r_hull = 20.0F; - const float32_t r_fuzz = 10.0F; - ASSERT_LT(r_fuzz, r_hull); - - std::vector hull; - - uint32_t hull_pts = 0U; - float32_t th = 0.0F; - // hull part 1 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 1 - uint32_t fuzz_pts = 0U; - for (uint32_t idx = 0U; idx < FUZZ_SIZE / 2U; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++fuzz_pts; - } - - // hull part 2 - for (uint32_t idx = 0U; idx < HULL_SIZE / 3U; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - ++hull_pts; - } - - // fuzz part 2 - for (uint32_t idx = 0U; idx < FUZZ_SIZE - fuzz_pts; ++idx) { - const auto pt = this->make(r_fuzz * cosf(th), r_fuzz * sinf(th), th); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - // hull part 3 - for (uint32_t idx = 0U; idx < HULL_SIZE - hull_pts; ++idx) { - const auto pt = this->make(r_hull * cosf(th), r_hull * sinf(th), th); - hull.push_back(pt); - this->list.push_back(pt); - th = fmodf(th + dth, 2.0F * 3.14159F); - } - - const auto last = this->convex_hull(); - - this->check_hull(last, hull); - ASSERT_EQ(std::distance(this->list.cbegin(), last), HULL_SIZE); -} - -TYPED_TEST(TypedConvexHullTest, Collinear) -{ - std::vector data( - {this->make(0, 0, 1), this->make(1, 0, 2), this->make(2, 0, 3), this->make(0, 2, 4), - this->make(1, 2, 8), this->make(2, 2, 7), this->make(1, 0, 6), this->make(1, 2, 5), - this->make(1, 1, 0)}); - const std::vector expect{{data[0], data[2], data[3], data[5]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - this->check_hull(last, expect); - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - - // check for order - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 1 - ASSERT_FLOAT_EQ(it->z, 7); - ++it; // node 2 - ASSERT_FLOAT_EQ(it->z, 4); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -// degenerate cases -TYPED_TEST(TypedConvexHullTest, OverlappingPoints) -{ - std::vector data( - {this->make(3, -1, 1), this->make(4, -2, 2), this->make(5, -7, 3), this->make(4, -2, 4), - this->make(5, -7, 8), this->make(3, -1, 7), this->make(5, -7, 6), this->make(4, -2, 5), - this->make(3, -1, 0)}); - const std::vector expect{{data[0], data[1], data[2]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 3U); - this->check_hull(last, expect, false); -} - -TYPED_TEST(TypedConvexHullTest, Line) -{ - std::vector data( - {this->make(-3, 3, 1), this->make(-2, 2, 2), this->make(-1, 1, 3), this->make(-8, 8, 4), - this->make(-6, 6, 8), this->make(-4, 4, 7), this->make(-10, 10, 6), this->make(-12, 12, 5), - this->make(-11, 11, 0)}); - const std::vector expect{{data[2], data[7]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 2U); - this->check_hull(last, expect, false); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 5); - ++it; // node 8 - ASSERT_FLOAT_EQ(it->z, 3); - ++it; // node 3 - ASSERT_EQ(it, last); -} - -/* -1 - 4 - - 3 - 2 -*/ -TYPED_TEST(TypedConvexHullTest, LowerHull) -{ - const std::vector data({ - this->make(1, 3, 1), - this->make(2, -2, 2), - this->make(3, -1, 3), - this->make(4, 1, 4), - }); - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 4U); - this->check_hull(last, data); - - // check for order: this part is a little loose - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_EQ(it, last); -} - -// Ensure the leftmost item is properly shuffled -/* - 5 -1 6 - 2 4 - 3 -*/ -TYPED_TEST(TypedConvexHullTest, Root) -{ - const std::vector data({ - this->make(0, 0, 1), - this->make(1, -1, 2), - this->make(3, -2, 3), - this->make(4, 0, 4), - this->make(3, 1, 5), - this->make(1, 0, 6), - }); - const std::vector expect{{data[0], data[1], data[2], data[3], data[4]}}; - this->list.insert(this->list.begin(), data.begin(), data.end()); - - const auto last = this->convex_hull(); - - ASSERT_EQ(std::distance(this->list.cbegin(), last), 5); - this->check_hull(last, expect); - - auto it = this->list.begin(); - ASSERT_FLOAT_EQ(it->z, 1); - ++it; - ASSERT_FLOAT_EQ(it->z, 2); - ++it; - ASSERT_FLOAT_EQ(it->z, 3); - ++it; - ASSERT_FLOAT_EQ(it->z, 4); - ++it; - ASSERT_FLOAT_EQ(it->z, 5); - ++it; - ASSERT_EQ(it, last); - EXPECT_NE(last, this->list.cend()); - EXPECT_EQ(last->z, 6); -} - -// TODO(c.ho) random input, fuzzing, stress tests diff --git a/common/autoware_auto_geometry/test/src/test_geometry.cpp b/common/autoware_auto_geometry/test/src/test_geometry.cpp deleted file mode 100644 index 0a7dd288d0d79..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_geometry.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "gtest/gtest.h" -#include "test_bounding_box.hpp" -#include "test_spatial_hash.hpp" - -int32_t main(int32_t argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp deleted file mode 100644 index 9477a83a488ed..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/hull_pockets.hpp" - -#include - -#include - -#include -#include -#include -#include - -using autoware::common::geometry::point_adapter::x_; -using autoware::common::geometry::point_adapter::y_; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - -template -class TypedHullPocketsTest : public ::testing::Test -{ -protected: - PointT make(const float32_t x, const float32_t y, const float32_t z) - { - PointT ret; - ret.x = x; - ret.y = y; - ret.z = z; - return ret; - } -}; // class TypedHullPocketsTest - -using PointTypes = ::testing::Types; -// cppcheck-suppress syntaxError -TYPED_TEST_SUITE(TypedHullPocketsTest, PointTypes, ); -/// NOTE: This is the older version due to 1.8.0 of GTest. v1.8.1 uses TYPED_TEST_SUITE - -// Inner test function, shared among all the tests -template -typename std::vector::value_type>> -compute_hull_and_pockets(const Iter polygon_start, const Iter polygon_end) -{ - auto convex_hull_list = - std::list::value_type>{polygon_start, polygon_end}; - const auto cvx_hull_end = autoware::common::geometry::convex_hull(convex_hull_list); - - const typename decltype(convex_hull_list)::const_iterator list_beginning = - convex_hull_list.begin(); - const auto pockets = autoware::common::geometry::hull_pockets( - polygon_start, polygon_end, list_beginning, cvx_hull_end); - - return pockets; -} - -// Test for a triangle - any triangle should really not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Triangle) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(1, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +--------------------+ -// | | -// | | -// | | -// | | -// | | -// | | -// +--------------------+ -// This should not result in any pockets. -TYPED_TEST(TypedHullPocketsTest, Box) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 1, 0), this->make(0, 1, 0)}; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - ASSERT_EQ(pockets.size(), 0u); -} - -// Test for the use case: -// +-----+ +-----+ -// / | | | -// / | | | -// + | | + -// | | | | -// | | | | -// | -------------- | -// | | -// | | -// | | -// | | -// +------------------------------+ -// This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, UShape) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), - this->make(4, 2, 0), this->make(2, 2, 0), this->make(2, 5, 0), this->make(0, 4.5, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 4u); - ASSERT_FLOAT_EQ(x_(pockets[0][0]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][0]), 5.0); - ASSERT_FLOAT_EQ(x_(pockets[0][1]), 4.0); - ASSERT_FLOAT_EQ(y_(pockets[0][1]), 2.0); -} - -// Test for the use case: -// +------+ -// | | -// | | -// | | -// +------------------+ +------+ -// | | -// | | -// | | -// +--------------------------------+ -// -// This should come up with two pockets, a triangle on the top left and one on the -// top right. -TYPED_TEST(TypedHullPocketsTest, TypicalGap) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(10, 0, 0), this->make(10, 2, 0), this->make(8, 2, 0), - this->make(8, 4, 0), this->make(6, 4, 0), this->make(6, 2, 0), this->make(0, 2, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 2u); - ASSERT_EQ(pockets[0].size(), 3u); - ASSERT_EQ(pockets[1].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} - -// Test for the use case: -// -// +-----------------+ -// | | -// | | -// + | -// / | -// / | -// +-----------------+ -// -// This should come up with one pocket, in particular a pocket that contains -// the segment of the final to the first point. -TYPED_TEST(TypedHullPocketsTest, EndsInPocket) -{ - const auto polygon = std::vectormake(0, 0, 0))>{ - this->make(0, 0, 0), this->make(2, 0, 0), this->make(2, 2, 0), - this->make(0, 2, 0), this->make(0.1, 1, 0), - }; - - const auto pockets = compute_hull_and_pockets(polygon.begin(), polygon.end()); - - ASSERT_EQ(pockets.size(), 1u); - ASSERT_EQ(pockets[0].size(), 3u); - // TODO(s.me) check for correct pocket positioning -} diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp deleted file mode 100644 index 1036c69573c49..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2021 Apex.AI, 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. - -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#include "autoware_auto_geometry/convex_hull.hpp" -#include "autoware_auto_geometry/intersection.hpp" - -#include - -#include - -struct TestPoint -{ - autoware::common::types::float32_t x; - autoware::common::types::float32_t y; -}; - -struct IntersectionTestParams -{ - std::list polygon1_pts; - std::list polygon2_pts; - std::list expected_intersection_pts; -}; - -void order_ccw(std::list & points) -{ - const auto end_it = autoware::common::geometry::convex_hull(points); - ASSERT_EQ(end_it, points.end()); // Points should have represent a shape -} - -class IntersectionTest : public ::testing::TestWithParam -{ -}; - -TEST_P(IntersectionTest, Basic) -{ - const auto get_ordered_polygon = [](auto polygon) { - order_ccw(polygon); - return polygon; - }; - - const auto polygon1 = get_ordered_polygon(GetParam().polygon1_pts); - const auto polygon2 = get_ordered_polygon(GetParam().polygon2_pts); - const auto expected_intersection = get_ordered_polygon(GetParam().expected_intersection_pts); - - const auto result = autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - - ASSERT_EQ(result.size(), expected_intersection.size()); - auto expected_shape_it = expected_intersection.begin(); - for (auto result_it = result.begin(); result_it != result.end(); ++result_it) { - EXPECT_FLOAT_EQ(result_it->x, expected_shape_it->x); - EXPECT_FLOAT_EQ(result_it->y, expected_shape_it->y); - ++expected_shape_it; - } -} - -INSTANTIATE_TEST_SUITE_P( - Basic, IntersectionTest, - ::testing::Values( - IntersectionTestParams{{}, {}, {}}, - IntersectionTestParams{// Partial intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {15.0F, 5.0F}, {5.0F, 15.0F}, {15.0F, 15.0F}}, - {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}}, - // Full intersection with overlapping edges - // TODO(yunus.caliskan): enable after #1231 - // IntersectionTestParams{ - // {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // {{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}, - // }, - IntersectionTestParams{ - // Fully contained - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}, {8.0F, 8.0F}}, - }, - IntersectionTestParams{ - // Fully contained triangle - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - {{5.0F, 5.0F}, {6.0F, 5.0F}, {5.0F, 7.0F}}, - }, - IntersectionTestParams{// Triangle rectangle intersection. - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {15.0F, 5.0F}}, - {{5.0F, 1.0F}, {5.0F, 9.0F}, {10.0F, 3.0F}, {10.0F, 7.0F}}}, - IntersectionTestParams{// No intersection - {{0.0F, 0.0F}, {10.0F, 0.0F}, {0.0F, 10.0F}, {10.0F, 10.0F}}, - {{15.0F, 15.0F}, {20.0F, 15.0F}, {15.0F, 20.0F}, {20.0F, 20.0F}}, - {}} // cppcheck-suppress syntaxError - )); - -TEST(PolygonPointTest, Basic) -{ - GTEST_SKIP(); // TODO(yunus.caliskan): enable after #1231 - std::list polygon{{5.0F, 5.0F}, {10.0F, 5.0F}, {5.0F, 10.0F}, {10.0F, 10.0F}}; - order_ccw(polygon); - EXPECT_FALSE(autoware::common::geometry::is_point_inside_polygon_2d( - polygon.begin(), polygon.end(), TestPoint{0.0F, 10.0F})); -} - -// IoU of two intersecting shapes: a pentagon and a square. The test includes pen and paper -// computations for the intermediate steps as assertions. -TEST(IoUTest, PentagonRectangleIntersection) -{ - std::list polygon1{ - {0.0F, 3.0F}, {3.0F, 4.0F}, {6.0F, 3.0F}, {4.0F, 1.0F}, {2.0F, 1.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - const auto intersection = - autoware::common::geometry::convex_polygon_intersection2d(polygon1, polygon2); - const auto expected_intersection_area = - autoware::common::geometry::area_2d(intersection.begin(), intersection.end()); - ASSERT_FLOAT_EQ(expected_intersection_area, 1.5F); // Pen & paper proof. - - const auto expected_union_area = - autoware::common::geometry::area_2d(polygon1.begin(), polygon1.end()) + - autoware::common::geometry::area_2d(polygon2.begin(), polygon2.end()) - - expected_intersection_area; - ASSERT_FLOAT_EQ(expected_union_area, (11.0F + 4.0F - 1.5F)); // Pen & paper proof. - - const auto computed_iou = - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2); - - EXPECT_FLOAT_EQ(computed_iou, expected_intersection_area / expected_union_area); -} - -// IoU of two non-intersecting rectangles. -TEST(IoUTest, NoIntersection) -{ - std::list polygon1{{0.0F, 0.0F}, {0.0F, 1.0F}, {1.0F, 1.0F}, {1.0F, 0.0F}}; - std::list polygon2{{3.0F, 0.0F}, {3.0F, 2.0F}, {5.0F, 2.0F}, {5.0F, 0.0F}}; - - order_ccw(polygon1); - order_ccw(polygon2); - - EXPECT_FLOAT_EQ( - autoware::common::geometry::convex_intersection_over_union_2d(polygon1, polygon2), 0.0F); -} diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp deleted file mode 100644 index 266ab123f5203..0000000000000 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ /dev/null @@ -1,259 +0,0 @@ -// Copyright 2020 Mapless AI, Inc. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include "autoware_auto_geometry/interval.hpp" - -#include - -#include -#include -#include - -using autoware::common::geometry::Interval; -using autoware::common::geometry::Interval_d; -using autoware::common::geometry::Interval_f; - -namespace -{ -const auto Inf = std::numeric_limits::infinity(); -const auto Min = std::numeric_limits::lowest(); -const auto Max = std::numeric_limits::max(); -const auto NaN = std::numeric_limits::quiet_NaN(); -const auto epsilon = 1e-5; -} // namespace - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, AbsEq) -{ - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-1.0 + 0.5 * epsilon, 1.0 + 0.5 * epsilon); - const auto shift = (2.0 * epsilon); - const auto i3 = Interval_d(-1.0 + shift, 1.0 + shift); - const auto i_empty = Interval_d(); - - EXPECT_TRUE(Interval_d::abs_eq(i1, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i1, i2, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i2, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i3, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i3, i1, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i1, i_empty, epsilon)); - EXPECT_FALSE(Interval_d::abs_eq(i_empty, i1, epsilon)); - EXPECT_TRUE(Interval_d::abs_eq(i_empty, i_empty, epsilon)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IsSubsetEq) -{ - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(-0.5, 0.5), Interval_d(-1.0, 1.0))); - EXPECT_TRUE(Interval_d::is_subset_eq(Interval_d(3.2, 4.2), Interval_d(3.2, 4.2))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(-3.0, -1.0), Interval_d(1.0, 3.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(1.0, 3.0), Interval_d(2.0, 4.0))); - EXPECT_FALSE(Interval_d::is_subset_eq(Interval_d(), Interval_d())); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ClampTo) -{ - const auto i = Interval_d(-1.0, 1.0); - { - const auto val = 0.0; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, val); - } - - { - const auto val = -3.4; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::min(i)); - } - - { - const auto val = 2.7; - const auto p = Interval_d::clamp_to(i, val); - EXPECT_EQ(p, Interval_d::max(i)); - } - - const auto val = 1.0; - const auto empty_interval = Interval_d(); - const auto projected = Interval_d::clamp_to(empty_interval, val); - EXPECT_TRUE(std::isnan(projected)); -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Comparisons) -{ - { - const auto i1 = Interval_d(0.25, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(-0.25, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 0.5); - const auto i2 = Interval_d(0, 1); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(0, 1); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } - - { - const auto i1 = Interval_d(0, 1); - const auto i2 = Interval_d(); - EXPECT_FALSE((i1 == i2)); - EXPECT_TRUE((i1 != i2)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - EXPECT_TRUE((i1 == i2)); - EXPECT_FALSE((i1 != i2)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Contains) -{ - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::contains(i, 0.0)); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_TRUE(Interval_d::contains(i, 0.0)); - EXPECT_FALSE(Interval_d::contains(i, 2.0)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, Empty) -{ - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } - - { - const auto i1 = Interval_d(); - const auto i2 = Interval_d(0.0, 1.0); - const auto i3 = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i3)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ZeroMeasure) -{ - { - const auto i = Interval_d(0, 1); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(); - EXPECT_FALSE(Interval_d::zero_measure(i)); - } - - { - const auto i = Interval_d(2, 2); - EXPECT_TRUE(Interval_d::zero_measure(i)); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, IntersectionMeasure) -{ - { - const auto i1 = Interval_d(-1.0, 1.0); - const auto i2 = Interval_d(-0.5, 1.5); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_EQ(Interval_d::min(i), -0.5); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 1.5, epsilon); - } - - { - const auto i1 = Interval_d(-2.0, -1.0); - const auto i2 = Interval_d(1.0, 2.0); - const auto i = Interval_d::intersect(i1, i2); - EXPECT_TRUE(Interval_d::empty(i)); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } -} - -//------------------------------------------------------------------------------ - -TEST(GeometryInterval, ConstructionMeasure) -{ - { - const auto i = Interval_d(); - EXPECT_TRUE(std::isnan(Interval_d::min(i))); - EXPECT_TRUE(std::isnan(Interval_d::max(i))); - EXPECT_TRUE(std::isnan(Interval_d::measure(i))); - } - - { - const auto i = Interval_d(-1.0, 1.0); - EXPECT_EQ(Interval_d::min(i), -1.0); - EXPECT_EQ(Interval_d::max(i), 1.0); - EXPECT_NEAR(Interval_d::measure(i), 2.0, epsilon); - } - - { - const auto i = Interval_d(0.0, 0.0); - EXPECT_EQ(Interval_d::min(i), 0.0); - EXPECT_EQ(Interval_d::max(i), 0.0); - EXPECT_FALSE(Interval_d::empty(i)); - EXPECT_EQ(Interval_d::measure(i), 0.0); - } - - { - EXPECT_THROW({ Interval_d(1.0, -1.0); }, std::runtime_error); - } -} - -//------------------------------------------------------------------------------ diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 49a67d68057d9..daf14ba4147c2 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -12,7 +12,6 @@ autoware_cmake - autoware_auto_geometry autoware_auto_mapping_msgs geometry_msgs image_geometry From 9a5e6c6efc0ec3159c5b3505bc6dea6c02b8cc9b Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 3 Jun 2024 12:22:16 +0900 Subject: [PATCH 087/120] refactor(multi_object_tracker): use constexpr properly (#7206) reafctor multi_object_tracker Signed-off-by: Ryuta Kambe --- .../src/tracker/model/bicycle_tracker.cpp | 6 +++--- .../src/tracker/model/big_vehicle_tracker.cpp | 6 +++--- .../src/tracker/model/normal_vehicle_tracker.cpp | 6 +++--- .../src/tracker/model/pedestrian_tracker.cpp | 6 +++--- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 4e5f14fde6834..fdc93419b326a 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -127,9 +127,9 @@ BicycleTracker::BicycleTracker( constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); - constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); - constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; + constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; + constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 15c6b2e722ed5..5e3ba8d614e7e 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -147,9 +147,9 @@ BigVehicleTracker::BigVehicleTracker( constexpr double p0_stddev_y = 0.5; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); - constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); - constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; + constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; + constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index bd04cc613ef12..9907da2105825 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -148,9 +148,9 @@ NormalVehicleTracker::NormalVehicleTracker( constexpr double p0_stddev_y = 0.3; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); - constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); - constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; + constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; + constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index fd9ef82dfdca3..6891bf6f63500 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -122,9 +122,9 @@ PedestrianTracker::PedestrianTracker( constexpr double p0_stddev_y = 2.0; // in object coordinate [m] constexpr double p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] - constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); - constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); - constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; + constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; + constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; const double cos_yaw = std::cos(yaw); const double sin_yaw = std::sin(yaw); From dfd3c13c0abd4a54abddc0c1a85c811d5aa22ff6 Mon Sep 17 00:00:00 2001 From: MohammadReza Mirdamadiyan Date: Mon, 3 Jun 2024 06:52:50 +0330 Subject: [PATCH 088/120] fix(radar_tracks_noise_filter): update param_path (#7199) Signed-off-by: Mohammadreza Mirdamadiyan --- .../launch/radar_tracks_noise_filter.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml index 028ca5de6a220..f980bcebf1b6b 100644 --- a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml +++ b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml @@ -2,7 +2,7 @@ - + From 228be2f1ab94125056530ebc97a8654a253fd0ed Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 13:27:54 +0900 Subject: [PATCH 089/120] feat(autoware_auto_perception_rviz_plugin)!: rename package to autoware_perception_rviz_plugin (#7221) feat(autoware_auto_perception_rviz_plugin): rename package to autoware_perception_rviz_plugin Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Yukihiro Saito --- .github/CODEOWNERS | 2 +- common/.pages | 2 +- .../visibility_control.hpp | 43 ---------- .../CMakeLists.txt | 16 ++-- .../README.md | 0 .../icons/classes/DetectedObjects.png | Bin .../icons/classes/PredictedObjects.png | Bin .../icons/classes/TrackedObjects.png | Bin ...ected-object-visualization-description.jpg | Bin ...icted-object-visualization-description.jpg | Bin ...acked-object-visualization-description.jpg | Bin .../common/color_alpha_property.hpp | 10 +-- .../detected_objects_display.hpp | 10 +-- .../object_polygon_detail.hpp | 76 +++++++++--------- .../object_polygon_display_base.hpp | 16 ++-- .../predicted_objects_display.hpp | 10 +-- .../tracked_objects_display.hpp | 10 +-- .../visibility_control.hpp | 43 ++++++++++ .../package.xml | 2 +- .../plugins_description.xml | 8 +- .../src/common/color_alpha_property.cpp | 2 +- .../detected_objects_display.cpp | 2 +- .../object_polygon_detail.cpp | 2 +- .../predicted_objects_display.cpp | 2 +- .../tracked_objects_display.cpp | 2 +- .../rviz/obstacle_avoidance_planner.rviz | 6 +- 26 files changed, 132 insertions(+), 132 deletions(-) delete mode 100644 common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/CMakeLists.txt (68%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/README.md (100%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/icons/classes/DetectedObjects.png (100%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/icons/classes/PredictedObjects.png (100%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/icons/classes/TrackedObjects.png (100%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/images/detected-object-visualization-description.jpg (100%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/images/predicted-object-visualization-description.jpg (100%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/images/tracked-object-visualization-description.jpg (100%) rename common/{autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin}/common/color_alpha_property.hpp (81%) rename common/{autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin}/object_detection/detected_objects_display.hpp (72%) rename common/{autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin}/object_detection/object_polygon_detail.hpp (77%) rename common/{autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin}/object_detection/object_polygon_display_base.hpp (97%) rename common/{autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin}/object_detection/predicted_objects_display.hpp (90%) rename common/{autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin}/object_detection/tracked_objects_display.hpp (88%) create mode 100644 common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/package.xml (96%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/plugins_description.xml (51%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/src/common/color_alpha_property.cpp (95%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/src/object_detection/detected_objects_display.cpp (98%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/src/object_detection/object_polygon_detail.cpp (99%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/src/object_detection/predicted_objects_display.cpp (99%) rename common/{autoware_auto_perception_rviz_plugin => autoware_perception_rviz_plugin}/src/object_detection/tracked_objects_display.cpp (98%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index e60173068f028..5c4ad77e756fa 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp +common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai diff --git a/common/.pages b/common/.pages index 5ce88e0561c34..105466bcba763 100644 --- a/common/.pages +++ b/common/.pages @@ -31,7 +31,7 @@ nav: - 'Introduction': common/tvm_utility - 'YOLOv2 Tiny Example': common/tvm_utility/tvm-utility-yolo-v2-tiny-tests - 'RVIZ2 Plugins': - - 'autoware_auto_perception_rviz_plugin': common/autoware_auto_perception_rviz_plugin + - 'autoware_perception_rviz_plugin': common/autoware_perception_rviz_plugin - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin - 'autoware_mission_details_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin - 'bag_time_manager_rviz_plugin': common/bag_time_manager_rviz_plugin diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp deleted file mode 100644 index 47cb8383fdada..0000000000000 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2019 the 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ - defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#else -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || -// defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_EXPORTS) -#endif -#elif defined(__linux__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) -#else // defined(_LINUX) -#error "Unsupported Build Configuration" -#endif // defined(_WINDOWS) - -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_perception_rviz_plugin/CMakeLists.txt similarity index 68% rename from common/autoware_auto_perception_rviz_plugin/CMakeLists.txt rename to common/autoware_perception_rviz_plugin/CMakeLists.txt index 8d0469e78c3ac..74671d74f7397 100644 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_perception_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_auto_perception_rviz_plugin) +project(autoware_perception_rviz_plugin) find_package(autoware_cmake REQUIRED) autoware_package() @@ -13,18 +13,18 @@ set(OD_PLUGIN_LIB_SRC ) set(OD_PLUGIN_LIB_HEADERS - include/autoware_auto_perception_rviz_plugin/visibility_control.hpp + include/autoware_perception_rviz_plugin/visibility_control.hpp ) set(OD_PLUGIN_LIB_HEADERS_TO_WRAP - include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp + include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp ) set(COMMON_HEADERS - include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp - include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp + include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp + include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp + include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp ) set(COMMON_SRC diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/README.md rename to common/autoware_perception_rviz_plugin/README.md diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/DetectedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/DetectedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/DetectedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/PredictedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/PredictedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/PredictedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png b/common/autoware_perception_rviz_plugin/icons/classes/TrackedObjects.png similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/icons/classes/TrackedObjects.png rename to common/autoware_perception_rviz_plugin/icons/classes/TrackedObjects.png diff --git a/common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/detected-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/detected-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/detected-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/predicted-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/predicted-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/predicted-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg b/common/autoware_perception_rviz_plugin/images/tracked-object-visualization-description.jpg similarity index 100% rename from common/autoware_auto_perception_rviz_plugin/images/tracked-object-visualization-description.jpg rename to common/autoware_perception_rviz_plugin/images/tracked-object-visualization-description.jpg diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp similarity index 81% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp index 10dc46e55ec70..e39faeb254add 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/common/color_alpha_property.hpp @@ -11,10 +11,10 @@ // 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 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -31,7 +31,7 @@ namespace rviz_plugins namespace common { /// \brief Class to define Color and Alpha values as plugin properties -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty { public: /// \brief Constructor @@ -56,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp similarity index 72% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp index 97479fb68ca9b..b5e0f5c548c31 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -11,10 +11,10 @@ // 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 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -25,7 +25,7 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize DetectedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay : public ObjectPolygonDisplayBase { Q_OBJECT @@ -43,4 +43,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp similarity index 77% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index 4f545d194b2c2..e1a911b55f719 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. /// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class -#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -85,7 +85,7 @@ const std::map< /// \param color_rgba Color and alpha values to use for the marker /// \param line_width Line thickness around the object /// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -93,7 +93,7 @@ get_shape_marker_ptr( const bool & is_orientation_available = true, const ObjectFillType fill_type = ObjectFillType::Skeleton); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -103,137 +103,137 @@ get_2d_shape_marker_ptr( /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \return Marker ptr. Id and header will have to be set by the caller -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( const std::string & uuid, const geometry_msgs::msg::Point & centroid, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & confidence_interval_coefficient); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & confidence_interval_coefficient, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & confidence_interval_coefficient); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & confidence_interval_coefficient, const double & line_width); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( const double start_angle, const double end_angle, const double radius, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( const double point_list[][3], const int point_pairs[][2], const int & num_pairs, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point /// \param val Point32 to be converted /// \return Point type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_point( const geometry_msgs::msg::Point32 & val) { geometry_msgs::msg::Point ret; @@ -247,7 +247,7 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Point to_ /// \param point /// \param orientation /// \return Pose type -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_pose( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Quaternion & orientation) { geometry_msgs::msg::Pose ret; @@ -256,7 +256,7 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose to_p return ret; } -inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() +inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose() { geometry_msgs::msg::Pose pose; pose.position.x = 0.0; @@ -275,7 +275,7 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose init /// \param logger_name Name to use for logger in case of a warning (if labels is empty) /// \return Id of the best classification, Unknown if there is no best label template -AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC autoware_auto_perception_msgs::msg::ObjectClassification::_label_type get_best_label(ClassificationContainerT labels, const std::string & logger_name) { @@ -297,4 +297,4 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp similarity index 97% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 1093f6e4f2414..2d86d8dd09db8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -11,12 +11,12 @@ // 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 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" -#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" +#include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_perception_rviz_plugin/visibility_control.hpp" #include #include @@ -46,7 +46,7 @@ namespace object_detection /// classes. /// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type template -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase : public rviz_common::RosTopicDisplay { public: @@ -431,7 +431,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return ss.str(); } - std_msgs::msg::ColorRGBA AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC + std_msgs::msg::ColorRGBA AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC get_color_from_uuid(const std::string & uuid) const { int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % @@ -576,4 +576,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp similarity index 90% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 775c18db6ba5c..0501d2ab3456d 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -11,10 +11,10 @@ // 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 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -38,7 +38,7 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay : public ObjectPolygonDisplayBase { Q_OBJECT @@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp similarity index 88% rename from common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp rename to common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 4e86a5ee93fd8..180494d742144 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -11,10 +11,10 @@ // 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 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -33,7 +33,7 @@ namespace rviz_plugins namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects -class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay +class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay : public ObjectPolygonDisplayBase { Q_OBJECT @@ -114,4 +114,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp new file mode 100644 index 0000000000000..e5e7693054ec8 --- /dev/null +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/visibility_control.hpp @@ -0,0 +1,43 @@ +// Copyright 2019 the 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. +// +// Co-developed by Tier IV, Inc. and Apex.AI, Inc. + +#ifndef AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ + defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllexport) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#else +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __declspec(dllimport) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || +// defined(AUTOWARE_PERCEPTION_RVIZ_PLUGIN_EXPORTS) +#endif +#elif defined(__linux__) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC __attribute__((visibility("default"))) +#define AUTOWARE_PERCEPTION_RVIZ_PLUGIN_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml similarity index 96% rename from common/autoware_auto_perception_rviz_plugin/package.xml rename to common/autoware_perception_rviz_plugin/package.xml index 2033239824d95..66b7fafc39de5 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -1,7 +1,7 @@ - autoware_auto_perception_rviz_plugin + autoware_perception_rviz_plugin 1.0.0 Contains plugins to visualize object detection outputs Apex.AI, Inc. diff --git a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml b/common/autoware_perception_rviz_plugin/plugins_description.xml similarity index 51% rename from common/autoware_auto_perception_rviz_plugin/plugins_description.xml rename to common/autoware_perception_rviz_plugin/plugins_description.xml index 3f56a43558494..50a0b5e5aff56 100644 --- a/common/autoware_auto_perception_rviz_plugin/plugins_description.xml +++ b/common/autoware_perception_rviz_plugin/plugins_description.xml @@ -1,22 +1,22 @@ - + - + Convert a PredictedObjects to markers and display them. autoware_auto_perception_msgs/msg/PredictedObjects - + Convert a TrackedObjects to markers and display them. autoware_auto_perception_msgs/msg/TrackedObjects - + Convert a DetectedObjects to markers and display them. diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp similarity index 95% rename from common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp rename to common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp index b3e542a02243b..9b8ac2e38b740 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp similarity index 98% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 53e935fa1850a..335a323d4ecd5 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp similarity index 99% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 7631acffafdf9..009091f33c272 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License.. -#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" #include #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp similarity index 99% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index b42ffe1804246..2a81732d93b95 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" #include #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp similarity index 98% rename from common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp rename to common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 504b51f850444..e8891e2bae897 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" +#include "autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" #include diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz index c3cd89bbce9b9..78df4fe53c4d8 100644 --- a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz +++ b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz @@ -761,7 +761,7 @@ Visualization Manager: CYCLIST: Alpha: 0.9990000128746033 Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Class: autoware_perception_rviz_plugin/DetectedObjects Display Acceleration: true Display Label: true Display PoseWithCovariance: true @@ -812,7 +812,7 @@ Visualization Manager: CYCLIST: Alpha: 0.9990000128746033 Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Class: autoware_perception_rviz_plugin/TrackedObjects Display Acceleration: true Display Label: true Display PoseWithCovariance: true @@ -863,7 +863,7 @@ Visualization Manager: CYCLIST: Alpha: 0.9990000128746033 Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Class: autoware_perception_rviz_plugin/PredictedObjects Display Acceleration: true Display Label: true Display PoseWithCovariance: false From 74ac782beb2db839359855815f1c51d92f15712a Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 17:07:18 +0900 Subject: [PATCH 090/120] feat: remove autoware_auto_msgs_adapter and make it a separate repository (#7220) feat: extract autoware_auto_msgs_adapter as independent repository Signed-off-by: Ryohsuke Mitsudome --- .github/CODEOWNERS | 1 - .../autoware_auto_msgs_adapter/CMakeLists.txt | 34 --- system/autoware_auto_msgs_adapter/README.md | 102 ------- ...oware_auto_msgs_adapter_control.param.yaml | 5 - .../autoware_auto_msgs_adapter_map.param.yaml | 5 - ...re_auto_msgs_adapter_perception.param.yaml | 5 - ...ware_auto_msgs_adapter_planning.param.yaml | 5 - .../autoware_auto_msgs_adapter.launch.xml | 20 -- system/autoware_auto_msgs_adapter/package.xml | 36 --- .../autoware_auto_msgs_adapter.schema.json | 46 --- .../src/autoware_auto_msgs_adapter_core.cpp | 99 ------- .../src/include/adapter_base.hpp | 57 ---- .../src/include/adapter_base_interface.hpp | 34 --- .../src/include/adapter_control.hpp | 69 ----- .../src/include/adapter_map.hpp | 59 ---- .../src/include/adapter_perception.hpp | 97 ------- .../src/include/adapter_planning.hpp | 70 ----- .../autoware_auto_msgs_adapter_core.hpp | 49 ---- .../test/test_autoware_auto_msgs_adapter.cpp | 26 -- .../test_msg_ackermann_control_command.cpp | 116 -------- .../test/test_msg_had_map_bin.cpp | 104 ------- .../test/test_msg_planning_trajectory.cpp | 149 ---------- .../test/test_msg_predicted_objects.cpp | 270 ------------------ 23 files changed, 1458 deletions(-) delete mode 100644 system/autoware_auto_msgs_adapter/CMakeLists.txt delete mode 100644 system/autoware_auto_msgs_adapter/README.md delete mode 100644 system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml delete mode 100644 system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml delete mode 100644 system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml delete mode 100644 system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml delete mode 100755 system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml delete mode 100644 system/autoware_auto_msgs_adapter/package.xml delete mode 100644 system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json delete mode 100644 system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp delete mode 100644 system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp delete mode 100644 system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp delete mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp delete mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp delete mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp delete mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5c4ad77e756fa..1ef667376b616 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -218,7 +218,6 @@ simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp -system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt deleted file mode 100644 index a8b72b8da688c..0000000000000 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_msgs_adapter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(NODE_NAME ${PROJECT_NAME}_node) -set(EXEC_NAME ${PROJECT_NAME}_exe) -set(TEST_NAME test_${PROJECT_NAME}) - -ament_auto_add_library(${NODE_NAME} - src/include/adapter_base.hpp - src/include/adapter_base_interface.hpp - src/include/adapter_control.hpp - src/include/adapter_map.hpp - src/include/adapter_perception.hpp - src/include/adapter_planning.hpp - src/include/autoware_auto_msgs_adapter_core.hpp - src/autoware_auto_msgs_adapter_core.cpp) - -rclcpp_components_register_node(${NODE_NAME} - PLUGIN "autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode" - EXECUTABLE ${EXEC_NAME}) - -if(BUILD_TESTING) - file(GLOB_RECURSE TEST_SOURCES test/*.cpp) - ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_SOURCES}) - target_include_directories(${TEST_NAME} PRIVATE src/include) - target_link_libraries(${TEST_NAME} ${NODE_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE - launch - config) diff --git a/system/autoware_auto_msgs_adapter/README.md b/system/autoware_auto_msgs_adapter/README.md deleted file mode 100644 index 67fabaaa46c9a..0000000000000 --- a/system/autoware_auto_msgs_adapter/README.md +++ /dev/null @@ -1,102 +0,0 @@ -# autoware_auto_msgs_adapter - -This package is used to convert `autoware_msgs` to `autoware_auto_msgs`. - -## Purpose - -As we transition from `autoware_auto_msgs` to `autoware_msgs`, we wanted to provide flexibility and compatibility for -users who are still using `autoware_auto_msgs`. - -This adapter package allows users to easily convert messages between the two formats. - -## Capabilities - -The `autoware_auto_msgs_adapter` package provides the following capabilities: - -- Conversion of supported `autoware_msgs` messages to `autoware_auto_msgs` messages. -- Can be extended to support conversion for any message type pairs. -- Each instance is designed to convert from a single source message type to a single target message type. -- Multiple instances can be launched to convert multiple message types. -- Can be launched as a standalone node or as a component. - -## Usage - -Customize the adapter configuration by replicating and editing the `autoware_auto_msgs_adapter_control.param.yaml` file located -in the `autoware_auto_msgs_adapter/config` directory. Example configuration: - -```yaml -/**: - ros__parameters: - msg_type_target: "autoware_auto_control_msgs/msg/AckermannControlCommand" - topic_name_source: "/control/command/control_cmd" - topic_name_target: "/control/command/control_cmd_auto" -``` - -Set the `msg_type_target` parameter to the desired target message type from `autoware_auto_msgs`. - -Make sure that the `msg_type_target` has the correspondence in either: - -- [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) -- OR [src/autoware_auto_msgs_adapter_core.cpp](src/autoware_auto_msgs_adapter_core.cpp) `AutowareAutoMsgsAdapterNode::create_adapter_map()` method. - -(If this package is maintained correctly, they should match each other.) - -Launch the adapter node by any of the following methods: - -### `ros2 launch` - -```bash -ros2 launch autoware_auto_msgs_adapter autoware_auto_msgs_adapter.launch.xml param_path:='full_path_to_param_file' -``` - -Make sure to set the `param_path` argument to the full path of the parameter file. - -Alternatively, - -- You can replicate and edit the launch file to suit to your needs. -- You can make use of the existing launch file in another launch file by providing the parameter file path as an - argument. - -### `ros2 run` - -```bash -ros2 run autoware_auto_msgs_adapter autoware_auto_msgs_adapter_exe --ros-args --params-file 'full_path_to_param_file' -``` - -Make sure to set the `param_path` argument to the full path of the parameter file. - -## Contributing - -### Current implementation details - -The entry point for the adapter executable is created with `RCLCPP_COMPONENTS_REGISTER_NODE` the [autoware_auto_msgs_adapter_core.cpp](src/Fautoware_auto_msgs_adapter_core.cpp). - -This allows it to be launched as a component or as a standalone node. - -In the `AutowareAutoMsgsAdapterNode` constructor, the adapter is selected by the type string provided in the -configuration file. The adapter is then initialized with the topic names provided. - -The constructors of the adapters are responsible for creating the publisher and subscriber (which makes use of the conversion method). - -### Adding a new message pair - -To add a new message pair, - -- Replicate and edit: - - [adapter_control.hpp](include/autoware_auto_msgs_adapter/adapter_control.hpp). - - Add the new header file to the [CMakeLists.txt](CMakeLists.txt). -- Add a new entry to the returned map instance in the `AutowareAutoMsgsAdapterNode::create_adapter_map()` method of the adapter node: - - [autoware_auto_msgs_adapter_core.cpp](src/autoware_auto_msgs_adapter_core.cpp) -- Add a new entry to the [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) file in the `definitions:autoware_auto_msgs_adapter:properties:msg_type_target:enum` section. - - Learn more about JSON schema usage in [here](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/parameters/#json-schema). -- Create a new config file by replicating and editing: - - [autoware_auto_msgs_adapter_control.param.yaml](config/autoware_auto_msgs_adapter_control.param.yaml) -- Add a new test file by replicating and editing: - - [test_msg_ackermann_control_command.cpp](test/test_msg_ackermann_control_command.cpp) - - No need to edit the `CMakeLists.txt` file as it will automatically detect the new test file. - -Also make sure to test the new adapter with: - -```bash -colcon test --event-handlers console_cohesion+ --packages-select autoware_auto_msgs_adapter -``` diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml deleted file mode 100644 index 4c6d5f101f380..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_control_msgs/msg/AckermannControlCommand" - topic_name_source: "/control/command/control_cmd" - topic_name_target: "/control/command/control_cmd_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml deleted file mode 100644 index dcaa49e089b44..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" - topic_name_source: "/map/vector_map" - topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml deleted file mode 100644 index d8b6e31543fad..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_perception_msgs/msg/PredictedObjects" - topic_name_source: "/perception/object_recognition/objects" - topic_name_target: "/perception/object_recognition/objects_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml deleted file mode 100644 index b4a8712d4c505..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_planning_msgs/msg/Trajectory" - topic_name_source: "/planning/scenario_planning/trajectory" - topic_name_target: "/planning/scenario_planning/trajectory_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml deleted file mode 100755 index 58f2fb4e06238..0000000000000 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml deleted file mode 100644 index 2941820550ba0..0000000000000 --- a/system/autoware_auto_msgs_adapter/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - autoware_auto_msgs_adapter - 1.0.0 - Converts an autoware_msgs message to autoware_auto_msgs version and publishes it. - - M. Fatih Cırıt - Takagi, Isamu - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - rosidl_default_generators - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_control_msgs - autoware_map_msgs - autoware_perception_msgs - autoware_planning_msgs - rclcpp - rclcpp_components - - - ament_cmake - - diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json deleted file mode 100644 index a5d6c93029d09..0000000000000 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for autoware_auto_msg_adapter", - "type": "object", - "definitions": { - "autoware_auto_msgs_adapter": { - "type": "object", - "properties": { - "msg_type_target": { - "type": "string", - "description": "Target message type", - "enum": [ - "autoware_auto_control_msgs/msg/AckermannControlCommand", - "autoware_auto_mapping_msgs/msg/HADMapBin", - "autoware_auto_perception_msgs/msg/PredictedObjects", - "autoware_auto_planning_msgs/msg/Trajectory" - ], - "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" - }, - "topic_name_source": { - "type": "string", - "description": "Topic name of the message to be converted.", - "default": "/control/command/control_cmd" - }, - "topic_name_target": { - "type": "string", - "description": "Target topic name which the message will be converted into.", - "default": "/control/command/control_cmd_auto" - } - }, - "required": ["msg_type_target", "topic_name_source", "topic_name_target"] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/autoware_auto_msgs_adapter" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp deleted file mode 100644 index 6a23970246866..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2023 The 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 "include/autoware_auto_msgs_adapter_core.hpp" - -#include "include/adapter_control.hpp" - -#include - -#include - -namespace autoware_auto_msgs_adapter -{ - -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_control_msgs::msg::Control; - -using MapStringAdapter = AutowareAutoMsgsAdapterNode::MapStringAdapter; - -AutowareAutoMsgsAdapterNode::AutowareAutoMsgsAdapterNode(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("autoware_auto_msgs_adapter", node_options) -{ - const std::string msg_type_target = declare_parameter("msg_type_target"); - const std::string topic_name_source = declare_parameter("topic_name_source"); - const std::string topic_name_target = declare_parameter("topic_name_target"); - - // Map of available adapters - auto map_adapter = create_adapter_map(topic_name_source, topic_name_target); - - print_adapter_options(map_adapter); - - // Initialize the adapter with the selected option - if (!initialize_adapter(map_adapter, msg_type_target)) { - RCLCPP_ERROR( - get_logger(), "Unknown msg type: %s. Please refer to previous log for available options.", - msg_type_target.c_str()); - } -} - -MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( - const std::string & topic_name_source, const std::string & topic_name_target) -{ - return { - {"autoware_auto_control_msgs/msg/AckermannControlCommand", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_mapping_msgs/msg/HADMapBin", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_perception_msgs/msg/PredictedObjects", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_planning_msgs/msg/Trajectory", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - }; -} - -void AutowareAutoMsgsAdapterNode::print_adapter_options(const MapStringAdapter & map_adapter) -{ - std::string std_options_available; - for (const auto & entry : map_adapter) { - std_options_available += entry.first + "\n"; - } - RCLCPP_INFO( - get_logger(), "Available msg_type_target options:\n%s", std_options_available.c_str()); -} - -bool AutowareAutoMsgsAdapterNode::initialize_adapter( - const MapStringAdapter & map_adapter, const std::string & msg_type_target) -{ - auto it = map_adapter.find(msg_type_target); - adapter_ = (it != map_adapter.end()) ? it->second() : nullptr; - return adapter_ != nullptr; -} - -} // namespace autoware_auto_msgs_adapter - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode) diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp deleted file mode 100644 index f506d66b1d8b0..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023 The 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. -#ifndef ADAPTER_BASE_HPP_ -#define ADAPTER_BASE_HPP_ - -#include "adapter_base_interface.hpp" - -#include - -#include -#include - -namespace autoware_auto_msgs_adapter -{ - -template -class AdapterBase : public AdapterBaseInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(AdapterBase) - - AdapterBase( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - { - pub_target_ = node.create_publisher(topic_name_target, qos); - sub_source_ = node.create_subscription( - topic_name_source, qos, std::bind(&AdapterBase::callback, this, std::placeholders::_1)); - } - -protected: - virtual TargetT convert(const SourceT & msg_source) = 0; - -private: - typename rclcpp::Publisher::SharedPtr pub_target_; - typename rclcpp::Subscription::SharedPtr sub_source_; - - void callback(const typename SourceT::SharedPtr msg_source) - { - pub_target_->publish(convert(*msg_source)); - } -}; - -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_BASE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp deleted file mode 100644 index 606993332fc4a..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2023 The 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. -#ifndef ADAPTER_BASE_INTERFACE_HPP_ -#define ADAPTER_BASE_INTERFACE_HPP_ - -#include - -#include - -namespace autoware_auto_msgs_adapter -{ - -class AdapterBaseInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(AdapterBaseInterface) - - virtual ~AdapterBaseInterface() = default; -}; - -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_BASE_INTERFACE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp deleted file mode 100644 index c02f4e03877a2..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2023 The 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. -#ifndef ADAPTER_CONTROL_HPP_ -#define ADAPTER_CONTROL_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_control_msgs::msg::Control; - -class AdapterControl -: public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterControl( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterControl is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - AckermannControlCommand convert(const Control & msg_source) override - { - autoware_auto_control_msgs::msg::AckermannControlCommand msg_auto; - msg_auto.stamp = msg_source.stamp; - - const auto & lateral = msg_source.lateral; - auto & lateral_auto = msg_auto.lateral; - lateral_auto.stamp = lateral.stamp; - lateral_auto.steering_tire_angle = lateral.steering_tire_angle; - lateral_auto.steering_tire_rotation_rate = lateral.steering_tire_rotation_rate; - - const auto & longitudinal = msg_source.longitudinal; - auto & longitudinal_auto = msg_auto.longitudinal; - longitudinal_auto.stamp = longitudinal.stamp; - longitudinal_auto.acceleration = longitudinal.acceleration; - longitudinal_auto.jerk = longitudinal.jerk; - longitudinal_auto.speed = longitudinal.velocity; - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_CONTROL_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp deleted file mode 100644 index 8150b7d7dba08..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2023 The 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. -#ifndef ADAPTER_MAP_HPP_ -#define ADAPTER_MAP_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_map_msgs::msg::LaneletMapBin; - -class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterMap( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - HADMapBin convert(const LaneletMapBin & msg_source) override - { - autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; - msg_auto.header = msg_source.header; - msg_auto.format_version = msg_source.version_map_format; - msg_auto.map_version = msg_source.version_map; - msg_auto.map_format = 0; - msg_auto.data = msg_source.data; - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp deleted file mode 100644 index 3821bbad8ce38..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2023 The 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. -#ifndef ADAPTER_PERCEPTION_HPP_ -#define ADAPTER_PERCEPTION_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using Objects_auto = autoware_auto_perception_msgs::msg::PredictedObjects; -using Objects = autoware_perception_msgs::msg::PredictedObjects; - -class AdapterPerception : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterPerception( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterPerception is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - Objects_auto convert(const Objects & msg_source) override - { - Objects_auto msg_auto; - msg_auto.header = msg_source.header; - - autoware_auto_perception_msgs::msg::PredictedObject object_auto; - for (size_t it_of_objects = 0; it_of_objects < msg_source.objects.size(); it_of_objects++) { - // convert id and probability - object_auto.object_id = msg_source.objects[it_of_objects].object_id; - object_auto.existence_probability = msg_source.objects[it_of_objects].existence_probability; - // convert classification - autoware_auto_perception_msgs::msg::ObjectClassification classification; - for (size_t i = 0; i < msg_source.objects[it_of_objects].classification.size(); i++) { - classification.label = msg_source.objects[it_of_objects].classification[i].label; - classification.probability = - msg_source.objects[it_of_objects].classification[i].probability; - object_auto.classification.push_back(classification); - } - // convert kinematics - object_auto.kinematics.initial_pose_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_pose_with_covariance; - object_auto.kinematics.initial_twist_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_twist_with_covariance; - object_auto.kinematics.initial_acceleration_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_acceleration_with_covariance; - for (size_t j = 0; j < msg_source.objects[it_of_objects].kinematics.predicted_paths.size(); - j++) { - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; - for (size_t k = 0; - k < msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path.size(); k++) { - predicted_path.path.push_back( - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path[k]); - } - predicted_path.time_step = - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].time_step; - predicted_path.confidence = - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].confidence; - object_auto.kinematics.predicted_paths.push_back(predicted_path); - } - // convert shape - object_auto.shape.type = msg_source.objects[it_of_objects].shape.type; - object_auto.shape.footprint = msg_source.objects[it_of_objects].shape.footprint; - object_auto.shape.dimensions = msg_source.objects[it_of_objects].shape.dimensions; - - // add to objects list - msg_auto.objects.push_back(object_auto); - } - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_PERCEPTION_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp deleted file mode 100644 index 79d73bdda0575..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 The 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. -#ifndef ADAPTER_PLANNING_HPP_ -#define ADAPTER_PLANNING_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using TrajectoryAuto = autoware_auto_planning_msgs::msg::Trajectory; -using PointAuto = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Trajectory = autoware_planning_msgs::msg::Trajectory; - -class AdapterPlanning : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterPlanning( - rclcpp::Node & node, const std::string & topic_name_source, - const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) - : AdapterBase(node, topic_name_source, topic_name_target, qos) - { - RCLCPP_DEBUG( - node.get_logger(), "AdapterPlanning is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - TrajectoryAuto convert(const Trajectory & msg_source) override - { - TrajectoryAuto msg_auto; - msg_auto.header = msg_source.header; - PointAuto trajectory_point_auto; - msg_auto.points.reserve(msg_source.points.size()); - for (size_t i = 0; i < msg_source.points.size(); i++) { - trajectory_point_auto.time_from_start = msg_source.points.at(i).time_from_start; - trajectory_point_auto.pose = msg_source.points.at(i).pose; - trajectory_point_auto.longitudinal_velocity_mps = - msg_source.points.at(i).longitudinal_velocity_mps; - trajectory_point_auto.lateral_velocity_mps = msg_source.points.at(i).lateral_velocity_mps; - trajectory_point_auto.acceleration_mps2 = msg_source.points.at(i).acceleration_mps2; - trajectory_point_auto.heading_rate_rps = msg_source.points.at(i).heading_rate_rps; - trajectory_point_auto.front_wheel_angle_rad = msg_source.points.at(i).front_wheel_angle_rad; - trajectory_point_auto.rear_wheel_angle_rad = msg_source.points.at(i).rear_wheel_angle_rad; - msg_auto.points.push_back(trajectory_point_auto); - } - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_PLANNING_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp deleted file mode 100644 index 258bf01244499..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 The 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. -#ifndef AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ -#define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ - -#include "adapter_control.hpp" -#include "adapter_map.hpp" -#include "adapter_perception.hpp" -#include "adapter_planning.hpp" - -#include - -#include -#include - -namespace autoware_auto_msgs_adapter -{ - -class AutowareAutoMsgsAdapterNode : public rclcpp::Node -{ -public: - explicit AutowareAutoMsgsAdapterNode(const rclcpp::NodeOptions & node_options); - using MapStringAdapter = std::map>; - -private: - AdapterBaseInterface::SharedPtr adapter_; - - MapStringAdapter create_adapter_map( - const std::string & topic_name_source, const std::string & topic_name_target); - - void print_adapter_options(const MapStringAdapter & map_adapter); - - bool initialize_adapter( - const MapStringAdapter & map_adapter, const std::string & msg_type_target); -}; -} // namespace autoware_auto_msgs_adapter - -#endif // AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp b/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp deleted file mode 100644 index 6b19ae6e30555..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 The 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 - -#include - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - bool result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp deleted file mode 100644 index 430b9a26b60e1..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2023 The 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 - -#include - -#include - -autoware_control_msgs::msg::Control generate_control_msg() -{ - // generate deterministic random float - std::mt19937 gen(0); - std::uniform_real_distribution<> dis(-100.0, 100.0); - auto rand_float = [&dis, &gen]() { return static_cast(dis(gen)); }; - - // generate deterministic random int - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_control_msgs::msg::Control msg_control; - msg_control.stamp = rclcpp::Time(rand_int()); - - msg_control.lateral.stamp = rclcpp::Time(rand_int()); - msg_control.lateral.steering_tire_angle = rand_float(); - msg_control.lateral.steering_tire_rotation_rate = rand_float(); - - msg_control.longitudinal.stamp = rclcpp::Time(rand_int()); - msg_control.longitudinal.velocity = rand_float(); - msg_control.longitudinal.jerk = rand_float(); - msg_control.longitudinal.acceleration = rand_float(); - return msg_control; -} - -TEST(AutowareAutoMsgsAdapter, TestMsgAckermannControlCommand) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_control_msgs/msg/AckermannControlCommand"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_control = generate_control_msg(); - auto sub = - node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_control, &test_completed]( - const autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) { - EXPECT_EQ(msg->stamp, msg_control.stamp); - - EXPECT_EQ(msg->lateral.stamp, msg_control.lateral.stamp); - EXPECT_FLOAT_EQ(msg->lateral.steering_tire_angle, msg_control.lateral.steering_tire_angle); - EXPECT_FLOAT_EQ( - msg->lateral.steering_tire_rotation_rate, - msg_control.lateral.steering_tire_rotation_rate); - - EXPECT_EQ(msg->longitudinal.stamp, msg_control.longitudinal.stamp); - EXPECT_FLOAT_EQ(msg->longitudinal.speed, msg_control.longitudinal.velocity); - EXPECT_FLOAT_EQ(msg->longitudinal.acceleration, msg_control.longitudinal.acceleration); - EXPECT_FLOAT_EQ(msg->longitudinal.jerk, msg_control.longitudinal.jerk); - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_control); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp deleted file mode 100644 index b312bd144f6f3..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2023 The 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 - -#include - -#include - -autoware_map_msgs::msg::LaneletMapBin generate_map_msg() -{ - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_map_msgs::msg::LaneletMapBin msg_map; - msg_map.header.stamp = rclcpp::Time(rand_int()); - msg_map.header.frame_id = "test_frame"; - - msg_map.version_map_format = "1.1.1"; - msg_map.version_map = "1.0.0"; - msg_map.name_map = "florence-prato-city-center"; - msg_map.data.push_back(rand_int()); - - return msg_map; -} - -TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_map = generate_map_msg(); - auto sub = node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); - - EXPECT_EQ(msg->map_format, 0); - EXPECT_EQ(msg->format_version, msg_map.version_map_format); - EXPECT_EQ(msg->map_version, msg_map.version_map); - EXPECT_EQ(msg->data, msg_map.data); - - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_map); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp deleted file mode 100644 index e2d44d6085c8a..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2023 The 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 - -#include - -#include - -autoware_planning_msgs::msg::Trajectory generate_planning_msg() -{ - using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_planning_msgs::msg::Trajectory msg_planning; - msg_planning.header.stamp = rclcpp::Time(rand_int()); - msg_planning.header.frame_id = "test_frame"; - - TrajectoryPoint point; - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - for (size_t i = 0; i < 100; i++) { - point.longitudinal_velocity_mps = 1.0; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); - point.pose = pose; - point.longitudinal_velocity_mps = 20.0; - point.lateral_velocity_mps = 0.0; - point.acceleration_mps2 = 1.0; - point.heading_rate_rps = 2.0; - point.front_wheel_angle_rad = 8.0; - point.rear_wheel_angle_rad = 10.0; - - msg_planning.points.push_back(point); - } - - return msg_planning; -} - -TEST(AutowareAutoMsgsAdapter, TestTrajectory) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_planning_msgs/msg/Trajectory"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_planning = generate_planning_msg(); - auto sub = node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_planning, - &test_completed](const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_planning.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_planning.header.frame_id); - for (size_t i = 0; i < msg_planning.points.size(); i++) { - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.x, msg_planning.points.at(i).pose.position.x); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.y, msg_planning.points.at(i).pose.position.y); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.z, msg_planning.points.at(i).pose.position.z); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.x, msg_planning.points.at(i).pose.orientation.x); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.y, msg_planning.points.at(i).pose.orientation.y); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.z, msg_planning.points.at(i).pose.orientation.z); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.w, msg_planning.points.at(i).pose.orientation.w); - EXPECT_FLOAT_EQ( - msg->points.at(i).longitudinal_velocity_mps, - msg_planning.points.at(i).longitudinal_velocity_mps); - EXPECT_FLOAT_EQ( - msg->points.at(i).lateral_velocity_mps, msg_planning.points.at(i).lateral_velocity_mps); - EXPECT_FLOAT_EQ( - msg->points.at(i).acceleration_mps2, msg_planning.points.at(i).acceleration_mps2); - EXPECT_FLOAT_EQ( - msg->points.at(i).heading_rate_rps, msg_planning.points.at(i).heading_rate_rps); - EXPECT_FLOAT_EQ( - msg->points.at(i).front_wheel_angle_rad, msg_planning.points.at(i).front_wheel_angle_rad); - EXPECT_FLOAT_EQ( - msg->points.at(i).rear_wheel_angle_rad, msg_planning.points.at(i).rear_wheel_angle_rad); - } - - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_planning); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp deleted file mode 100644 index a84985770059d..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp +++ /dev/null @@ -1,270 +0,0 @@ -// Copyright 2023 The 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 - -#include - -#include - -autoware_perception_msgs::msg::PredictedObjects generate_perception_msg() -{ - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_perception_msgs::msg::PredictedObjects msg_perception; - msg_perception.header.stamp = rclcpp::Time(rand_int()); - msg_perception.header.frame_id = "test_frame"; - - autoware_perception_msgs::msg::PredictedObject obj; - // // { - unique_identifier_msgs::msg::UUID uuid_; - std::independent_bits_engine bit_eng(gen); - std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); - obj.object_id = uuid_; - obj.existence_probability = 0.5; - autoware_perception_msgs::msg::ObjectClassification obj_class; - obj_class.label = 0; - obj_class.probability = 0.5; - obj.classification.push_back(obj_class); - - // { - autoware_perception_msgs::msg::PredictedObjectKinematics kin; - kin.initial_pose_with_covariance.pose.position.x = 10; - kin.initial_pose_with_covariance.pose.position.y = 10; - kin.initial_pose_with_covariance.pose.position.z = 0; - kin.initial_pose_with_covariance.pose.orientation.x = 0; - kin.initial_pose_with_covariance.pose.orientation.y = 0; - kin.initial_pose_with_covariance.pose.orientation.z = 0; - kin.initial_pose_with_covariance.pose.orientation.w = 1; - - kin.initial_twist_with_covariance.twist.linear.x = 1; - kin.initial_twist_with_covariance.twist.linear.y = 0; - kin.initial_twist_with_covariance.twist.linear.z = 0; - kin.initial_twist_with_covariance.twist.angular.x = 0; - kin.initial_twist_with_covariance.twist.angular.y = 0; - kin.initial_twist_with_covariance.twist.angular.z = 0; - - kin.initial_acceleration_with_covariance.accel.linear.x = 0; - kin.initial_acceleration_with_covariance.accel.linear.y = 0; - kin.initial_acceleration_with_covariance.accel.linear.z = 0; - kin.initial_acceleration_with_covariance.accel.angular.x = 0; - kin.initial_acceleration_with_covariance.accel.angular.y = 0; - kin.initial_acceleration_with_covariance.accel.angular.z = 0; - - constexpr size_t path_size = 10; - kin.predicted_paths.resize(1); - kin.predicted_paths[0].path.resize(path_size); - for (size_t i = 0; i < path_size; i++) { - kin.predicted_paths[0].path[i].position.x = i; - kin.predicted_paths[0].path[i].position.y = 0; - kin.predicted_paths[0].path[i].position.z = 0; - } - obj.kinematics = kin; - // } - // { - autoware_perception_msgs::msg::Shape s; - s.type = 1; - geometry_msgs::msg::Point32 p; - p.x = 9.0f; - p.y = 11.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 11.0f; - p.y = 11.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 11.0f; - p.y = 9.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 9.0f; - p.y = 9.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - - s.dimensions.x = 2; - s.dimensions.y = 2; - s.dimensions.z = 2; - - obj.shape = s; - // } - // } - - msg_perception.objects.push_back(obj); - return msg_perception; -} - -TEST(AutowareAutoMsgsAdapter, TestPredictedObjects) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_perception_msgs/msg/PredictedObjects"; - const std::string topic_name_source = "topic_name_source"; - const std::string topic_name_target = "topic_name_target"; - - std::cout << "Creating the adapter node..." << std::endl; - - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("msg_type_target", msg_type_target); - node_options.append_parameter_override("topic_name_source", topic_name_source); - node_options.append_parameter_override("topic_name_target", topic_name_target); - - using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; - AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; - node_adapter = std::make_shared(node_options); - - std::cout << "Creating the subscriber node..." << std::endl; - - auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); - - bool test_completed = false; - - const auto msg_perception = generate_perception_msg(); - auto sub = node_subscriber->create_subscription< - autoware_auto_perception_msgs::msg::PredictedObjects>( - topic_name_target, rclcpp::QoS{1}, - [&msg_perception, - &test_completed](const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_perception.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_perception.header.frame_id); - EXPECT_EQ(msg->objects[0].object_id.uuid, msg_perception.objects[0].object_id.uuid); - EXPECT_FLOAT_EQ( - msg->objects[0].existence_probability, msg_perception.objects[0].existence_probability); - EXPECT_EQ( - msg->objects[0].classification[0].label, msg_perception.objects[0].classification[0].label); - EXPECT_FLOAT_EQ( - msg->objects[0].classification[0].probability, - msg_perception.objects[0].classification[0].probability); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.x, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.y, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.z, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w); - - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.x, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.y, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.z, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.x, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.y, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.z, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.z); - - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z); - - for (size_t i = 0; i < msg->objects[0].kinematics.predicted_paths[0].path.size(); i++) { - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.x, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.y, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.z, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.z); - } - - EXPECT_EQ(msg->objects[0].shape.type, msg_perception.objects[0].shape.type); - for (size_t i = 0; i < msg_perception.objects[0].shape.footprint.points.size(); i++) { - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].x, - msg_perception.objects[0].shape.footprint.points[i].x); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].y, - msg_perception.objects[0].shape.footprint.points[i].y); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].z, - msg_perception.objects[0].shape.footprint.points[i].z); - } - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.x, msg_perception.objects[0].shape.dimensions.x); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.y, msg_perception.objects[0].shape.dimensions.y); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.z, msg_perception.objects[0].shape.dimensions.z); - - test_completed = true; - }); - - std::cout << "Creating the publisher node..." << std::endl; - - auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); - auto pub = node_publisher->create_publisher( - topic_name_source, rclcpp::QoS{1}); - pub->publish(msg_perception); - - auto start_time = std::chrono::system_clock::now(); - auto max_test_dur = std::chrono::seconds(5); - auto timed_out = false; - - while (rclcpp::ok() && !test_completed) { - rclcpp::spin_some(node_subscriber); - rclcpp::spin_some(node_adapter); - rclcpp::spin_some(node_publisher); - rclcpp::sleep_for(std::chrono::milliseconds(50)); - if (std::chrono::system_clock::now() - start_time > max_test_dur) { - timed_out = true; - break; - } - } - - EXPECT_TRUE(test_completed); - EXPECT_FALSE(timed_out); - - // rclcpp::shutdown(); -} From 25cf70066d2250d721cc4320ca08f25ad74e76ab Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Mon, 3 Jun 2024 10:08:45 +0200 Subject: [PATCH 091/120] refactor(behavior_velocity_template_module): prefix package and namespace with autoware_ (#6639) Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- planning/.pages | 2 +- .../autoware_behavior_velocity_planner/package.xml | 2 +- .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../config/template.param.yaml | 0 .../package.xml | 4 ++-- .../plugins.xml | 3 +++ .../src/manager.cpp | 9 +++++---- .../src/manager.hpp | 11 ++++++----- .../src/scene.cpp | 4 ++-- .../src/scene.hpp | 6 ++++-- .../behavior_velocity_template_module/plugins.xml | 3 --- 13 files changed, 27 insertions(+), 23 deletions(-) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/CMakeLists.txt (85%) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/README.md (98%) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/config/template.param.yaml (100%) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/package.xml (88%) create mode 100644 planning/autoware_behavior_velocity_template_module/plugins.xml rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/src/manager.cpp (86%) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/src/manager.hpp (89%) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/src/scene.cpp (93%) rename planning/{behavior_velocity_template_module => autoware_behavior_velocity_template_module}/src/scene.hpp (91%) delete mode 100644 planning/behavior_velocity_template_module/plugins.xml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1ef667376b616..b7fc70f042251 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -147,6 +147,7 @@ perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @@ -173,7 +174,6 @@ planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.taka planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp -planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/planning/.pages b/planning/.pages index 4514894605bbc..c886c6a027753 100644 --- a/planning/.pages +++ b/planning/.pages @@ -19,7 +19,7 @@ nav: - 'Start Planner': planning/behavior_path_start_planner_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - - 'Template for Custom Module': planning/behavior_velocity_template_module + - 'Template for Custom Module': planning/autoware_behavior_velocity_template_module - 'Available Module': - 'Blind Spot': planning/behavior_velocity_blind_spot_module - 'Crosswalk': planning/behavior_velocity_crosswalk_module diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 573d862f1725b..c97a83fbff644 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -80,7 +80,7 @@ behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module - + rosidl_interface_packages diff --git a/planning/behavior_velocity_template_module/CMakeLists.txt b/planning/autoware_behavior_velocity_template_module/CMakeLists.txt similarity index 85% rename from planning/behavior_velocity_template_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_template_module/CMakeLists.txt index 103a3d0a4ceb3..88e6167dd4433 100644 --- a/planning/behavior_velocity_template_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_template_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_template_module) +project(autoware_behavior_velocity_template_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_template_module/README.md b/planning/autoware_behavior_velocity_template_module/README.md similarity index 98% rename from planning/behavior_velocity_template_module/README.md rename to planning/autoware_behavior_velocity_template_module/README.md index 1995b3e040061..2cc3e6db8a555 100644 --- a/planning/behavior_velocity_template_module/README.md +++ b/planning/autoware_behavior_velocity_template_module/README.md @@ -41,7 +41,7 @@ The managing of your modules is defined in manager.hpp and manager.cpp. The mana #### Constructor `TemplateModuleManager` - This is the constructor of the `TemplateModuleManager` class, and it takes an `rclcpp::Node` reference as a parameter. -- It initializes a member variable `dummy_parameter` to 0.0. +- It initializes a member variable `dummy_parameter_` to 0.0. #### `getModuleName()` Method diff --git a/planning/behavior_velocity_template_module/config/template.param.yaml b/planning/autoware_behavior_velocity_template_module/config/template.param.yaml similarity index 100% rename from planning/behavior_velocity_template_module/config/template.param.yaml rename to planning/autoware_behavior_velocity_template_module/config/template.param.yaml diff --git a/planning/behavior_velocity_template_module/package.xml b/planning/autoware_behavior_velocity_template_module/package.xml similarity index 88% rename from planning/behavior_velocity_template_module/package.xml rename to planning/autoware_behavior_velocity_template_module/package.xml index 93a1cb4042e93..fa3b20f5b5af8 100644 --- a/planning/behavior_velocity_template_module/package.xml +++ b/planning/autoware_behavior_velocity_template_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_template_module + autoware_behavior_velocity_template_module 0.1.0 - The behavior_velocity_template_module package + The autoware_behavior_velocity_template_module package Daniel Sanchez diff --git a/planning/autoware_behavior_velocity_template_module/plugins.xml b/planning/autoware_behavior_velocity_template_module/plugins.xml new file mode 100644 index 0000000000000..54aa67a9228bc --- /dev/null +++ b/planning/autoware_behavior_velocity_template_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_template_module/src/manager.cpp b/planning/autoware_behavior_velocity_template_module/src/manager.cpp similarity index 86% rename from planning/behavior_velocity_template_module/src/manager.cpp rename to planning/autoware_behavior_velocity_template_module/src/manager.cpp index 22763f44af7bf..c5861d032dee1 100644 --- a/planning/behavior_velocity_template_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.cpp @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -34,7 +34,7 @@ TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { std::string ns(getModuleName()); - dummy_parameter = getOrDeclareParameter(node, ns + ".dummy"); + dummy_parameter_ = getOrDeclareParameter(node, ns + ".dummy"); } void TemplateModuleManager::launchNewModules( @@ -56,8 +56,9 @@ TemplateModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TemplateModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::TemplateModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_template_module/src/manager.hpp b/planning/autoware_behavior_velocity_template_module/src/manager.hpp similarity index 89% rename from planning/behavior_velocity_template_module/src/manager.hpp rename to planning/autoware_behavior_velocity_template_module/src/manager.hpp index ef4e1f00cae92..aaaf0710eff46 100644 --- a/planning/behavior_velocity_template_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.hpp @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief Constructor for the TemplateModuleManager class. @@ -37,7 +37,7 @@ namespace behavior_velocity_planner * * @param node A reference to the ROS node. */ -class TemplateModuleManager : public SceneModuleManagerInterface +class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleManagerInterface { public: explicit TemplateModuleManager(rclcpp::Node & node); @@ -53,7 +53,7 @@ class TemplateModuleManager : public SceneModuleManagerInterface const char * getModuleName() override { return "template"; } private: - double dummy_parameter{0.0}; + double dummy_parameter_{0.0}; /** * @brief Launch new modules based on the provided path. @@ -84,10 +84,11 @@ class TemplateModuleManager : public SceneModuleManagerInterface * The TemplateModulePlugin class is used to integrate the TemplateModuleManager into the Behavior * Velocity Planner. */ -class TemplateModulePlugin : public PluginWrapper +class TemplateModulePlugin +: public ::behavior_velocity_planner::PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_template_module/src/scene.cpp b/planning/autoware_behavior_velocity_template_module/src/scene.cpp similarity index 93% rename from planning/behavior_velocity_template_module/src/scene.cpp rename to planning/autoware_behavior_velocity_template_module/src/scene.cpp index 558f67b49817c..644e59e6b3011 100644 --- a/planning/behavior_velocity_template_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.cpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { TemplateModule::TemplateModule( @@ -49,4 +49,4 @@ bool TemplateModule::modifyPathVelocity( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_template_module/src/scene.hpp b/planning/autoware_behavior_velocity_template_module/src/scene.hpp similarity index 91% rename from planning/behavior_velocity_template_module/src/scene.hpp rename to planning/autoware_behavior_velocity_template_module/src/scene.hpp index fab0b23efb25f..e8c00efcb3ee1 100644 --- a/planning/behavior_velocity_template_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.hpp @@ -21,9 +21,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopReason; class TemplateModule : public SceneModuleInterface { @@ -64,6 +66,6 @@ class TemplateModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_template_module/plugins.xml b/planning/behavior_velocity_template_module/plugins.xml deleted file mode 100644 index 3560c84e6a080..0000000000000 --- a/planning/behavior_velocity_template_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - From d2f41bc62e6a25bec56b767b0ef19987a6e4c0d6 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 3 Jun 2024 21:15:46 +0900 Subject: [PATCH 092/120] chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (#7202) * chore(autoware_path_optimizer): rename package and namespace Signed-off-by: satoshi-ota * chore(autoware_static_centerline_generator): rename package and namespace Signed-off-by: satoshi-ota * chore: update module name Signed-off-by: satoshi-ota * chore(autoware_velocity_smoother): rename package and namespace Signed-off-by: satoshi-ota * chore(tier4_planning_launch): update module name Signed-off-by: satoshi-ota * chore: update module name Signed-off-by: satoshi-ota * fix: test Signed-off-by: satoshi-ota * fix: test Signed-off-by: satoshi-ota * fix: test Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../launch/planning_evaluator.launch.xml | 2 +- .../launch/planning.launch.xml | 2 +- .../behavior_planning.launch.xml | 2 +- .../motion_planning.launch.xml | 20 ++-- .../scenario_planning.launch.xml | 12 +-- launch/tier4_planning_launch/package.xml | 4 +- planning/.pages | 8 +- planning/README.md | 2 +- .../README.md | 2 +- .../package.xml | 2 +- .../src/node.cpp | 4 +- .../test/src/test_node_interface.cpp | 45 ++++---- .../CMakeLists.txt | 14 +-- .../README.md | 6 +- .../config/path_optimizer.param.yaml} | 0 .../docs/debug.md | 14 +-- .../docs/mpt.md | 0 .../common_structs.hpp | 14 +-- .../autoware_path_optimizer}/debug_marker.hpp | 14 +-- .../mpt_optimizer.hpp | 16 +-- .../include/autoware_path_optimizer}/node.hpp | 22 ++-- .../replan_checker.hpp | 16 +-- .../state_equation_generator.hpp | 16 +-- .../autoware_path_optimizer}/type_alias.hpp | 10 +- .../utils/geometry_utils.hpp | 18 ++-- .../utils/trajectory_utils.hpp | 24 ++--- .../vehicle_model_bicycle_kinematics.hpp | 8 +- .../vehicle_model/vehicle_model_interface.hpp | 6 +- .../launch/launch_visualization.launch.xml | 2 +- .../launch/path_optimizer.launch.xml} | 4 +- .../media/avoid_sudden_steering.drawio.svg | 0 .../media/avoidance_cost.drawio.svg | 0 .../media/bounds.svg | 0 .../media/debug/bounds_visualization.png | Bin .../media/debug/calculation_time_plot.png | Bin .../debug/drivable_area_visualization.png | Bin .../debug/mpt_fixed_traj_visualization.png | Bin .../debug/mpt_ref_traj_visualization.png | Bin .../media/debug/mpt_traj_visualization.png | Bin .../debug/path_footprint_visualization.png | Bin .../media/debug/path_visualization.png | Bin .../debug/traj_footprint_visualization.png | Bin .../media/debug/traj_visualization.png | Bin .../debug/vehicle_circles_visualization.png | Bin .../vehicle_traj_circles_visualization.png | Bin .../keep_minimum_boundary_width.drawio.svg | 0 .../media/mpt_optimization_offset.svg | 0 .../media/vehicle_circles.svg | 0 .../media/vehicle_error_kinematics.png | Bin .../package.xml | 4 +- .../rviz/autoware_path_optimizer.rviz} | 14 +-- .../scripts/calculation_time_plotter.py | 2 +- .../src/debug_marker.cpp | 8 +- .../src/mpt_optimizer.cpp | 10 +- .../src/node.cpp | 53 +++++----- .../src/replan_checker.cpp | 8 +- .../src/state_equation_generator.cpp | 8 +- .../src/utils/geometry_utils.cpp | 8 +- .../src/utils/trajectory_utils.cpp | 16 +-- .../vehicle_model_bicycle_kinematics.cpp | 2 +- .../vehicle_model/vehicle_model_interface.cpp | 2 +- .../test_path_optimizer_node_interface.cpp} | 21 ++-- .../autoware_planning_test_manager/README.md | 26 ++--- .../README.md | 2 +- .../static_centerline_generator.launch.xml | 7 +- .../package.xml | 2 +- ...timization_trajectory_based_centerline.cpp | 8 +- .../test_static_centerline_generator.test.py | 4 +- .../CMakeLists.txt | 18 ++-- .../README.ja.md | 6 +- .../README.md | 8 +- .../config/Analytical.param.yaml | 0 .../config/JerkFiltered.param.yaml | 0 .../config/L2.param.yaml | 0 .../config/Linf.param.yaml | 0 .../config/default_common.param.yaml | 0 .../default_velocity_smoother.param.yaml} | 0 .../config/rqt_multiplot.xml | 20 ++-- .../autoware_velocity_smoother/node.hpp} | 30 +++--- .../autoware_velocity_smoother}/resample.hpp | 10 +- .../analytical_jerk_constrained_smoother.hpp | 14 +-- .../velocity_planning_utils.hpp | 10 +- .../smoother/jerk_filtered_smoother.hpp | 12 +-- .../smoother/l2_pseudo_jerk_smoother.hpp | 12 +-- .../smoother/linf_pseudo_jerk_smoother.hpp | 12 +-- .../smoother/smoother_base.hpp | 12 +-- .../trajectory_utils.hpp | 10 +- .../launch/velocity_smoother.launch.xml} | 8 +- .../motion_velocity_smoother_flow.drawio.svg | 0 .../package.xml | 4 +- .../src/node.cpp} | 99 +++++++++--------- .../src/resample.cpp | 8 +- .../analytical_jerk_constrained_smoother.cpp | 8 +- .../velocity_planning_utils.cpp | 6 +- .../src/smoother/jerk_filtered_smoother.cpp | 10 +- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 10 +- .../smoother/linf_pseudo_jerk_smoother.cpp | 10 +- .../src/smoother/smoother_base.cpp | 10 +- .../src/trajectory_utils.cpp | 20 ++-- .../test/test_smoother_functions.cpp | 6 +- ...test_velocity_smoother_node_interface.cpp} | 30 +++--- .../src/behavior_path_planner_node.cpp | 2 +- .../planner_data.hpp | 4 +- .../package.xml | 2 +- .../src/utilization/trajectory_utils.cpp | 4 +- .../planner_data.hpp | 4 +- .../package.xml | 2 +- .../README.md | 2 +- .../package.xml | 2 +- .../src/node.cpp | 8 +- .../test/src/test_node_interface.cpp | 19 ++-- .../config/plot_juggler_slow_down.xml | 16 +-- .../script/velocity_visualizer.py | 2 +- .../invalid_trajectory_publisher.launch.xml | 4 +- .../launch/planning_validator.launch.xml | 2 +- .../path_sampler/README.md | 4 +- 116 files changed, 492 insertions(+), 502 deletions(-) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/CMakeLists.txt (77%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/README.md (97%) rename planning/{obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml => autoware_path_optimizer/config/path_optimizer.param.yaml} (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/docs/debug.md (87%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/docs/mpt.md (100%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/common_structs.hpp (91%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/debug_marker.hpp (75%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/mpt_optimizer.hpp (96%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/node.hpp (89%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/replan_checker.hpp (83%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/state_equation_generator.hpp (76%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/type_alias.hpp (87%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/utils/geometry_utils.hpp (77%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/utils/trajectory_utils.hpp (89%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/vehicle_model/vehicle_model_bicycle_kinematics.hpp (76%) rename planning/{obstacle_avoidance_planner/include/obstacle_avoidance_planner => autoware_path_optimizer/include/autoware_path_optimizer}/vehicle_model/vehicle_model_interface.hpp (84%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/launch/launch_visualization.launch.xml (86%) rename planning/{obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml => autoware_path_optimizer/launch/path_optimizer.launch.xml} (65%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/avoid_sudden_steering.drawio.svg (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/avoidance_cost.drawio.svg (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/bounds.svg (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/bounds_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/calculation_time_plot.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/drivable_area_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/mpt_fixed_traj_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/mpt_ref_traj_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/mpt_traj_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/path_footprint_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/path_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/traj_footprint_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/traj_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/vehicle_circles_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/debug/vehicle_traj_circles_visualization.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/keep_minimum_boundary_width.drawio.svg (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/mpt_optimization_offset.svg (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/vehicle_circles.svg (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/media/vehicle_error_kinematics.png (100%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/package.xml (93%) rename planning/{obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz => autoware_path_optimizer/rviz/autoware_path_optimizer.rviz} (99%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/scripts/calculation_time_plotter.py (98%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/debug_marker.cpp (98%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/mpt_optimizer.cpp (99%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/node.cpp (93%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/replan_checker.cpp (97%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/state_equation_generator.cpp (92%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/utils/geometry_utils.cpp (97%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/utils/trajectory_utils.cpp (93%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp (94%) rename planning/{obstacle_avoidance_planner => autoware_path_optimizer}/src/vehicle_model/vehicle_model_interface.cpp (92%) rename planning/{obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp => autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp} (70%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/CMakeLists.txt (72%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/README.ja.md (98%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/README.md (98%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/config/Analytical.param.yaml (100%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/config/JerkFiltered.param.yaml (100%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/config/L2.param.yaml (100%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/config/Linf.param.yaml (100%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/config/default_common.param.yaml (100%) rename planning/{motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml => autoware_velocity_smoother/config/default_velocity_smoother.param.yaml} (100%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/config/rqt_multiplot.xml (96%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp => autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp} (92%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/resample.hpp (90%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp (85%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp (82%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/smoother/jerk_filtered_smoother.hpp (86%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/smoother/l2_pseudo_jerk_smoother.hpp (82%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/smoother/linf_pseudo_jerk_smoother.hpp (82%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/smoother/smoother_base.hpp (91%) rename planning/{motion_velocity_smoother/include/motion_velocity_smoother => autoware_velocity_smoother/include/autoware_velocity_smoother}/trajectory_utils.hpp (91%) rename planning/{motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml => autoware_velocity_smoother/launch/velocity_smoother.launch.xml} (77%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/media/motion_velocity_smoother_flow.drawio.svg (100%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/package.xml (93%) rename planning/{motion_velocity_smoother/src/motion_velocity_smoother_node.cpp => autoware_velocity_smoother/src/node.cpp} (91%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/resample.cpp (98%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp (98%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp (98%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/smoother/jerk_filtered_smoother.cpp (98%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/smoother/l2_pseudo_jerk_smoother.cpp (96%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/smoother/linf_pseudo_jerk_smoother.cpp (96%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/smoother/smoother_base.cpp (97%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/src/trajectory_utils.cpp (96%) rename planning/{motion_velocity_smoother => autoware_velocity_smoother}/test/test_smoother_functions.cpp (89%) rename planning/{motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp => autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp} (74%) diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml index 1b7510c2ced69..4f36a614b5d09 100644 --- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 0e693943a4d03..97bf6414189ab 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -33,7 +33,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 51f0a32284626..29fe7f7071126 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -242,7 +242,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index f8deccb188b71..24006e7df580e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -60,18 +60,18 @@ - + - + - + - + @@ -84,7 +84,7 @@ - + @@ -101,7 +101,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -131,8 +131,8 @@ - - + + @@ -146,7 +146,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 0a30204ca3c99..be85ee704ff95 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -21,22 +21,22 @@ - + - - - + + + - + - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 9d39c397f9417..cf96cd39043ce 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,7 +58,9 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_path_optimizer autoware_remaining_distance_time_calculator + autoware_velocity_smoother behavior_path_planner costmap_generator external_cmd_selector @@ -66,8 +68,6 @@ freespace_planner glog_component mission_planner - motion_velocity_smoother - obstacle_avoidance_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator diff --git a/planning/.pages b/planning/.pages index c886c6a027753..a9a9e3dd7e55b 100644 --- a/planning/.pages +++ b/planning/.pages @@ -43,10 +43,10 @@ nav: - 'RRT*': planning/freespace_planning_algorithms/rrtstar - 'Mission Planner': planning/mission_planner - 'Motion Planning': - - 'Obstacle Avoidance Planner': - - 'About Obstacle Avoidance': planning/obstacle_avoidance_planner - - 'Model Predictive Trajectory (MPT)': planning/obstacle_avoidance_planner/docs/mpt - - 'How to Debug': planning/obstacle_avoidance_planner/docs/debug + - 'Path Optimizer': + - 'About Path Optimizer': planning/autoware_path_optimizer + - 'Model Predictive Trajectory (MPT)': planning/autoware_path_optimizer/docs/mpt + - 'How to Debug': planning/autoware_path_optimizer/docs/debug - 'Obstacle Cruise Planner': - 'About Obstacle Cruise': planning/obstacle_cruise_planner - 'How to Debug': planning/obstacle_cruise_planner/docs/debug diff --git a/planning/README.md b/planning/README.md index 0302d37a1180b..ccf8288df3911 100644 --- a/planning/README.md +++ b/planning/README.md @@ -105,7 +105,7 @@ Interested in joining our meetings? We’d love to have you! For more informatio Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. -If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./motion_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. +If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index e0fec11d166a6..0b29d463bb105 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -4,7 +4,7 @@ This module is under development. ## Purpose / Role -This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the [obstacle_avoidance_planner](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/). +This module provides avoidance functions for vehicles, pedestrians, and obstacles in the vicinity of the ego's path in combination with the [autoware_path_optimizer](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_path_optimizer/). Each module performs the following roles. Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided. Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area. diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index c97a83fbff644..a9a096dae7d59 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -37,6 +37,7 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_velocity_smoother behavior_velocity_planner_common diagnostic_msgs eigen @@ -44,7 +45,6 @@ lanelet2_extension libboost-dev motion_utils - motion_velocity_smoother pcl_conversions pluginlib rclcpp diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index d4ae3897bca65..dbbbe143658f4 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -14,11 +14,11 @@ #include "node.hpp" +#include #include #include #include #include -#include #include #include @@ -310,7 +310,7 @@ void BehaviorVelocityPlannerNode::onParam() // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = - std::make_unique(*this); + std::make_unique(*this); planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 06c1bc48cf529..3f60c904d6eb4 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -51,8 +51,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "behavior_velocity_" + module + "_module"; @@ -83,27 +83,26 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - motion_velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane"), - get_behavior_velocity_module_config("no_drivable_lane")}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", + get_behavior_velocity_module_config("blind_spot"), + get_behavior_velocity_module_config("crosswalk"), + get_behavior_velocity_module_config("walkway"), + get_behavior_velocity_module_config("detection_area"), + get_behavior_velocity_module_config("intersection"), + get_behavior_velocity_module_config("no_stopping_area"), + get_behavior_velocity_module_config("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), + get_behavior_velocity_module_config("speed_bump"), + get_behavior_velocity_module_config("stop_line"), + get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config("virtual_traffic_light"), + get_behavior_velocity_module_config("out_of_lane"), + get_behavior_velocity_module_config("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/autoware_path_optimizer/CMakeLists.txt similarity index 77% rename from planning/obstacle_avoidance_planner/CMakeLists.txt rename to planning/autoware_path_optimizer/CMakeLists.txt index 2662555d92341..3ceeae1022b10 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/autoware_path_optimizer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(obstacle_avoidance_planner) +project(autoware_path_optimizer) include(CheckCXXCompilerFlag) @@ -17,7 +17,7 @@ autoware_package() find_package(Eigen3 REQUIRED) -ament_auto_add_library(obstacle_avoidance_planner SHARED +ament_auto_add_library(autoware_path_optimizer SHARED # node src/node.cpp # core algorithms @@ -34,20 +34,20 @@ ament_auto_add_library(obstacle_avoidance_planner SHARED src/utils/geometry_utils.cpp ) -target_include_directories(obstacle_avoidance_planner +target_include_directories(autoware_path_optimizer SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) # register node -rclcpp_components_register_node(obstacle_avoidance_planner - PLUGIN "obstacle_avoidance_planner::ObstacleAvoidancePlanner" - EXECUTABLE obstacle_avoidance_planner_node +rclcpp_components_register_node(autoware_path_optimizer + PLUGIN "autoware_path_optimizer::PathOptimizer" + EXECUTABLE path_optimizer_node ) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_${PROJECT_NAME}_node_interface.cpp + test/test_path_optimizer_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/autoware_path_optimizer/README.md similarity index 97% rename from planning/obstacle_avoidance_planner/README.md rename to planning/autoware_path_optimizer/README.md index c079a57c3a2da..29ffee76b0c49 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/autoware_path_optimizer/README.md @@ -1,4 +1,4 @@ -# Obstacle Avoidance Planner +# Path optimizer ## Purpose @@ -158,7 +158,7 @@ Therefore, we have to make sure that the optimized trajectory is inside the driv - Computation cost is sometimes high. - Because of the approximation such as linearization, some narrow roads cannot be run by the planner. -- Roles of planning for `behavior_path_planner` and `obstacle_avoidance_planner` are not decided clearly. Both can avoid obstacles. +- Roles of planning for `behavior_path_planner` and `path_optimizer` are not decided clearly. Both can avoid obstacles. ## Comparison to other methods @@ -195,7 +195,7 @@ Although it has a cons to converge to the local minima, it can get a good soluti - The point on the vehicle, offset forward with this parameter from the base link` tries to follow the reference path. - change or tune the method to approximate footprints with a set of circles. - - See [here](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/docs/mpt/#collision-free) + - See [here](https://autowarefoundation.github.io/autoware.universe/main/planning/path_optimizer/docs/mpt/#collision-free) - Tuning means changing the ratio of circle's radius. ### Computation time diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/autoware_path_optimizer/config/path_optimizer.param.yaml similarity index 100% rename from planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml rename to planning/autoware_path_optimizer/config/path_optimizer.param.yaml diff --git a/planning/obstacle_avoidance_planner/docs/debug.md b/planning/autoware_path_optimizer/docs/debug.md similarity index 87% rename from planning/obstacle_avoidance_planner/docs/debug.md rename to planning/autoware_path_optimizer/docs/debug.md index 293f5b2ec1a40..1c07d29650a6a 100644 --- a/planning/obstacle_avoidance_planner/docs/debug.md +++ b/planning/autoware_path_optimizer/docs/debug.md @@ -7,7 +7,7 @@ The visualization markers of the planning flow (Input, Model Predictive Trajecto All the following markers can be visualized by ```bash -ros2 launch obstacle_avoidance_planner launch_visualiation.launch.xml vehilce_model:=sample_vehicle +ros2 launch autoware_path_optimizer launch_visualiation.launch.xml vehilce_model:=sample_vehicle ``` The `vehicle_model` must be specified to make footprints with vehicle's size. @@ -33,7 +33,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. - The skyblue left and right line strings, that is visualized by default. - NOTE: - Check if the path is almost inside the drivable area. - - Then, the `obstacle_avoidance_planner` will try to make the trajectory fully inside the drivable area. + - Then, the `path_optimizer` will try to make the trajectory fully inside the drivable area. - During avoidance or lane change by the `behavior` planner, please make sure that the drivable area is expanded correctly. ![drivable_area](../media/debug/drivable_area_visualization.png) @@ -57,7 +57,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. - **Vehicle Circles** - The vehicle's shape is represented by a set of circles. - - The `obstacle_avoidance_planner` will try to make the these circles inside the above boundaries' width. + - The `path_optimizer` will try to make the these circles inside the above boundaries' width. ![vehicle_circles](../media/debug/vehicle_circles_visualization.png) @@ -88,7 +88,7 @@ The `vehicle_model` must be specified to make footprints with vehicle's size. ## Calculation time -The `obstacle_avoidance_planner` consists of many functions such as boundaries' width calculation, collision-free planning, etc. +The `path_optimizer` consists of many functions such as boundaries' width calculation, collision-free planning, etc. We can see the calculation time for each function as follows. ### Raw data @@ -96,7 +96,7 @@ We can see the calculation time for each function as follows. Enable `option.enable_calculation_time_info` or echo the topic as follows. ```sh -$ ros2 topic echo /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time --field data +$ ros2 topic echo /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/calculation_time --field data --- insertFixedPoint:= 0.008 [ms] getPaddedTrajectoryPoints:= 0.002 [ms] @@ -135,7 +135,7 @@ onPath:= 20.737 [ms] With the following script, any calculation time of the above functions can be plot. ```sh -ros2 run obstacle_avoidance_planner calculation_time_plotter.py +ros2 run autoware_path_optimizer calculation_time_plotter.py ``` ![calculation_time_plot](../media/debug/calculation_time_plot.png) @@ -143,7 +143,7 @@ ros2 run obstacle_avoidance_planner calculation_time_plotter.py You can specify functions to plot with the `-f` option. ```sh -ros2 run obstacle_avoidance_planner calculation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" +ros2 run autoware_path_optimizer calculation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" ``` ## Q&A for Debug diff --git a/planning/obstacle_avoidance_planner/docs/mpt.md b/planning/autoware_path_optimizer/docs/mpt.md similarity index 100% rename from planning/obstacle_avoidance_planner/docs/mpt.md rename to planning/autoware_path_optimizer/docs/mpt.md diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp similarity index 91% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp index 4f4f01bf1ec9f..b27eff787ca5a 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ -#include "obstacle_avoidance_planner/type_alias.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -25,7 +25,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { struct ReferencePoint; struct Bounds; @@ -65,7 +65,7 @@ struct TimeKeeper latest_stream.str(""); if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("obstacle_avoidance_planner.time"), msg); + RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); } } @@ -153,6 +153,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp similarity index 75% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index ce12cbca8ddb6..3ef47b865bf8e 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -11,11 +11,11 @@ // 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 OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "rclcpp/clock.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" @@ -25,11 +25,11 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp similarity index 96% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp index edcabec14930d..33da339c62e40 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/state_equation_generator.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/state_equation_generator.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -34,7 +34,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { struct Bounds { @@ -308,5 +308,5 @@ class MPTOptimizer size_t getNumberOfSlackVariables() const; std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp similarity index 89% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp index db17496288766..a0e776628e3cb 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/replan_checker.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/replan_checker.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" @@ -33,12 +33,12 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { -class ObstacleAvoidancePlanner : public rclcpp::Node +class PathOptimizer : public rclcpp::Node { public: - explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); + explicit PathOptimizer(const rclcpp::NodeOptions & node_options); // NOTE: This is for the static_centerline_generator package which utilizes the following // instance. @@ -141,6 +141,6 @@ class ObstacleAvoidancePlanner : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp similarity index 83% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp index 1e4699556ef24..8270bb631e0ae 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/replan_checker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { class ReplanChecker { @@ -66,6 +66,6 @@ class ReplanChecker bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__REPLAN_CHECKER_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp similarity index 76% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp index b6f4eadb682fd..6ee4fcb0ab7a5 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { struct ReferencePoint; @@ -58,5 +58,5 @@ class StateEquationGenerator std::unique_ptr vehicle_model_ptr_; mutable std::shared_ptr time_keeper_ptr_; }; -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__STATE_EQUATION_GENERATOR_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp similarity index 87% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp index b02d2232a4aa1..f249b7486bce6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { // std_msgs using std_msgs::msg::Header; @@ -45,6 +45,6 @@ using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer -#endif // OBSTACLE_AVOIDANCE_PLANNER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp similarity index 77% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index 1ec61f583f661..d21d353f951c4 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -39,13 +39,13 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace geometry_utils { @@ -68,5 +68,5 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__GEOMETRY_UTILS_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp similarity index 89% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index d119acf09b6ea..c4d3f9c063433 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware_path_optimizer/common_structs.hpp" +#include "autoware_path_optimizer/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/common_structs.hpp" -#include "obstacle_avoidance_planner/type_alias.hpp" #include @@ -38,16 +38,16 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); template <> -double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p); +double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace trajectory_utils { @@ -181,7 +181,7 @@ std::optional updateFrontPointForFix( motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + rclcpp::get_logger("autoware_path_optimizer.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; } @@ -192,7 +192,7 @@ std::optional updateFrontPointForFix( constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { RCLCPP_DEBUG( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), + rclcpp::get_logger("autoware_path_optimizer.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } @@ -214,5 +214,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace obstacle_avoidance_planner -#endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__TRAJECTORY_UTILS_HPP_ +} // namespace autoware_path_optimizer +#endif // AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 76% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp index f892b0b8d13de..ec46a96e3d12b 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -32,4 +32,4 @@ class KinematicsBicycleModel : public VehicleModelInterface Eigen::MatrixXd & Ad, Eigen::MatrixXd & Bd, Eigen::MatrixXd & Wd, const double curvature, const double ds) const override; }; -#endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp similarity index 84% rename from planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp rename to planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp index 3fd0129315edd..85180bd33b34c 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #include #include @@ -44,4 +44,4 @@ class VehicleModelInterface const double ds) const = 0; }; -#endif // OBSTACLE_AVOIDANCE_PLANNER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE_PATH_OPTIMIZER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml b/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml similarity index 86% rename from planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml rename to planning/autoware_path_optimizer/launch/launch_visualization.launch.xml index d8320e09cc001..38b3d83e94991 100644 --- a/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml +++ b/planning/autoware_path_optimizer/launch/launch_visualization.launch.xml @@ -13,6 +13,6 @@ - + diff --git a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml b/planning/autoware_path_optimizer/launch/path_optimizer.launch.xml similarity index 65% rename from planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml rename to planning/autoware_path_optimizer/launch/path_optimizer.launch.xml index aec23bf3e60ad..273e3fecb377d 100644 --- a/planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml +++ b/planning/autoware_path_optimizer/launch/path_optimizer.launch.xml @@ -2,8 +2,8 @@ - - + + diff --git a/planning/obstacle_avoidance_planner/media/avoid_sudden_steering.drawio.svg b/planning/autoware_path_optimizer/media/avoid_sudden_steering.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/avoid_sudden_steering.drawio.svg rename to planning/autoware_path_optimizer/media/avoid_sudden_steering.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/avoidance_cost.drawio.svg b/planning/autoware_path_optimizer/media/avoidance_cost.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/avoidance_cost.drawio.svg rename to planning/autoware_path_optimizer/media/avoidance_cost.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/bounds.svg b/planning/autoware_path_optimizer/media/bounds.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/bounds.svg rename to planning/autoware_path_optimizer/media/bounds.svg diff --git a/planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png b/planning/autoware_path_optimizer/media/debug/bounds_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png rename to planning/autoware_path_optimizer/media/debug/bounds_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png b/planning/autoware_path_optimizer/media/debug/calculation_time_plot.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png rename to planning/autoware_path_optimizer/media/debug/calculation_time_plot.png diff --git a/planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png b/planning/autoware_path_optimizer/media/debug/drivable_area_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png rename to planning/autoware_path_optimizer/media/debug/drivable_area_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_fixed_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_fixed_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_ref_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_ref_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png b/planning/autoware_path_optimizer/media/debug/mpt_traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/mpt_traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png b/planning/autoware_path_optimizer/media/debug/path_footprint_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png rename to planning/autoware_path_optimizer/media/debug/path_footprint_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/path_visualization.png b/planning/autoware_path_optimizer/media/debug/path_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/path_visualization.png rename to planning/autoware_path_optimizer/media/debug/path_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png b/planning/autoware_path_optimizer/media/debug/traj_footprint_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png rename to planning/autoware_path_optimizer/media/debug/traj_footprint_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_visualization.png b/planning/autoware_path_optimizer/media/debug/traj_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/traj_visualization.png rename to planning/autoware_path_optimizer/media/debug/traj_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png b/planning/autoware_path_optimizer/media/debug/vehicle_circles_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png rename to planning/autoware_path_optimizer/media/debug/vehicle_circles_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png b/planning/autoware_path_optimizer/media/debug/vehicle_traj_circles_visualization.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png rename to planning/autoware_path_optimizer/media/debug/vehicle_traj_circles_visualization.png diff --git a/planning/obstacle_avoidance_planner/media/keep_minimum_boundary_width.drawio.svg b/planning/autoware_path_optimizer/media/keep_minimum_boundary_width.drawio.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/keep_minimum_boundary_width.drawio.svg rename to planning/autoware_path_optimizer/media/keep_minimum_boundary_width.drawio.svg diff --git a/planning/obstacle_avoidance_planner/media/mpt_optimization_offset.svg b/planning/autoware_path_optimizer/media/mpt_optimization_offset.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/mpt_optimization_offset.svg rename to planning/autoware_path_optimizer/media/mpt_optimization_offset.svg diff --git a/planning/obstacle_avoidance_planner/media/vehicle_circles.svg b/planning/autoware_path_optimizer/media/vehicle_circles.svg similarity index 100% rename from planning/obstacle_avoidance_planner/media/vehicle_circles.svg rename to planning/autoware_path_optimizer/media/vehicle_circles.svg diff --git a/planning/obstacle_avoidance_planner/media/vehicle_error_kinematics.png b/planning/autoware_path_optimizer/media/vehicle_error_kinematics.png similarity index 100% rename from planning/obstacle_avoidance_planner/media/vehicle_error_kinematics.png rename to planning/autoware_path_optimizer/media/vehicle_error_kinematics.png diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/autoware_path_optimizer/package.xml similarity index 93% rename from planning/obstacle_avoidance_planner/package.xml rename to planning/autoware_path_optimizer/package.xml index 40caf7ef300aa..9b2d1e952d91a 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -1,9 +1,9 @@ - obstacle_avoidance_planner + autoware_path_optimizer 0.1.0 - The obstacle_avoidance_planner package + The autoware_path_optimizer package Takayuki Murooka Kosuke Takeuchi Takamasa Horibe diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz similarity index 99% rename from planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz rename to planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz index 78df4fe53c4d8..4d4feb38f27a6 100644 --- a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz +++ b/planning/autoware_path_optimizer/rviz/autoware_path_optimizer.rviz @@ -1038,7 +1038,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_fixed_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/eb_fixed_traj Value: false View Footprint: Alpha: 1 @@ -1079,7 +1079,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/eb_traj Value: false View Footprint: Alpha: 1 @@ -1124,7 +1124,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_ref_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_ref_traj Value: false View Footprint: Alpha: 1 @@ -1165,7 +1165,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_fixed_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_fixed_traj Value: false View Footprint: Alpha: 1 @@ -1206,7 +1206,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/marker Value: false - Class: rviz_plugins/Trajectory Color Border Vel Max: 3 @@ -1218,7 +1218,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_traj + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/mpt_traj Value: false View Footprint: Alpha: 1 @@ -1263,7 +1263,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory + Value: /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory Value: true View Footprint: Alpha: 1 diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py similarity index 98% rename from planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py rename to planning/autoware_path_optimizer/scripts/calculation_time_plotter.py index 1904b45fb8225..29dfccb533fbe 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py @@ -33,7 +33,7 @@ def __init__(self, args): self.calculation_cost_hist = [] self.sub_calculation_cost = self.create_subscription( StringStamped, - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time", + "/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/calculation_time", self.CallbackCalculationCost, 1, ) diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp similarity index 98% rename from planning/obstacle_avoidance_planner/src/debug_marker.cpp rename to planning/autoware_path_optimizer/src/debug_marker.cpp index 2f8babf103877..3127d521160c2 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/debug_marker.hpp" +#include "autoware_path_optimizer/debug_marker.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -432,4 +432,4 @@ MarkerArray getDebugMarker( return marker_array; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp similarity index 99% rename from planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp rename to planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 94dc62b1335d4..ece301e64c97e 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/normalization.hpp" @@ -29,7 +29,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace { @@ -1783,4 +1783,4 @@ std::optional MPTOptimizer::calcNormalizedAvoidanceCost( } return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp similarity index 93% rename from planning/obstacle_avoidance_planner/src/node.cpp rename to planning/autoware_path_optimizer/src/node.cpp index b05d3f9da0c57..49d41e6b07884 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/node.hpp" +#include "autoware_path_optimizer/node.hpp" +#include "autoware_path_optimizer/debug_marker.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/debug_marker.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace { @@ -82,8 +82,8 @@ std::vector calcSegmentLengthVector(const std::vector & } } // namespace -ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) -: Node("obstacle_avoidance_planner", node_options), +PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) +: Node("path_optimizer", node_options), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), time_keeper_ptr_(std::make_shared()) @@ -94,7 +94,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // interface subscriber path_sub_ = create_subscription( - "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); + "~/input/path", 1, std::bind(&PathOptimizer::onPath, this, std::placeholders::_1)); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -148,13 +148,13 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been // initialized. set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); + std::bind(&PathOptimizer::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); published_time_publisher_ = std::make_unique(this); } -rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( +rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -203,7 +203,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( return result; } -void ObstacleAvoidancePlanner::initializePlanning() +void PathOptimizer::initializePlanning() { RCLCPP_DEBUG(get_logger(), "Initialize planning"); @@ -212,12 +212,12 @@ void ObstacleAvoidancePlanner::initializePlanning() resetPreviousData(); } -void ObstacleAvoidancePlanner::resetPreviousData() +void PathOptimizer::resetPreviousData() { mpt_optimizer_ptr_->resetPreviousData(); } -void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) +void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -282,7 +282,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } -bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock clock) const +bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const { if (path.points.size() < 2) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1."); @@ -298,7 +298,7 @@ bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock c return true; } -PlannerData ObstacleAvoidancePlanner::createPlannerData( +PlannerData PathOptimizer::createPlannerData( const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const { // create planner data @@ -314,7 +314,7 @@ PlannerData ObstacleAvoidancePlanner::createPlannerData( return planner_data; } -std::vector ObstacleAvoidancePlanner::generateOptimizedTrajectory( +std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -341,8 +341,7 @@ std::vector ObstacleAvoidancePlanner::generateOptimizedTrajecto return optimized_traj_points; } -std::vector ObstacleAvoidancePlanner::optimizeTrajectory( - const PlannerData & planner_data) +std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); const auto & p = planner_data; @@ -375,7 +374,7 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( return mpt_traj; } -std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajectory( +std::vector PathOptimizer::getPrevOptimizedTrajectory( const std::vector & traj_points) const { const auto prev_optimized_traj_points = mpt_optimizer_ptr_->getPrevOptimizedTrajectoryPoints(); @@ -385,7 +384,7 @@ std::vector ObstacleAvoidancePlanner::getPrevOptimizedTrajector return traj_points; } -void ObstacleAvoidancePlanner::applyInputVelocity( +void PathOptimizer::applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const @@ -488,7 +487,7 @@ void ObstacleAvoidancePlanner::applyInputVelocity( time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( +void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { time_keeper_ptr_->tic(__func__); @@ -555,7 +554,7 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const +void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { time_keeper_ptr_->tic(__func__); @@ -570,7 +569,7 @@ void ObstacleAvoidancePlanner::publishVirtualWall(const geometry_msgs::msg::Pose time_keeper_ptr_->toc(__func__, " "); } -void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( +void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { if (!enable_pub_debug_marker_) { @@ -592,7 +591,7 @@ void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( time_keeper_ptr_->toc(__func__, " "); } -std::vector ObstacleAvoidancePlanner::extendTrajectory( +std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { @@ -656,7 +655,7 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( return resampled_traj_points; } -void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const +void PathOptimizer::publishDebugData(const Header & header) const { time_keeper_ptr_->tic(__func__); @@ -667,7 +666,7 @@ void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const time_keeper_ptr_->toc(__func__, " "); } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_avoidance_planner::ObstacleAvoidancePlanner) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_path_optimizer::PathOptimizer) diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp similarity index 97% rename from planning/obstacle_avoidance_planner/src/replan_checker.cpp rename to planning/autoware_path_optimizer/src/replan_checker.cpp index 5f495051cc2bd..797041ee75416 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/replan_checker.hpp" +#include "autoware_path_optimizer/replan_checker.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) : ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) @@ -212,4 +212,4 @@ bool ReplanChecker::isPathGoalChanged( return true; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/state_equation_generator.cpp rename to planning/autoware_path_optimizer/src/state_equation_generator.cpp index 7460ce9c1f764..7712fbbf6c3cf 100644 --- a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/state_equation_generator.hpp" +#include "autoware_path_optimizer/state_equation_generator.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { // state equation: x = B u + W (u includes x_0) // NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. @@ -69,4 +69,4 @@ Eigen::VectorXd StateEquationGenerator::predict( { return mat.B * U + mat.W; } -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp similarity index 97% rename from planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp rename to planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 3f35de9147e6a..e05098d126a74 100644 --- a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "tf2/utils.h" #include @@ -36,7 +36,7 @@ #include #include -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace bg = boost::geometry; using tier4_autoware_utils::LinearRing2d; @@ -207,4 +207,4 @@ bool isOutsideDrivableAreaFromRectangleFootprint( return false; } } // namespace geometry_utils -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp similarity index 93% rename from planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp rename to planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 651b11b28e8bc..66d0fc08ccf06 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" +#include "autoware_path_optimizer/utils/trajectory_utils.hpp" +#include "autoware_path_optimizer/mpt_optimizer.hpp" +#include "autoware_path_optimizer/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/mpt_optimizer.hpp" -#include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "autoware_auto_planning_msgs/msg/path_point.hpp" @@ -36,25 +36,25 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const obstacle_avoidance_planner::ReferencePoint & p) +geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const obstacle_avoidance_planner::ReferencePoint & p) +geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const obstacle_avoidance_planner::ReferencePoint & p) +double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } } // namespace tier4_autoware_utils -namespace obstacle_avoidance_planner +namespace autoware_path_optimizer { namespace trajectory_utils { @@ -242,4 +242,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace obstacle_avoidance_planner +} // namespace autoware_path_optimizer diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 94% rename from planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 19ff1775a1211..89e9e0e2c9b74 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp similarity index 92% rename from planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp rename to planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp index 07a2ab00e7546..57e4671a4e0cc 100644 --- a/planning/obstacle_avoidance_planner/src/vehicle_model/vehicle_model_interface.cpp +++ b/planning/autoware_path_optimizer/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_path_optimizer/vehicle_model/vehicle_model_interface.hpp" VehicleModelInterface::VehicleModelInterface( const int dim_x, const int dim_u, const int dim_y, const double wheelbase, diff --git a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp similarity index 70% rename from planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp rename to planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index d5af4c7e1180f..8ef099ba09f24 100644 --- a/planning/obstacle_avoidance_planner/test/test_obstacle_avoidance_planner_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_avoidance_planner/node.hpp" +#include "autoware_path_optimizer/node.hpp" #include #include @@ -32,27 +32,26 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto obstacle_avoidance_planner_dir = - ament_index_cpp::get_package_share_directory("obstacle_avoidance_planner"); + const auto path_optimizer_dir = + ament_index_cpp::get_package_share_directory("autoware_path_optimizer"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", - obstacle_avoidance_planner_dir + "/config/obstacle_avoidance_planner.param.yaml"}); + path_optimizer_dir + "/config/path_optimizer.param.yaml"}); - auto test_target_node = - std::make_shared(node_options); + auto test_target_node = std::make_shared(node_options); // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "obstacle_avoidance_planner/input/odometry"); + test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); - // set subscriber with topic name: obstacle_avoidance_planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_avoidance_planner/output/path"); + // set subscriber with topic name: path_optimizer → test_node_ + test_manager->setTrajectorySubscriber("path_optimizer/output/path"); - // set obstacle_avoidance_planner's input topic name(this topic is changed to test node) - test_manager->setPathInputTopicName("obstacle_avoidance_planner/input/path"); + // set path_optimizer's input topic name(this topic is changed to test node) + test_manager->setPathInputTopicName("path_optimizer/input/path"); // test with normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index b72625be999dd..fe5484ca498ee 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -49,7 +49,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // publish the necessary topics from test_manager second argument is topic name test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); // set scenario_selector's input topic name(this topic is changed to test node) test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); @@ -70,18 +70,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) ## Implemented tests -| Node | Test name | exceptional input | output | Exceptional input pattern | -| -------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| motion_velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_avoidance_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | +| Node | Test name | exceptional input | output | Exceptional input pattern | +| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | +| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | +| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | +| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | ## Important Notes diff --git a/planning/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md index 844d0af15f2a5..426d5487cf0cb 100644 --- a/planning/autoware_static_centerline_generator/README.md +++ b/planning/autoware_static_centerline_generator/README.md @@ -5,7 +5,7 @@ This package statically calculates the centerline satisfying path footprints inside the drivable area. On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. -To make path footprints inside the drivable area, we use online path shape optimization by [the obstacle_avoidance_planner package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/obstacle_avoidance_planner/). +To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). Instead of online path shape optimization, we introduce static centerline optimization. With this static centerline optimization, we have following advantages. diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 8e016e2b63391..486ea547e3dc1 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -28,10 +28,7 @@ default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml" /> - + @@ -76,7 +73,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 56d30a715c006..62cdecc1f1eb2 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -18,6 +18,7 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_path_optimizer behavior_path_planner_common geography_utils geometry_msgs @@ -28,7 +29,6 @@ map_projection_loader mission_planner motion_utils - obstacle_avoidance_planner osqp_interface path_smoother rclcpp diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 2825381937674..9eb9a2b432a21 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -14,9 +14,9 @@ #include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "autoware_path_optimizer/node.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator_node.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" @@ -130,7 +130,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto eb_path_smoother_ptr = path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); const auto mpt_optimizer_ptr = - obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); + autoware_path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; @@ -156,9 +156,9 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto smoothed_traj_points = eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // road collision avoidance by model predictive trajectory in the autoware_path_optimizer // package - const obstacle_avoidance_planner::PlannerData planner_data{ + const autoware_path_optimizer::PlannerData planner_data{ raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, virtual_ego_pose}; const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index dc3c26798b12d..7a5cdfd905fe9 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -67,8 +67,8 @@ def generate_test_description(): "config/elastic_band_smoother.param.yaml", ), os.path.join( - get_package_share_directory("obstacle_avoidance_planner"), - "config/obstacle_avoidance_planner.param.yaml", + get_package_share_directory("autoware_path_optimizer"), + "config/path_optimizer.param.yaml", ), os.path.join( get_package_share_directory("map_loader"), diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt similarity index 72% rename from planning/motion_velocity_smoother/CMakeLists.txt rename to planning/autoware_velocity_smoother/CMakeLists.txt index 7fd9c5681315c..fee13513a7a97 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(motion_velocity_smoother) +project(autoware_velocity_smoother) find_package(autoware_cmake REQUIRED) autoware_package() @@ -14,7 +14,7 @@ include_directories( ) set(MOTION_VELOCITY_SMOOTHER_SRC - src/motion_velocity_smoother_node.cpp + src/node.cpp ) set(SMOOTHER_SRC @@ -32,17 +32,17 @@ ament_auto_add_library(smoother SHARED ${SMOOTHER_SRC} ) -ament_auto_add_library(motion_velocity_smoother_node SHARED +ament_auto_add_library(${PROJECT_NAME}_node SHARED ${MOTION_VELOCITY_SMOOTHER_SRC} ) -target_link_libraries(motion_velocity_smoother_node +target_link_libraries(${PROJECT_NAME}_node smoother ) -rclcpp_components_register_node(motion_velocity_smoother_node - PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode" - EXECUTABLE motion_velocity_smoother +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware_velocity_smoother::VelocitySmootherNode" + EXECUTABLE velocity_smoother_node ) if(BUILD_TESTING) @@ -53,10 +53,10 @@ if(BUILD_TESTING) smoother ) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_motion_velocity_smoother_node_interface.cpp + test/test_velocity_smoother_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} - motion_velocity_smoother_node + ${PROJECT_NAME}_node ) endif() diff --git a/planning/motion_velocity_smoother/README.ja.md b/planning/autoware_velocity_smoother/README.ja.md similarity index 98% rename from planning/motion_velocity_smoother/README.ja.md rename to planning/autoware_velocity_smoother/README.ja.md index 62e97b6bbc906..68957078e2bdb 100644 --- a/planning/motion_velocity_smoother/README.ja.md +++ b/planning/autoware_velocity_smoother/README.ja.md @@ -1,10 +1,10 @@ -# Motion Velocity Smoother +# Velocity Smoother ## Purpose -`motion_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 +`autoware_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。 -加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`motion_velocity_smoother`と呼んでいる。 +加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`autoware_velocity_smoother`と呼んでいる。 ## Inner-workings / Algorithms diff --git a/planning/motion_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md similarity index 98% rename from planning/motion_velocity_smoother/README.md rename to planning/autoware_velocity_smoother/README.md index 1ff7f5982b4eb..b8a8a9eb7bb5f 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/autoware_velocity_smoother/README.md @@ -1,10 +1,10 @@ -# Motion Velocity Smoother +# Velocity Smoother ## Purpose -`motion_velocity_smoother` outputs a desired velocity profile on a reference trajectory. +`autoware_velocity_smoother` outputs a desired velocity profile on a reference trajectory. This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. -We call this module `motion_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. +We call this module `autoware_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile. ## Inner-workings / Algorithms @@ -18,7 +18,7 @@ For the point on the reference trajectory closest to the center of the rear whee #### Apply external velocity limit -It applies the velocity limit input from the external of `motion_velocity_smoother`. +It applies the velocity limit input from the external of `autoware_velocity_smoother`. Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter. diff --git a/planning/motion_velocity_smoother/config/Analytical.param.yaml b/planning/autoware_velocity_smoother/config/Analytical.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/Analytical.param.yaml rename to planning/autoware_velocity_smoother/config/Analytical.param.yaml diff --git a/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml b/planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/JerkFiltered.param.yaml rename to planning/autoware_velocity_smoother/config/JerkFiltered.param.yaml diff --git a/planning/motion_velocity_smoother/config/L2.param.yaml b/planning/autoware_velocity_smoother/config/L2.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/L2.param.yaml rename to planning/autoware_velocity_smoother/config/L2.param.yaml diff --git a/planning/motion_velocity_smoother/config/Linf.param.yaml b/planning/autoware_velocity_smoother/config/Linf.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/Linf.param.yaml rename to planning/autoware_velocity_smoother/config/Linf.param.yaml diff --git a/planning/motion_velocity_smoother/config/default_common.param.yaml b/planning/autoware_velocity_smoother/config/default_common.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/default_common.param.yaml rename to planning/autoware_velocity_smoother/config/default_common.param.yaml diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml similarity index 100% rename from planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml rename to planning/autoware_velocity_smoother/config/default_velocity_smoother.param.yaml diff --git a/planning/motion_velocity_smoother/config/rqt_multiplot.xml b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml similarity index 96% rename from planning/motion_velocity_smoother/config/rqt_multiplot.xml rename to planning/autoware_velocity_smoother/config/rqt_multiplot.xml index ad0ff4b68251e..694f18c0d9b4f 100644 --- a/planning/motion_velocity_smoother/config/rqt_multiplot.xml +++ b/planning/autoware_velocity_smoother/config/rqt_multiplot.xml @@ -35,7 +35,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_velocity + /planning/scenario_planning/velocity_smoother/closest_velocity std_msgs/Float32 @@ -48,7 +48,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_velocity + /planning/scenario_planning/velocity_smoother/closest_velocity std_msgs/Float32 @@ -86,7 +86,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + /planning/scenario_planning/velocity_smoother/closest_merged_velocity std_msgs/Float32 @@ -99,7 +99,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity + /planning/scenario_planning/velocity_smoother/closest_merged_velocity std_msgs/Float32 @@ -137,7 +137,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_max_velocity + /planning/scenario_planning/velocity_smoother/closest_max_velocity std_msgs/Float32 @@ -150,7 +150,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_max_velocity + /planning/scenario_planning/velocity_smoother/closest_max_velocity std_msgs/Float32 @@ -188,7 +188,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_acceleration + /planning/scenario_planning/velocity_smoother/closest_acceleration std_msgs/Float32 @@ -201,7 +201,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/closest_acceleration + /planning/scenario_planning/velocity_smoother/closest_acceleration std_msgs/Float32 @@ -264,7 +264,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/calculation_time + /planning/scenario_planning/velocity_smoother/calculation_time std_msgs/Float32 @@ -277,7 +277,7 @@ -1000 0 - /planning/scenario_planning/motion_velocity_smoother/calculation_time + /planning/scenario_planning/velocity_smoother/calculation_time std_msgs/Float32 diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp similarity index 92% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index 868c1ab12cce8..c2fe415091a0a 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ - +#ifndef AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ + +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -53,7 +53,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -77,10 +77,10 @@ struct Motion Motion(const double v, const double a) : vel(v), acc(a) {} }; -class MotionVelocitySmootherNode : public rclcpp::Node +class VelocitySmootherNode : public rclcpp::Node { public: - explicit MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options); + explicit VelocitySmootherNode(const rclcpp::NodeOptions & node_options); private: rclcpp::Publisher::SharedPtr pub_trajectory_; @@ -275,6 +275,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp similarity index 90% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index 80f691b5f7b40..4c820b4d04baa 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace resampling { @@ -48,6 +48,6 @@ TrajectoryPoints resampleTrajectory( const double nearest_dist_threshold, const double nearest_yaw_threshold, const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp similarity index 85% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 9cddba5ca500a..08d0a1cd1a8e1 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -28,7 +28,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class AnalyticalJerkConstrainedSmoother : public SmootherBase { @@ -113,8 +113,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase std::string strTimes(const std::vector & times) const; std::string strStartIndices(const std::vector & start_indices) const; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother // clang-format off -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT // clang-format on diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp similarity index 82% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 554bdcca890ef..596a8aa94c707 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { @@ -51,8 +51,8 @@ double integ_x(double x0, double v0, double a0, double j0, double t); double integ_v(double v0, double a0, double j0, double t); double integ_a(double a0, double j0, double t); } // namespace analytical_velocity_planning_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother // clang-format off -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT // clang-format on diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp similarity index 86% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 4b55fbbf184b9..57414b8e8109f 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class JerkFilteredSmoother : public SmootherBase { @@ -69,6 +69,6 @@ class JerkFilteredSmoother : public SmootherBase const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp similarity index 82% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 392fef2429736..3d94102495839 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class L2PseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class L2PseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp similarity index 82% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 9cc6ffef16090..c8bdc9d3c939c 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -26,7 +26,7 @@ #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { class LinfPseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class LinfPseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp similarity index 91% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index beb571635f70c..63d5e5e887124 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "motion_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -86,6 +86,6 @@ class SmootherBase protected: BaseParam base_param_; }; -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother -#endif // MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp similarity index 91% rename from planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp rename to planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index 35e8ab53dd75d..775f4109a4ad1 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "geometry_msgs/msg/detail/pose__struct.hpp" @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother::trajectory_utils +namespace autoware_velocity_smoother::trajectory_utils { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -77,6 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace motion_velocity_smoother::trajectory_utils +} // namespace autoware_velocity_smoother::trajectory_utils -#endif // MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml similarity index 77% rename from planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml rename to planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml index 86f767560fd7c..11932ad125adc 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/autoware_velocity_smoother/launch/velocity_smoother.launch.xml @@ -1,6 +1,6 @@ - + @@ -12,10 +12,10 @@ - - + + - + diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg similarity index 100% rename from planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg rename to planning/autoware_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg diff --git a/planning/motion_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml similarity index 93% rename from planning/motion_velocity_smoother/package.xml rename to planning/autoware_velocity_smoother/package.xml index b9b368d917535..b390dcc12edc5 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -1,9 +1,9 @@ - motion_velocity_smoother + autoware_velocity_smoother 0.1.0 - The motion_velocity_smoother package + The autoware_velocity_smoother package Fumiya Watanabe Takamasa Horibe diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/autoware_velocity_smoother/src/node.cpp similarity index 91% rename from planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp rename to planning/autoware_velocity_smoother/src/node.cpp index eb8592bb99637..0ea10534eb126 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "autoware_velocity_smoother/node.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "motion_utils/marker/marker_helper.hpp" -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -32,10 +32,10 @@ #include // clang-format on -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { -MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("motion_velocity_smoother", node_options) +VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) +: Node("velocity_smoother", node_options) { using std::placeholders::_1; @@ -57,13 +57,13 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( - "~/input/trajectory", 1, std::bind(&MotionVelocitySmootherNode::onCurrentTrajectory, this, _1)); + "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); sub_current_odometry_ = create_subscription( "/localization/kinematic_state", 1, - std::bind(&MotionVelocitySmootherNode::onCurrentOdometry, this, _1)); + std::bind(&VelocitySmootherNode::onCurrentOdometry, this, _1)); sub_external_velocity_limit_ = create_subscription( "~/input/external_velocity_limit_mps", 1, - std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); + std::bind(&VelocitySmootherNode::onExternalVelocityLimit, this, _1)); sub_current_acceleration_ = create_subscription( "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { current_acceleration_ptr_ = msg; @@ -73,8 +73,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&MotionVelocitySmootherNode::onParameter, this, _1)); + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1)); // debug publish_debug_trajs_ = declare_parameter("publish_debug_trajs"); @@ -107,7 +107,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions published_time_publisher_ = std::make_unique(this); } -void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) +void VelocitySmootherNode::setupSmoother(const double wheelbase) { switch (node_param_.algorithm_type) { case AlgorithmType::JERK_FILTERED: { @@ -137,13 +137,13 @@ void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) break; } default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); } smoother_->setWheelBase(wheelbase); } -rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter( +rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter( const std::vector & parameters) { auto update_param = [&](const std::string & name, double & v) { @@ -263,7 +263,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter break; } default: - throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); + throw std::domain_error("[VelocitySmootherNode] invalid algorithm"); } rcl_interfaces::msg::SetParametersResult result{}; @@ -272,7 +272,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter return result; } -void MotionVelocitySmootherNode::initCommonParam() +void VelocitySmootherNode::initCommonParam() { auto & p = node_param_; p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); @@ -310,7 +310,7 @@ void MotionVelocitySmootherNode::initCommonParam() declare_parameter("plan_from_ego_speed_on_manual_mode"); } -void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const +void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const { Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; @@ -319,17 +319,17 @@ void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & traj pub_trajectory_, publishing_trajectory.header.stamp); } -void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) +void VelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; } -void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +void VelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { external_velocity_limit_ptr_ = msg; } -void MotionVelocitySmootherNode::calcExternalVelocityLimit() +void VelocitySmootherNode::calcExternalVelocityLimit() { if (!external_velocity_limit_ptr_) { return; @@ -418,7 +418,7 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() return; } -bool MotionVelocitySmootherNode::checkData() const +bool VelocitySmootherNode::checkData() const { if (!current_odometry_ptr_ || !base_traj_raw_ptr_ || !current_acceleration_ptr_) { RCLCPP_DEBUG( @@ -434,7 +434,7 @@ bool MotionVelocitySmootherNode::checkData() const return true; } -void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -516,7 +516,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } -void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() +void VelocitySmootherNode::updateDataForExternalVelocityLimit() { if (prev_output_.empty()) { return; @@ -532,7 +532,7 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() return; } -TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( +TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { TrajectoryPoints output{}; // velocity is optimized by qp solver @@ -578,7 +578,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( return output; } -bool MotionVelocitySmootherNode::smoothVelocity( +bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { @@ -685,7 +685,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( return true; } -void MotionVelocitySmootherNode::insertBehindVelocity( +void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { const bool keep_closest_vel_for_behind = @@ -727,7 +727,7 @@ void MotionVelocitySmootherNode::insertBehindVelocity( } } -void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const +void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const { const size_t closest = findNearestIndexFromEgo(trajectory); @@ -746,8 +746,7 @@ void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & tr pub_dist_to_stopline_->publish(dist_to_stopline); } -std::pair -MotionVelocitySmootherNode::calcInitialMotion( +std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); @@ -830,7 +829,7 @@ MotionVelocitySmootherNode::calcInitialMotion( return {initial_motion, InitializeType::NORMAL}; } -void MotionVelocitySmootherNode::overwriteStopPoint( +void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { const auto stop_idx = motion_utils::searchZeroVelocityIndex(input); @@ -877,7 +876,7 @@ void MotionVelocitySmootherNode::overwriteStopPoint( } } -void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const +void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { if (traj.size() < 1) { return; @@ -915,7 +914,7 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); } -void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const +void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { const auto stop_idx = motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { @@ -933,7 +932,7 @@ void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & } } -void MotionVelocitySmootherNode::publishDebugTrajectories( +void VelocitySmootherNode::publishDebugTrajectories( const std::vector & debug_trajectories) const { auto debug_trajectories_tmp = debug_trajectories; @@ -955,7 +954,7 @@ void MotionVelocitySmootherNode::publishDebugTrajectories( } } -void MotionVelocitySmootherNode::publishClosestVelocity( +void VelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { @@ -967,7 +966,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( pub->publish(vel_data); } -void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) +void VelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); @@ -1002,13 +1001,13 @@ void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & tr *prev_time_ = curr_time; } -void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) +void VelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) { prev_output_ = final_result; prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); } -MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType( +VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType( const std::string & algorithm_name) const { if (algorithm_name == "JerkFiltered") { @@ -1024,11 +1023,11 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit return AlgorithmType::ANALYTICAL; } - throw std::domain_error("[MotionVelocitySmootherNode] undesired algorithm is selected."); + throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected."); return AlgorithmType::INVALID; } -double MotionVelocitySmootherNode::calcTravelDistance() const +double VelocitySmootherNode::calcTravelDistance() const { const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); @@ -1041,14 +1040,14 @@ double MotionVelocitySmootherNode::calcTravelDistance() const return 0.0; } -bool MotionVelocitySmootherNode::isEngageStatus(const double target_vel) const +bool VelocitySmootherNode::isEngageStatus(const double target_vel) const { const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio; return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity; } -Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( +Trajectory VelocitySmootherNode::toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header) const { auto trajectory = motion_utils::convertToTrajectory(points); @@ -1056,28 +1055,28 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( return trajectory; } -size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const +size_t VelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { return motion_utils::findFirstNearestIndexWithSoftConstraints( points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } -bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const +bool VelocitySmootherNode::isReverse(const TrajectoryPoints & points) const { if (points.empty()) return true; return std::any_of( points.begin(), points.end(), [](auto & pt) { return pt.longitudinal_velocity_mps < 0; }); } -void MotionVelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const +void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const { for (auto & pt : points) { pt.longitudinal_velocity_mps *= -1.0; } } -void MotionVelocitySmootherNode::publishStopWatchTime() +void VelocitySmootherNode::publishStopWatchTime() { Float32Stamped calculation_time_data{}; calculation_time_data.stamp = this->now(); @@ -1085,7 +1084,7 @@ void MotionVelocitySmootherNode::publishStopWatchTime() debug_calculation_time_->publish(calculation_time_data); } -TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPoint( const TrajectoryPoints & trajectory, const Pose & pose) const { const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -1094,13 +1093,13 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); } -TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( +TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( const TrajectoryPoints & trajectory) const { return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(motion_velocity_smoother::MotionVelocitySmootherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_velocity_smoother::VelocitySmootherNode) diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/resample.cpp rename to planning/autoware_velocity_smoother/src/resample.cpp index ae638f8e4db1a..17ea1eb9fc9bd 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace resampling { @@ -264,4 +264,4 @@ TrajectoryPoints resampleTrajectory( } } // namespace resampling -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 84d6331b3429d..d2956e67c6ac1 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include @@ -64,7 +64,7 @@ bool applyMaxVelocity( } // namespace -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) : SmootherBase(node) @@ -658,4 +658,4 @@ std::string AnalyticalJerkConstrainedSmoother::strStartIndices( return ss.str(); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp rename to planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 9fa4d697f06cb..84ac29d14bdc5 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { @@ -353,4 +353,4 @@ double integ_a(double a0, double j0, double t) } } // namespace analytical_velocity_planning_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp similarity index 98% rename from planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 6c2bef3ae08c4..d212cfc0bcedf 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" +#include "autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -27,7 +27,7 @@ #define VERBOSE_TRAJECTORY_VELOCITY false -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -322,7 +322,7 @@ bool JerkFilteredSmoother::apply( output.at(i).acceleration_mps2 = a_stop_decel; } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const int status_polish = std::get<2>(result); if (status_polish != 1) { @@ -480,4 +480,4 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( smoother_param_.jerk_filter_ds); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index d46deeeeb9f10..3c2872f2e58e3 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -223,7 +223,7 @@ bool L2PseudoJerkSmoother::apply( // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); // } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const auto tf2 = std::chrono::system_clock::now(); const double dt_ms2 = @@ -242,4 +242,4 @@ TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp rename to planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index e8900b1947454..f434d8d4514ca 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" +#include "autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include @@ -23,7 +23,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -236,7 +236,7 @@ bool LinfPseudoJerkSmoother::apply( // v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); // } - qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]"); + qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); const auto tf2 = std::chrono::system_clock::now(); const double dt_ms2 = @@ -254,4 +254,4 @@ TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp similarity index 97% rename from planning/motion_velocity_smoother/src/smoother/smoother_base.cpp rename to planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index bf193b7251382..5704113067244 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/smoother/smoother_base.hpp" +#include "autoware_velocity_smoother/resample.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -26,7 +26,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { namespace @@ -270,4 +270,4 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( return output; } -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp similarity index 96% rename from planning/motion_velocity_smoother/src/trajectory_utils.cpp rename to planning/autoware_velocity_smoother/src/trajectory_utils.cpp index acc3673d66925..aff1a0b0e3213 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -25,7 +25,7 @@ #include #include -namespace motion_velocity_smoother +namespace autoware_velocity_smoother { using geometry_msgs::msg::Point; namespace trajectory_utils @@ -202,7 +202,7 @@ std::vector calcTrajectoryCurvatureFrom3Points( } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "%s", e.what()); if (i > 1) { curvature = k_arr.at(i - 1); // previous curvature @@ -374,7 +374,7 @@ bool calcStopDistWithJerkConstraints( double t1 = calculateTime(a0, 0.0, jerk); if (t1 < 0) { RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "t1 < 0. unexpected condition."); return false; } else if (t1 < t_threshold) { @@ -399,7 +399,7 @@ bool calcStopDistWithJerkConstraints( stop_dist = x; if (!isValidStopDist(v, a, target_vel, a_target, v_margin, a_margin)) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid error. type = " << static_cast(type)); return false; } @@ -416,13 +416,13 @@ bool isValidStopDist( const double a_max = a_target + std::abs(a_margin); if (v_end < v_min || v_max < v_end) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid check error! v_target = " << v_target << ", v_end = " << v_end); return false; } if (a_end < a_min || a_max < a_end) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "valid check error! a_target = " << a_target << ", a_end = " << a_end); return false; } @@ -486,7 +486,7 @@ std::optional applyDecelFilterWithJerkConstraint( const double a_margin{0.1}; if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "validation check error"); return {}; } @@ -562,7 +562,7 @@ std::optional> updateStateWithJerkCon } RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + rclcpp::get_logger("autoware_velocity_smoother").get_child("trajectory_utils"), "Invalid jerk profile"); return std::nullopt; } @@ -610,4 +610,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } } // namespace trajectory_utils -} // namespace motion_velocity_smoother +} // namespace autoware_velocity_smoother diff --git a/planning/motion_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp similarity index 89% rename from planning/motion_velocity_smoother/test/test_smoother_functions.cpp rename to planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index 67196edc17a2e..ace1d1f14d776 100644 --- a/planning/motion_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/trajectory_utils.hpp" +#include "autoware_velocity_smoother/trajectory_utils.hpp" #include #include using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using motion_velocity_smoother::trajectory_utils::TrajectoryPoints; +using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) { @@ -48,7 +48,7 @@ TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { const auto trajectory_points = genStraightTrajectory(trajectory_size); const auto curvatures = - motion_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + autoware_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( trajectory_points, idx_dist); EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; }; diff --git a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp similarity index 74% rename from planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp rename to planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index 56813a37941a6..f0145ea5a32a7 100644 --- a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "autoware_velocity_smoother/node.hpp" #include #include @@ -22,37 +22,36 @@ #include -using motion_velocity_smoother::MotionVelocitySmootherNode; +using autoware_velocity_smoother::VelocitySmootherNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); - test_manager->setTrajectorySubscriber("motion_velocity_smoother/output/trajectory"); - test_manager->setTrajectoryInputTopicName("motion_velocity_smoother/input/trajectory"); + test_manager->setTrajectorySubscriber("velocity_smoother/output/trajectory"); + test_manager->setTrajectoryInputTopicName("velocity_smoother/input/trajectory"); test_manager->setOdometryTopicName("/localization/kinematic_state"); return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generateNode() { auto node_options = rclcpp::NodeOptions{}; node_options.append_parameter_override("algorithm_type", "JerkFiltered"); node_options.append_parameter_override("publish_debug_trajs", false); const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - "--params-file", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - "--params-file", motion_velocity_smoother_dir + "/config/default_common.param.yaml", - "--params-file", motion_velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); + "--params-file", velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + "--params-file", velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", + velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); - return std::make_shared(node_options); + return std::make_shared(node_options); } void publishMandatoryTopics( @@ -62,11 +61,10 @@ void publishMandatoryTopics( // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); + test_target_node, "velocity_smoother/input/external_velocity_limit_mps"); test_manager->publishOperationModeState( - test_target_node, "motion_velocity_smoother/input/operation_mode_state"); - test_manager->publishAcceleration( - test_target_node, "motion_velocity_smoother/input/acceleration"); + test_target_node, "velocity_smoother/input/operation_mode_state"); + test_manager->publishAcceleration(test_target_node, "velocity_smoother/input/acceleration"); } TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index f7d204d2bff87..a12cadb0c4994 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -229,7 +229,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // size of a drivable area. // The drivable area has to cover not the base link but the vehicle itself. Therefore // rear_overhang must be added to backward_path_length. In addition, because of the - // calculation of the drivable area in the obstacle_avoidance_planner package, the drivable + // calculation of the drivable area in the autoware_path_optimizer package, the drivable // area has to be a little longer than the backward_path_length parameter by adding // min_backward_offset. constexpr double min_backward_offset = 1.0; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 1511d430ddd0c..c85da1e551391 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -17,8 +17,8 @@ #include "route_handler/route_handler.hpp" +#include #include -#include #include #include @@ -88,7 +88,7 @@ struct PlannerData bool is_simulation = false; // velocity smoother - std::shared_ptr velocity_smoother_; + std::shared_ptr velocity_smoother_; // route handler std::shared_ptr route_handler_; // parameters diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 784b7cabfe9ad..ee3e33f6e2987 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -24,13 +24,13 @@ autoware_auto_planning_msgs autoware_auto_tf2 autoware_perception_msgs + autoware_velocity_smoother diagnostic_msgs eigen geometry_msgs interpolation lanelet2_extension motion_utils - motion_velocity_smoother nav_msgs objects_of_interest_marker_interface pcl_conversions diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 314281a954b49..725c717082620 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -15,9 +15,9 @@ // #include #include "motion_utils/trajectory/conversion.hpp" +#include #include #include -#include #include #include @@ -83,7 +83,7 @@ bool smoothPath( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index 84591f9429a66..c8d0ac54ca969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#include +#include #include #include @@ -81,7 +81,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_{}; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index ad791d247ee11..b3eaaccd11d13 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -17,13 +17,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_velocity_smoother behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension libboost-dev motion_utils - motion_velocity_smoother rclcpp route_handler tier4_autoware_utils diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index f8c5eb172abd2..9190adf1ff67e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -54,5 +54,5 @@ In addition, the following parameters should be provided to the node: - [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml); - [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml); - [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml); -- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/motion_velocity_smoother/#parameters) +- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/#parameters) - Parameters of each plugin that will be loaded. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 2b52610c0276a..0cbf220cd0955 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -21,13 +21,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_motion_velocity_planner_common + autoware_velocity_smoother diagnostic_msgs eigen geometry_msgs lanelet2_extension libboost-dev motion_utils - motion_velocity_smoother pcl_conversions pluginlib rclcpp diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index c0373678270dd..7c2c56c332865 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -14,10 +14,10 @@ #include "node.hpp" +#include +#include #include #include -#include -#include #include #include #include @@ -241,7 +241,7 @@ void MotionVelocityPlannerNode::on_acceleration( void MotionVelocityPlannerNode::set_velocity_smoother_params() { planner_data_.velocity_smoother_ = - std::make_shared(*this); + std::make_shared(*this); } void MotionVelocityPlannerNode::on_lanelet_map( @@ -397,7 +397,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp index 3ad5bab8e070b..46887a56c84f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp @@ -49,8 +49,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto motion_velocity_planner_dir = ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); - const auto motion_velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); const auto get_motion_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_motion_velocity_" + module + "_module"; @@ -67,14 +67,13 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", - motion_velocity_smoother_dir + "/config/Analytical.param.yaml", - motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", - get_motion_velocity_module_config("out_of_lane")}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + motion_velocity_planner_dir + "/config/motion_velocity_planner.param.yaml", + get_motion_velocity_module_config("out_of_lane")}); return std::make_shared(node_options); } diff --git a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml index 92b56c2d2e6e4..4c47fa994cc9b 100644 --- a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml +++ b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml @@ -8,10 +8,10 @@ - - - - + + + + @@ -69,10 +69,10 @@ - - - - + + + + diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index c9255dae679c0..a54b238ad56e1 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -31,7 +31,7 @@ def __init__(self): self.sub_original_traj = self.create_subscription( Trajectory, - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", + "/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/trajectory", self.plotOriginalTrajectory, 1, ) diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index fb64d2e0841bd..3f73e0f462cc2 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -1,6 +1,6 @@ - - + + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 382c2d22b2adf..81573adb4dc24 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index 2d7fe52e5dcad..b82896f4fd93f 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -69,9 +69,9 @@ If the reference path is not smooth, the resulting candidates will probably be u Failure to find a valid trajectory current results in a suddenly stopping trajectory. -## Comparison with the `obstacle_avoidance_planner` +## Comparison with the `autoware_path_optimizer` -The `obstacle_avoidance_planner` uses an optimization based approach, +The `autoware_path_optimizer` uses an optimization based approach, finding the optimal solution of a mathematical problem if it exists. When no solution can be found, it is often hard to identify the issue due to the intermediate mathematical representation of the problem. From 19ea1014b65bd3aad71f612a508fd5d42cf0b8b1 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 21:16:53 +0900 Subject: [PATCH 093/120] feat!: remove autoware_auto_tf2 package (#7218) * feat!: remove autoware_auto_geometry package Signed-off-by: Ryohsuke Mitsudome * docs: remove autoware_auto_geometry package from docs Signed-off-by: Ryohsuke Mitsudome * feat!: remove autoware_auto_tf2 package Signed-off-by: Ryohsuke Mitsudome * fix: remove from autoware_auto_tf2 packages from docs page Signed-off-by: Ryohsuke Mitsudome --------- Signed-off-by: Ryohsuke Mitsudome --- .github/CODEOWNERS | 1 - common/.pages | 1 - common/autoware_auto_tf2/CMakeLists.txt | 22 -- .../design/autoware-auto-tf2-design.md | 183 ---------- .../tf2_autoware_auto_msgs.hpp | 147 -------- common/autoware_auto_tf2/package.xml | 32 -- .../test/test_tf2_autoware_auto_msgs.cpp | 318 ------------------ .../package.xml | 1 - planning/behavior_path_planner/package.xml | 1 - .../behavior_path_planner_common/package.xml | 1 - .../package.xml | 1 - .../package.xml | 1 - .../src/scene_crosswalk.cpp | 1 - .../package.xml | 1 - .../surround_obstacle_checker/package.xml | 1 - .../surround_obstacle_checker/src/node.cpp | 1 - .../sim_model_delay_steer_map_acc_geared.hpp | 2 - .../simple_planning_simulator/package.xml | 1 - .../simple_planning_simulator_core.cpp | 1 - 19 files changed, 717 deletions(-) delete mode 100755 common/autoware_auto_tf2/CMakeLists.txt delete mode 100644 common/autoware_auto_tf2/design/autoware-auto-tf2-design.md delete mode 100644 common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp delete mode 100644 common/autoware_auto_tf2/package.xml delete mode 100644 common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b7fc70f042251..dd73e823b6c05 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,7 +2,6 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_point_types/** taichi.higashide@tier4.jp diff --git a/common/.pages b/common/.pages index 105466bcba763..38cffcb1dc027 100644 --- a/common/.pages +++ b/common/.pages @@ -6,7 +6,6 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons - - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils - 'Geography Utils': common/geography_utils diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt deleted file mode 100755 index a8ae9ec2d962e..0000000000000 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_tf2) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) - target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") - ament_target_dependencies(test_tf2_autoware_auto_msgs - "autoware_auto_common" - "autoware_auto_perception_msgs" - "autoware_auto_system_msgs" - "autoware_auto_geometry_msgs" - "geometry_msgs" - "tf2" - "tf2_geometry_msgs" - "tf2_ros" - ) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md deleted file mode 100644 index aaa719d617373..0000000000000 --- a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md +++ /dev/null @@ -1,183 +0,0 @@ -# autoware_auto_tf2 - -This is the design document for the `autoware_auto_tf2` package. - -## Purpose / Use cases - -In general, users of ROS rely on tf (and its successor, tf2) for publishing and utilizing coordinate -frame transforms. This is true even to the extent that the tf2 contains the packages -`tf2_geometry_msgs` and `tf2_sensor_msgs` which allow for easy conversion to and from the message -types defined in `geometry_msgs` and `sensor_msgs`, respectively. However, AutowareAuto contains -some specialized message types which are not transformable between frames using the ROS 2 library. -The `autoware_auto_tf2` package aims to provide developers with tools to transform applicable -`autoware_auto_msgs` types. In addition to this, this package also provides transform tools for -messages types in `geometry_msgs` missing in `tf2_geometry_msgs`. - -## Design - -While writing `tf2_some_msgs` or contributing to `tf2_geometry_msgs`, compatibility and design -intent was ensured with the following files in the existing tf2 framework: - -- `tf2/convert.h` -- `tf2_ros/buffer_interface.h` - -For example: - -```cpp -void tf2::convert( const A & a,B & b) -``` - -The method `tf2::convert` is dependent on the following: - -```cpp -template - B tf2::toMsg(const A& a); -template - void tf2::fromMsg(const A&, B& b); - -// New way to transform instead of using tf2::doTransform() directly -tf2_ros::BufferInterface::transform(...) -``` - -Which, in turn, is dependent on the following: - -```cpp -void tf2::convert( const A & a,B & b) -const std::string& tf2::getFrameId(const T& t) -const ros::Time& tf2::getTimestamp(const T& t); -``` - -## Current Implementation of tf2_geometry_msgs - -In both ROS 1 and ROS 2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated -functions like: - -- `getTimestamp` -- `getFrameId` -- `doTransform` -- `toMsg` -- `fromMsg` - -In ROS 1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped -underlying data like `Vector3`, `Point`, have implementations of the following functions: - -- `toMsg` -- `fromMsg` - -In ROS 2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 -are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data -were not added; such as `Vector3`, `Point`, or ported in this commit ros/geometry2/commit/6f2a82. -The non-stamped data with `toMsg` and `fromMsg` are `Quaternion`, `Transform`. `Pose` has the -modified `toMsg` and not used by `PoseStamped`. - -## Plan for autoware_auto_perception_msgs::msg::BoundingBoxArray - -The initial rough plan was to implement some of the common tf2 functions like `toMsg`, `fromMsg`, -and `doTransform`, as needed for all the underlying data types in `BoundingBoxArray`. Examples -of the data types include: `BoundingBox`, `Quaternion32`, and `Point32`. In addition, the -implementation should be done such that upstream contributions could also be made to `geometry_msgs`. - -## Assumptions / Known limits - -Due to conflicts in a function signatures, the predefined template of `convert.h`/ -`transform_functions.h` is not followed and compatibility with `tf2::convert(..)` is broken and -`toMsg` is written differently. - -```cpp -// Old style -geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) -geometry_msgs::Point& toMsg(const tf2::Vector3& in) - -// New style -geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) -``` - -## Inputs / Outputs / API - - - -The library provides API `doTransform` for the following data-types that are either not available -in `tf2_geometry_msgs` or the messages types are part of `autoware_auto_msgs` and are therefore -custom and not inherently supported by any of the tf2 libraries. The following APIs are provided -for the following data types: - -- `Point32` - -```cpp -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, - geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `Quaternion32` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBox` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBoxArray` - -```cpp -inline void doTransform( - const BoundingBoxArray & t_in, - BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -In addition, the following helper methods are also added: - -- `BoundingBoxArray` - -```cpp -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) - -inline std::string getFrameId(const BoundingBoxArray & t) -``` - - - - - - - - - - - - - - -## Future extensions / Unimplemented parts - -## Challenges - -- `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped data types, but it is - possible with the same function template. It is needed when transforming sub-data, with main data - that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution? -- `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the - implementation of non-standard `toMsg` would not help the convert. -- `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used - repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`, - `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be - implemented? It may not be simple to template. - - - diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp deleted file mode 100644 index 50c16ba8eaf7d..0000000000000 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2020, The 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. -/// \file -/// \brief This file includes common transform functionality for autoware_auto_msgs - -#ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; -using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray; -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -namespace tf2 -{ -/******************/ -/** Quaternion32 **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs Quaternion32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The Quaternion32 message to transform. - * \param t_out The transformed Quaternion32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w); - KDL::Rotation out = gmTransformToKDL(transform).M * r_in; - - double qx, qy, qz, qw; - out.GetQuaternion(qx, qy, qz, qw); - t_out.x = static_cast(qx); - t_out.y = static_cast(qy); - t_out.z = static_cast(qz); - t_out.w = static_cast(qw); -} - -/******************/ -/** BoundingBox **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBox type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBox message to transform. - * \param t_out The transformed BoundingBox message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - doTransform(t_in.orientation, t_out.orientation, transform); - doTransform(t_in.centroid, t_out.centroid, transform); - doTransform(t_in.corners[0], t_out.corners[0], transform); - doTransform(t_in.corners[1], t_out.corners[1], transform); - doTransform(t_in.corners[2], t_out.corners[2], transform); - doTransform(t_in.corners[3], t_out.corners[3], transform); - // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size -} - -/**********************/ -/** BoundingBoxArray **/ -/**********************/ - -/** \brief Extract a timestamp from the header of a BoundingBoxArray message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) -{ - return tf2_ros::fromMsg(t.header.stamp); -} - -/** \brief Extract a frame ID from the header of a BoundingBoxArray message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline std::string getFrameId(const BoundingBoxArray & t) -{ - return t.header.frame_id; -} - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBoxArray type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBoxArray to transform, as a timestamped BoundingBoxArray message. - * \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBoxArray & t_in, BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - for (auto idx = 0U; idx < t_in.boxes.size(); idx++) { - doTransform(t_out.boxes[idx], t_out.boxes[idx], transform); - } - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; -} - -} // namespace tf2 - -#endif // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml deleted file mode 100644 index b84e2d77befda..0000000000000 --- a/common/autoware_auto_tf2/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - autoware_auto_tf2 - 1.0.0 - Transform related utilities for different msg types - Jit Ray Chowdhury - Tomoya Kimura - Shumpei Wakabayashi - Satoshi Ota - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_system_msgs - geometry_msgs - tf2 - tf2_geometry_msgs - tf2_ros - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp deleted file mode 100644 index ce81dd2276870..0000000000000 --- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2020, The 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. -/// \file -/// \brief This file includes common transform functionally for autoware_auto_msgs - -#include -#include - -#include -#include -#include - -#include - -std::unique_ptr tf_buffer = nullptr; -constexpr double EPS = 1e-3; - -geometry_msgs::msg::TransformStamped filled_transform() -{ - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = 10; - t.transform.translation.y = 20; - t.transform.translation.z = 30; - t.transform.rotation.w = 0; - t.transform.rotation.x = 1; - t.transform.rotation.y = 0; - t.transform.rotation.z = 0; - t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - t.header.frame_id = "A"; - t.child_frame_id = "B"; - - return t; -} - -TEST(Tf2AutowareAuto, DoTransformPoint32) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - - // doTransform - geometry_msgs::msg::Point32 p_out; - tf2::doTransform(p1, p_out, trans); - - EXPECT_NEAR(p_out.x, 11, EPS); - EXPECT_NEAR(p_out.y, 18, EPS); - EXPECT_NEAR(p_out.z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformPolygon) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Polygon poly; - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - poly.points.push_back(p1); - // doTransform - geometry_msgs::msg::Polygon poly_out; - tf2::doTransform(poly, poly_out, trans); - - EXPECT_NEAR(poly_out.points[0].x, 11, EPS); - EXPECT_NEAR(poly_out.points[0].y, 18, EPS); - EXPECT_NEAR(poly_out.points[0].z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformQuaternion32) -{ - const auto trans = filled_transform(); - autoware_auto_geometry_msgs::msg::Quaternion32 q1; - q1.w = 0; - q1.x = 0; - q1.y = 0; - q1.z = 1; - - // doTransform - autoware_auto_geometry_msgs::msg::Quaternion32 q_out; - tf2::doTransform(q1, q_out, trans); - - EXPECT_NEAR(q_out.x, 0.0, EPS); - EXPECT_NEAR(q_out.y, 1.0, EPS); - EXPECT_NEAR(q_out.z, 0.0, EPS); - EXPECT_NEAR(q_out.w, 0.0, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformBoundingBox) -{ - const auto trans = filled_transform(); - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 1; - bb1.centroid.y = 2; - bb1.centroid.z = 3; - bb1.corners[0].x = 4; - bb1.corners[0].y = 5; - bb1.corners[0].z = 6; - bb1.corners[1].x = 7; - bb1.corners[1].y = 8; - bb1.corners[1].z = 9; - bb1.corners[2].x = 10; - bb1.corners[2].y = 11; - bb1.corners[2].z = 12; - bb1.corners[3].x = 13; - bb1.corners[3].y = 14; - bb1.corners[3].z = 15; - - // doTransform - BoundingBox bb_out; - tf2::doTransform(bb1, bb_out, trans); - - EXPECT_NEAR(bb_out.orientation.x, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.y, 1.0, EPS); - EXPECT_NEAR(bb_out.orientation.z, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.w, 0.0, EPS); - EXPECT_NEAR(bb_out.centroid.x, 11, EPS); - EXPECT_NEAR(bb_out.centroid.y, 18, EPS); - EXPECT_NEAR(bb_out.centroid.z, 27, EPS); - EXPECT_NEAR(bb_out.corners[0].x, 14, EPS); - EXPECT_NEAR(bb_out.corners[0].y, 15, EPS); - EXPECT_NEAR(bb_out.corners[0].z, 24, EPS); - EXPECT_NEAR(bb_out.corners[1].x, 17, EPS); - EXPECT_NEAR(bb_out.corners[1].y, 12, EPS); - EXPECT_NEAR(bb_out.corners[1].z, 21, EPS); - EXPECT_NEAR(bb_out.corners[2].x, 20, EPS); - EXPECT_NEAR(bb_out.corners[2].y, 9, EPS); - EXPECT_NEAR(bb_out.corners[2].z, 18, EPS); - EXPECT_NEAR(bb_out.corners[3].x, 23, EPS); - EXPECT_NEAR(bb_out.corners[3].y, 6, EPS); - EXPECT_NEAR(bb_out.corners[3].z, 15, EPS); - - // Testing unused fields are unmodified - EXPECT_EQ(bb_out.size, bb1.size); - EXPECT_EQ(bb_out.heading, bb1.heading); - EXPECT_EQ(bb_out.heading_rate, bb1.heading_rate); - EXPECT_EQ(bb_out.variance, bb1.variance); - EXPECT_EQ(bb_out.vehicle_label, bb1.vehicle_label); - EXPECT_EQ(bb_out.signal_label, bb1.signal_label); - EXPECT_EQ(bb_out.class_likelihood, bb1.class_likelihood); -} - -TEST(Tf2AutowareAuto, TransformBoundingBoxArray) -{ - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 20; - bb1.centroid.y = 21; - bb1.centroid.z = 22; - bb1.corners[0].x = 23; - bb1.corners[0].y = 24; - bb1.corners[0].z = 25; - bb1.corners[1].x = 26; - bb1.corners[1].y = 27; - bb1.corners[1].z = 28; - bb1.corners[2].x = 29; - bb1.corners[2].y = 30; - bb1.corners[2].z = 31; - bb1.corners[3].x = 32; - bb1.corners[3].y = 33; - bb1.corners[3].z = 34; - - BoundingBox bb2; - bb2.orientation.w = 0.707f; - bb2.orientation.x = -0.706f; - bb2.orientation.y = 0; - bb2.orientation.z = 0; - bb2.centroid.x = 50; - bb2.centroid.y = 51; - bb2.centroid.z = 52; - bb2.corners[0].x = 53; - bb2.corners[0].y = 54; - bb2.corners[0].z = 55; - bb2.corners[1].x = 56; - bb2.corners[1].y = 57; - bb2.corners[1].z = 58; - bb2.corners[2].x = 59; - bb2.corners[2].y = 50; - bb2.corners[2].z = 51; - bb2.corners[3].x = 52; - bb2.corners[3].y = 53; - bb2.corners[3].z = 54; - - BoundingBoxArray bba1; - bba1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - bba1.header.frame_id = "A"; - bba1.boxes.push_back(bb1); - bba1.boxes.push_back(bb2); - - // simple api - const auto bba_simple = tf_buffer->transform(bba1, "B", tf2::durationFromSec(2.0)); - - EXPECT_EQ(bba_simple.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_simple.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_simple.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].z, -24, EPS); - - // advanced api - const auto bba_advanced = - tf_buffer->transform(bba1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); - - EXPECT_EQ(bba_advanced.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_advanced.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_advanced.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].z, -24, EPS); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf_buffer = std::make_unique(clock); - tf_buffer->setUsingDedicatedThread(true); - - // populate buffer - const geometry_msgs::msg::TransformStamped t = filled_transform(); - tf_buffer->setTransform(t, "test"); - - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 3643257d243b4..f243e166e6790 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -23,7 +23,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs behavior_path_planner behavior_path_planner_common diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 14220c09880de..91a2ef5951bda 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -39,7 +39,6 @@ autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_auto_vehicle_msgs autoware_perception_msgs autoware_planning_test_manager diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index 7bc40974b9a62..cc6bb451719c8 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -45,7 +45,6 @@ autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs freespace_planning_algorithms geometry_msgs diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index f016cb6de1e7c..c3c5624f0d182 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -15,7 +15,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_auto_vehicle_msgs autoware_perception_msgs autoware_planning_test_manager diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 2c38a9836e0af..90b5592bb3ddb 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -23,7 +23,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0f9c753523f13..721af25e75ece 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,7 +16,6 @@ #include "occluded_crosswalk.hpp" -#include #include #include #include diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index ee3e33f6e2987..30916709c82f7 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -22,7 +22,6 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs autoware_velocity_smoother diagnostic_msgs diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 1f99ba8d2fcba..49165f5475ad2 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -16,7 +16,6 @@ autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 4c7d0589f843b..e499d6b43dea3 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -14,7 +14,6 @@ #include "surround_obstacle_checker/node.hpp" -#include #include #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index d7ca032aa6c48..4ac91eadc0593 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -21,8 +21,6 @@ #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include - #include #include #include diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 7618ef4204335..ef80cea466277 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -20,7 +20,6 @@ autoware_auto_control_msgs autoware_auto_mapping_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_auto_vehicle_msgs geometry_msgs lanelet2_core diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 7fba0431706f0..2d7325d90eb89 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -14,7 +14,6 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" -#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" From 2332541f9c6e9de20262f52a46f34a419bb8b1ce Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 4 Jun 2024 21:50:30 +0900 Subject: [PATCH 094/120] feat!: replace autoware_auto_msgs with autoware_msgs for map modules (#7244) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- map/map_height_fitter/package.xml | 1 - map/map_height_fitter/src/map_height_fitter.cpp | 10 +++++----- map/map_loader/README.md | 8 ++++---- .../include/map_loader/lanelet2_map_loader_node.hpp | 8 ++++---- .../map_loader/lanelet2_map_visualization_node.hpp | 6 +++--- map/map_loader/package.xml | 1 - .../lanelet2_map_loader/lanelet2_map_loader_node.cpp | 10 +++++----- .../lanelet2_map_visualization_node.cpp | 6 +++--- map/map_tf_generator/README.md | 6 +++--- .../src/vector_map_tf_generator_node.cpp | 8 ++++---- 10 files changed, 31 insertions(+), 33 deletions(-) diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 568c77f2509c6..c10e65cdfaab1 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -18,7 +18,6 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs geometry_msgs lanelet2_extension diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 0c99d33772aea..e01201f90f7e3 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -38,7 +38,7 @@ struct MapHeightFitter::Impl explicit Impl(rclcpp::Node * node); void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool get_partial_point_cloud_map(const Point & point); double get_ground_height(const Point & point) const; std::optional fit(const Point & position, const std::string & frame); @@ -59,7 +59,7 @@ struct MapHeightFitter::Impl // for fitting by vector_map_loader lanelet::LaneletMapPtr vector_map_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; }; MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node) @@ -95,7 +95,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n } else if (fit_target_ == "vector_map") { const auto durable_qos = rclcpp::QoS(1).transient_local(); - sub_vector_map_ = node_->create_subscription( + sub_vector_map_ = node_->create_subscription( "~/vector_map", durable_qos, std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1)); @@ -163,7 +163,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } void MapHeightFitter::Impl::on_vector_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { vector_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, vector_map_); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 8a31ecee50be5..21bd39303250f 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -130,7 +130,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature -lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. @@ -144,7 +144,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Published Topics -- ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map ### Parameters @@ -156,7 +156,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -164,7 +164,7 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Subscribed Topics -- ~input/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map ### Published Topics diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 0adc7612e9f61..4e85ddec056c1 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using tier4_map_msgs::msg::MapProjectorInfo; class Lanelet2MapLoaderNode : public rclcpp::Node @@ -38,7 +38,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static HADMapBin create_map_bin_msg( + static LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +48,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index e5a5fe3e3a6fa..cb640e4dc83d5 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -29,12 +29,12 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Subscription::SharedPtr sub_map_bin_; rclcpp::Publisher::SharedPtr pub_marker_; bool viz_lanelets_centerline_; - void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index a9b657f843447..4eaf8600dcab4 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,7 +19,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs component_interface_specs component_interface_utils diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 617f3dd503ce0..b097e1809a385 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -86,7 +86,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( // create publisher and publish pub_map_bin_ = - create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } @@ -141,18 +141,18 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( +LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}, map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - HADMapBin map_bin_msg; + LaneletMapBin map_bin_msg; map_bin_msg.header.stamp = now; map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); return map_bin_msg; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index bdfbcf904cf36..c81236bec86c0 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -73,7 +73,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt viz_lanelets_centerline_ = true; - sub_map_bin_ = this->create_subscription( + sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); @@ -82,7 +82,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt } void Lanelet2MapVisualizationNode::onMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); diff --git a/map/map_tf_generator/README.md b/map/map_tf_generator/README.md index 643f4233c06f0..eef36c34750ca 100644 --- a/map/map_tf_generator/README.md +++ b/map/map_tf_generator/README.md @@ -25,9 +25,9 @@ The following are the supported methods to calculate the position of the `viewer #### vector_map_tf_generator -| Name | Type | Description | -| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | +| Name | Type | Description | +| ----------------- | --------------------------------------- | ------------------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Subscribe vector map to calculate position of `viewer` frames | ### Output diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 20ca1c037a578..f242254a456a1 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node map_frame_ = declare_parameter("map_frame"); viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( + sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); @@ -44,12 +44,12 @@ class VectorMapTFGeneratorNode : public rclcpp::Node private: std::string map_frame_; std::string viewer_frame_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); From b7f18a1a3df35b58c64cbf2ded14f5a2fc682158 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 4 Jun 2024 22:47:10 +0900 Subject: [PATCH 095/120] feat!: replace autoware_auto_msgs with autoware_msgs for simulator modules (#7248) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- .../dummy_perception_publisher/CMakeLists.txt | 4 +- .../dummy_perception_publisher/README.md | 2 +- .../dummy_perception_publisher/node.hpp | 8 +- .../dummy_perception_publisher/msg/Object.msg | 4 +- .../dummy_perception_publisher/package.xml | 2 +- .../src/empty_objects_publisher.cpp | 12 ++- .../dummy_perception_publisher/src/node.cpp | 10 +-- simulator/simple_planning_simulator/README.md | 22 ++--- .../simple_planning_simulator_core.hpp | 81 ++++++++----------- .../sim_model_delay_steer_acc_geared.hpp | 2 +- .../sim_model_delay_steer_map_acc_geared.hpp | 2 +- .../sim_model_ideal_steer_acc_geared.hpp | 2 +- .../vehicle_model/sim_model_interface.hpp | 8 +- .../simple_planning_simulator/package.xml | 11 ++- .../simple_planning_simulator_core.cpp | 29 +++---- .../sim_model_delay_steer_acc_geared.cpp | 4 +- .../sim_model_delay_steer_map_acc_geared.cpp | 4 +- .../sim_model_ideal_steer_acc_geared.cpp | 4 +- .../test/test_simple_planning_simulator.cpp | 19 +++-- 19 files changed, 106 insertions(+), 124 deletions(-) diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index 63d07faf0634c..a27667edf536b 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -7,14 +7,14 @@ autoware_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/InitialState.msg" "msg/Object.msg" - DEPENDENCIES autoware_auto_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs + DEPENDENCIES autoware_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs ) # See ndt_omp package for documentation on why PCL is special find_package(PCL REQUIRED COMPONENTS common filters) set(${PROJECT_NAME}_DEPENDENCIES - autoware_auto_perception_msgs + autoware_perception_msgs tier4_perception_msgs pcl_conversions rclcpp diff --git a/simulator/dummy_perception_publisher/README.md b/simulator/dummy_perception_publisher/README.md index 48036948ece65..5d969dd292832 100644 --- a/simulator/dummy_perception_publisher/README.md +++ b/simulator/dummy_perception_publisher/README.md @@ -21,7 +21,7 @@ This node publishes the result of the dummy detection with the type of perceptio | ----------------------------------- | -------------------------------------------------------- | ----------------------- | | `output/dynamic_object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | dummy detection objects | | `output/points_raw` | `sensor_msgs::msg::PointCloud2` | point cloud of objects | -| `output/debug/ground_truth_objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | ground truth objects | +| `output/debug/ground_truth_objects` | `autoware_perception_msgs::msg::TrackedObjects` | ground truth objects | ## Parameters diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 770663e84dc0c..c3f7976d3efa1 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include @@ -59,7 +59,7 @@ struct ObjectInfo geometry_msgs::msg::PoseWithCovariance pose_covariance_; // convert to TrackedObject // (todo) currently need object input to get id and header information, but it should be removed - autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject( + autoware_perception_msgs::msg::TrackedObject toTrackedObject( const dummy_perception_publisher::msg::Object & object) const; }; @@ -114,7 +114,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr detected_object_with_feature_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr ground_truth_objects_pub_; rclcpp::Subscription::SharedPtr object_sub_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/simulator/dummy_perception_publisher/msg/Object.msg b/simulator/dummy_perception_publisher/msg/Object.msg index 30279299dbfb1..11ac1b3d39daa 100644 --- a/simulator/dummy_perception_publisher/msg/Object.msg +++ b/simulator/dummy_perception_publisher/msg/Object.msg @@ -1,8 +1,8 @@ std_msgs/Header header unique_identifier_msgs/UUID id dummy_perception_publisher/InitialState initial_state -autoware_auto_perception_msgs/ObjectClassification classification -autoware_auto_perception_msgs/Shape shape +autoware_perception_msgs/ObjectClassification classification +autoware_perception_msgs/Shape shape float32 max_velocity float32 min_velocity diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index 0632bd90b2c22..4704024214d9e 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -18,7 +18,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs libpcl-all-dev pcl_conversions diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp index 9b3f01fa4b267..2d1ea626fb1ac 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -14,7 +14,7 @@ #include -#include +#include #include #include @@ -24,9 +24,8 @@ class EmptyObjectsPublisher : public rclcpp::Node public: EmptyObjectsPublisher() : Node("empty_objects_publisher") { - empty_objects_pub_ = - this->create_publisher( - "~/output/objects", 1); + empty_objects_pub_ = this->create_publisher( + "~/output/objects", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -34,13 +33,12 @@ class EmptyObjectsPublisher : public rclcpp::Node } private: - rclcpp::Publisher::SharedPtr - empty_objects_pub_; + rclcpp::Publisher::SharedPtr empty_objects_pub_; rclcpp::TimerBase::SharedPtr timer_; void timerCallback() { - autoware_auto_perception_msgs::msg::PredictedObjects empty_objects; + autoware_perception_msgs::msg::PredictedObjects empty_objects; empty_objects.header.frame_id = "map"; empty_objects.header.stamp = this->now(); empty_objects_pub_->publish(empty_objects); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 3ac663d2c8647..9c58961978161 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -33,8 +33,8 @@ #include #include -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; ObjectInfo::ObjectInfo( const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time) @@ -169,7 +169,7 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() // optional ground truth publisher if (publish_ground_truth_objects_) { ground_truth_objects_pub_ = - this->create_publisher( + this->create_publisher( "~/output/debug/ground_truth_objects", qos); } @@ -182,7 +182,7 @@ void DummyPerceptionPublisherNode::timerCallback() { // output msgs tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg; - autoware_auto_perception_msgs::msg::TrackedObjects output_ground_truth_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_ground_truth_objects_msg; geometry_msgs::msg::PoseStamped output_moved_object_pose; sensor_msgs::msg::PointCloud2 output_pointcloud_msg; std_msgs::msg::Header header; @@ -282,7 +282,7 @@ void DummyPerceptionPublisherNode::timerCallback() feature_object.object.kinematics.twist_with_covariance = object.initial_state.twist_covariance; feature_object.object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; feature_object.object.kinematics.has_twist = false; tf2::toMsg( tf_base_link2noised_moved_object, diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 1f8762b722a29..4902832f836a9 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -18,23 +18,23 @@ The purpose of this simulator is for the integration test of planning and contro ### input - input/initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose -- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle -- input/manual_ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : manual target command to drive a vehicle (used when control_mode_request = Manual) -- input/gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command. -- input/manual_gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual) -- input/turn_indicators_command [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command -- input/hazard_lights_command [`autoware_auto_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command +- input/ackermann_control_command [`autoware_control_msgs/msg/Control`] : target command to drive a vehicle +- input/manual_ackermann_control_command [`autoware_control_msgs/msg/Control`] : manual target command to drive a vehicle (used when control_mode_request = Manual) +- input/gear_command [`autoware_vehicle_msgs/msg/GearCommand`] : target gear command. +- input/manual_gear_command [`autoware_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual) +- input/turn_indicators_command [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command +- input/hazard_lights_command [`autoware_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command - input/control_mode_request [`tier4_vehicle_msgs::srv::ControlModeRequest`] : mode change for Auto/Manual driving ### output - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) - /output/odometry [`nav_msgs/msg/Odometry`] : simulated vehicle pose and twist -- /output/steering [`autoware_auto_vehicle_msgs/msg/SteeringReport`] : simulated steering angle -- /output/control_mode_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual) -- /output/gear_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated gear -- /output/turn_indicators_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status -- /output/hazard_lights_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status +- /output/steering [`autoware_vehicle_msgs/msg/SteeringReport`] : simulated steering angle +- /output/control_mode_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual) +- /output/gear_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated gear +- /output/turn_indicators_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status +- /output/hazard_lights_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status ## Inner-workings / Algorithms diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index e81d4fef00abb..18a3a3bae501a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -20,23 +20,20 @@ #include "simple_planning_simulator/visibility_control.hpp" #include "tier4_api_utils/tier4_api_utils.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_geometry_msgs/msg/complex32.hpp" -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/engage.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "autoware_auto_vehicle_msgs/srv/control_mode_command.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/control_mode_report.hpp" +#include "autoware_vehicle_msgs/msg/engage.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_vehicle_msgs/msg/hazard_lights_command.hpp" +#include "autoware_vehicle_msgs/msg/hazard_lights_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/turn_indicators_command.hpp" +#include "autoware_vehicle_msgs/msg/turn_indicators_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/srv/control_mode_command.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -62,22 +59,20 @@ namespace simulation namespace simple_planning_simulator { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_geometry_msgs::msg::Complex32; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::ControlModeReport; -using autoware_auto_vehicle_msgs::msg::Engage; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::GearReport; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsReport; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; -using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using autoware_auto_vehicle_msgs::srv::ControlModeCommand; +using autoware_control_msgs::msg::Control; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::ControlModeReport; +using autoware_vehicle_msgs::msg::Engage; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::GearReport; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::HazardLightsReport; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsReport; +using autoware_vehicle_msgs::msg::VelocityReport; +using autoware_vehicle_msgs::srv::ControlModeCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -143,10 +138,9 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_; rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; - rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -176,8 +170,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node VelocityReport current_velocity_{}; Odometry current_odometry_{}; SteeringReport current_steer_{}; - AckermannControlCommand current_ackermann_cmd_{}; - AckermannControlCommand current_manual_ackermann_cmd_{}; + Control current_ackermann_cmd_{}; + Control current_manual_ackermann_cmd_{}; GearCommand current_gear_cmd_{}; GearCommand current_manual_gear_cmd_{}; TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_{}; @@ -215,15 +209,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer - /** - * @brief set current_vehicle_cmd_ptr_ with received message - */ - void on_vehicle_cmd(const VehicleControlCommand::ConstSharedPtr msg); - /** * @brief set input steering, velocity, and acceleration of the vehicle model */ - void set_input(const AckermannControlCommand & cmd, const double acc_by_slope); + void set_input(const Control & cmd, const double acc_by_slope); /** * @brief set current_vehicle_state_ with received message @@ -238,7 +227,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief subscribe lanelet map */ - void on_map(const HADMapBin::ConstSharedPtr msg); + void on_map(const LaneletMapBin::ConstSharedPtr msg); /** * @brief set initial pose for simulation with received message diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 35b95bf4b1ae5..1ecb74be41780 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -152,7 +152,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 4ac91eadc0593..b83a831341ac1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -200,7 +200,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index 55b5ddb8fcec5..c73cc54f4ea99 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -107,7 +107,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 981fb0f416d72..1274a1ec28a07 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" /** * @class SimModelInterface @@ -31,8 +31,8 @@ class SimModelInterface Eigen::VectorXd state_; //!< @brief vehicle state vector Eigen::VectorXd input_; //!< @brief vehicle input vector - //!< @brief gear command defined in autoware_auto_msgs/GearCommand - uint8_t gear_ = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + //!< @brief gear command defined in autoware_vehicle_msgs/GearCommand + uint8_t gear_ = autoware_vehicle_msgs::msg::GearCommand::DRIVE; public: /** @@ -73,7 +73,7 @@ class SimModelInterface /** * @brief set gear - * @param [in] gear gear command defined in autoware_auto_msgs/GearCommand + * @param [in] gear gear command defined in autoware_vehicle_msgs/GearCommand */ void setGear(const uint8_t gear); diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index ef80cea466277..5f04fba4fdb8e 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -15,12 +15,10 @@ ament_cmake_auto autoware_cmake - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs + autoware_planning_msgs + autoware_vehicle_msgs geometry_msgs lanelet2_core lanelet2_extension @@ -38,6 +36,7 @@ tier4_external_api_msgs tier4_vehicle_msgs vehicle_info_util + autoware_cmake launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 2d7325d90eb89..3b65218aaf03d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -43,10 +43,10 @@ using namespace std::literals::chrono_literals; namespace { -autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( +autoware_vehicle_msgs::msg::VelocityReport to_velocity_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::VelocityReport velocity; + autoware_vehicle_msgs::msg::VelocityReport velocity; velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); velocity.lateral_velocity = 0.0F; velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); @@ -67,10 +67,10 @@ nav_msgs::msg::Odometry to_odometry( return odometry; } -autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( +autoware_vehicle_msgs::msg::SteeringReport to_steering_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::SteeringReport steer; + autoware_vehicle_msgs::msg::SteeringReport steer; steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); return steer; } @@ -108,21 +108,19 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using std::placeholders::_1; using std::placeholders::_2; - sub_map_ = create_subscription( + sub_map_ = create_subscription( "input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&SimplePlanningSimulator::on_map, this, _1)); sub_init_pose_ = create_subscription( "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); - sub_ackermann_cmd_ = create_subscription( + sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); - sub_manual_ackermann_cmd_ = create_subscription( + [this](const Control::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); + sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { - current_manual_ackermann_cmd_ = *msg; - }); + [this](const Control::ConstSharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, [this](const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ = *msg; }); @@ -419,7 +417,7 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } -void SimplePlanningSimulator::on_map(const HADMapBin::ConstSharedPtr msg) +void SimplePlanningSimulator::on_map(const LaneletMapBin::ConstSharedPtr msg) { auto lanelet_map_ptr = std::make_shared(); @@ -469,14 +467,13 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input( - const AckermannControlCommand & cmd, const double acc_by_slope) +void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; - const auto vel = cmd.longitudinal.speed; + const auto vel = cmd.longitudinal.velocity; const auto accel = cmd.longitudinal.acceleration; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); const auto gear = vehicle_model_ptr_->getGear(); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 10e6a97d703cd..26b252805db47 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -160,7 +160,7 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 72273f0b21ec2..fe847cba946a1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -134,7 +134,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( void SimModelDelaySteerMapAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index b2bfe56209938..c2297f1fd1d73 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -92,7 +92,7 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( state(IDX::YAW) = prev_state(IDX::YAW); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index cedfec395110e..dc49bf17a11fc 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -24,8 +24,8 @@ #include -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; @@ -51,14 +51,14 @@ class PubSubNode : public rclcpp::Node "output/odometry", rclcpp::QoS{1}, [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = - create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); + create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); pub_initialpose_ = create_publisher("input/initialpose", rclcpp::QoS{1}); pub_gear_cmd_ = create_publisher("input/gear_command", rclcpp::QoS{1}); } rclcpp::Subscription::SharedPtr current_odom_sub_; - rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_ackermann_command_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; @@ -66,7 +66,7 @@ class PubSubNode : public rclcpp::Node }; /** - * @brief Generate an AckermannControlCommand message + * @brief Generate an Control message * @param [in] t timestamp * @param [in] steer [rad] steering * @param [in] steer_rate [rad/s] steering rotation rate @@ -74,17 +74,17 @@ class PubSubNode : public rclcpp::Node * @param [in] acc [m/s²] acceleration * @param [in] jerk [m/s3] jerk */ -AckermannControlCommand cmdGen( +Control cmdGen( const builtin_interfaces::msg::Time & t, double steer, double steer_rate, double vel, double acc, double jerk) { - AckermannControlCommand cmd; + Control cmd; cmd.stamp = t; cmd.lateral.stamp = t; cmd.lateral.steering_tire_angle = steer; cmd.lateral.steering_tire_rotation_rate = steer_rate; cmd.longitudinal.stamp = t; - cmd.longitudinal.speed = vel; + cmd.longitudinal.velocity = vel; cmd.longitudinal.acceleration = acc; cmd.longitudinal.jerk = jerk; return cmd; @@ -125,8 +125,7 @@ void sendGear( * @param [in] pub_sub_node pointer to the node used for communication */ void sendCommand( - const AckermannControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, - std::shared_ptr pub_sub_node) + const Control & cmd, rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) { for (int i = 0; i < 150; ++i) { pub_sub_node->pub_ackermann_command_->publish(cmd); From 72989d9a8640e4d0993e030bf700a0e5d669ce40 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 4 Jun 2024 23:47:57 +0900 Subject: [PATCH 096/120] feat!: replace autoware_auto_msgs with autoware_msgs for common modules (#7239) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- .../CMakeLists.txt | 2 +- .../autoware_overlay_rviz_plugin/README.md | 12 +- .../include/gear_display.hpp | 4 +- .../include/signal_display.hpp | 26 +- .../include/speed_display.hpp | 4 +- .../include/speed_limit_display.hpp | 4 +- .../include/steering_wheel_display.hpp | 5 +- .../include/traffic_display.hpp | 9 +- .../include/turn_signals_display.hpp | 8 +- .../autoware_overlay_rviz_plugin/package.xml | 2 +- .../src/gear_display.cpp | 17 +- .../src/signal_display.cpp | 98 +-- .../src/speed_display.cpp | 4 +- .../src/speed_limit_display.cpp | 2 +- .../src/steering_wheel_display.cpp | 2 +- .../src/traffic_display.cpp | 6 +- .../src/turn_signals_display.cpp | 12 +- .../autoware_perception_rviz_plugin/README.md | 20 +- .../detected_objects_display.hpp | 6 +- .../object_polygon_detail.hpp | 64 +- .../object_polygon_display_base.hpp | 14 +- .../predicted_objects_display.hpp | 6 +- .../tracked_objects_display.hpp | 8 +- .../package.xml | 2 +- .../plugins_description.xml | 6 +- .../detected_objects_display.cpp | 2 +- .../object_polygon_detail.cpp | 36 +- .../predicted_objects_display.cpp | 2 +- .../tracked_objects_display.cpp | 2 +- .../component_interface_specs/perception.hpp | 4 +- .../component_interface_specs/planning.hpp | 4 +- .../component_interface_specs/vehicle.hpp | 16 +- common/component_interface_specs/package.xml | 5 +- .../test/test_fake_test_node.cpp | 3 +- common/goal_distance_calculator/Readme.md | 8 +- .../goal_distance_calculator.hpp | 4 +- .../goal_distance_calculator_node.hpp | 8 +- common/goal_distance_calculator/package.xml | 2 +- .../src/goal_distance_calculator.cpp | 2 +- .../src/goal_distance_calculator_node.cpp | 4 +- .../test_spline_interpolation_points_2d.cpp | 2 +- common/motion_utils/README.md | 4 +- common/motion_utils/docs/vehicle/vehicle.md | 8 +- .../motion_utils/resample/resample.hpp | 39 +- .../motion_utils/trajectory/conversion.hpp | 51 +- .../motion_utils/trajectory/interpolation.hpp | 14 +- .../trajectory/path_with_lane_id.hpp | 16 +- .../trajectory/tmp_conversion.hpp | 48 ++ .../motion_utils/trajectory/trajectory.hpp | 617 +++++++++--------- .../vehicle/vehicle_state_checker.hpp | 4 +- common/motion_utils/package.xml | 5 +- .../src/factor/velocity_factor_interface.cpp | 22 +- common/motion_utils/src/resample/resample.cpp | 42 +- .../src/trajectory/conversion.cpp | 24 +- .../src/trajectory/interpolation.cpp | 8 +- .../src/trajectory/path_with_lane_id.cpp | 16 +- .../src/trajectory/trajectory.cpp | 601 ++++++++--------- .../test/src/resample/test_resample.cpp | 100 +-- .../benchmark_calcLateralOffset.cpp | 2 +- .../src/trajectory/test_interpolation.cpp | 20 +- .../src/trajectory/test_path_with_lane_id.cpp | 6 +- .../test/src/trajectory/test_trajectory.cpp | 22 +- .../test_vehicle_state_checker_helper.hpp | 8 +- .../object_recognition_utils/conversion.hpp | 12 +- .../object_recognition_utils/geometry.hpp | 15 +- .../object_classification.hpp | 4 +- .../predicted_path_utils.hpp | 16 +- common/object_recognition_utils/package.xml | 2 +- .../src/conversion.cpp | 10 +- .../src/predicted_path_utils.cpp | 15 +- .../test/src/test_conversion.cpp | 12 +- .../test/src/test_geometry.cpp | 6 +- .../test/src/test_matching.cpp | 68 +- .../test/src/test_object_classification.cpp | 46 +- .../test/src/test_predicted_path_utils.cpp | 2 +- common/path_distance_calculator/Readme.md | 8 +- common/path_distance_calculator/package.xml | 2 +- .../src/path_distance_calculator.cpp | 4 +- .../src/path_distance_calculator.hpp | 6 +- .../geometry/boost_polygon_utils.hpp | 18 +- .../geometry/geometry.hpp | 40 +- common/tier4_autoware_utils/package.xml | 7 +- .../src/geometry/boost_polygon_utils.cpp | 22 +- .../src/geometry/test_boost_polygon_utils.cpp | 28 +- .../test/src/geometry/test_geometry.cpp | 20 +- .../test_path_with_lane_id_geometry.cpp | 10 +- .../src/tools/car_pose.hpp | 4 +- .../src/tools/interactive_object.hpp | 4 +- .../src/tools/pedestrian_pose.hpp | 4 +- .../src/tools/unknown_pose.hpp | 4 +- common/tier4_planning_rviz_plugin/README.md | 10 +- .../path/display.hpp | 16 +- .../path/display_base.hpp | 2 +- common/tier4_planning_rviz_plugin/package.xml | 1 - .../plugins/plugin_description.xml | 6 +- .../src/path/display.cpp | 5 +- common/tier4_state_rviz_plugin/README.md | 2 +- common/tier4_state_rviz_plugin/package.xml | 3 +- .../src/include/autoware_state_panel.hpp | 6 +- common/tier4_system_rviz_plugin/README.md | 6 +- common/tier4_system_rviz_plugin/package.xml | 2 +- .../src/mrm_summary_overlay_display.cpp | 8 +- .../src/mrm_summary_overlay_display.hpp | 8 +- .../tier4_traffic_light_rviz_plugin/README.md | 6 +- .../package.xml | 2 +- .../src/traffic_light_publish_panel.cpp | 98 +-- .../src/traffic_light_publish_panel.hpp | 20 +- common/tier4_vehicle_rviz_plugin/README.md | 12 +- common/tier4_vehicle_rviz_plugin/package.xml | 2 +- .../src/tools/console_meter.cpp | 2 +- .../src/tools/console_meter.hpp | 8 +- .../src/tools/steering_angle.cpp | 2 +- .../src/tools/steering_angle.hpp | 8 +- .../src/tools/turn_signal.cpp | 8 +- .../src/tools/turn_signal.hpp | 10 +- .../src/tools/velocity_history.cpp | 4 +- .../src/tools/velocity_history.hpp | 12 +- .../Readme.md | 8 +- .../package.xml | 4 +- ...fic_light_recognition_marker_publisher.cpp | 14 +- ...fic_light_recognition_marker_publisher.hpp | 16 +- .../traffic_light_utils.hpp | 10 +- .../src/traffic_light_utils.cpp | 16 +- 123 files changed, 1419 insertions(+), 1431 deletions(-) create mode 100644 common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt index aafc62fc90d25..95224b9967d4b 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -68,7 +68,7 @@ pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_rendering - autoware_auto_vehicle_msgs + autoware_vehicle_msgs tier4_planning_msgs autoware_perception_msgs ) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md index 0d0def1a46997..943d566ad109c 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -15,13 +15,13 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | Name | Type | Description | | ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------ | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | -| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | +| `/vehicle/status/turn_indicators_status` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic is status of gear | | `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | The topic is status of traffic light | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The topic is status of traffic light | ## Parameter diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp index 3fe473d5d5123..cb7e49685d0b3 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_vehicle_msgs/msg/gear_report.hpp" #include #include @@ -37,7 +37,7 @@ class GearDisplay public: GearDisplay(); void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); private: int current_gear_; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index ec1acb9a5dc68..0831ded468c99 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -95,29 +95,29 @@ private Q_SLOTS: std::unique_ptr traffic_display_; std::unique_ptr speed_limit_display_; - rclcpp::Subscription::SharedPtr gear_sub_; - rclcpp::Subscription::SharedPtr steering_sub_; - rclcpp::Subscription::SharedPtr speed_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr turn_signals_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr hazard_lights_sub_; - rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr + traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp index 0669028dba3b2..2ae8e9a3fe0b9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -37,7 +37,7 @@ class SpeedDisplay public: SpeedDisplay(); void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_speed_; // Internal variable to store current speed diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index b59f4acf380db..fcdb293fe8c72 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -39,7 +39,7 @@ class SpeedLimitDisplay SpeedLimitDisplay(); void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_limit; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index 121ee9a0a4d84..dada3cddab911 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include #include @@ -37,8 +37,7 @@ class SteeringWheelDisplay public: SteeringWheelDisplay(); void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); private: float steering_angle_ = 0.0f; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index efa30f37ccb3e..fd15f542021f1 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -23,8 +23,9 @@ #include #include -#include -#include +#include +#include +#include #include #include @@ -39,8 +40,8 @@ class TrafficDisplay TrafficDisplay(); void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg); - autoware_perception_msgs::msg::TrafficSignal current_traffic_; + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_; private: QImage traffic_light_image_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp index ca10c92c06a3e..022de6d8c22c9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -41,9 +41,9 @@ class TurnSignalsDisplay TurnSignalsDisplay(); void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); private: QImage arrowImage; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index 4d2f53e1e27ed..73e0dbea0866e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -11,8 +11,8 @@ BSD-3-Clause - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_vehicle_msgs boost rviz_common rviz_ogre_vendor diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp index 825c7c1620e2c..956172bef6ed6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -49,8 +49,7 @@ GearDisplay::GearDisplay() : current_gear_(0) } } -void GearDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +void GearDisplay::updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { current_gear_ = msg->report; // Assuming msg->report contains the gear information } @@ -60,19 +59,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun // we deal with the different gears here std::string gearString; switch (current_gear_) { - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: gearString = "N"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + case autoware_vehicle_msgs::msg::GearReport::LOW: + case autoware_vehicle_msgs::msg::GearReport::LOW_2: gearString = "L"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::NONE: - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + case autoware_vehicle_msgs::msg::GearReport::NONE: + case autoware_vehicle_msgs::msg::GearReport::PARK: gearString = "P"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_vehicle_msgs::msg::GearReport::REVERSE_2: gearString = "R"; break; // all the drive gears from DRIVE to DRIVE_16 diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 9add6336ece46..ccfdaa69f5d2a 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -76,31 +76,29 @@ void SignalDisplay::onInitialize() auto rviz_ros_node = context_->getRosNodeAbstraction(); gear_topic_property_ = std::make_unique( - "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_vehicle_msgs/msg/GearReport", "Topic for Gear Data", this, SLOT(topic_updated_gear())); gear_topic_property_->initialize(rviz_ros_node); turn_signals_topic_property_ = std::make_unique( "Turn Signals Topic", "/vehicle/status/turn_indicators_status", - "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + "autoware_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, SLOT(topic_updated_turn_signals())); turn_signals_topic_property_->initialize(rviz_ros_node); speed_topic_property_ = std::make_unique( - "Speed Topic", "/vehicle/status/velocity_status", - "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, - SLOT(topic_updated_speed())); + "Speed Topic", "/vehicle/status/velocity_status", "autoware_vehicle_msgs/msg/VelocityReport", + "Topic for Speed Data", this, SLOT(topic_updated_speed())); speed_topic_property_->initialize(rviz_ros_node); steering_topic_property_ = std::make_unique( - "Steering Topic", "/vehicle/status/steering_status", - "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, - SLOT(topic_updated_steering())); + "Steering Topic", "/vehicle/status/steering_status", "autoware_vehicle_msgs/msg/SteeringReport", + "Topic for Steering Data", this, SLOT(topic_updated_steering())); steering_topic_property_->initialize(rviz_ros_node); hazard_lights_topic_property_ = std::make_unique( "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", - "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + "autoware_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, SLOT(topic_updated_hazard_lights())); hazard_lights_topic_property_->initialize(rviz_ros_node); @@ -111,10 +109,9 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_->initialize(rviz_ros_node); traffic_topic_property_ = std::make_unique( - "Traffic Topic", - "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", - "autoware_perception/msgs/msg/TrafficSignal", "Topic for Traffic Light Data", this, - SLOT(topic_updated_traffic())); + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data", + this, SLOT(topic_updated_traffic())); traffic_topic_property_->initialize(rviz_ros_node); } @@ -192,7 +189,7 @@ void SignalDisplay::onDisable() } void SignalDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -213,7 +210,7 @@ void SignalDisplay::updateSpeedLimitData( } void SignalDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -224,7 +221,7 @@ void SignalDisplay::updateHazardLightsData( } void SignalDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -235,7 +232,7 @@ void SignalDisplay::updateGearData( } void SignalDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -246,7 +243,7 @@ void SignalDisplay::updateSteeringData( } void SignalDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -258,7 +255,7 @@ void SignalDisplay::updateSpeedData( } void SignalDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -372,11 +369,10 @@ void SignalDisplay::topic_updated_gear() gear_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); gear_sub_ = - rviz_ros_node->get_raw_node()->create_subscription( - gear_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { - updateGearData(msg); - }); + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { updateGearData(msg); }); } void SignalDisplay::topic_updated_steering() @@ -384,12 +380,13 @@ void SignalDisplay::topic_updated_steering() // resubscribe to the topic steering_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - steering_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - steering_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { - updateSteeringData(msg); - }); + steering_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); } void SignalDisplay::topic_updated_speed() @@ -397,12 +394,13 @@ void SignalDisplay::topic_updated_speed() // resubscribe to the topic speed_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - speed_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - speed_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { - updateSpeedData(msg); - }); + speed_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); } void SignalDisplay::topic_updated_speed_limit() @@ -427,9 +425,10 @@ void SignalDisplay::topic_updated_turn_signals() turn_signals_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - turn_signals_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { updateTurnSignalsData(msg); }); } @@ -442,9 +441,10 @@ void SignalDisplay::topic_updated_hazard_lights() hazard_lights_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - hazard_lights_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { updateHazardLightsData(msg); }); } @@ -454,12 +454,14 @@ void SignalDisplay::topic_updated_traffic() // resubscribe to the topic traffic_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - traffic_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - traffic_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) { - updateTrafficLightData(msg); - }); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); } } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 5c5342259005b..b9c22ec5f53ac 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -50,7 +50,7 @@ SpeedDisplay::SpeedDisplay() : current_speed_(0.0) } void SpeedDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in @@ -64,7 +64,7 @@ void SpeedDisplay::updateSpeedData( } // void SpeedDisplay::processMessage(const -// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) // { // try // { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index 4c83b4a73c0c2..3dec6a8e7d4a0 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -57,7 +57,7 @@ void SpeedLimitDisplay::updateSpeedLimitData( } void SpeedLimitDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { float speed = msg->longitudinal_velocity; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index 45ebcde086165..d2390b16e5373 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -56,7 +56,7 @@ SteeringWheelDisplay::SteeringWheelDisplay() } void SteeringWheelDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { try { // Assuming msg->steering_angle is the field you're interested in diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index f6d232709a333..2dc9c23583a52 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay() } void TrafficDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg) { current_traffic_ = *msg; } @@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF backgroundRect.top() + circleRect.height() / 2)); painter.drawEllipse(circleRect); - if (!current_traffic_.elements.empty()) { - switch (current_traffic_.elements[0].color) { + if (!current_traffic_.traffic_light_groups.empty()) { + switch (current_traffic_.traffic_light_groups[0].elements[0].color) { case 1: painter.setBrush(QBrush(tl_red_)); painter.drawEllipse(circleRect); diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp index 1aaa5871015a8..b42b5d953fcc6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -46,7 +46,7 @@ TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) } void TurnSignalsDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -58,7 +58,7 @@ void TurnSignalsDisplay::updateTurnSignalsData( } void TurnSignalsDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -80,11 +80,11 @@ void TurnSignalsDisplay::drawArrows( int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175; bool leftActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); bool rightActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); // Color the arrows based on the state of the turn signals and hazard lights by having them blink // on and off diff --git a/common/autoware_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md index cb5deb48f411c..ed6f3e1675ace 100644 --- a/common/autoware_perception_rviz_plugin/README.md +++ b/common/autoware_perception_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_auto_perception_plugin +# autoware_perception_rviz_plugin ## Purpose @@ -19,9 +19,9 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | +| Name | Type | Description | +| ---- | ------------------------------------------------ | ---------------------- | +| | `autoware_perception_msgs::msg::DetectedObjects` | detection result array | #### Visualization Result @@ -31,9 +31,9 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ---------------------------------------------------- | --------------------- | -| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | +| Name | Type | Description | +| ---- | ----------------------------------------------- | --------------------- | +| | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array | #### Visualization Result @@ -45,9 +45,9 @@ Overwrite tracking results with detection results. #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------------ | ----------------------- | -| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | +| Name | Type | Description | +| ---- | ------------------------------------------------- | ----------------------- | +| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | #### Visualization Result diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp index b5e0f5c548c31..4dcc4e9ea1545 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -16,7 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include namespace autoware { @@ -26,12 +26,12 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize DetectedObjects class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; + using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; DetectedObjectsDisplay(); diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index e1a911b55f719..8d29900e1da26 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -22,10 +22,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -62,21 +62,19 @@ enum class ObjectFillType { Skeleton, Fill }; // Map defining colors according to value of label field in ObjectClassification msg const std::map< - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> + autoware_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> // Color map is based on cityscapes color kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {autoware_perception_msgs::msg::ObjectClassification::UNKNOWN, {"UNKNOWN", {255, 255, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN, {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {autoware_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + {autoware_perception_msgs::msg::ObjectClassification::TRAILER, {"TRAILER", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param shape_msg Shape msg to be converted. Corners should be in object-local frame @@ -87,7 +85,7 @@ const std::map< /// \return Marker ptr. Id and header will have to be set by the caller AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available = true, @@ -95,7 +93,7 @@ get_shape_marker_ptr( AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available = true); @@ -162,13 +160,13 @@ get_yaw_rate_covariance_marker_ptr( AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( @@ -183,35 +181,35 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( @@ -219,15 +217,15 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( std::vector & points, const int n); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point @@ -276,7 +274,7 @@ inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose( /// \return Id of the best classification, Unknown if there is no best label template AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type + autoware_perception_msgs::msg::ObjectClassification::_label_type get_best_label(ClassificationContainerT labels, const std::string & logger_name) { const auto best_class_label = std::max_element( @@ -287,7 +285,7 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC rclcpp::get_logger(logger_name), "Empty classification field. " "Treating as unknown"); - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; } return best_class_label->label; } diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 2d86d8dd09db8..b05ba5f27f551 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -53,7 +53,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase using Color = std::array; using Marker = visualization_msgs::msg::Marker; using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; - using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; + using ObjectClassificationMsg = autoware_perception_msgs::msg::ObjectClassification; using RosTopicDisplay = rviz_common::RosTopicDisplay; using PolygonPropertyMap = @@ -189,7 +189,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \return Marker ptr. Id and header will have to be set by the caller template std::optional get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const ClassificationContainerT & labels, const double & line_width, const bool & is_orientation_available) const @@ -212,7 +212,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available); @@ -363,8 +363,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_predicted_paths_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); @@ -379,7 +379,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_path_confidence_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_path_confidence_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 0501d2ab3456d..1445f02e34290 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -16,7 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -39,12 +39,12 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); ~PredictedObjectsDisplay() diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 180494d742144..9ccaa5cd150f6 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -16,7 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -34,13 +34,13 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; - using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; + using TrackedObject = autoware_perception_msgs::msg::TrackedObject; + using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml index 66b7fafc39de5..460186c33b7d8 100644 --- a/common/autoware_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -18,7 +18,7 @@ libboost-dev qtbase5-dev - autoware_auto_perception_msgs + autoware_perception_msgs rviz_common rviz_default_plugins diff --git a/common/autoware_perception_rviz_plugin/plugins_description.xml b/common/autoware_perception_rviz_plugin/plugins_description.xml index 50a0b5e5aff56..710e3aa2bfa26 100644 --- a/common/autoware_perception_rviz_plugin/plugins_description.xml +++ b/common/autoware_perception_rviz_plugin/plugins_description.xml @@ -6,21 +6,21 @@ Convert a PredictedObjects to markers and display them. - autoware_auto_perception_msgs/msg/PredictedObjects + autoware_perception_msgs/msg/PredictedObjects Convert a TrackedObjects to markers and display them. - autoware_auto_perception_msgs/msg/TrackedObjects + autoware_perception_msgs/msg/TrackedObjects Convert a DetectedObjects to markers and display them. - autoware_auto_perception_msgs/msg/DetectedObjects + autoware_perception_msgs/msg/DetectedObjects diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 335a323d4ecd5..9dd0b2923f09f 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -40,7 +40,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 009091f33c272..1d06454582a2f 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -50,7 +50,7 @@ namespace detail using Marker = visualization_msgs::msg::Marker; visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color) { auto marker_ptr = std::make_shared(); @@ -70,8 +70,8 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) { auto marker_ptr = std::make_shared(); @@ -492,7 +492,7 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available, const ObjectFillType fill_type) @@ -502,7 +502,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->color = color_rgba; marker_ptr->scale.x = line_width; - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { if (fill_type == ObjectFillType::Skeleton) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -542,7 +542,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available) @@ -550,7 +550,7 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); @@ -597,7 +597,7 @@ void calc_line_list_from_points( } void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -620,7 +620,7 @@ void calc_bounding_box_line_list( } void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { // direction triangle @@ -646,7 +646,7 @@ void calc_bounding_box_direction_line_list( } void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -671,7 +671,7 @@ void calc_bounding_box_orientation_line_list( } void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -697,7 +697,7 @@ void calc_2d_bounding_box_bottom_line_list( } void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -721,7 +721,7 @@ void calc_2d_bounding_box_bottom_direction_line_list( } void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -746,7 +746,7 @@ void calc_2d_bounding_box_bottom_orientation_line_list( } void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -789,7 +789,7 @@ void calc_cylinder_line_list( } void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -837,7 +837,7 @@ void calc_circle_line_list( } void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -890,7 +890,7 @@ void calc_polygon_line_list( } void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -917,7 +917,7 @@ void calc_2d_polygon_bottom_line_list( } void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple) { const int circle_line_num = is_simple ? 5 : 10; diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 2a81732d93b95..d11aba912854d 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -295,7 +295,7 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) lock.unlock(); - ObjectPolygonDisplayBase::update( + ObjectPolygonDisplayBase::update( wall_dt, ros_dt); } diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index e8891e2bae897..214ed9ce70e63 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -61,7 +61,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp index 8665b9c35157d..0c72dbdb12078 100644 --- a/common/component_interface_specs/include/component_interface_specs/perception.hpp +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -17,14 +17,14 @@ #include -#include +#include namespace perception_interface { struct ObjectRecognition { - using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + using Message = autoware_perception_msgs::msg::PredictedObjects; static constexpr char name[] = "/perception/object_recognition/objects"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 9efd8c871f68f..58aba53d8ac8a 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include @@ -66,7 +66,7 @@ struct LaneletRoute struct Trajectory { - using Message = autoware_auto_planning_msgs::msg::Trajectory; + using Message = autoware_planning_msgs::msg::Trajectory; static constexpr char name[] = "/planning/scenario_planning/trajectory"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp index 6358b35c3c674..e7ce2c5212a25 100644 --- a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp +++ b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp @@ -20,10 +20,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace vehicle_interface @@ -31,7 +31,7 @@ namespace vehicle_interface struct SteeringStatus { - using Message = autoware_auto_vehicle_msgs::msg::SteeringReport; + using Message = autoware_vehicle_msgs::msg::SteeringReport; static constexpr char name[] = "/vehicle/status/steering_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -40,7 +40,7 @@ struct SteeringStatus struct GearStatus { - using Message = autoware_auto_vehicle_msgs::msg::GearReport; + using Message = autoware_vehicle_msgs::msg::GearReport; static constexpr char name[] = "/vehicle/status/gear_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -49,7 +49,7 @@ struct GearStatus struct TurnIndicatorStatus { - using Message = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using Message = autoware_vehicle_msgs::msg::TurnIndicatorsReport; static constexpr char name[] = "/vehicle/status/turn_indicators_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -58,7 +58,7 @@ struct TurnIndicatorStatus struct HazardLightStatus { - using Message = autoware_auto_vehicle_msgs::msg::HazardLightsReport; + using Message = autoware_vehicle_msgs::msg::HazardLightsReport; static constexpr char name[] = "/vehicle/status/hazard_lights_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 13f135c925258..fa57bb1641eed 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -12,10 +12,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs nav_msgs rcl rclcpp diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index e065f332b75e4..163bd761407c5 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,6 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include #include #include @@ -29,7 +28,7 @@ #include #include -using autoware::common::types::bool8_t; +using bool8_t = bool; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; using FakeNodeFixtureParametrized = autoware::tools::testing::FakeTestNodeParametrized; diff --git a/common/goal_distance_calculator/Readme.md b/common/goal_distance_calculator/Readme.md index 3a25e147cf8e6..062b23baabe47 100644 --- a/common/goal_distance_calculator/Readme.md +++ b/common/goal_distance_calculator/Readme.md @@ -10,10 +10,10 @@ This node publishes deviation of self-pose from goal pose. ### Input -| Name | Type | Description | -| ---------------------------------- | ----------------------------------------- | --------------------- | -| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::Route` | Used to get goal pose | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ---------------------------------- | ------------------------------------ | --------------------- | +| `/planning/mission_planning/route` | `autoware_planning_msgs::msg::Route` | Used to get goal pose | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index b4a98d90c74e4..3329c1349b707 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ struct Param struct Input { geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; - autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route; + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route; }; struct Output diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 50051d928b6c1..680c4a3cdfff1 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -46,14 +46,14 @@ class GoalDistanceCalculatorNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_initial_pose_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - autoware_auto_planning_msgs::msg::Route::SharedPtr route_; + autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_; // Callback - void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg); + void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); // Publisher tier4_autoware_utils::DebugPublisher debug_publisher_; diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index 49cb674f0a256..eb71dc45f31a3 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -10,7 +10,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs geometry_msgs rclcpp rclcpp_components diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index 1405d5bd62f7d..719baef283791 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - tier4_autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose); + tier4_autoware_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index c6062d2a156ee..ab1c35e246719 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -48,9 +48,9 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_->setParam(param_); // Subscriber - sub_route_ = create_subscription( + sub_route_ = create_subscription( "/planning/mission_planning/route", queue_size, - [&](const autoware_auto_planning_msgs::msg::Route::SharedPtr msg_ptr) { route_ = msg_ptr; }); + [&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; }); // Wait for first self pose self_pose_listener_.waitForFirstPose(); diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index a7d7012c19e22..09973826387a2 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -199,7 +199,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; std::vector points; diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md index 925ee5b162db3..fd18540a9d0cf 100644 --- a/common/motion_utils/README.md +++ b/common/motion_utils/README.md @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`. ```cpp size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); ``` @@ -112,6 +112,6 @@ const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_ ## For developers -Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. +Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. `motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/docs/vehicle/vehicle.md b/common/motion_utils/docs/vehicle/vehicle.md index 9d33b5ed372c4..c768e6115731c 100644 --- a/common/motion_utils/docs/vehicle/vehicle.md +++ b/common/motion_utils/docs/vehicle/vehicle.md @@ -83,10 +83,10 @@ This class check whether the vehicle arrive at stop point based on localization #### Subscribed Topics -| Name | Type | Description | -| ---------------------------------------- | ---------------------------------------------- | ---------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | -| `/planning/scenario_planning/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | trajectory | +| Name | Type | Description | +| ---------------------------------------- | ----------------------------------------- | ---------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | +| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory | #### Parameters diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 258386ca236a3..91322360ce5b8 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,9 +15,9 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -113,8 +113,8 @@ std::vector resamplePoseVector( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -136,11 +136,10 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, - const bool resample_input_path_stop_point = true); +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a path. Note that in a default setting, position xy are @@ -159,8 +158,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -181,8 +180,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_path_stop_point = true); @@ -205,8 +204,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation * @return resampled trajectory */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); @@ -230,10 +229,10 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( * trajectory * @return resampled trajectory */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 7d4be216e89fe..8d009730a925d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -15,52 +15,52 @@ #ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "autoware_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include namespace motion_utils { -using TrajectoryPoints = std::vector; +using TrajectoryPoints = std::vector; /** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. * @attention This function just clips - * std::vector up to the capacity of Trajectory. + * std::vector up to the capacity of Trajectory. * Therefore, the error handling out of this function is necessary if the size of the input greater * than the capacity. * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, const std_msgs::msg::Header & header = std_msgs::msg::Header{}); /** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); template -autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); throw std::logic_error("Only specializations of convertToPath can be used."); } template <> -inline autoware_auto_planning_msgs::msg::Path convertToPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +inline autoware_planning_msgs::msg::Path convertToPath( + const tier4_planning_msgs::msg::PathWithLaneId & input) { - autoware_auto_planning_msgs::msg::Path output{}; + autoware_planning_msgs::msg::Path output{}; output.header = input.header; output.left_bound = input.left_bound; output.right_bound = input.right_bound; @@ -80,11 +80,11 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) template <> inline TrajectoryPoints convertToTrajectoryPoints( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) + const tier4_planning_msgs::msg::PathWithLaneId & input) { TrajectoryPoints output{}; for (const auto & p : input.points) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + autoware_planning_msgs::msg::TrajectoryPoint tp; tp.pose = p.point.pose; tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; // since path point doesn't have acc for now @@ -95,20 +95,19 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - [[maybe_unused]] const T & input) +tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); } template <> -inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( const TrajectoryPoints & input) { - autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + tier4_planning_msgs::msg::PathWithLaneId output{}; for (const auto & p : input) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + tier4_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose = p.pose; pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; output.points.emplace_back(pp); diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index aec329f9f540e..0a0b14cb7488d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -17,8 +17,10 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" + +#include #include #include @@ -34,8 +36,8 @@ namespace motion_utils * twist information * @return resampled path(poses) */ -autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, +autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( + const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -49,8 +51,8 @@ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( * twist information * @return resampled path(poses) */ -autoware_auto_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index d0ed761b76d9a..d6d4fbf559323 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -23,23 +23,23 @@ namespace motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle // center follow the input path // @param [in] path with position to be followed by the center of the vehicle // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle // center follow the input path NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp new file mode 100644 index 0000000000000..1eb0a43976daa --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include + +namespace motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory); + +/** + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); + +} // namespace motion_utils + +#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 7e2d92c9434fb..3715f466e7684 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -56,14 +56,12 @@ void validateNonEmpty(const T & points) } } -extern template void validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); /** * @brief validate a point is in a non-sharp angle between two points or not @@ -114,14 +112,14 @@ std::optional isDrivingForward(const T & points) } extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); /** * @brief checks whether a path of trajectory has forward driving direction using its longitudinal @@ -149,14 +147,14 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) } extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); /** * @brief remove overlapping points through points container. @@ -194,17 +192,16 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) return dst; } -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); /** @@ -237,8 +234,8 @@ std::optional searchZeroVelocityIndex( } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); /** @@ -262,8 +259,8 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx); /** @@ -279,8 +276,8 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist) } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); +searchZeroVelocityIndex>( + const std::vector & points_with_twist); /** * @brief find nearest point index through points container for a given point. @@ -310,16 +307,14 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin return min_idx; } -extern template size_t findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); /** @@ -378,18 +373,18 @@ std::optional findNearestIndex( } extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -462,19 +457,17 @@ double calcLongitudinalOffsetToSegment( } extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); /** * @brief find nearest segment index to point. @@ -505,17 +498,16 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point return nearest_idx; } -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +extern template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); /** @@ -557,18 +549,18 @@ std::optional findNearestSegmentIndex( } extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -631,18 +623,17 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } -extern template double calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); @@ -695,16 +686,15 @@ double calcLateralOffset( return calcLateralOffset(points, p_target, seg_idx, throw_exception); } -extern template double calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); /** @@ -738,18 +728,17 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t return dist_sum; } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); /** * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, @@ -789,17 +778,17 @@ std::vector calcSignedArcLengthPartialSum( } extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); /** * @brief calculate length of 2D distance between two points, specified by start point and end point @@ -831,17 +820,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); /** @@ -868,18 +856,17 @@ double calcSignedArcLength( return -calcSignedArcLength(points, dst_point, src_idx); } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); /** * @brief calculate length of 2D distance between two points, specified by start point and end @@ -916,17 +903,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); /** @@ -947,14 +933,12 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } -extern template double calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); /** * @brief calculate curvature through points container. @@ -986,14 +970,14 @@ std::vector calcCurvature(const T & points) } extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); /** * @brief calculate curvature through points container and length of 2d distance for segment used @@ -1026,14 +1010,14 @@ std::vector> calcCurvatureAndArcLength(const T & point } extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); /** * @brief calculate length of 2D distance between given start point index in points container and @@ -1063,8 +1047,8 @@ std::optional calcDistanceToForwardStopPoint( } extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const size_t src_idx = 0); /** @@ -1139,17 +1123,17 @@ std::optional calcLongitudinalOffsetPoint( } extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); /** * @brief calculate the point offset from source point along the trajectory (or path) (points @@ -1184,16 +1168,16 @@ std::optional calcLongitudinalOffsetPoint( } extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); /** @@ -1284,20 +1268,20 @@ std::optional calcLongitudinalOffsetPose( } extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); /** * @brief calculate the point offset from source point along the trajectory (or path) (points @@ -1331,18 +1315,18 @@ std::optional calcLongitudinalOffsetPose( } extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); @@ -1433,19 +1417,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1488,19 +1472,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1561,19 +1545,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1614,21 +1598,21 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); @@ -1666,19 +1650,19 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1715,19 +1699,19 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1770,21 +1754,21 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); @@ -1817,9 +1801,9 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1859,10 +1843,10 @@ std::optional insertDecelPoint( } extern template std::optional -insertDecelPoint>( +insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, - std::vector & points_with_twist); + std::vector & points_with_twist); /** * @brief Insert orientation to each point in points container (trajectory, path, ...) @@ -1901,15 +1885,13 @@ void insertOrientation(T & points, const bool is_driving_forward) } } -extern template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); /** @@ -1968,19 +1950,18 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); @@ -2009,17 +1990,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); /** @@ -2047,18 +2027,17 @@ double calcSignedArcLength( return signed_length_on_traj + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); /** * @brief find first nearest point index through points container for a given pose with soft @@ -2149,20 +2128,20 @@ size_t findFirstNearestIndexWithSoftConstraints( } extern template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2205,20 +2184,20 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints( } extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2270,18 +2249,18 @@ std::optional calcDistanceToForwardStopPoint( } extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -2309,19 +2288,19 @@ T cropForwardPoints( return points; } -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); @@ -2349,19 +2328,19 @@ T cropBackwardPoints( return points; } -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); @@ -2391,19 +2370,19 @@ T cropPoints( return cropped_points; } -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); @@ -2459,16 +2438,14 @@ double calcYawDeviation( return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); } -extern template double calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); /** @@ -2495,18 +2472,16 @@ bool isTargetPointFront( return s_target - s_base > threshold; } -extern template bool isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 82433d8e3c241..859bbdcc851ea 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -26,7 +26,7 @@ namespace motion_utils { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index dec5287b0a520..21471a4a6a75e 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -22,8 +22,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_planning_msgs + autoware_vehicle_msgs builtin_interfaces geometry_msgs interpolation @@ -32,6 +32,7 @@ tf2 tf2_geometry_msgs tier4_autoware_utils + tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index 20742af0b6c0c..6878c056b6f7a 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -15,9 +15,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace motion_utils { @@ -36,14 +36,14 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, - const Pose &, const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, - const Pose &, const VelocityFactorStatus, const std::string &); } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 834d07a87ea09..4a4e8dff1e4ad 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -181,8 +181,8 @@ std::vector resamplePoseVector( return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -333,13 +333,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( return input_path; } - autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path; + tier4_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; + autoware_planning_msgs::msg::PathPoint path_point; path_point.pose = interpolated_pose.at(i); path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); path_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -352,9 +352,9 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( return resampled_path; } -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments @@ -363,7 +363,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } // transform input_path - std::vector transformed_input_path( + std::vector transformed_input_path( input_path.points.size()); for (size_t i = 0; i < input_path.points.size(); ++i) { transformed_input_path.at(i) = input_path.points.at(i).point; @@ -418,8 +418,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( use_zero_order_hold_for_v); } -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -483,13 +483,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath( return input_path; } - autoware_auto_planning_msgs::msg::Path resampled_path; + autoware_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; + autoware_planning_msgs::msg::PathPoint path_point; path_point.pose = interpolated_pose.at(i); path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); path_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -500,8 +500,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( return resampled_path; } -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) { @@ -559,8 +559,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( use_zero_order_hold_for_twist); } -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { @@ -646,11 +646,11 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( return input_trajectory; } - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; resampled_trajectory.header = input_trajectory.header; resampled_trajectory.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; traj_point.pose = interpolated_pose.at(i); traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -665,9 +665,9 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( return resampled_trajectory; } -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { // validate arguments diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp index f198003d84091..e3a221c61a2a7 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -19,34 +19,34 @@ namespace motion_utils { /** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. * @attention This function just clips - * std::vector up to the capacity of Trajectory. + * std::vector up to the capacity of Trajectory. * Therefore, the error handling out of this function is necessary if the size of the input greater * than the capacity. * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, const std_msgs::msg::Header & header) { - autoware_auto_planning_msgs::msg::Trajectory output{}; + autoware_planning_msgs::msg::Trajectory output{}; output.header = header; for (const auto & pt : trajectory) output.points.push_back(pt); return output; } /** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory) { - std::vector output(trajectory.points.size()); + std::vector output(trajectory.points.size()); std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); return output; } diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 6ee3e665f746a..13df39daab59d 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -17,10 +17,10 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; namespace motion_utils { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index c7e85554dbddd..4c86135811003 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -25,7 +25,7 @@ namespace motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,8 +56,8 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); if (opt_range) { @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,8 +77,8 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp index 51c07a75076c8..42c750e3c109f 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -18,607 +18,584 @@ namespace motion_utils { // -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); // +template std::optional isDrivingForward>( + const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); // template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); // -template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); +searchZeroVelocityIndex>( + const std::vector & points_with_twist); // -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); // template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); // -template size_t findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); // template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); // -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); // template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); // -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); // +template std::vector calcCurvature>( + const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); // template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); // template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const size_t src_idx); // template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); // template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); // template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); // template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist, const double max_yaw, const double overlap_threshold); + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); + std::vector & points_with_twist, const double max_dist, + const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); // template std::optional -insertStopPoint>( +insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); // template std::optional -insertDecelPoint>( +insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, - std::vector & points_with_twist); + std::vector & points_with_twist); // -template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); // template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); // -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); // -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); // -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double -calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); // -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool -isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index 373a12c5bbd76..ef374f0b484db 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -27,15 +27,15 @@ namespace { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -297,7 +297,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -403,7 +403,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -437,7 +437,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -470,7 +470,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -504,7 +504,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -543,7 +543,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) using motion_utils::resamplePath; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -651,7 +651,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -775,7 +775,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -852,7 +852,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -931,7 +931,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -1626,7 +1626,7 @@ TEST(resample_path, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -1708,7 +1708,7 @@ TEST(resample_path, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -1742,7 +1742,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1776,7 +1776,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1815,7 +1815,7 @@ TEST(resample_path, resample_path_by_vector_backward) using motion_utils::resamplePath; { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -1896,7 +1896,7 @@ TEST(resample_path, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -1994,7 +1994,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2053,7 +2053,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2115,7 +2115,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2179,7 +2179,7 @@ TEST(resample_path, resample_path_by_same_interval) // Same point resampling { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2241,7 +2241,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2269,7 +2269,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2314,7 +2314,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2360,7 +2360,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2390,7 +2390,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2532,7 +2532,7 @@ TEST(resample_path, resample_path_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -2565,7 +2565,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2598,7 +2598,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2698,7 +2698,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2787,7 +2787,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -2821,7 +2821,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2855,7 +2855,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2895,7 +2895,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2959,7 +2959,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( @@ -3025,7 +3025,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp twist { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3094,7 +3094,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Same point resampling { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3160,7 +3160,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3190,7 +3190,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3238,7 +3238,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3287,7 +3287,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3319,7 +3319,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3470,7 +3470,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -3503,7 +3503,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3536,7 +3536,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 549ca4c0ae5bb..7aae860b76533 100644 --- a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -22,7 +22,7 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 5794ed61e9e44..90b11e535c486 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -27,13 +27,13 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -127,7 +127,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) using motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -319,7 +319,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) // Duplicated Points { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -351,7 +351,7 @@ TEST(Interpolation, interpolate_path_for_path) using motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -543,7 +543,7 @@ TEST(Interpolation, interpolate_points_with_length) using motion_utils::calcInterpolatedPose; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -631,7 +631,7 @@ TEST(Interpolation, interpolate_points_with_length) // one point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index a65147a65b12d..6dd60c499d181 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -22,9 +22,9 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::createPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -54,8 +54,8 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using autoware_auto_planning_msgs::msg::PathWithLaneId; using motion_utils::getPathIndexRangeWithLaneId; + using tier4_planning_msgs::msg::PathWithLaneId; // Usual cases { diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 8e84b111b0688..9ebc712b3bf1a 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -26,8 +26,8 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPointArray = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPointArray = std::vector; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -69,7 +69,7 @@ TrajectoryPointArray generateTestTrajectoryPointArray( const size_t num_points, const double point_interval, const double vel = 0.0, const double init_theta = 0.0, const double delta_theta = 0.0) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPointArray traj; for (size_t i = 0; i < num_points; ++i) { const double theta = init_theta + i * delta_theta; @@ -106,7 +106,7 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; @@ -135,7 +135,7 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::validateNonSharpAngle; using tier4_autoware_utils::pi; @@ -1299,7 +1299,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; @@ -1385,7 +1385,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; @@ -1563,7 +1563,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; @@ -1637,7 +1637,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; @@ -5393,7 +5393,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) TEST(trajectory, calcYawDeviation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcYawDeviation; constexpr double tolerance = 1e-3; @@ -5420,7 +5420,7 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::isTargetPointFront; using tier4_autoware_utils::createPoint; diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp index aa26a64cb87ce..29802e70bfd5f 100644 --- a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp +++ b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp @@ -15,11 +15,11 @@ #ifndef VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ #define VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ -#include -#include +#include +#include -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; inline Trajectory generateTrajectoryWithStopPoint(const geometry_msgs::msg::Pose & goal_pose) { diff --git a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp index c6dd0104b1e11..6d07d161bf069 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp @@ -15,15 +15,15 @@ #ifndef OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ #define OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ -#include -#include +#include +#include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object); DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); diff --git a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp index ae3013c728032..28cc9a786ecac 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp @@ -15,9 +15,9 @@ #ifndef OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ #define OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ -#include -#include -#include +#include +#include +#include #include namespace object_recognition_utils @@ -36,22 +36,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::Pose & p) } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::DetectedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::DetectedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::TrackedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::TrackedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::PredictedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::PredictedObject & obj) { return obj.kinematics.initial_pose_with_covariance.pose; } diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp index 9c0745bb28af9..4ffafc22339d6 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp @@ -15,14 +15,14 @@ #ifndef OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ #define OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; inline ObjectClassification getHighestProbClassification( const std::vector & object_classifications) diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 73cdce6c6444e..1cd838e090560 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ namespace object_recognition_utils * @return interpolated pose */ boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time); + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time); /** * @brief Resampling predicted path by time step vector. Note this function does not substitute @@ -44,8 +44,8 @@ boost::optional calcInterpolatedPose( * time_step*(num_of_path_points)] * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); @@ -57,10 +57,10 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( * @param sampling_horizon sampling time horizon * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, - const bool use_spline_for_xy = true, const bool use_spline_for_z = false); +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 796e276c376d6..fdf73e6b26057 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs interpolation libboost-dev diff --git a/common/object_recognition_utils/src/conversion.cpp b/common/object_recognition_utils/src/conversion.cpp index f6a25cee2c7d5..c8b8e6585f5a7 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/object_recognition_utils/src/conversion.cpp @@ -16,10 +16,10 @@ namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object) { @@ -43,7 +43,7 @@ DetectedObject toDetectedObject(const TrackedObject & tracked_object) DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects) { - autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + autoware_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = tracked_objects.header; for (auto & tracked_object : tracked_objects.objects) { diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index 81a22c98d88e5..fe9499b4eec33 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -23,7 +23,7 @@ namespace object_recognition_utils { boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) { // Check if relative time is in the valid range if (path.path.empty() || relative_time < 0.0) { @@ -45,8 +45,8 @@ boost::optional calcInterpolatedPose( return boost::none; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy, const bool use_spline_for_z) { @@ -83,7 +83,7 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); const auto interpolated_quat = slerp(quat); - autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + autoware_perception_msgs::msg::PredictedPath resampled_path; const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); resampled_path.confidence = path.confidence; resampled_path.path.resize(resampled_size); @@ -99,10 +99,9 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return resampled_path; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, - const bool use_spline_for_z) +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) { if (path.path.empty()) { throw std::invalid_argument("Predicted Path is empty"); diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/object_recognition_utils/test/src/test_conversion.cpp index 61fa0efd07d04..378f9a1245746 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/object_recognition_utils/test/src/test_conversion.cpp @@ -28,10 +28,10 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -42,10 +42,10 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat // NOTE: covariance is not checked TEST(conversion, test_toDetectedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toDetectedObject; - autoware_auto_perception_msgs::msg::TrackedObject tracked_obj; + autoware_perception_msgs::msg::TrackedObject tracked_obj; // existence probability tracked_obj.existence_probability = 1.0; // classification @@ -160,10 +160,10 @@ TEST(conversion, test_toDetectedObject) // NOTE: covariance is not checked TEST(conversion, test_toTrackedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toTrackedObject; - autoware_auto_perception_msgs::msg::DetectedObject detected_obj; + autoware_perception_msgs::msg::DetectedObject detected_obj; // existence probability detected_obj.existence_probability = 1.0; // classification diff --git a/common/object_recognition_utils/test/src/test_geometry.cpp b/common/object_recognition_utils/test/src/test_geometry.cpp index fe9f61b2424c6..ab3ba8fcec7d4 100644 --- a/common/object_recognition_utils/test/src/test_geometry.cpp +++ b/common/object_recognition_utils/test/src/test_geometry.cpp @@ -50,7 +50,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::DetectedObject p; + autoware_perception_msgs::msg::DetectedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::TrackedObject p; + autoware_perception_msgs::msg::TrackedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -90,7 +90,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::PredictedObject p; + autoware_perception_msgs::msg::PredictedObject p; p.kinematics.initial_pose_with_covariance.pose.position.x = x_ans; p.kinematics.initial_pose_with_covariance.pose.position.y = y_ans; p.kinematics.initial_pose_with_covariance.pose.position.z = z_ans; diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 8603dd54c433c..2ad34c3ae6bbe 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/object_recognition_utils/test/src/test_matching.cpp @@ -15,7 +15,7 @@ #include "object_recognition_utils/matching.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include @@ -37,7 +37,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(matching, test_get2dIoU) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dIoU; const double quart_circle = 0.16237976320958225; @@ -45,13 +45,13 @@ TEST(matching, test_get2dIoU) { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -61,13 +61,13 @@ TEST(matching, test_get2dIoU) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -77,13 +77,13 @@ TEST(matching, test_get2dIoU) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -103,15 +103,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) const double quart_circle = 0.16237976320958225; { // non overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -119,15 +119,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // partially overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -135,15 +135,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // fully overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -153,20 +153,20 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) TEST(matching, test_get2dPrecision) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dPrecision; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -180,13 +180,13 @@ TEST(matching, test_get2dPrecision) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -200,13 +200,13 @@ TEST(matching, test_get2dPrecision) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -220,20 +220,20 @@ TEST(matching, test_get2dPrecision) TEST(matching, test_get2dRecall) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dRecall; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -247,13 +247,13 @@ TEST(matching, test_get2dRecall) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -267,13 +267,13 @@ TEST(matching, test_get2dRecall) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); diff --git a/common/object_recognition_utils/test/src/test_object_classification.cpp b/common/object_recognition_utils/test/src/test_object_classification.cpp index 1637dc16346cd..697a7e8447732 100644 --- a/common/object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/object_recognition_utils/test/src/test_object_classification.cpp @@ -20,10 +20,10 @@ constexpr double epsilon = 1e-06; namespace { -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -34,17 +34,17 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat TEST(object_classification, test_getHighestProbLabel) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbLabel; { // empty - std::vector classifications; + std::vector classifications; std::uint8_t label = getHighestProbLabel(classifications); EXPECT_EQ(label, ObjectClassification::UNKNOWN); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -54,7 +54,7 @@ TEST(object_classification, test_getHighestProbLabel) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -67,7 +67,7 @@ TEST(object_classification, test_getHighestProbLabel) // Test isVehicle TEST(object_classification, test_isVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isVehicle; { // True Case with uint8_t @@ -87,7 +87,7 @@ TEST(object_classification, test_isVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -96,7 +96,7 @@ TEST(object_classification, test_isVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::PEDESTRIAN, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); EXPECT_FALSE(isVehicle(classification)); @@ -106,7 +106,7 @@ TEST(object_classification, test_isVehicle) // TEST isCarLikeVehicle TEST(object_classification, test_isCarLikeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isCarLikeVehicle; { // True Case with uint8_t @@ -126,7 +126,7 @@ TEST(object_classification, test_isCarLikeVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); @@ -135,7 +135,7 @@ TEST(object_classification, test_isCarLikeVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); EXPECT_FALSE(isCarLikeVehicle(classification)); @@ -146,7 +146,7 @@ TEST(object_classification, test_isCarLikeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When car and non-car label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_FALSE( @@ -157,7 +157,7 @@ TEST(object_classification, test_isCarLikeVehicle) // TEST isLargeVehicle TEST(object_classification, test_isLargeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isLargeVehicle; { // True Case with uint8_t @@ -176,7 +176,7 @@ TEST(object_classification, test_isLargeVehicle) } { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); @@ -188,7 +188,7 @@ TEST(object_classification, test_isLargeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When large-vehicle and non-large-vehicle label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_TRUE(isLargeVehicle(classification)); // evaluated with earlier appended "BUS" label @@ -197,18 +197,18 @@ TEST(object_classification, test_isLargeVehicle) TEST(object_classification, test_getHighestProbClassification) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbClassification; { // empty - std::vector classifications; + std::vector classifications; auto classification = getHighestProbClassification(classifications); EXPECT_EQ(classification.label, ObjectClassification::UNKNOWN); EXPECT_DOUBLE_EQ(classification.probability, 0.0); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -219,7 +219,7 @@ TEST(object_classification, test_getHighestProbClassification) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -232,7 +232,7 @@ TEST(object_classification, test_getHighestProbClassification) TEST(object_classification, test_fromString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toLabel; using object_recognition_utils::toObjectClassification; using object_recognition_utils::toObjectClassifications; @@ -266,7 +266,7 @@ TEST(object_classification, test_fromString) TEST(object_classification, test_convertLabelToString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::convertLabelToString; // from label @@ -290,7 +290,7 @@ TEST(object_classification, test_convertLabelToString) // from ObjectClassifications { - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index b466273f4defd..a7e6737cd2d42 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-06; namespace { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; diff --git a/common/path_distance_calculator/Readme.md b/common/path_distance_calculator/Readme.md index acbdc6998c46f..fed0476047ba9 100644 --- a/common/path_distance_calculator/Readme.md +++ b/common/path_distance_calculator/Readme.md @@ -11,10 +11,10 @@ Note that the distance means the arc-length along the path, not the Euclidean di ### Input -| Name | Type | Description | -| ----------------------------------------------------------------- | ---------------------------------------- | -------------- | -| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_auto_planning_msgs::msg::Path` | Reference path | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ----------------------------------------------------------------- | ----------------------------------- | -------------- | +| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_planning_msgs::msg::Path` | Reference path | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index e796bbd0d20f7..0ccc6419b13de 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,7 +10,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs motion_utils rclcpp rclcpp_components diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index aa20dd1ffc7ce..798f5df70c596 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -25,9 +25,9 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options) : Node("path_distance_calculator", options), self_pose_listener_(this) { - sub_path_ = create_subscription( + sub_path_ = create_subscription( "~/input/path", rclcpp::QoS(1), - [this](const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); + [this](const autoware_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); pub_dist_ = create_publisher("~/output/distance", rclcpp::QoS(1)); diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index f709701024f51..838afb903c55f 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include class PathDistanceCalculator : public rclcpp::Node @@ -27,11 +27,11 @@ class PathDistanceCalculator : public rclcpp::Node explicit PathDistanceCalculator(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_path_; + rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - autoware_auto_planning_msgs::msg::Path::SharedPtr path_; + autoware_planning_msgs::msg::Path::SharedPtr path_; }; #endif // PATH_DISTANCE_CALCULATOR_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 5d9001affa308..665e959ed2460 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -17,9 +17,9 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -36,14 +36,16 @@ geometry_msgs::msg::Polygon rotatePolygon( /// @return rotated polygon Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle); Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::DetectedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::TrackedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::PredictedObject & object); Polygon2d toFootprint( const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front, const double base_to_rear, const double width); -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); +double getArea(const autoware_perception_msgs::msg::Shape & shape); Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 63cf6305e8158..334df8f32af91 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -27,9 +27,8 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include #include @@ -129,21 +129,19 @@ inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseWithCova } template <> -inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Point getPoint(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose.position; } @@ -168,20 +166,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::PoseStamped & } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Pose getPose(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose; } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose; } @@ -194,20 +191,19 @@ double getLongitudinalVelocity([[maybe_unused]] const T & p) } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPoint & p) { return p.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline double getLongitudinalVelocity(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.longitudinal_velocity_mps; } @@ -233,21 +229,21 @@ inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::P template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::PathPoint & p) { p.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.pose = pose; } @@ -269,21 +265,21 @@ void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unus template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index c34d3b5fdfdd0..334ee226a5f22 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs autoware_internal_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs builtin_interfaces diagnostic_msgs geometry_msgs @@ -26,6 +26,7 @@ tf2 tf2_geometry_msgs tier4_debug_msgs + tier4_planning_msgs unique_identifier_msgs visualization_msgs diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index 79c9f9937cd4d..01e02d1cf0038 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -118,11 +118,11 @@ Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle) } Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape) + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape) { Polygon2d polygon; - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const auto point0 = tier4_autoware_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; @@ -140,7 +140,7 @@ Polygon2d toPolygon2d( appendPointToPolygon(polygon, point1); appendPointToPolygon(polygon, point2); appendPointToPolygon(polygon, point3); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { const double radius = shape.dimensions.x / 2.0; constexpr int circle_discrete_num = 6; for (int i = 0; i < circle_discrete_num; ++i) { @@ -157,7 +157,7 @@ Polygon2d toPolygon2d( pose.position.y; appendPointToPolygon(polygon, point); } - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { const double poly_yaw = tf2::getYaw(pose.orientation); const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw); for (const auto rel_point : rotated_footprint.points) { @@ -180,21 +180,21 @@ Polygon2d toPolygon2d( } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object) + const autoware_perception_msgs::msg::DetectedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object) + const autoware_perception_msgs::msg::TrackedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::PredictedObject & object) + const autoware_perception_msgs::msg::PredictedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); @@ -223,13 +223,13 @@ Polygon2d toFootprint( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +double getArea(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return getRectangleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return getCircleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { return getPolygonArea(shape.footprint); } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp index 7370c3b650887..7689544bd79cc 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -137,8 +137,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -159,8 +159,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double diameter = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const auto poly = toPolygon2d(pose, shape); @@ -183,8 +183,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 0.5; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.push_back(createPoint32(-x, -y)); shape.footprint.points.push_back(createPoint32(-x, y)); shape.footprint.points.push_back(createPoint32(x, y)); @@ -240,8 +240,8 @@ TEST(boost_geometry, boost_getArea) const double x = 1.0; const double y = 2.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -252,8 +252,8 @@ TEST(boost_geometry, boost_getArea) { // cylinder const double diameter = 1.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const double area = getArea(shape); @@ -265,8 +265,8 @@ TEST(boost_geometry, boost_getArea) const double y = 2.0; // clock wise - autoware_auto_perception_msgs::msg::Shape clock_wise_shape; - clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape clock_wise_shape; + clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); clock_wise_shape.footprint.points.push_back(createPoint32(0.0, y)); clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); @@ -276,8 +276,8 @@ TEST(boost_geometry, boost_getArea) EXPECT_DOUBLE_EQ(clock_wise_area, -x * y); // anti clock wise - autoware_auto_perception_msgs::msg::Shape anti_clock_wise_shape; - anti_clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape anti_clock_wise_shape; + anti_clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; anti_clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index c10e04736b5bb..b492da2d72e88 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -90,7 +90,7 @@ TEST(geometry, getPoint) } { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -101,7 +101,7 @@ TEST(geometry, getPoint) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -163,7 +163,7 @@ TEST(geometry, getPose) } { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -182,7 +182,7 @@ TEST(geometry, getPose) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -208,13 +208,13 @@ TEST(geometry, getLongitudinalVelocity) const double velocity = 1.0; { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -266,7 +266,7 @@ TEST(geometry, setPose) } { - autoware_auto_planning_msgs::msg::PathPoint p_out{}; + autoware_planning_msgs::msg::PathPoint p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); @@ -278,7 +278,7 @@ TEST(geometry, setPose) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p_out{}; + autoware_planning_msgs::msg::TrajectoryPoint p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); @@ -313,13 +313,13 @@ TEST(geometry, setLongitudinalVelocity) const double velocity = 1.0; { - autoware_auto_planning_msgs::msg::PathPoint p{}; + autoware_planning_msgs::msg::PathPoint p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p{}; + autoware_planning_msgs::msg::TrajectoryPoint p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index a8179f3b3cd60..5c55a09af95b1 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -24,7 +24,7 @@ TEST(geometry, getPoint_PathWithLaneId) const double y_ans = 2.0; const double z_ans = 3.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -46,7 +46,7 @@ TEST(geometry, getPose_PathWithLaneId) const double q_z_ans = 0.3; const double q_w_ans = 0.4; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -96,7 +96,7 @@ TEST(geometry, setPose_PathWithLaneId) p.orientation.z = q_z_ans; p.orientation.w = q_w_ans; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p_out{}; + tier4_planning_msgs::msg::PathPointWithLaneId p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); @@ -113,7 +113,7 @@ TEST(geometry, setLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p{}; + tier4_planning_msgs::msg::PathPointWithLaneId p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity); } diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp index 4fc6aeb71771d..7dbbb65c94560 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class CarInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index fc40b08be822b..c1ec65a0488bd 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -82,8 +82,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class InteractiveObject diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index 195ec8e2a3a6d..642159aceaf3d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class PedestrianInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp index 2bc3129b51321..3f1a9b55c30ab 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -48,8 +48,8 @@ namespace rviz_plugins { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; class UnknownInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md index ef9df25f2b657..5b6286139af0d 100644 --- a/common/tier4_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -11,11 +11,11 @@ This plugin displays the path, trajectory, and maximum speed. ### Input -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------------- | ------------------------------------------ | -| `/input/path` | `autoware_auto_planning_msgs::msg::Path` | The topic on which to subscribe path | -| `/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | -| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------------- | ------------------------------------------ | +| `/input/path` | `autoware_planning_msgs::msg::Path` | The topic on which to subscribe path | +| `/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | +| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | ### Output diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 66b4a31f0993f..ab6a0b25fd3b4 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -17,9 +17,9 @@ #include "tier4_planning_rviz_plugin/path/display_base.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -188,7 +188,7 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay }; class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT public: @@ -198,9 +198,9 @@ class AutowarePathWithLaneIdDisplay private: void preProcessMessageDetail() override; void preVisualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; void visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; rviz_common::properties::BoolProperty property_lane_id_view_; @@ -212,7 +212,7 @@ class AutowarePathWithLaneIdDisplay }; class AutowarePathDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT private: @@ -220,7 +220,7 @@ class AutowarePathDisplay }; class AutowareTrajectoryDisplay -: public AutowarePathBaseDisplay +: public AutowarePathBaseDisplay { Q_OBJECT private: diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index c915e38977012..ed79e122c0a01 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 4af4a371ef651..5d755d212c859 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -11,7 +11,6 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs autoware_planning_msgs libqt5-core libqt5-gui diff --git a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml index 0a80327199606..dac8c41670f3c 100644 --- a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml @@ -2,12 +2,12 @@ - Display path points of autoware_auto_planning_msg::Path + Display path points of autoware_planning_msg::Path - Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId + Display path_with_lane_id of autoware_planning_msg::PathWithLaneId - Display trajectory points of autoware_auto_planning_msg::Trajectory + Display trajectory points of autoware_planning_msg::Trajectory points.size(); // clear previous text @@ -73,8 +73,7 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) { const auto & point = msg_ptr->points.at(p_idx); diff --git a/common/tier4_state_rviz_plugin/README.md b/common/tier4_state_rviz_plugin/README.md index ac1e188fb36cd..e45be3bea13dc 100644 --- a/common/tier4_state_rviz_plugin/README.md +++ b/common/tier4_state_rviz_plugin/README.md @@ -16,7 +16,7 @@ This plugin also can engage from the panel. | `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` | The topic represents the state of localization initialization | | `/api/motion/state` | `autoware_adapi_v1_msgs::msg::MotionState` | The topic represents the state of motion | | `/api/autoware/get/emergency` | `tier4_external_api_msgs::msg::Emergency` | The topic represents the state of external emergency | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | ### Output diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 1db152e9632f8..65ccd217d8016 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -13,8 +13,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 9863cbbbf802b..c1ab9018e12f9 100644 --- a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -109,12 +109,12 @@ public Q_SLOTS: // NOLINT for Qt QVBoxLayout * makeFailSafeGroup(); // QVBoxLayout * makeDiagnosticGroup(); - // void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + // void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_gear_; rclcpp::Client::SharedPtr client_emergency_stop_; rclcpp::Subscription::SharedPtr sub_emergency_; diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md index 61260ecfda723..763504c4709a9 100644 --- a/common/tier4_system_rviz_plugin/README.md +++ b/common/tier4_system_rviz_plugin/README.md @@ -6,6 +6,6 @@ This plugin display the Hazard information from Autoware; and output notices whe ## Input -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml index adae997ea07aa..bd72ecf97def4 100644 --- a/common/tier4_system_rviz_plugin/package.xml +++ b/common/tier4_system_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs + autoware_system_msgs diagnostic_msgs libqt5-core libqt5-gui diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index 61c2bd378dab1..c0db8cc86450b 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include @@ -162,7 +162,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) // MRM summary std::vector mrm_comfortable_stop_summary_list = {}; std::vector mrm_emergency_stop_summary_list = {}; - int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + int hazard_level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; { std::lock_guard message_lock(mutex_); if (last_msg_ptr_) { @@ -207,7 +207,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) std::ostringstream output_text; // Display the MRM Summary only when there is a fault - if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + if (hazard_level != autoware_system_msgs::msg::HazardStatus::NO_FAULT) { // Broadcasting the Basic Error Infos int number_of_comfortable_stop_messages = static_cast(mrm_comfortable_stop_summary_list.size()); @@ -243,7 +243,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) } void MrmSummaryOverlayDisplay::processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp index d0b0e32c3788f..0f59ba5582fed 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -59,14 +59,14 @@ #include #include -#include +#include #endif namespace rviz_plugins { class MrmSummaryOverlayDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -85,7 +85,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -101,7 +101,7 @@ private Q_SLOTS: static constexpr int hand_width_ = 4; std::mutex mutex_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; + autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 91fb5bc124cb7..cc37d17768d49 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 2b118b429f1af..b123f8e2bdc3c 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs + autoware_map_msgs autoware_perception_msgs lanelet2_extension libqt5-core diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index 35c5a88a2f8a6..db33abd72283d 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -139,66 +139,66 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficSignalElement traffic_light; + TrafficLightElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficSignalElement::RED; + traffic_light.color = TrafficLightElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficSignalElement::AMBER; + traffic_light.color = TrafficLightElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficSignalElement::GREEN; + traffic_light.color = TrafficLightElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficSignalElement::WHITE; + traffic_light.color = TrafficLightElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficSignalElement::UNKNOWN; + traffic_light.color = TrafficLightElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficSignalElement::CIRCLE; + traffic_light.shape = TrafficLightElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::LEFT_ARROW; + traffic_light.shape = TrafficLightElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficSignalElement::UP_ARROW; + traffic_light.shape = TrafficLightElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficSignalElement::UNKNOWN; + traffic_light.shape = TrafficLightElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficSignalElement::SOLID_OFF; + traffic_light.status = TrafficLightElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficSignalElement::SOLID_ON; + traffic_light.status = TrafficLightElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficSignalElement::FLASHING; + traffic_light.status = TrafficLightElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficSignalElement::UNKNOWN; + traffic_light.status = TrafficLightElement::UNKNOWN; } - TrafficSignal traffic_signal; + TrafficLightGroup traffic_signal; traffic_signal.elements.push_back(traffic_light); - traffic_signal.traffic_signal_id = traffic_light_id; + traffic_signal.traffic_light_group_id = traffic_light_id; - for (auto & signal : extra_traffic_signals_.signals) { - if (signal.traffic_signal_id == traffic_light_id) { + for (auto & signal : extra_traffic_signals_.traffic_light_groups) { + if (signal.traffic_light_group_id == traffic_light_id) { signal = traffic_signal; return; } } - extra_traffic_signals_.signals.push_back(traffic_signal); + extra_traffic_signals_.traffic_light_groups.push_back(traffic_signal); } void TrafficLightPublishPanel::onResetTrafficLightState() { - extra_traffic_signals_.signals.clear(); + extra_traffic_signals_.traffic_light_groups.clear(); enable_publish_ = false; publish_button_->setText("PUBLISH"); @@ -218,10 +218,10 @@ void TrafficLightPublishPanel::onInitialize() using std::placeholders::_1; raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pub_traffic_signals_ = raw_node_->create_publisher( + pub_traffic_signals_ = raw_node_->create_publisher( "/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); - sub_vector_map_ = raw_node_->create_subscription( + sub_vector_map_ = raw_node_->create_subscription( "/map/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightPublishPanel::onVectorMap, this, _1)); createWallTimer(); @@ -252,20 +252,20 @@ void TrafficLightPublishPanel::onTimer() pub_traffic_signals_->publish(extra_traffic_signals_); } - traffic_table_->setRowCount(extra_traffic_signals_.signals.size()); + traffic_table_->setRowCount(extra_traffic_signals_.traffic_light_groups.size()); - if (extra_traffic_signals_.signals.empty()) { + if (extra_traffic_signals_.traffic_light_groups.empty()) { return; } - for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { - const auto & signal = extra_traffic_signals_.signals.at(i); + for (size_t i = 0; i < extra_traffic_signals_.traffic_light_groups.size(); ++i) { + const auto & signal = extra_traffic_signals_.traffic_light_groups.at(i); if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); + auto id_label = new QLabel(QString::number(signal.traffic_light_group_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); @@ -273,23 +273,23 @@ void TrafficLightPublishPanel::onTimer() const auto & light = signal.elements.front(); switch (light.color) { - case TrafficSignalElement::RED: + case TrafficLightElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficSignalElement::AMBER: + case TrafficLightElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficSignalElement::GREEN: + case TrafficLightElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficSignalElement::WHITE: + case TrafficLightElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -301,28 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficSignalElement::CIRCLE: + case TrafficLightElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficSignalElement::LEFT_ARROW: + case TrafficLightElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficSignalElement::RIGHT_ARROW: + case TrafficLightElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficSignalElement::UP_ARROW: + case TrafficLightElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficSignalElement::DOWN_ARROW: + case TrafficLightElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficSignalElement::DOWN_LEFT_ARROW: + case TrafficLightElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficSignalElement::DOWN_RIGHT_ARROW: + case TrafficLightElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -333,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficSignalElement::SOLID_OFF: + case TrafficLightElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficSignalElement::SOLID_ON: + case TrafficLightElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficSignalElement::FLASHING: + case TrafficLightElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -361,7 +361,7 @@ void TrafficLightPublishPanel::onTimer() traffic_table_->update(); } -void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) +void TrafficLightPublishPanel::onVectorMap(const LaneletMapBin::ConstSharedPtr msg) { if (received_vector_map_) return; // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index e54b6a301873b..2979563062fea 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include +#include #endif #include @@ -35,10 +35,10 @@ namespace rviz_plugins { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalArray; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT @@ -56,12 +56,12 @@ public Q_SLOTS: protected: void onTimer(); void createWallTimer(); - void onVectorMap(const HADMapBin::ConstSharedPtr msg); + void onVectorMap(const LaneletMapBin::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr pub_timer_; - rclcpp::Publisher::SharedPtr pub_traffic_signals_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Publisher::SharedPtr pub_traffic_signals_; + rclcpp::Subscription::SharedPtr sub_vector_map_; QSpinBox * publishing_rate_input_; QComboBox * traffic_light_id_input_; @@ -74,7 +74,7 @@ public Q_SLOTS: QPushButton * publish_button_; QTableWidget * traffic_table_; - TrafficSignalArray extra_traffic_signals_; + TrafficLightGroupArray extra_traffic_signals_; bool enable_publish_{false}; std::set traffic_light_ids_; diff --git a/common/tier4_vehicle_rviz_plugin/README.md b/common/tier4_vehicle_rviz_plugin/README.md index 09576ac327bec..9560963b1f5c0 100644 --- a/common/tier4_vehicle_rviz_plugin/README.md +++ b/common/tier4_vehicle_rviz_plugin/README.md @@ -11,12 +11,12 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t ### Input -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------------- | ---------------------------------- | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | -| `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | +| Name | Type | Description | +| --------------------------------- | -------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/control/turn_signal_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | ## Parameter diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index f6c131fdc99f3..922ebd2c5e318 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 5d3684d0351c6..5a9f7e602efc7 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -164,7 +164,7 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt) } void ConsoleMeterDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index cba0fa16b50fe..8f1a62fc35361 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -28,13 +28,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class ConsoleMeterDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -52,7 +52,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -86,7 +86,7 @@ private Q_SLOTS: Arc outer_arc_; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index 60a81e45c9c29..ffc2484c3673a 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -167,7 +167,7 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt) } void SteeringAngleDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp index d56b5a8d742b9..867a4c6110bb0 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp @@ -26,13 +26,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class SteeringAngleDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -50,7 +50,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; @@ -65,7 +65,7 @@ private Q_SLOTS: private: std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp index b9ff54ef44ecd..ec16b693a5974 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp @@ -84,7 +84,7 @@ void TurnSignalDisplay::onDisable() } void TurnSignalDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; @@ -123,19 +123,19 @@ void TurnSignalDisplay::update(float wall_dt, float ros_dt) // turn signal color QColor white_color(Qt::white); white_color.setAlpha(255); - if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { + if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(left_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { + } else if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(right_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(left_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE) { + } else if (signal_type == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE) { painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp index 06a1d2e5f0d54..eb64edd2d595e 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp @@ -24,14 +24,14 @@ #include #include -#include -#include +#include +#include #endif namespace rviz_plugins { class TurnSignalDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -49,7 +49,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::IntProperty * property_left_; rviz_common::properties::IntProperty * property_top_; @@ -62,7 +62,7 @@ private Q_SLOTS: QPointF left_arrow_polygon_[7]; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp index b5db539f437cb..ccf84d4e64895 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp @@ -102,7 +102,7 @@ void VelocityHistoryDisplay::reset() } bool VelocityHistoryDisplay::validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) { if (!rviz_common::validateFloats(msg_ptr->longitudinal_velocity)) { return false; @@ -120,7 +120,7 @@ void VelocityHistoryDisplay::update(float wall_dt, float ros_dt) } void VelocityHistoryDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp index 9b5819df98bef..96345205289e6 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ namespace rviz_plugins { class VelocityHistoryDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -53,7 +53,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; std::unique_ptr setColorDependsOnVelocity( const double vel_max, const double cmd_vel); std::unique_ptr gradation( @@ -67,11 +67,9 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_vel_max_; private: - std::deque< - std::tuple> + std::deque> histories_; - bool validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); + bool validateFloats(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); std::mutex mutex_; }; diff --git a/common/traffic_light_recognition_marker_publisher/Readme.md b/common/traffic_light_recognition_marker_publisher/Readme.md index 6a51499c4e5da..d87fc33aefd33 100644 --- a/common/traffic_light_recognition_marker_publisher/Readme.md +++ b/common/traffic_light_recognition_marker_publisher/Readme.md @@ -12,10 +12,10 @@ This node publishes a marker array for visualizing traffic signal recognition re ### Input -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Vector map for getting traffic signal information | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | The result of traffic signal recognition | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Vector map for getting traffic signal information | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The result of traffic signal recognition | ### Output diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml index d12022c01684e..2b65c5b868313 100644 --- a/common/traffic_light_recognition_marker_publisher/package.xml +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -12,8 +12,8 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs + autoware_perception_msgs geometry_msgs lanelet2_extension rclcpp diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp index 6076d3fa32f26..5d887a2296137 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp @@ -26,17 +26,17 @@ TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher( { using std::placeholders::_1; - sub_map_ptr_ = this->create_subscription( + sub_map_ptr_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightRecognitionMarkerPublisher::onMap, this, _1)); - sub_tlr_ptr_ = this->create_subscription( + sub_tlr_ptr_ = this->create_subscription( "~/input/traffic_signals", rclcpp::QoS{1}, std::bind(&TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray, this, _1)); pub_marker_ptr_ = this->create_publisher("~/output/marker", rclcpp::QoS{1}); } -void TrafficLightRecognitionMarkerPublisher::onMap(const HADMapBin::ConstSharedPtr msg_ptr) +void TrafficLightRecognitionMarkerPublisher::onMap(const LaneletMapBin::ConstSharedPtr msg_ptr) { is_map_ready_ = false; lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -70,10 +70,10 @@ void TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray( } MarkerArray markers; marker_id = 0; - for (const auto & tl : msg_ptr->signals) { - if (tl_position_map_.count(tl.map_primitive_id) == 0) continue; - auto tl_position = tl_position_map_[tl.map_primitive_id]; - for (const auto tl_light : tl.lights) { + for (const auto & tl : msg_ptr->traffic_light_groups) { + if (tl_position_map_.count(tl.traffic_light_group_id) == 0) continue; + auto tl_position = tl_position_map_[tl.traffic_light_group_id]; + for (const auto tl_light : tl.elements) { const auto marker = getTrafficLightMarker(tl_position, tl_light.color, tl_light.shape); markers.markers.emplace_back(marker); marker_id++; diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp index de9aea14b7e5a..c1cac302647a1 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -32,21 +32,21 @@ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node { public: - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; - using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; + using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; + using TrafficLight = autoware_perception_msgs::msg::TrafficLightElement; using MarkerArray = visualization_msgs::msg::MarkerArray; using Pose = geometry_msgs::msg::Pose; explicit TrafficLightRecognitionMarkerPublisher(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_map_ptr_; + rclcpp::Subscription::SharedPtr sub_tlr_ptr_; rclcpp::Publisher::SharedPtr pub_marker_ptr_; - void onMap(const HADMapBin::ConstSharedPtr msg_ptr); + void onMap(const LaneletMapBin::ConstSharedPtr msg_ptr); void onTrafficSignalArray(const TrafficSignalArray::ConstSharedPtr msg_ptr); visualization_msgs::msg::Marker getTrafficLightMarker( const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape); diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 9c3acd4f45fa4..cf4ecdd9774b8 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,8 +15,8 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ -#include "autoware_perception_msgs/msg/traffic_signal.hpp" -#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_group.hpp" #include "tier4_perception_msgs/msg/traffic_light.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" @@ -50,7 +50,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c * @return True if a circle-shaped light with the specified color is found, false otherwise. */ bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color); /** * @brief Checks if a traffic light state includes a light with the specified shape. @@ -62,7 +62,7 @@ bool hasTrafficLightCircleColor( * @return True if a light with the specified shape is found, false otherwise. */ bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape); /** * @brief Determines if a traffic signal indicates a stop for the given lanelet. @@ -78,7 +78,7 @@ bool hasTrafficLightShape( */ bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state); tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 0bd3d85a9b71f..8aea884510ec2 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -52,11 +52,11 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c } bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color) { const auto it_lamp = std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + return x.shape == autoware_perception_msgs::msg::TrafficLightElement::CIRCLE && x.color == lamp_color; }); @@ -64,7 +64,7 @@ bool hasTrafficLightCircleColor( } bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape) { const auto it_lamp = std::find_if( tl_state.elements.begin(), tl_state.elements.end(), @@ -75,10 +75,10 @@ bool hasTrafficLightShape( bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state) { if (hasTrafficLightCircleColor( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::GREEN)) { return false; } @@ -90,18 +90,18 @@ bool isTrafficSignalStop( if ( turn_direction == "right" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW)) { return false; } if ( turn_direction == "left" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW)) { return false; } From f2ee838f50d372e1a8ae780c82100017dc694325 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 00:03:50 +0900 Subject: [PATCH 097/120] feat!: replace autoware_auto_msgs with autoware_msgs for sensing modules (#7247) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- sensing/pointcloud_preprocessor/docs/vector-map-filter.md | 8 ++++---- .../docs/vector-map-inside-area-filter.md | 8 ++++---- .../concatenate_data/concatenate_pointclouds.hpp | 4 ++-- .../vector_map_filter/lanelet2_map_filter_nodelet.hpp | 6 +++--- .../vector_map_filter/vector_map_inside_area_filter.hpp | 4 ++-- .../src/vector_map_filter/lanelet2_map_filter_nodelet.cpp | 4 ++-- .../vector_map_filter/vector_map_inside_area_filter.cpp | 4 ++-- sensing/vehicle_velocity_converter/README.md | 8 ++++---- .../vehicle_velocity_converter.hpp | 7 +++---- sensing/vehicle_velocity_converter/package.xml | 2 +- .../src/vehicle_velocity_converter.cpp | 4 ++-- 11 files changed, 29 insertions(+), 30 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md index fedd3c081cdeb..c38a4c719fcf3 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md @@ -10,10 +10,10 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ### Input -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ---------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| -------------------- | --------------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | ### Output diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index 4bafac7872e78..bd2f7d98919d5 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -19,10 +19,10 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please ### Input -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ------------------------------------ | -| `~/input` | `sensor_msgs::msg::PointCloud2` | input points | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map used for filtering points | +| Name | Type | Description | +| -------------------- | --------------------------------------- | ------------------------------------ | +| `~/input` | `sensor_msgs::msg::PointCloud2` | input points | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map used for filtering points | ### Output diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index f46d6595b2a76..4e183a9816c2e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,7 +68,7 @@ #include #include -#include +#include #include #include #include @@ -127,7 +127,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node /** \brief A vector of subscriber. */ std::vector::SharedPtr> filters_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index ed0d753a68ecc..42078922d39b9 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -59,7 +59,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr filtered_pointcloud_pub_; @@ -71,7 +71,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node void pointcloudCallback(const PointCloud2ConstPtr msg); - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool transformPointCloud( const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr, diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 74209c1a22e4c..894768d385438 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -36,10 +36,10 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; lanelet::ConstPolygons3d polygon_lanelets_; - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); // parameter std::string polygon_type_; diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp index 502c85d1746e8..8f7cb2abec6a4 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp @@ -51,7 +51,7 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set subscriber { - map_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapFilterComponent::mapCallback, this, _1)); pointcloud_sub_ = this->create_subscription( @@ -259,7 +259,7 @@ void Lanelet2MapFilterComponent::pointcloudCallback(const PointCloud2ConstPtr cl } void Lanelet2MapFilterComponent::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 5b8755714b375..8fcf15326829f 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -71,7 +71,7 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( using std::placeholders::_1; // Set subscriber - map_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); } @@ -107,7 +107,7 @@ void VectorMapInsideAreaFilterComponent::filter( } void VectorMapInsideAreaFilterComponent::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg) { tf_input_frame_ = map_msg->header.frame_id; diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/vehicle_velocity_converter/README.md index 92881f9321f28..3c7292f3fcdc4 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/vehicle_velocity_converter/README.md @@ -2,15 +2,15 @@ ## Purpose -This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. +This package converts autoware_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ----------------- | ------------------------------------------------ | ---------------- | -| `velocity_status` | `autoware_auto_vehicle_msgs::msg::VehicleReport` | vehicle velocity | +| Name | Type | Description | +| ----------------- | ------------------------------------------- | ---------------- | +| `velocity_status` | `autoware_vehicle_msgs::msg::VehicleReport` | vehicle velocity | ### Output diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 660ad330f07f3..4a1a66b842892 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -32,10 +32,9 @@ class VehicleVelocityConverter : public rclcpp::Node ~VehicleVelocityConverter() = default; private: - void callbackVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg); + void callbackVelocityReport(const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg); - rclcpp::Subscription::SharedPtr - vehicle_report_sub_; + rclcpp::Subscription::SharedPtr vehicle_report_sub_; rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index c44c55bcd40eb..39780deaccc28 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs geometry_msgs rclcpp ament_lint_auto diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 0f4a22bbc9730..360ec4cae58d5 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -22,7 +22,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co frame_id_ = declare_parameter("frame_id"); speed_scale_factor_ = declare_parameter("speed_scale_factor"); - vehicle_report_sub_ = create_subscription( + vehicle_report_sub_ = create_subscription( "velocity_status", rclcpp::QoS{100}, std::bind(&VehicleVelocityConverter::callbackVelocityReport, this, std::placeholders::_1)); @@ -31,7 +31,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co } void VehicleVelocityConverter::callbackVelocityReport( - const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) + const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { if (msg->header.frame_id != frame_id_) { RCLCPP_WARN(get_logger(), "frame_id is not base_link."); From 09b68993e3bb20b1e2fb60bd5e3cd58c019ab477 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 00:10:54 +0900 Subject: [PATCH 098/120] feat!: replace autoware_auto_msgs with autoware_msgs for perception modules (#7245) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan Co-authored-by: M. Fatih Cırıt Co-authored-by: Yukihito Saito --- perception/bytetrack/package.xml | 2 +- perception/bytetrack/src/bytetrack_node.cpp | 6 +-- perception/cluster_merger/README.md | 6 +-- .../README.md | 16 +++--- .../node.hpp | 16 +++--- .../package.xml | 3 +- .../src/node.cpp | 40 ++++++++------- .../detected_object_feature_remover/README.md | 6 +-- .../detected_object_feature_remover.hpp | 4 +- .../package.xml | 2 +- .../object_lanelet_filter.hpp | 29 ++++++----- .../object_position_filter.hpp | 12 ++--- .../debugger.hpp | 12 ++--- .../obstacle_pointcloud_based_validator.hpp | 32 ++++++------ .../occupancy_grid_based_validator.hpp | 16 +++--- .../utils/utils.hpp | 10 ++-- .../object-lanelet-filter.md | 14 +++--- .../object-position-filter.md | 14 +++--- .../obstacle-pointcloud-based-validator.md | 14 +++--- .../occupancy-grid-based-validator.md | 14 +++--- .../detected_object_validation/package.xml | 4 +- .../src/object_lanelet_filter.cpp | 32 ++++++------ .../src/object_position_filter.cpp | 10 ++-- .../obstacle_pointcloud_based_validator.cpp | 24 ++++----- .../src/occupancy_grid_based_validator.cpp | 16 +++--- .../detected_object_validation/src/utils.cpp | 4 +- .../test/test_utils.cpp | 4 +- perception/detection_by_tracker/README.md | 6 +-- .../include/detection_by_tracker/debugger.hpp | 42 +++++++--------- .../detection_by_tracker_core.hpp | 24 ++++----- .../src/detection_by_tracker_core.cpp | 50 +++++++++---------- perception/detection_by_tracker/src/utils.cpp | 4 +- perception/elevation_map_loader/README.md | 2 +- .../elevation_map_loader_node.hpp | 6 +-- perception/elevation_map_loader/package.xml | 1 - .../src/elevation_map_loader_node.cpp | 4 +- perception/euclidean_cluster/lib/utils.cpp | 10 ++-- .../voxel_grid_based_euclidean_cluster.cpp | 4 +- perception/euclidean_cluster/package.xml | 2 +- .../docs/pointpainting-fusion.md | 10 ++-- .../docs/roi-detected-object-fusion.md | 14 +++--- .../fusion_node.hpp | 8 +-- .../pointpainting_fusion/node.hpp | 2 +- .../roi_detected_object_fusion/node.hpp | 2 +- .../utils/geometry.hpp | 4 +- .../utils/utils.hpp | 2 +- .../image_projection_based_fusion/package.xml | 2 +- .../src/pointpainting_fusion/node.cpp | 30 +++++------ .../src/roi_cluster_fusion/node.cpp | 4 +- .../src/roi_pointcloud_fusion/node.cpp | 2 +- .../src/utils/utils.cpp | 4 +- .../test/test_geometry.cpp | 6 +-- .../CMakeLists.txt | 2 + .../package.xml | 2 +- .../src/cluster2d.cpp | 10 ++-- .../src/debugger.cpp | 4 +- .../cluster2d.hpp | 1 - .../lidar_apollo_segmentation_tvm/package.xml | 2 +- .../src/cluster2d.cpp | 8 +-- perception/lidar_centerpoint/README.md | 10 ++-- .../detection_class_remapper.hpp | 10 ++-- .../include/lidar_centerpoint/node.hpp | 10 ++-- .../postprocess/non_maximum_suppression.hpp | 4 +- .../include/lidar_centerpoint/ros_utils.hpp | 10 ++-- .../lib/detection_class_remapper.cpp | 2 +- .../lidar_centerpoint/lib/ros_utils.cpp | 10 ++-- perception/lidar_centerpoint/package.xml | 2 +- perception/lidar_centerpoint/src/node.cpp | 8 +-- .../test/test_detection_class_remapper.cpp | 18 +++---- .../lidar_centerpoint/test/test_nms.cpp | 16 +++--- .../lidar_centerpoint/test/test_ros_utils.cpp | 27 +++++----- perception/lidar_centerpoint_tvm/README.md | 10 ++-- .../include/lidar_centerpoint_tvm/node.hpp | 10 ++-- .../lidar_centerpoint_tvm/ros_utils.hpp | 10 ++-- .../single_inference_node.hpp | 12 ++--- .../lidar_centerpoint_tvm/lib/ros_utils.cpp | 10 ++-- perception/lidar_centerpoint_tvm/package.xml | 2 +- perception/lidar_centerpoint_tvm/src/node.cpp | 6 +-- .../src/single_inference_node.cpp | 8 +-- perception/map_based_prediction/README.md | 24 ++++----- .../map_based_prediction_node.hpp | 49 +++++++++--------- .../map_based_prediction/path_generator.hpp | 20 ++++---- perception/map_based_prediction/package.xml | 1 - .../src/map_based_prediction_node.cpp | 35 +++++++------ .../test_path_generator.cpp | 16 +++--- perception/multi_object_tracker/README.md | 20 ++++---- .../data_association/data_association.hpp | 4 +- .../debugger/debug_object.hpp | 6 +-- .../debugger/debugger.hpp | 10 ++-- .../multi_object_tracker_core.hpp | 10 ++-- .../processor/input_manager.hpp | 6 +-- .../processor/processor.hpp | 14 +++--- .../tracker/model/bicycle_tracker.hpp | 16 +++--- .../tracker/model/big_vehicle_tracker.hpp | 16 +++--- .../model/multiple_vehicle_tracker.hpp | 6 +-- .../tracker/model/normal_vehicle_tracker.hpp | 16 +++--- .../tracker/model/pass_through_tracker.hpp | 10 ++-- .../model/pedestrian_and_bicycle_tracker.hpp | 6 +-- .../tracker/model/pedestrian_tracker.hpp | 16 +++--- .../tracker/model/tracker_base.hpp | 21 ++++---- .../tracker/model/unknown_tracker.hpp | 16 +++--- .../multi_object_tracker/utils/utils.hpp | 29 ++++++----- perception/multi_object_tracker/package.xml | 2 +- .../src/data_association/data_association.cpp | 6 +-- .../src/debugger/debug_object.cpp | 6 +-- .../src/debugger/debugger.cpp | 6 +-- .../src/multi_object_tracker_core.cpp | 2 +- .../src/processor/input_manager.cpp | 2 +- .../src/processor/processor.cpp | 23 ++++----- .../src/tracker/model/bicycle_tracker.cpp | 30 ++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 37 +++++++------- .../model/multiple_vehicle_tracker.cpp | 10 ++-- .../tracker/model/normal_vehicle_tracker.cpp | 37 +++++++------- .../tracker/model/pass_through_tracker.cpp | 6 +-- .../model/pedestrian_and_bicycle_tracker.cpp | 10 ++-- .../src/tracker/model/pedestrian_tracker.cpp | 36 ++++++------- .../src/tracker/model/tracker_base.cpp | 10 ++-- .../src/tracker/model/unknown_tracker.cpp | 17 +++---- perception/object_merger/README.md | 34 ++++++------- .../data_association/data_association.hpp | 6 +-- .../include/object_merger/node.hpp | 16 +++--- .../include/object_merger/utils/utils.hpp | 4 +- perception/object_merger/package.xml | 2 +- .../data_association/data_association.cpp | 8 +-- .../src/object_association_merger/node.cpp | 18 +++---- perception/object_range_splitter/README.md | 14 +++--- .../include/object_range_splitter/node.hpp | 10 ++-- perception/object_range_splitter/package.xml | 2 +- perception/object_range_splitter/src/node.cpp | 16 +++--- perception/object_velocity_splitter/README.md | 8 +-- .../object_velocity_splitter_node.hpp | 6 +-- .../object_velocity_splitter/package.xml | 2 +- .../object_velocity_splitter_node.cpp | 4 +- .../package.xml | 2 +- .../README.md | 8 +-- ...dar_crossing_objects_noise_filter_node.hpp | 6 +-- .../package.xml | 2 +- ...dar_crossing_objects_noise_filter_node.cpp | 4 +- ...radar_crossing_objects_filter_is_noise.cpp | 4 +- .../radar_fusion_to_detected_object/README.md | 8 +-- .../radar_fusion_to_detected_object.hpp | 6 +-- .../radar_fusion_to_detected_object_node.hpp | 6 +-- .../package.xml | 2 +- .../src/radar_fusion_to_detected_object.cpp | 4 +- ..._object_fusion_to_detected_object_node.cpp | 4 +- perception/radar_object_clustering/README.md | 6 +-- .../radar_object_clustering_node.hpp | 6 +-- .../radar_object_clustering/package.xml | 2 +- .../radar_object_clustering_node.cpp | 8 +-- perception/radar_object_tracker/README.md | 14 +++--- .../data_association/data_association.hpp | 4 +- .../radar_object_tracker_node.hpp | 29 ++++++----- .../constant_turn_rate_motion_tracker.hpp | 14 +++--- .../tracker/model/linear_motion_tracker.hpp | 14 +++--- .../tracker/model/tracker_base.hpp | 19 ++++--- .../utils/radar_object_tracker_utils.hpp | 10 ++-- .../radar_object_tracker/utils/utils.hpp | 6 +-- perception/radar_object_tracker/package.xml | 2 +- .../src/data_association/data_association.cpp | 6 +-- .../radar_object_tracker_node.cpp | 28 +++++------ .../constant_turn_rate_motion_tracker.cpp | 30 +++++------ .../tracker/model/linear_motion_tracker.cpp | 30 +++++------ .../src/tracker/model/tracker_base.cpp | 6 +-- .../src/utils/radar_object_tracker_utils.cpp | 8 +-- .../test/test_radar_object_tracker_utils.cpp | 2 +- .../radar_tracks_msgs_converter/README.md | 10 ++-- .../radar_tracks_msgs_converter_node.hpp | 28 +++++------ .../radar_tracks_msgs_converter/package.xml | 2 +- .../radar_tracks_msgs_converter_node.cpp | 2 +- perception/shape_estimation/README.md | 6 +-- .../corrector/corrector_interface.hpp | 4 +- .../corrector/no_corrector.hpp | 2 +- .../reference_shape_size_corrector.hpp | 2 +- .../shape_estimation/corrector/utils.hpp | 8 +-- .../corrector/vehicle_corrector.hpp | 2 +- .../shape_estimation/filter/bus_filter.hpp | 2 +- .../shape_estimation/filter/car_filter.hpp | 2 +- .../filter/filter_interface.hpp | 5 +- .../shape_estimation/filter/no_filter.hpp | 2 +- .../filter/trailer_filter.hpp | 2 +- .../shape_estimation/filter/truck_filter.hpp | 2 +- .../include/shape_estimation/filter/utils.hpp | 6 +-- .../shape_estimation/model/bounding_box.hpp | 5 +- .../shape_estimation/model/convex_hull.hpp | 2 +- .../shape_estimation/model/cylinder.hpp | 2 +- .../model/model_interface.hpp | 4 +- .../shape_estimation/shape_estimator.hpp | 16 +++--- .../lib/corrector/no_corrector.cpp | 2 +- .../shape_estimation/lib/corrector/utils.cpp | 8 +-- .../lib/filter/bus_filter.cpp | 2 +- .../lib/filter/car_filter.cpp | 2 +- .../shape_estimation/lib/filter/no_filter.cpp | 2 +- .../lib/filter/trailer_filter.cpp | 2 +- .../lib/filter/truck_filter.cpp | 2 +- .../shape_estimation/lib/filter/utils.cpp | 6 +-- .../lib/model/bounding_box.cpp | 8 +-- .../lib/model/convex_hull.cpp | 6 +-- .../shape_estimation/lib/model/cylinder.cpp | 6 +-- .../shape_estimation/lib/shape_estimator.cpp | 12 ++--- perception/shape_estimation/package.xml | 2 +- perception/shape_estimation/src/node.cpp | 12 ++--- perception/shape_estimation/src/node.hpp | 4 +- .../shape_estimation/test_shape_estimator.cpp | 18 +++---- perception/simple_object_merger/README.md | 6 +-- .../simple_object_merger_node.hpp | 6 +-- perception/simple_object_merger/package.xml | 2 +- .../simple_object_merger_node.cpp | 8 +-- perception/tensorrt_yolox/package.xml | 2 +- .../src/tensorrt_yolox_node.cpp | 2 +- perception/tracking_object_merger/README.md | 12 ++--- .../data_association/data_association.hpp | 14 +++--- .../decorative_tracker_merger.hpp | 35 ++++++------- .../utils/tracker_state.hpp | 14 +++--- .../tracking_object_merger/utils/utils.hpp | 18 +++---- perception/tracking_object_merger/package.xml | 2 +- .../src/data_association/data_association.cpp | 10 ++-- .../src/decorative_tracker_merger.cpp | 10 ++-- .../src/utils/tracker_state.cpp | 8 +-- .../src/utils/utils.cpp | 34 ++++++------- perception/traffic_light_arbiter/README.md | 16 +++--- .../signal_match_validator.hpp | 8 +-- .../traffic_light_arbiter.hpp | 12 ++--- perception/traffic_light_arbiter/package.xml | 2 +- .../src/signal_match_validator.cpp | 36 ++++++------- .../src/traffic_light_arbiter.cpp | 18 +++---- perception/traffic_light_classifier/README.md | 4 +- .../README.md | 2 +- .../traffic_light_map_based_detector/node.hpp | 6 +-- .../package.xml | 3 +- .../src/node.cpp | 4 +- .../node.hpp | 12 ++--- .../package.xml | 2 +- .../src/node.cpp | 14 +++--- .../README.md | 18 +++---- .../nodelet.hpp | 6 +-- .../package.xml | 2 +- .../src/nodelet.cpp | 4 +- .../traffic_light_visualization/README.md | 8 +-- .../traffic_light_map_visualizer/node.hpp | 14 +++--- .../src/traffic_light_map_visualizer/node.cpp | 18 +++---- 240 files changed, 1225 insertions(+), 1265 deletions(-) diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 0d1ccc284f888..231a1845bb0ea 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -15,7 +15,7 @@ cudnn_cmake_module tensorrt_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs cuda_utils cv_bridge eigen diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp index eb5e73312a890..64adc83a51abd 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include @@ -62,7 +62,7 @@ void ByteTrackNode::on_connect() void ByteTrackNode::on_rect( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + using Label = autoware_perception_msgs::msg::ObjectClassification; tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; tier4_perception_msgs::msg::DynamicObjectArray out_objects_uuid; @@ -97,7 +97,7 @@ void ByteTrackNode::on_rect( object.feature.roi.height = static_cast(output_height); object.object.existence_probability = tracked_object.score; object.object.classification.emplace_back( - autoware_auto_perception_msgs::build diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 24006e7df580e..0612f67de4c2b 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -50,7 +50,7 @@ - + @@ -221,7 +221,7 @@ - + From df99718a2917521cf664204fbfb4d0d74e99ba5c Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 02:46:52 +0900 Subject: [PATCH 108/120] feat(build_depends.repos): remove autoware_auto_msgs from build_depends.repos (#7252) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Yukihiro Saito --- build_depends.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index 32854cc34e362..a8f431f97df25 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -20,10 +20,6 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git version: main - core/external/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main # universe universe/external/tier4_autoware_msgs: type: git From 97173bcecd7e4cff8d315d06c302712bf13a8d60 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 09:59:13 +0900 Subject: [PATCH 109/120] feat(object_recognition_utils): fix failing test (#7268) * feat(object_recognition_utils): fix failing test Signed-off-by: Ryohsuke Mitsudome * Update common/object_recognition_utils/test/src/test_predicted_path_utils.cpp --------- Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Yukihiro Saito --- .../test/src/test_predicted_path_utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index a7e6737cd2d42..c2c11b7d5859f 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -174,7 +174,7 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) } } - // Resample which exceeds the maximum size + // Resample the path with more than 100 points { std::vector resampling_vec(101); for (size_t i = 0; i < 101; ++i) { @@ -186,7 +186,7 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) EXPECT_EQ(resampled_path.path.size(), resampled_path.path.max_size()); EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); - for (size_t i = 0; i < resampled_path.path.max_size(); ++i) { + for (size_t i = 0; i < resampled_path.path.size(); ++i) { EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i), epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); From 10a83d9de3d7d1b82e040aa4e776f61e592304ca Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 14:58:22 +0900 Subject: [PATCH 110/120] fix(object_recognition_utils): fix test for resamplePredictedPath_by_vector (#7275) Signed-off-by: Ryohsuke Mitsudome --- .../test/src/test_predicted_path_utils.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index c2c11b7d5859f..3dc3a8bcba3f7 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -183,7 +183,6 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) const auto resampled_path = resamplePredictedPath(path, resampling_vec); - EXPECT_EQ(resampled_path.path.size(), resampled_path.path.max_size()); EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); for (size_t i = 0; i < resampled_path.path.size(); ++i) { From 9ec94f0db4dfa312b86aede5d4ffaf936ee2763e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 5 Jun 2024 20:17:32 +0900 Subject: [PATCH 111/120] chore(ci): add a workflow for DCO check to avoid DCO bot system failure (#7270) Signed-off-by: Kotaro Yoshimoto pythagora.yoshimoto@gmail.com --- .github/workflows/dco.yaml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/dco.yaml diff --git a/.github/workflows/dco.yaml b/.github/workflows/dco.yaml new file mode 100644 index 0000000000000..db7ace467c658 --- /dev/null +++ b/.github/workflows/dco.yaml @@ -0,0 +1,21 @@ +name: DCO +# ref: https://github.com/anchore/syft/pull/2926/files +on: + pull_request: +jobs: + dco: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Setup Python 3.x + uses: actions/setup-python@v5 + with: + python-version: 3.x + + - name: Check DCO + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + pip3 install -U dco-check + dco-check --verbose --exclude-pattern 'pre-commit-ci\[bot\]@users\.noreply\.github\.com' From 0160f36c156b7791bb1915ec6a09dc797b770fca Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 5 Jun 2024 22:43:18 +0900 Subject: [PATCH 112/120] ci(cppcheck-all, cppcheck-differential): add cppcheck (#7262) * ci: add cppcheck Signed-off-by: kminoda * add more suppression Signed-off-by: kminoda * update cppcheck version Signed-off-by: kminoda * update suppressions list Signed-off-by: kminoda * add two workflows Signed-off-by: kminoda * change name Signed-off-by: kminoda * change timing of all ci Signed-off-by: kminoda * update Signed-off-by: kminoda * update Signed-off-by: kminoda * update Signed-off-by: kminoda * fix Signed-off-by: kminoda * update name Signed-off-by: kminoda * fix mistake Signed-off-by: kminoda * add echo in diff Signed-off-by: kminoda * update regex Signed-off-by: kminoda * add statistics for cppcheck-all Signed-off-by: kminoda * avoid checking cppcheck dir Signed-off-by: kminoda * error-exit Signed-off-by: kminoda * dummy test Signed-off-by: kminoda * upload file even if it fails Signed-off-by: kminoda * fix bug Signed-off-by: kminoda * revert unnecessary commit Signed-off-by: kminoda * minor fix Signed-off-by: kminoda * dummy test Signed-off-by: kminoda * dummy test Signed-off-by: kminoda * modify to show cppcheck result even when failure Signed-off-by: kminoda * finalize PR Signed-off-by: kminoda * fix pre-commit Signed-off-by: kminoda * fix pre-commit Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .cppcheck_suppressions | 59 ++++++++++++++++++ .github/workflows/cppcheck-all.yaml | 60 ++++++++++++++++++ .github/workflows/cppcheck-differential.yaml | 65 ++++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 .cppcheck_suppressions create mode 100644 .github/workflows/cppcheck-all.yaml create mode 100644 .github/workflows/cppcheck-differential.yaml diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 0000000000000..5b140152e7f08 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,59 @@ +arrayIndexThenCheck +assignBoolToFloat +checkersReport +constParameterPointer +constParameterReference +constStatement +constVariable +constVariablePointer +constVariableReference +containerOutOfBounds +cstyleCast +ctuOneDefinitionRuleViolation +current_deleted_index +duplicateAssignExpression +duplicateBranch +duplicateBreak +duplicateCondition +duplicateExpression +funcArgNamesDifferent +functionConst +functionStatic +invalidPointerCast +knownConditionTrueFalse +missingInclude +missingIncludeSystem +multiCondition +noConstructor +noExplicitConstructor +noValidConfiguration +obstacle_cruise_planner +passedByValue +preprocessorErrorDirective +redundantAssignment +redundantContinue +redundantIfRemove +redundantInitialization +returnByReference +selfAssignment +shadowArgument +shadowFunction +shadowVariable +stlFindInsert +syntaxError +uninitMemberVar +unknownMacro +unmatchedSuppression +unpreciseMathCall +unreadVariable +unsignedLessThanZero +unusedFunction +unusedScopedObject +unusedStructMember +unusedVariable +useInitializationList +useStlAlgorithm +uselessCallsSubstr +uselessOverride +variableScope +virtualCallInConstructor diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-all.yaml new file mode 100644 index 0000000000000..db3bd5d259895 --- /dev/null +++ b/.github/workflows/cppcheck-all.yaml @@ -0,0 +1,60 @@ +name: cppcheck-all + +on: + pull_request: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-all: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 0000000000000..914abd7df86ea --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,65 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Get changed files + id: changed-files + run: | + git fetch origin ${{ github.base_ref }} --depth=1 + git diff --name-only FETCH_HEAD ${{ github.sha }} > changed_files.txt + cat changed_files.txt + + - name: Run Cppcheck on changed files + continue-on-error: true + id: cppcheck + run: | + files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) + if [ -n "$files" ]; then + echo "Running Cppcheck on changed files: $files" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt + else + echo "No C++ files changed." + touch cppcheck-report.txt + fi + shell: bash + + - name: Show cppcheck-report result + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 From f0610736e1ebebe57a35c8df7cf1f99bede0dacb Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 5 Jun 2024 16:01:59 +0200 Subject: [PATCH 113/120] refactor(autoware_velocity_virtual_traffic_light_module): prefix package with autoware_ and move code to the autoware namespace (#7155) Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- .../behavior_planning.launch.xml | 2 +- planning/.pages | 2 +- .../README.md | 2 +- .../package.xml | 2 +- .../test/src/test_node_interface.cpp | 37 +++++++++++------- .../CMakeLists.txt | 2 +- .../README.md | 0 .../config/virtual_traffic_light.param.yaml | 0 .../docs/V2X_support_traffic_light.png | Bin .../docs/intersection-coordination.png | Bin .../docs/keep_stopping.svg | 0 .../docs/restart.svg | 0 .../docs/restart_prevention.svg | 0 .../package.xml | 4 +- .../plugins.xml | 3 ++ .../src/debug.cpp | 4 +- .../src/manager.cpp | 7 ++-- .../src/manager.hpp | 7 +++- .../src/scene.cpp | 9 ++++- .../src/scene.hpp | 8 +++- .../plugins.xml | 3 -- 22 files changed, 57 insertions(+), 37 deletions(-) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/CMakeLists.txt (83%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/README.md (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/config/virtual_traffic_light.param.yaml (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/docs/V2X_support_traffic_light.png (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/docs/intersection-coordination.png (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/docs/keep_stopping.svg (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/docs/restart.svg (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/docs/restart_prevention.svg (100%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/package.xml (89%) create mode 100644 planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/src/debug.cpp (98%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/src/manager.cpp (92%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/src/manager.hpp (86%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/src/scene.cpp (98%) rename planning/{behavior_velocity_virtual_traffic_light_module => autoware_behavior_velocity_virtual_traffic_light_module}/src/scene.hpp (93%) delete mode 100644 planning/behavior_velocity_virtual_traffic_light_module/plugins.xml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index dd73e823b6c05..451ade3771f61 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -147,6 +147,7 @@ perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp +planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @@ -174,7 +175,6 @@ planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kur planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 29fe7f7071126..154a3019f1c17 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -136,7 +136,7 @@ /> ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_virtual_traffic_light_module autoware_lint_common behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module @@ -78,7 +79,6 @@ behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module - behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 3f60c904d6eb4..7cbe0f65a1fc2 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -54,12 +54,19 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - const auto get_behavior_velocity_module_config = [](const std::string & module) { + // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* + const auto get_behavior_velocity_module_config_no_prefix = [](const std::string & module) { const auto package_name = "behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); return package_path + "/config/" + module + ".param.yaml"; }; + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + std::vector module_names; module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); module_names.emplace_back("behavior_velocity_planner::WalkwayModulePlugin"); @@ -68,7 +75,7 @@ std::shared_ptr generateNode() module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::VirtualTrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); @@ -89,20 +96,20 @@ std::shared_ptr generateNode() velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", velocity_smoother_dir + "/config/Analytical.param.yaml", behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config_no_prefix("blind_spot"), + get_behavior_velocity_module_config_no_prefix("crosswalk"), + get_behavior_velocity_module_config_no_prefix("walkway"), + get_behavior_velocity_module_config_no_prefix("detection_area"), + get_behavior_velocity_module_config_no_prefix("intersection"), + get_behavior_velocity_module_config_no_prefix("no_stopping_area"), + get_behavior_velocity_module_config_no_prefix("occlusion_spot"), + get_behavior_velocity_module_config_no_prefix("run_out"), + get_behavior_velocity_module_config_no_prefix("speed_bump"), + get_behavior_velocity_module_config_no_prefix("stop_line"), + get_behavior_velocity_module_config_no_prefix("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane"), - get_behavior_velocity_module_config("no_drivable_lane")}); + get_behavior_velocity_module_config_no_prefix("out_of_lane"), + get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt similarity index 83% rename from planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index a1c00cf49db29..9d1fe762262dd 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_virtual_traffic_light_module) +project(autoware_behavior_velocity_virtual_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_virtual_traffic_light_module/README.md b/planning/autoware_behavior_velocity_virtual_traffic_light_module/README.md similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/README.md rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/README.md diff --git a/planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/restart.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/restart.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml similarity index 89% rename from planning/behavior_velocity_virtual_traffic_light_module/package.xml rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 2cf3847c66935..cd35d4308c26a 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_virtual_traffic_light_module + autoware_behavior_velocity_virtual_traffic_light_module 0.1.0 - The behavior_velocity_virtual_traffic_light_module package + The autoware_behavior_velocity_virtual_traffic_light_module package Kosuke Takeuchi Tomoya Kimura diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..2402fc13469b9 --- /dev/null +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 83b5e6317aeb0..2f389d5d1104f 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -28,7 +28,7 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::toMsg; using namespace std::literals::string_literals; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -147,4 +147,4 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp similarity index 92% rename from planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index cd8047cff24e0..5a32cfd2f74f0 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -24,10 +24,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; using tier4_autoware_utils::getOrDeclareParameter; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -76,9 +77,9 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( return id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::VirtualTrafficLightModulePlugin, + autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp similarity index 86% rename from planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index fa41e50d96e3f..c73bb0d706008 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -27,8 +27,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PluginWrapper; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::SceneModuleManagerInterface; class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface { public: @@ -48,6 +51,6 @@ class VirtualTrafficLightModulePlugin : public PluginWrapper #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PlanningBehavior; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::VelocityFactor; +namespace arc_lane_utils = ::behavior_velocity_planner::arc_lane_utils; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { using tier4_autoware_utils::calcDistance2d; @@ -618,4 +623,4 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp similarity index 93% rename from planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 7715bb127962f..c83ff4e0607ef 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -30,8 +30,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PathWithLaneId; +using ::behavior_velocity_planner::Pose; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopReason; class VirtualTrafficLightModule : public SceneModuleInterface { public: @@ -126,5 +130,5 @@ class VirtualTrafficLightModule : public SceneModuleInterface tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml deleted file mode 100644 index 943ef175aa8f8..0000000000000 --- a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - From 4daa6a9f4a9a81fd3cacccb7362e5eb845d70a69 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 5 Jun 2024 23:18:40 +0900 Subject: [PATCH 114/120] fix(bpp): overwrite turn signal by latter module (#7045) * fix(bpp): overwrite turn signal by latter module Signed-off-by: satoshi-ota * fix(avoidance): return previous module turn signal if it's in success state Signed-off-by: satoshi-ota * fix(avoidance): don't output turn signal if there is huge lateral deveation Signed-off-by: satoshi-ota * refactor: small update Signed-off-by: satoshi-ota * chore: use lower case Signed-off-by: satoshi-ota * refactor: remove redundant function call Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 14 ++++- .../src/goal_planner_module.cpp | 2 +- .../src/scene.cpp | 8 +-- .../turn_signal_decider.hpp | 16 ++++++ .../src/turn_signal_decider.cpp | 55 ++++++++++++++++--- 5 files changed, 81 insertions(+), 14 deletions(-) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 0a6d490f7fbf1..48dae73231b9f 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -867,10 +867,12 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() if (data.state == AvoidanceState::SUCCEEDED) { removeRegisteredShiftLines(State::SUCCEEDED); + return getPreviousModuleOutput(); } if (data.state == AvoidanceState::CANCEL) { removeRegisteredShiftLines(State::FAILED); + return getPreviousModuleOutput(); } if (data.yield_required) { @@ -915,11 +917,21 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; }; + const auto is_large_deviation = [this](const auto & path) { + constexpr double threshold = 1.0; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto lateral_deviation = + motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + return std::abs(lateral_deviation) > threshold; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_large_deviation(spline_shift_path.path)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; @@ -930,7 +942,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9bfef90668e8e..4acc4dce743ad 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1092,7 +1092,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2d88a820e0fae..e1c4289b57612 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -204,7 +204,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() } // check the priority of turn signals - return getTurnSignalDecider().use_prior_turn_signal( + return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } @@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const output.path = abort_path_->path; extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -252,7 +252,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -297,7 +297,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index da39bf0d0d0ba..7e335b09e4fd3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -64,6 +64,17 @@ struct TurnSignalInfo hazard_signal.command = HazardLightsCommand::NO_COMMAND; } + TurnSignalInfo(const Pose & start, const Pose & end) + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + + desired_start_point = start; + desired_end_point = end; + required_start_point = start; + required_end_point = end; + } + // desired turn signal TurnIndicatorsCommand turn_signal; HazardLightsCommand hazard_signal; @@ -93,6 +104,11 @@ class TurnSignalDecider const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const; + TurnSignalInfo use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index fb15391550980..146fb03cc00f2 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -392,6 +392,40 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) { + return new_signal; + } + + if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + return new_signal; + } + + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, + nearest_seg_idx) - + base_link2front_; + }; + + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point); + if (dist_to_new_desired_start > dist_to_original_desired_end) { + return original_signal; + } + + return new_signal; +} + TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, @@ -615,6 +649,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { + using tier4_autoware_utils::getPose; + const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -667,16 +703,19 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const auto relative_shift_length = end_shift_length - start_shift_length; + const auto p_path_start = getPose(path.path.points.front()); + const auto p_path_end = getPose(path.path.points.back()); + // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the vehicle does not shift anymore, we turn off the blinker if ( std::fabs(end_shift_length - current_shift_length) < p.turn_signal_remaining_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto get_command = [](const auto & shift_length) { @@ -691,7 +730,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -708,13 +747,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } lanelet::ConstLanelet lanelet; const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -729,13 +768,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( !is_pull_out && !existShiftSideLane( start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // Check if the ego will cross lane bounds. // Note that pull out requires blinkers, even if the ego does not cross lane bounds if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the ego has stopped and its close to completing its shift, turn off the blinkers @@ -744,7 +783,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( if (isNearEndOfShift( start_shift_length, end_shift_length, ego_pose.position, current_lanelets, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } } From 34437034fb18bf4f74f648df26cb1f677d0837b7 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 6 Jun 2024 08:59:53 +0900 Subject: [PATCH 115/120] docs(planning): fix path to documents (#7233) * docs(avoidance): fix path to documents Signed-off-by: Zulfaqar Azmi * fix naming Signed-off-by: Zulfaqar Azmi * fix velocity smoother and add remaining dist time calculator Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- planning/.pages | 11 ++++++----- .../README.md | 4 ++-- .../schema/static_obstacle_avoidance.schema.json | 6 +++--- .../README.md | 2 +- planning/behavior_path_lane_change_module/README.md | 2 +- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/planning/.pages b/planning/.pages index dfa8d397a5834..544e4c0cb2772 100644 --- a/planning/.pages +++ b/planning/.pages @@ -10,13 +10,13 @@ nav: - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance': planning/behavior_path_avoidance_module - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module + - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module + - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/autoware_behavior_velocity_template_module @@ -67,9 +67,9 @@ nav: - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ - 'Available Modules': - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - - 'Motion Velocity Smoother': - - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja + - 'Velocity Smoother': + - 'About Velocity Smoother': planning/autoware_velocity_smoother + - 'About Velocity Smoother (Japanese)': planning/autoware_velocity_smoother/README.ja - 'Scenario Selector': planning/scenario_selector - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': @@ -79,6 +79,7 @@ nav: - 'Route Handler': planning/route_handler - 'RTC Interface': planning/rtc_interface - 'Additional Tools': + - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - 'RTC Replayer': planning/rtc_replayer - 'Planning Debug Tools': - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index 0b29d463bb105..94784fe6771c0 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -9,8 +9,8 @@ Each module performs the following roles. Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided. Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area. -Avoidance functions are also provided by the [Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_avoidance_module/), but these modules have different roles. -The Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. +Static obstacle's avoidance functions are also provided by the [Static Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_behavior_path_static_obstacle_avoidance_module/), but these modules have different roles. +The Static Obstacle Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. On the other hand, this module can avoid moving objects. For this reason, the word "dynamic" is used in the module's name. The table below lists the avoidance modules that can handle each situation. diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index e0f1156172932..cb150514ac372 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -1,9 +1,9 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for behavior_path_avoidance_module", + "title": "Parameters for behavior_path_static_obstacle_avoidance_module", "type": "object", "definitions": { - "behavior_path_avoidance_module": { + "behavior_path_static_obstacle_avoidance_module": { "type": "object", "properties": { "resample_interval_for_planning": { @@ -1473,7 +1473,7 @@ "type": "object", "properties": { "avoidance": { - "$ref": "#/definitions/behavior_path_avoidance_module" + "$ref": "#/definitions/behavior_path_static_obstacle_avoidance_module" } }, "required": ["avoidance"], diff --git a/planning/behavior_path_avoidance_by_lane_change_module/README.md b/planning/behavior_path_avoidance_by_lane_change_module/README.md index d91d7116ee056..036de718ccde8 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/README.md +++ b/planning/behavior_path_avoidance_by_lane_change_module/README.md @@ -13,7 +13,7 @@ This module is designed as one of the obstacle avoidance features and generates ## Inner-workings / Algorithms -Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](../behavior_path_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Static Object Avoidance Module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. Check that the following conditions are satisfied after the filtering process for the avoidance target. diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 9cbd864be9dbb..09603bedf27df 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -296,7 +296,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. From bc8c08b89079c79fe87f63a53f758da218063a66 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 6 Jun 2024 09:49:11 +0900 Subject: [PATCH 116/120] fix(autoware_pose_covariance_modifier): change log output from screen to both (#7198) change log output from screen to both Signed-off-by: a-maumau Co-authored-by: SakodaShintaro --- .../launch/pose_covariance_modifier.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml index 0395496ae0b78..e2b90b2160a62 100755 --- a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml +++ b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml @@ -5,7 +5,7 @@ - + From 5935138b77d847656a18f3415cf7f467a8fd4d26 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Thu, 6 Jun 2024 10:05:58 +0900 Subject: [PATCH 117/120] feat(yabloc_image_processing): componentize yabloc_image_processing nodes (#7196) * replace executable with component Signed-off-by: Kento Yabuuchi * modify launch Signed-off-by: Kento Yabuuchi * fix line_segments_overlay namespace & node_name Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix * uncomment lanelet2_overlay Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../yabloc_image_processing/CMakeLists.txt | 85 +++++++++---------- .../graph_segment/graph_segment.hpp | 2 +- .../lanelet2_overlay/lanelet2_overlay.hpp | 2 +- .../line_segment_detector.hpp | 2 +- .../line_segments_overlay.hpp | 2 +- .../segment_filter/segment_filter.hpp | 2 +- .../launch/image_processing.launch.xml | 8 +- .../launch/overlay.launch.xml | 4 +- .../yabloc_image_processing/package.xml | 1 + .../src/graph_segment/graph_segment_core.cpp | 7 +- .../src/graph_segment/graph_segment_node.cpp | 23 ----- .../lanelet2_overlay_core.cpp | 7 +- .../lanelet2_overlay_node.cpp | 23 ----- .../line_segment_detector_core.cpp | 6 +- .../line_segment_detector_node.cpp | 23 ----- .../line_segments_overlay_core.cpp | 7 +- .../line_segments_overlay_node.cpp | 23 ----- .../segment_filter/segment_filter_core.cpp | 7 +- .../segment_filter/segment_filter_node.cpp | 23 ----- .../src/undistort/undistort_node.cpp | 13 +-- 20 files changed, 83 insertions(+), 187 deletions(-) delete mode 100644 localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp delete mode 100644 localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp delete mode 100644 localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp delete mode 100644 localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp delete mode 100644 localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp diff --git a/localization/yabloc/yabloc_image_processing/CMakeLists.txt b/localization/yabloc/yabloc_image_processing/CMakeLists.txt index 58437c085c4b2..2af75ab237585 100644 --- a/localization/yabloc/yabloc_image_processing/CMakeLists.txt +++ b/localization/yabloc/yabloc_image_processing/CMakeLists.txt @@ -14,56 +14,55 @@ find_package(OpenCV REQUIRED) # PCL find_package(PCL REQUIRED COMPONENTS common) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/line_segment_detector/line_segment_detector_core.cpp + src/graph_segment/graph_segment_core.cpp + src/graph_segment/similar_area_searcher.cpp + src/segment_filter/segment_filter_core.cpp + src/undistort/undistort_node.cpp + src/line_segments_overlay/line_segments_overlay_core.cpp + src/lanelet2_overlay/lanelet2_overlay_core.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + # =================================================== # Executable -# line segment detector -set(TARGET line_segment_detector_node) -ament_auto_add_executable(${TARGET} - src/line_segment_detector/line_segment_detector_node.cpp - src/line_segment_detector/line_segment_detector_core.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::graph_segment::GraphSegment" + EXECUTABLE yabloc_graph_segment_node + EXECUTOR SingleThreadedExecutor +) -# graph based segmentation -set(TARGET graph_segment_node) -ament_auto_add_executable(${TARGET} - src/graph_segment/graph_segment_node.cpp - src/graph_segment/graph_segment_core.cpp - src/graph_segment/similar_area_searcher.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::lanelet2_overlay::Lanelet2Overlay" + EXECUTABLE yabloc_lanelet2_overlay_node + EXECUTOR SingleThreadedExecutor +) -# segment filter -set(TARGET segment_filter_node) -ament_auto_add_executable(${TARGET} - src/segment_filter/segment_filter_node.cpp - src/segment_filter/segment_filter_core.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segment_detector::LineSegmentDetector" + EXECUTABLE yabloc_line_segment_detector_node + EXECUTOR SingleThreadedExecutor +) -# undistort -set(TARGET undistort_node) -ament_auto_add_executable(${TARGET} - src/undistort/undistort_node.cpp) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segments_overlay::LineSegmentsOverlay" + EXECUTABLE yabloc_line_segments_overlay_node + EXECUTOR SingleThreadedExecutor +) -# line_segments_overlay -set(TARGET line_segments_overlay_node) -ament_auto_add_executable(${TARGET} - src/line_segments_overlay/line_segments_overlay_core.cpp - src/line_segments_overlay/line_segments_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::segment_filter::SegmentFilter" + EXECUTABLE yabloc_segment_filter_node + EXECUTOR SingleThreadedExecutor +) -# lanelet2_overlay -set(TARGET lanelet2_overlay_node) -ament_auto_add_executable(${TARGET} - src/lanelet2_overlay/lanelet2_overlay_core.cpp - src/lanelet2_overlay/lanelet2_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::undistort::UndistortNode" + EXECUTABLE yabloc_undistort_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp index a1e9c90eaa362..6c2e670a7a72e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp @@ -32,7 +32,7 @@ class GraphSegment : public rclcpp::Node public: using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - GraphSegment(); + explicit GraphSegment(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float target_height_ratio_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 468e765b74175..7d644376ba591 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -47,7 +47,7 @@ class Lanelet2Overlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using Float32Array = std_msgs::msg::Float32MultiArray; - Lanelet2Overlay(); + explicit Lanelet2Overlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: common::StaticTfSubscriber tf_subscriber_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 74ef82dd94f59..761c581200369 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -42,7 +42,7 @@ class LineSegmentDetector : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; - LineSegmentDetector(); + explicit LineSegmentDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Subscription::SharedPtr sub_image_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp index abbc2f75725ed..00f01be5984e5 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp @@ -34,7 +34,7 @@ class LineSegmentsOverlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using LineSegment = pcl::PointXYZLNormal; using LineSegments = pcl::PointCloud; - LineSegmentsOverlay(); + explicit LineSegmentsOverlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: void on_image(const Image::ConstSharedPtr & img_msg); diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index 766bb77d4da85..c8f036f2b12de 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -39,7 +39,7 @@ class SegmentFilter : public rclcpp::Node using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - SegmentFilter(); + explicit SegmentFilter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: using ProjectFunc = std::function(const Eigen::Vector3f &)>; diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index a0cad16302c2b..214772751f931 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -5,7 +5,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -27,7 +27,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index 150ffed58138d..134f3ee765476 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -10,7 +10,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 209f09fdaa7ac..416acfdc76a16 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -19,6 +19,7 @@ cv_bridge pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs tier4_autoware_utils diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index 75e14b5a4cd4b..f8d4c74dd5bf8 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::graph_segment { -GraphSegment::GraphSegment() -: Node("graph_segment"), +GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) +: Node("graph_segment", options), target_height_ratio_(declare_parameter("target_height_ratio")), target_candidate_box_width_(declare_parameter("target_candidate_box_width")) { @@ -159,3 +159,6 @@ void GraphSegment::draw_and_publish_image( } } // namespace yabloc::graph_segment + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::graph_segment::GraphSegment) diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp deleted file mode 100644 index d11701e115eff..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/graph_segment/graph_segment.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 367ff51567147..107d861364038 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -28,8 +28,8 @@ namespace yabloc::lanelet2_overlay { -Lanelet2Overlay::Lanelet2Overlay() -: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40} +Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options) +: Node("lanelet2_overlay", options), tf_subscriber_(get_clock()), pose_buffer_{40} { using std::placeholders::_1; @@ -211,3 +211,6 @@ void Lanelet2Overlay::make_vis_marker( } } // namespace yabloc::lanelet2_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::lanelet2_overlay::Lanelet2Overlay) diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp deleted file mode 100644 index 941ac9d84ab16..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 57f2302fd5c5a..c613642628499 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -23,7 +23,8 @@ namespace yabloc::line_segment_detector { -LineSegmentDetector::LineSegmentDetector() : Node("line_detector") +LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options) +: Node("line_detector", options) { using std::placeholders::_1; @@ -106,3 +107,6 @@ std::vector LineSegmentDetector::remove_too_outer_elements( } } // namespace yabloc::line_segment_detector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segment_detector::LineSegmentDetector) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp deleted file mode 100644 index 42965ae853ea7..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 0ee4115a39760..7e06de81fbd18 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::line_segments_overlay { -LineSegmentsOverlay::LineSegmentsOverlay() -: Node("overlay_lanelet2"), +LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) +: Node("line_segments_overlay", options), max_buffer_size_(static_cast(declare_parameter("max_buffer_size", 5))) { using std::placeholders::_1; @@ -90,3 +90,6 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l } } // namespace yabloc::line_segments_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segments_overlay::LineSegmentsOverlay) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp deleted file mode 100644 index cac6a6a0c5a66..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index cb1505903b210..df0aa7d65c617 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::segment_filter { -SegmentFilter::SegmentFilter() -: Node("segment_filter"), +SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) +: Node("segment_filter", options), image_size_(declare_parameter("image_size")), max_range_(declare_parameter("max_range")), min_segment_length_(declare_parameter("min_segment_length")), @@ -282,3 +282,6 @@ std::set SegmentFilter::filter_by_mask( } } // namespace yabloc::segment_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::segment_filter::SegmentFilter) diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp deleted file mode 100644 index ea51babceee60..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/segment_filter/segment_filter.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7fc9ad785dbe2..0714b6c8091c8 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -37,8 +37,8 @@ class UndistortNode : public rclcpp::Node using CameraInfo = sensor_msgs::msg::CameraInfo; using Image = sensor_msgs::msg::Image; - UndistortNode() - : Node("undistort"), + explicit UndistortNode(const rclcpp::NodeOptions & options) + : Node("undistort", options), OUTPUT_WIDTH(declare_parameter("width")), OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) { @@ -166,10 +166,5 @@ class UndistortNode : public rclcpp::Node }; } // namespace yabloc::undistort -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::undistort::UndistortNode) From 42a63401c2fbbc1f9bbe6d1da5f4e750b419473d Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 6 Jun 2024 11:23:51 +0900 Subject: [PATCH 118/120] fix(map_loader): add log output (#7203) add log output Signed-off-by: a-maumau Co-authored-by: SakodaShintaro --- map/map_loader/launch/lanelet2_map_loader.launch.xml | 6 +++--- map/map_loader/launch/pointcloud_map_loader.launch.xml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index ea0157620ce43..5baee911d6572 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -4,16 +4,16 @@ - + - + - + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 3e447456bb623..3633a8fa39a6b 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -3,7 +3,7 @@ - + From dd3a7a822a615c80f3b5266a051cef80fb4c6a61 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 6 Jun 2024 12:51:40 +0900 Subject: [PATCH 119/120] refactor(simple_planning_simulator): remove static odom tf publisher (#7265) Signed-off-by: Maxime CLEMENT --- .../launch/simple_planning_simulator.launch.py | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 0070646a20d49..a8eb88f55df19 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -14,7 +14,6 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -78,22 +77,7 @@ def launch_setup(context, *args, **kwargs): ], ) - map_to_odom_tf_publisher = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_map_to_odom_tf_publisher", - output="screen", - arguments=[ - "--frame-id", - "map", - "--child-frame-id", - "odom", - ], - ) - - group = GroupAction([simple_planning_simulator_node, map_to_odom_tf_publisher]) - - return [group] + return [simple_planning_simulator_node] def generate_launch_description(): From 043a7e9a9a33d98763a2d1e1812b6fb913c931a0 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 6 Jun 2024 13:49:58 +0900 Subject: [PATCH 120/120] feat(gyro_odometer): add diagnostic (#7188) * add diagnostic Signed-off-by: Yamato Ando * fix diag time stamp Signed-off-by: Yamato Ando * style(pre-commit): autofix * fix spellcheck Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro --- localization/gyro_odometer/CMakeLists.txt | 1 + localization/gyro_odometer/README.md | 15 + .../gyro_odometer/diagnostics_module.hpp | 65 ++++ .../gyro_odometer/gyro_odometer_core.hpp | 6 + .../gyro_odometer/media/diagnostic.png | Bin 0 -> 41146 bytes localization/gyro_odometer/package.xml | 1 + .../gyro_odometer/src/diagnostics_module.cpp | 110 +++++++ .../gyro_odometer/src/gyro_odometer_core.cpp | 300 ++++++++++-------- 8 files changed, 361 insertions(+), 137 deletions(-) create mode 100644 localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp create mode 100644 localization/gyro_odometer/media/diagnostic.png create mode 100644 localization/gyro_odometer/src/diagnostics_module.cpp diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index a832383ff4761..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- | +| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none | +| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none | +| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none | +| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none | +| `imu_queue_size` | the size of gyro_queue. | none | none | +| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..49b881900b997 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 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. + +#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void addKeyValue(const std::string & key, const T & value); + void updateLevelAndMessage(const int8_t level, const std::string & message); + void publish(const rclcpp::Time & publish_time_stamp); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 2926589bd2da9..3b2da504f938a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,7 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "gyro_odometer/diagnostics_module.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" @@ -52,6 +53,7 @@ class GyroOdometerNode : public rclcpp::Node void callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concatGyroAndOdometer(); void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr @@ -74,8 +76,12 @@ class GyroOdometerNode : public rclcpp::Node bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_ros_time_; + rclcpp::Time latest_imu_ros_time_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000000000000000000000000000000..f2324f4079d0db7f1c407841cbc2c925e645eef9 GIT binary patch literal 41146 zcmdqIV|OM^^fwyYHYT=h+s4GUc?A>OoY=PQWMbR4?UVa|&U5hM`2wfc>Q$?&@uI4F z@A_efE6Pj2!{ERG0Rh2FNs1~10Rcz;oLEp0KRs0_Yi2(Un6t2yD%8&bfHDdDxyE)8 z({xd>H*;|}bTS1px3jl3rE@lRGBvexwy<}(2JPYhd5FbQRntYp$<)xr(%z0p)za4V zrxOT>nUR_Gu&t4i?I<&hh3P1@h=_^hC_R9U`7kVmk^Q(NfdqSp8VHC8NJ>;l)g$v_ z)71riWEQUE1_?L$8Ey z3pw3e?ft!*vDZNG9UenZ4@EhTA&|-Vx!(3V&7!a6@Ba6Bh+$8&vpHgn2`~#lOaAy) zQEmGkfULt>szvYKN)5_fvFMFf;i?Z9`jVtbhCD_ymwB?l}KB!A%N&ad@1m>9!pc>#w|oJR>pmXqyX zn?Yu2gVG!Wsamjs_25f+mJ=-QHFNldFR_gg4UWJgsN+~vZX)SD+kSZ5oW89zI*;FY zX58I0EDM>R2PZ^t4b><#C;#b6DMj@+2Vpy>+lV|bkFe|V%6^*EXR>d z08^`TM>Yq8^X?YZGFMWSxqQ#rTGwr<0RkJqSKa2gh=EL}0iG5czT3X#C zp2+3ybZ5;SW{9kf=FNcsL2h)u(p;+EkBi_1&&p0v%IhT{3-oe=<%_$4MaEZ|*We~{ z`v!V*-`AsrIQhPEFCwsj;QA+d3PLY_lIbCKUUIT248Vi1?N*hk_0_x;X2k}7JIyO; zs;l{DS|>F)HVZ-hXTY|Me>HDUQE|-AVxQ<1uzviu%~Po`Jk$n}<4^ySKER!CRf=2L zr0zhsg*C9GKdt5qp{vJ(a#<17KPiqV@onMqY6qFkjdw@)u_6d7l|WP$aJx%S0z~w< z@C&fe$qSNIWq61ZrTyvKznfI4JX*r5GQ#&jeC%AgXtfWfMO~@ox zsDLUeYt63CC$m$B13^iuuC>k#i6^(ksIkfOT6_88GbnJMq9p-QUsT*?kYF>kUS_%^ z@prYcbD0yac~VhHNw0YXGcSyXNJFc(w(JI+n$*6Y|8FTx=F*wm)V?-qvktWpOs2H+8AG+%XZ?};wYuBKwqOMi$1Js_+tt_ zL*u>#(P~ESa?}mBdR8`Jc>kk40-kLM$N6QWX`|kfs>Dw-bxWG=e6mw2y%1nd=8dK@ z&*cSVUKzDJEYNK>;y|M<27kP@P)S#HEr#jJ>HS?F?&z5DdfaOEphkV7cuOb$=}gjM zr~Qus$3yXOEg`EyoRoR{P8N0Krb!^gNl46e zV}O_#$BW}LqQVV-ukSZ*tHY2G`S|N`AnR@al9+CrDaG@}+Qc@0r8tEvlE4ei{a4bq zSNgVh?RIe#MFz*mBXD42(TiATU2i8EnLju@7J6=>ZLb!G=~xS9ZFECGT%!%^ zJH07?2ZCjHOEjUl6_t?CG5vVjhwwT%yg5Z}yZkS-tBVe%z`{Qrp+Sn!;8}maCcsFf z_D#POr$Z774If8SEqqaElitNw zFNeaZd)JMW>tc=$OkP~q6hE&s$u=a_g;uO=s!iSycQ~aaUpp!BV3x}u921kI*Vz*J zEcya?j+CanPW|e$=-v-C$|NB#v@~Xeu&s3tw2ZugH=tDDPB=nEBFbI3j4SeoVZR&B=zYAX2%R$fKBH(x5A8t{}|JLg5c z1w8KYn}ia(SI`{1y`QAmjM;;E#+H)WTkv{b_%7V|E?x`|+KDjplh&=t*SnG}G*C*tIn! zx+Aj^-*bJ_1wFXAddX@jQpm?76~;Z<=tme5QvLc8A?%9u89^3MrV2OvV{AtZOYy;qJI?))R(HMvZd7Ab0e4JZ=lHsDbrHj zzeZCu+b(7R{HokrV59YthG=jelayIQc-^0O=6-SZKe@?p|dGjJ_ zcuHhyt;l{DP%si69#a$(+g$f~;`ZQt1SC{k&m`#tWaB}=n0t<+>!2twINuHmUMe9zEMUJ-6K(xmahi`K@E`mx16Q;A>GS(YQIPn%K_ME4O6Zx z)6RT?yBEM3ha#DLK6Km1L4bSZq=V|jG0uCE3~y6*{{7Y1a!n?f!_dYv{JW?a9= z7_R7vH%Z_2bL4c5_km=~$|B&r>fRdpbV+BBl7$&0#fH6?dDcU1$bJ)3_ZK3F`|ox(OZ?q$dYf}ua6u_k8AJhlZmyQy>rs>8tKBH4JA-}f<7|7pNzQ*t zhYt&A)?@Lo9L^FK((-;CzwlL8A2+oK>Q8CBUObMB^6Du4amXzHd)#i)N48A()Q*=`0Oj4n&&qxFgt1Dqv zkIy>N#{|~SN^My)+#Ajs4$oOMH7d;C$xeSZ7}b7}LO3)bP1r+i*3Har=i2lih8V`x z=0%1pqlf{jjgbTs+w8Xc!$Sg&hmN3=8pjRMp$Sh)d@q;%j~f^r=@mzEM>QP@EbHKR zR0{vI6f}wErRcMmLn~UYkMltN92Z~A9@O8RdY7UAckxr!0&Im<<7S8au>yL5?1sM?Ew+bi_8IfP6Bo^t6!jvIc;AC}z3}`z zYS0TrHY_UCl!}^)bQHPRFSruMr_Id1Zp(%~TE) z;EYBE&Cl4p+Cw;Yc7jWCtXAGT)~9GR;|>ZEBVV;TEFju7m5Nm7$5wMpx3H$VS+l1lFD3@l-t@;MB04|$d%?LcsuZ}JNmg}dc5~G2=n=Q2UptHx5>#TG zh-MA{y&}VLy7|}#BzdPxP_#No!ivAHsZ^*cnO~32z(jKyIm;K=oTU%ptyQw}*|IKh zzp%8Ad)T$btDyLOi;E;+cQ(4A@rXmtY1I@K4p6T;`yEK-W+8#k5au*vC#NP zKklAlw)H`frTdSVZftH@NFsSHg8t7ez*=c(k|pO3ZRF-rAOt*~&}OfME#S0LLn&`z zma3L{`@_y#ed~TSbQeVfFqc%sXIrXHSytzkui?PumgCNKzH*e~cV9(^w_c6sbHS=w z?$YRtXr#LN=YyorhL}MtR1_nac+6BcTN&0c(A__|d-%2|8^%n72IktA`nH zC&)|*&Y__|zA7(Thq9uFD<-4b^y`G$Q%l3kLaot>W(b3XO|{ z2?%|PudUfaEf&Le4-3Qdw4!5HX{@6%7v^xLEBH0OAWjFlECl7tvW!E1cO$FwJy3?) z98CPBU!C^@!}8}10+s5W!J@$lyUytZ7Y9V>(2~V(UY!~?WZECH0xWV6@PBg@KU@U{ zA3Cjas#{Q%a6lnU-_B*30>h}c=GrQ#v{XQGzlNCo%Z6Xt=@G&N)&>fCpsr!TeBTra z-XahDKmdtU+gaRy;|!|pgG)R1xUYBLba;Ya9Qux{;y}t4I|wd>6w#{NCpD*zpJiv`yb4{8?*^qZrWB42oF%zy?XZyv$AslPvCda(3t^wiDYRB z$Q(&_Pd{LVBZ84-m&o;!C*w~jg*7WxRUNK5o=T8ytk$?y0s7>`{nzuIVcJtex#*?r zZd0AxhN9zoUu?U*(NWjaBi(RggrnK4DVLPQavc@?g^UV2zIjOIOHPVK*TS(1T#b#? z*FxCZYBo&%U(p&VbqbEw?aSXWkObWZer!L~(=Q^8TFL!-`R1hD+{9Fh#{|((3};Uj z^2uY0x^}(%x%tp+X{57=kGH7s!io3uZNo6}U!;7S0RyMzHThuSZcXI(jp0ex^aj=v z;c==iaMa_{hyACg3;rzjCNbv%MIq>9k_+u2b$}ttkXFyy`GiUy>pk9P2Z!7eKd6C# z4a=f9D5-&j0mM1U1Se)=fv{Y85V9h^&jRZD${u`lpk!i^)^))-j|7!7PG#V5YHxuM z$PN{TKMBXusX?=%w?9RRNlZ#YG*hSd$-d#4j^ z$E*15Rl0=;z339gFC_vRn4ftUcM1pwB(j`aLzimpHiTd7R^=8_jn9q{IE}pbsY-h- zVF|=%U62Z6|3h@ck(|f&MXkysJ=HS(a4b$ty8h^kxq=Z^3&_D|oO8L55iesI3C3|H zJ(n9YnZWU&NeTQ&3No35m}i(5OsJ0^rVtoonKenU^zaDPVEDarHLF&dEGX9`)oDRK zxolSVCs~v-a~0qQ5>2Xd)RIfDQR~sz4!MbzJJ-Uav5_oQ%;amrRZcuxya+MDkaR@Pn2ufHfe%_2&p!ZW0 z+aDxmXw6Wu2C6hlg3)^69ZTT)*U22p^%EtixwqI+y%ckt8oVbPF987|vfVetk$Uh81gQcsEbdksM{F%x!fd!8!Ine%H{W8!H zI?jlLN5VMZpm_U(cxDM*XW@ z@9~tNM-|Sqb85g+a|ILs0vQATwE~crVc9T)lcq^IUKL;RwFl;Ah7`oUrbUplR+ES}~1cvPNy;mp`61=7)^ z#?m8Cu+5y!SQXzE26Q&5`0a60g;-$<&hiV)_Q0^4S9qeo2|Vy4!SPSyK%Wvd`5&F({O$1)Lt6iQg<2h*Mtz_q1LQu=KmGC!Wt*?1i%#(c+RL*&n4p)O^uKBim<^Fn79Eqef%`)bm z8|i(1^ByG=L4DU8f<-P2*_e|d!`OE>kljDo^0EhR;;@-SPIH(nU1p$iHklpqqM1q% zmV}#aX>O!M_Kl-yaELQ0B;!o7XXvP>1?cHlRJ_658c^XDL<%|%%;{|JBj>|(vqt4zM&S<)LfUc0&YB}QHSS;*c`PhN>@W7dBA-RRp zbrgGcs9>13qlu;Dq1wl~7f%Nk(bu&jMJiq`b+iJu&}cm^|lGPGZsDjc;6y z(cNS{G4XX8#h(?9cWvtQ(mt2%CaiX~dKGrjZ*17%BYUFy({Jtf3LanO%&DLP6kWQG zK%r5nmg$K%$&l>0fsS}IH)~L|KNEm#=eZ;`7xC$h(BywoSL&0Og6|+{C7vPRofd{_Jyh{r0vIF=U6^@GI&JmRu_Nz6o*0p1!$W z&&XxpTzHqzt|;z)LXhLPZ!P0jaE*4;V8*9}K{^!1w|&}F&0vsJB=YypcGV&4wm>)? zjr&eMmXjEgo^*9x9=(wf8y%@su>Z4r|H}a)IUbH4ij`CV`I!vfJL2{(V4qaIAwSM;n1Gw_L0E`L zxC@+hp!hHP+S$^)I!6)7$Lbe$c4u(Fh2VWvJ>B1dNt#k6wV5P(abJxLN(B|<-I*E8)W}a`y zU?H$qhYv{9gT;MjT)1L^@3{K>^m0>6pRRkC|D{Y&vU7H&KT{J$ck@E(d&&U?YiHIS zocUK7@rr&H%bW6=8+qW64Qn;;*9X3RLc-og`X;5dX>4^I_)6UfV+UF7quKUaken8JSVA8B%FIBet`Sk2p5HG% z2L;A2whqQg549mvYw$zoh`rO0iO!!WS(4nGu=a}x!I8-rOIOGitX@3I+2wzjci)a` zW{RAE@E*BWFe}0k3YLibZcOYQZlB#1IjFG+p1l4?6SU74jCwC_UOVuPCAL=HAJzI^TWX^0f%hyTiSJ4Iz z=illxyF&V|O`>^H)KYM+*)tpYQ`TFm4;JWm7o@$!UB3SI)U5ZXnQqtF=KfuBEn(@C zO*rLcEa$b&q`BW*&1m&rhL}3hidbS-JjuVW>=;JZE4u?Dvvxq#K2Oaq}Ya#T|l98`PKo4N4q)kzI0O5Ig+5eVmi;DkQH z%ltouneoF9Q3D_ONfG`L#P&7Ezgb2@$5&RByijN@czdU%OQA;p;HUDNye$Dgs0H{Z z_y!uK$2W%A+K>tVjxlbynj%k3KYB3PYIh1ZUq&JxS{X<1A+p2C@* zv!ERul$(Cz=?e(yc`=Mj@J|pT{2Brj%a0lDt=i|8HIf%I=RoJkx=!O_r_j(egQo5%q< z?%vTqt*^`SnJc)Ve6i4Kx5XAH`Cy4p23E(+4h^pUq543=xN^hLsDiJ^1jR>Ln91^x zYQ*GrUR=!e#dH2i+a|aQh5I+%1qP=f7CVUwleztnbUf;iEesNO^bu<& zrxQe2X^2G$;A!6DCr{!cDRv+@74s_fwz&Nc%Gj^_E@hftB!wjOSpVzz{PbB#%>cf^ zw~@=p-y0Yjboyb4CnrDhgeYuSG%r*#FLb;D(zC_1d(@QiuO)xd10^)3h}hDT!{fAJ zY2G_;`LAl4o!$W5?79Y0cH+p2BFv9!k6})s7KkHgW?#x&%6c@INJe0f6=GMgyw~iC z<^^~r!137VPcR&)eSm@G$}dNT5b>2MS8Db|eUbYcWxal6l=4Jds7z>}`MS+$@+JOJ zd&6%xP94N<8v>OK{XX*CJ2PaS2WK0|B%(vYNCIKJoe_vCCXrP=XM+${V^h3DJEQJ2lL3c4rZ&#)=|S=Ja|k3- z;AZ--M}!mb>7lcpq*mKMcQN=zoL(Dt0WHxYC6NF)t*HrIGH=!#j8I25=V@9pq019U z*KW{mq(YSL%_GCfxPK*M3Uh70b`BA`h(3BAK;So+Q5bXu^WjQ`wf6`5HfQUMos(Rt zM3vP|6}y9A5&($SW)lN#51p9vs#bv*LrIMIpYZnx|0vaaxV)MwAnx(jqt8S;s;ufXsH}%MsV$Ur~&Qqx#a23G>~z zg)H>b9=dxoc2wp;ba}$vbI8e zwfq{U(}g1e(9U<|as@s;Q*74-bFiV=yE-Ve=52!Ku{%N1qTdkCRr=rQ6-5X|V<|u} zHlnh=0=+5{xqw2P)IB4|(^1%rsPnpY2XQ5wLuvFz#8(w?iTuJz55pxmZ8ks%Xa$tkt!^1U$Xvi|6o20_KE$A09aYdEI7JR;s}@PJ?tJ0;WI09)vfUBEc5X+fZ#%T~OJsZbA}mG}TP zX1i*fAW9C2R&UOM;WccsEv-NAe)laV#RDWtQ(G|FG#a9;I1#3p%tTzOgA(`T`9%#u z0p22Iip9%73rQMA4(oX@uJiI2n%^|!bo^hkZ1M1CUGu2G?-Y5 zz7qytER6AViD5`)(Vn z+2Acr;Dov5T>E=3E8G-+!@7+%SO+}6rw&G^Z_d0W=F~aK;2Cky_Qh`32tS>c(y3vP z)uIZ(ePiPqaTvcc$T;lOac~1x;C)9%3T#UlnCvgv2W@pi#peA6k8tx5q>=6(3Lp2b zA3_omJ#R)@G=CgmwhJ)Q((tyr5p^a_>Y266y`4C%&V~1Te`har*@CK&qB-ne_4ovHXfne*X#>uAOHRf|&9(~}+4zO}%wgJfRrZr<%)+r9A7Xsi<> zv1Bl581zTCD>!P6oZlf+CKt&Y>olz#XqHD=m#8aEURZoKjg3#VlWXXorS<3i=xvEi z`uo$-7nEh=P7}AfbZ6kK|BADcSpN_K>uVE9bkl}qmJ8}M`lf>rfM8}xuMyveW(?hPm5(DdI<-;>1m7hby#B~ z>z4F>gQE?!fVZklU1ydrAJmWE`d50NjI~6au?tbwqgA;}%tNHrbUKAB#(z07aG%uX9oy1G})Ami3kXm&$AvyEcfZ)Tey!&{w{&hot>%}>_op*|gTI%@_pESRfTTFQI zu-}U2Z8vjvbXMcw?EFXp6Y`i0w<`T+1-;7V2|ka#=Lbi78bU(eftpQJMFRW3Sa;E? zmLFrFLHz$?5d4pP4>$P1xI~(y0m1)&xmh^&C84Xk(UXq&gTeElO0^oyvbQUozA4z* zpZwx6|LY8+R;>l{M z7Nt-4r>!mRH6Sjb*t@&+(3L%^*k(pWW+?>O!26hec;9F21_UsY>JN+tnmaPMyXn8W zePMrFX#Eh4orNEPL;C`=vqRv(rfYklsKpvRruX1Bzfog5Mm1_pH~C4s4*2G3U5P-l zJ{9Y)urAm?oY!9^KITp7Ix(2(>4dhSbb{bkKOaP*0qEPyOTr0-nXz-I_Cp7mC&NLm zSc}!XfXk+tHLpCEhS<6T-M0$40RXDbu&bb_KF`TR3{==vz^#sbe`XuI5#*~-GSBBl zY&ZB31*ku;28V*+HTSMV!>sVNCfwmV;7^l=Q_em4pq~=VS;?GY!NH9PBk)didTGhH zF>V6RZqC(+iUuN~!Syoyj@=?y5BZVnZpq$WahXSroNLmJTYyS~zQc3+(8M!>QQyC8 z3r%_?yO^QdS$$7FJO1H3=*OMng@VI?&|umc;bKE#LTpTRBh_VX$}2yo&X5sW*&9+=a%>V~#@$c9(K&W7?a!<)*{orw3avrU5PIhkT}_Pz<;yZ7>z``DMz zac#=EgQR&)9%DRlKnK)F0@U~LG;MFFq;IL{DzWvgih0ch1+Nr*Dv|5aHasL_yQj{C zg9qkzO;$8xc5i1aoIL;?NHAcFg!SI+a1CPKJy{~+qtGD^>z=C};0X_Z9x9mL z#?snm%}*YM6P>P8L#(#C^D4}tKXfNFdHci-*?6&xx(|US!PJAqDam!pzaapPzvdf# zj_#fNEFoZ^S<3(tA>cjEx-$`rKWj9nRhsSf(5$PMGGCjC*u;!pCLeussxYuy&>iX1 z5Nd2PEngeS=^r0o-k+r(JVaYpSBJ;tM4IE#7+2*=6nC%m25(gPhcvpg`4!i#23gp= zE5z#+IrSUo3q~L45?L3CdIz3c(Mp;5|bSKtr zHm+-Pfkl_Ql8)w$N4Z9Qez*ZmN1jKR1U99vjF1nl?Ve_~eP6lwxDMaBAEkS{keTEo z)3QoQ$@XyU<_(ndE2GawdpM&Rhwc(ER{vNU-Dx+rDt@+Fgtyd==X2S(u;#&>ze_X~ zu25`4Y)>ln%I~glIeOMNwNTO2=y+=?y2;SSQngdhuCLe2KTG^jJO7hxX8R}-ks;;eHm%w)`s}W9(UlxxJcsd(iRV%6FPy)ent1Q9};DPk;bDJ5%$G zQEbW(hcPbng#*#ubAS4gKpM@a^$e^tjxxK+*B~BGtXe*{aHT{*W_DH?OT+Vmrb0?5tlzo}I0lXlEMA zOwV*(q2XJb{TTpQIZM39Ixht9$-aXsH~%@apjKVUPiUD%4>Z*tSmjmdd>gN#e}=Pp>R2_hQlkU6ggY_5uP2% zk01feF3d~W9#C0TD6!Rq{q;)tOb)xYNdACJcbA$)g8h_{{XcR{Xbs+B^SC8>&$wN+ zHzAoL>e(qc_f%^L*@0P@@D9(86F@~WEDG_;b zWf+6wizF5NJep`HGBRBOUy$bqq9J_7lkY7Y# zU#pm$Dh|i;5H?A%@eU-|At3s#@SLot#vf)Dkqn`#V^lo9;bo%)BIyJ=gDsYMukf&W zZaxow`x`=T(H$d5P}nB{jFEi#-WboTB!`=Vw++AK;H#&s0)WgVS|P*>=2x(Er`4v1 z)@c|^*NxC}?r0rms)~3dthj`OA|gD zE8zAZ{SG5UidjC$wPm2C-`w;DDsf(HwEg>hK(gM4gwbGM|%fKx)}JL3p;Rn7~@e|B5o{ zX-*S4fy;8P1gnV79H*1OoMR>{Zro>@)rKZVo*okF?(sMWens#*SQ~mlHDfUH2y->{ zcsXyEI`s*v5G9v+htG~-f+Xrj7zW_QLU44$oBeU%?N=+|!Q{n#QX0450%t!#EHlM) z%*w%Rc*;X-Y%jKLWnb^NJoa#7J3$pn(A~~@pp&`W|L1JyO4orpoqY9Q=yJ zNzI{B6VdkXK{xuP%C}AIzuDg`Qg^rc?9*LZSo=K&RM9NffESwOYwp39&GFYm+-z0v zu2EBW(*!Erg{6@4+L+8v)|B^sM5l2DAI-J32=rmIMW=r)ZHXXC`?}QSHoii2X6oSE|+ZmspySR@%c!LcXzf5Jop*^I+z{PZ^Q$;2J3nBbW+u zS0RYR1qUCmAJ=fs7nRkHz+udXLMn2oQeU39@fc{KW!gGcwzb)b=*(R&@+Q23sjAF+ z3blzD>Y7#{Fi2zugT;iDHZ#1*$r$boS^4BIBv72a-iTa# zf!8s;g(eqF{45>QMJS@u*A~n~tQ7D;N2}vJkXrcB$!0tGm_&+L7Qf1k8l(>1>!$)9 z8!%#vPk1bc#$c*(MlV^-S)_1M>JTeTeCz}>O$fpLlfmP6_~S8>|q%S3t-g_z&764Qys08aIZaRz0>|YwplV76HM6 zJ36NOPOk5I_MCyJ76^l>?hXf^eU=H`S|sPK$PjvGiF+z1nfC+^sfs}Hv9Ul4YleK) zTG-NcF zMqqyj79a^?qay(pZNhas(>-uJ2DO0>IeCJ2xpV?%tODW6TJ21b>hN(a=~M8 zyAM}+dK%*|v~EzVsTV9Ypv<40gq8^+U0_tDj>Tw7HN4yr9h~^po2>CMk^0RiZ;B{^ z3r@7%7rx&@44OL%mDr7!PqG+y;H~+6?xf2CGE5`G$Z-U0A{F>g+BA1EIcn4(2(B04 z%$9N`3rdFGFB5$1h*E}xr-svvV!+Et$nxEu^V&G&LP{aX|0 zYero8O8M3CTfDCH1d9pNxUr`!3#c^w*oDK7%t6-fxv z8NKrI>S5Dh83UXJ>a-~dx6)vSN)W^@UJ!R&6VV82Jk#tBCoQ|-@3|?G)2?x#07As4 zmcy{U66PApR=V2AZga0gM_68~Oj+F?=;O`6Uxv(>#TCP1#sdb?E|ljWA{xYVVBAsn zt+WIJ2veaz0s1z0p$#&A#lg6b zch{mK^%plLXyRa?_^xg^yDQZgM|Mux7&#Im+xbk!hl?$^{TvdQeZRj-TRbz@)m?$^ zvxXGRNC`s&%|*BfnQjLc{xhN#o)*to1&Vki$8uI*(l8P7QO>tJE<65#vP7|+gF za^lWmvYej*1F5|~h@|Te8=R~s8{R9i7H)3u-K$ZvW-dYtNkhX-BL#ULNwe;wU7(?; zX6HeW^fx4Rw#76k1!25NJq>B@L@CH!(uZcIX;O0p3bGN1CG)0=_Mxv`p zvsGn&Y8^%IZX_u@ci@p{ztq7N1pc4`5>t-`1Y|xmjsf^ZGauUtBqdh7u zP*zqfa?drl@MOp84AQu_*rA{3=GBro9iCD4b(yiW*vB4C8CW)B--7*4>)Gzj8skG< zZ?H#}P$`lE%@d9LY+j5z_Am z>hean2WqXH&*H0t%MzR!IbNn_JT|xAt^LBssXw%=l(Ib6M9y~gBoZbco0mq4#RpTW z{={a(z`u%fiF&!!%By4F*$aMG#QUy=9P3Pa6Hl%GCOXZ-%T9lZ?l94FdY)^Jp&S}= zawFsU^NtL+m5lj5)uzTR^HbubE19OflJVC|0Y2xe?V*OI>#t7Q`98z2D1y+$vEejl zV|Xu4^ih54uO8B)2BZjTnPW5Ho*jB&55mazg<(uT?7NSK_I^g2%yo3VZ*?_qrRf|Z zsD2qQz5sAp$#ypk^QnW)nJP!Wn=xGHuk8G6f&VNp_lV%IxGs9xAyYNfT%-7(?8OD! z71GygG-t2gA>|vInT#=?Ee*s_{nnzYQU6sKWAHtjA!A}b+aHJ^eW?t(l6Tr=!(#jp z2H(Q$KQ+5zy5{}oM{Neg?GMoX0unA+=n>@?&MX4?lmEk>NrL@2#z~edXD@O1h6&yu4BzEy>6R3<2pTU+mZ%Usyr^eccv$f0 zG3vcrVczXuWxeOf@e0#ixgJ{RN8%UzEh>SD36^9TNhsgBK|~XaP$@BSXtKf6j*~I1 zEh*C-L3qY9z}E`$M%$^~dau+SbL!d@M@n@aI|m8ce8{(zM)Xm0_`X$ha~(d~fKs>9 zDZu>*|EV*N@x_2P`GH+`K7H6~m9n$uh%EoTg|Q0SUaS=@^1V(Ir|u1fquuj@@v{xo zTnG^TaTdU3vtjEx5-N4A4?OCZ)w2xP__Bc?%)HTjcu^riUSt?LkjdrF3M0o~YaU_at}g!;l8pHfc$(tr zjM%g8z25Q_@j|2`Z3N2u@p?!|cuAB){hEv*G6+^cE+|D4q78$BQC2O4%1fOQxke%) z&MQWqJMVHoekT#4D)R?(**;WNBb=Hk+ErWHGVT(|pbhOWw<}a}6q~(mRQI z>hE%j!+MfQstw!whv6N#9=?X2DautPZZX~#Ha3}!sechR-ov4hsmCk+V5_F1>Of85 zg40z?qpuUCmrASL-;G}LVDsMT@bbUB0OVF4Yp@0CBvA(dZfRpipV1lmvSVT?gu;ox?ys@vI78zE_@HbzV+W-g zkor-`)D^t+8Y?uETD8xE#mz}J$^{rO^qbI>ru;^VU*7S(ZYMUdluSgUFKqYJ7@7p< zBA4{_M;Y$%>h@g;^Tbh>HhnRm>yezs9B=1Wrt*uux$@@Mp|8EV2H*X3(LD8D=zdwL zJLiECW8()`WC@)_P31d98t$Ff>WB?fj&{{c$>PrmtGd>qXjdgW+}e{AUyRY##y~4& z;tj=h%RaDg;4=tavS1@U$-yX>2#8gJGO0`9ox3eCw8Y}+qP}nw)4cc zotyuA_P%xAea_wY)~!0@%lfcZ&6>4pRjr@q7>JVYD?cF<>~LFUjGzdFPO5UNDu|gu z;`-&|e)W2)cq-^p@l&1n091^CSX4sE0}(Vmxw?H&ZuVMM+5xs1ItG*CalaDB44k1i zHh?-<W|r))Ajo|rb5=A%m^RfdfU8euQ?yF zK(ztv*F%qQxTz8OHN8DK*c?p&dQPksl*Vv8)yp1YbBe>gKdek3<=XObACJ2vT$#t4 zP^Tzm;%hB2JSp!-1_=Y55jpu#D3RxFbG?*tpm>hiURUyrPBKp_j$|y9;l^?9cC40U z`gm;p(Vbx~T#tq-uCzy1_V!|=gp>_R3>~*3?}(hDcyC%T_z8LK5kKr7iJ5ir_ks$n zLXe6h9@tk?#l`1$j?gF8Wd@S!MpykpGaHL%wykx?P3PHiG(}YpCZqPJ3(tu=U%t~O zB3Z4vgN7O9yG1>)t4}f%sakHXXNHeXrj2fLXz@0Nmb^#BHD90uou0S|Q-)60*)J(ZEBFW;29*{1KE&@OhfqJv=ikr@2$KQJ=Sf_uQU74`f|%gD_&gbpD1xTziS3kqJy2p~F9=OWmR>>in2XW) zNBNyHTT-^CwGr0_*(DF(c+HjbPFp;{e(vdh3@ZQpBbJTq-XQ-}Tm_Dz$|^vK9EGHc zF0o8V=n$WnO2fN8#0w%4K=42r=B|;nWdaNPS2h~iBStVsbGhM>4BVqECeFF`{U~EB z+k!G;L&D0F(41l@1@X>>tk@iVJZ9O|rhu&GJ!16nC++rfCt5{0$zDH=|JhVvI;g*u zIKK+VY^SQ_=)k+kRk@*2u_e8YCUVlG_Rj3fQgQj{!Tj6v*{JVcGzT?%7IdR^iW^~M z^Ku$?hQcSvceau%jK*-q?QQ=ouHO1a;XCwL9zX9e0$zfI`Z3_Fj%F=jkrm$Lv&F8r zPqsF@%Ygx>IOd>(bGsDL=nP8V14?buT2C+^V`Mp9Cu4nh{Ru3~pOnB2USC*<`H)tm z?z8X3YTg$mK_sI@GQifnMrKe{`8rtJbT}YN%T<4K)fC@;SxVlON^L- z)pzd+r(L~6uTNiuVvig#flV+egOHm$y>5*RF+ax6qYw$U9x*XYA6tsv-eyRqQj>oV zSO3vh3J`U^YnRA>zTqp=aG}}_up*W}{?6DerUU>wqKgN%F+O zK9;zrf^-U$EH#NLYAS!L)4i}Hc$#impl#lVlolX1J{IP7Pp#2=9n?qEGJRCFn{JB( zRlDKOFg{JkmIl=^%n@{~%&Z}~o*ZS1qp_X`7qKJ^))Fq^`<8`3rKVhq<8F&{W3o)3 zJ-gtaXKJR@6+&@DXX`G~-dHp$Hp&s9{tzakpVNz%!da!@`~jQUG%I2c2-yti)7(-V zXun$)L66mSBE;4vSEq@4Ki}f&NFh&IL65few|jjB*JFDC_onWx#nHq>PGopJ{`h1s zh)dAPI_6$jNJ=_j08B_o^+&R3+iF|eA12I780?9i-fle|3yZAi(Bo0hzI)^plv$IG zu=Ds*qAN(KJ(D zVgoN_$rr|NYxFtCW)#(lt1t!J@<9B0{`9D2>Yp+)3~3QPgdl5dix=*hkdGuf*FFBy zZ~>p|(iR%d0X{hnV}7u=XA0J>$|)`1g-0M^+}QuL-~W!m=>>DXVW`evPw1nCP@wsX zT7g%T6aM7yZ7GSCdRsb23s$p%{O*d!FY9uUmDnahVa0$8!&mv!$)3(a#QRZku_mtS zb?i|qc!`+(_3y`x(EtiG#fS;FN?9=PWR!_Q-LAgdUU4Z(4|iT54jN>o+KoGW!xB?F z^WMcRu|9aUxd_$p?rx1D{W0Yeg)qeVLBhcuAUyzRR0M>BEV%bwvQx zn2&*FA(3?}OrEN!ZvV=_8**aU-b@7VL>OXzVkmPbD0Y zssvype8nn|&qtRG zN}WOBt9w)I<1$BEvKkPS>`~&!ytPD=Im8GGqB~H+6@^1Wej8H_o{l9s(o5p?rNRv8 z_5+&k1GHjzIb6Q)G@B?W9lP3sGgRN#jS7x(%D0_u7Oz9h`+7yMiVs)!3p;~A%C$EW(_hRygG>1-Z zb;Sv$pj(L($M6UN1nfdkr_kw}b-S}R&_qkna+q_loVJslvt`%9w12S(hbfZB4D!f~ z^~G9)OCp(WJg=yVX!4?RdCiUS>5tmET)-g3hmWYL-n7!i1^zoypPCT=pGTyCj>@khSdUWF0TWLL~ui><{_^2X^Xvf=! z5ftnmu5oNs^&L&vw1w-reAp%NSe;z&IZ)3tQ=J3Nqe$s@ge;=`{lv9TMeBusSpl4U z%;QpQYmdBZMvtu-Y7;@?Y=rE7V4&7PR-H`kc2VQBMW&(R60m@QY=K+0sFQI1DYZZ0 zU?w}Q-fx>h` zlZedZCH{gk`jdb|c&xv58g6|Jh<4wm;fV zQqI_N0JTROEvAx^=dmbi#3jEI8Z3sry^%(n0kbhTr{ZbGva`Vpp6;w*o$uqTOTP9H zuhhD6zI_`1`ocFvIGiqn(^8k8?H^3LFV0&jlT){oZ`MMIBj(}(ML6QY?Gu@ySzcfP zKDY%L6x=1qs%&_^hpf;s4zDwYiz$Dl30|DXf2Ij02^2oaA|3Q)&29$>YtfmZR{;t? z-*7zV>tSTpLxsGtW>VoFP_1sJ!KIDnru-;A=WWYBFXvgn0>~mncMh1y7*e4Rr_ij{ z*nUJm&@se>jd z#s{QgB1J`WU2kXI<>y&5q?z5G)uQft6SIj}kD4f{k6lOI0!u8Rpky&+JM;y9f7N!@ zVQ0#~WCqum032ArC$a>WqU&n2th#?-V4^QN6x1h8ymR4Iy?s`#T;m-D`I`f|rYTCM zlFiaYuiPRv@;? zQS2qNazz;mlu(S4P}CF_OKSVKN7&<1)unuJCK%IZ25SMEx8`hbs(@rMq6< zl7VC8WSM1Lu;wGDQwB~kryYYlywm8il>0bXWM%|K0-Kh@)8k#aS(Ic)ja73Ad#s|I z963$^B{YXK9l@(jG$xkF@P#GGt;Gwv8{_#t7#?oH{TlIVnV z2`N&82@5zQ-lN-z^kgkOQCXi!BV?OQt+7SuPt-#^S&kd0PU|w! z*!;gf7IZpm@+SJt$jBGDa!m1~kHRKiK4ByXoiNzy_nqiw#uITASs`9sRXe4^y~f>u z-lT^f2B1}f@jXS=UrU zzp6SG_X-e?Db99;Sb-DYOV{J zViWZd)9-mdRVx=Txk8VPmf%l}VfN@{Rl|TZe(0@#6HoWNbAD_kfuP4R_7?%iOl<;s z2*AnAi8Z=(-m}J3cmrtrwmy2y!1*%cI86Nj9QAP@1Z1*Eh^3B(15Ljh<(f9}(}^Ep z`t$^l8M;3r=j3^2pw?R~gsN0%%vR^pP&QDpXx5LmE!ILR!-ys`6|*#WrB}}$R|6_1 zmuXUn-`8RNuGJGijeQ&JK5j+Oxpnt1T_szDf}{#{=SrJ3u7i}Q!ZDDygqf>ykDI< z7vyU*LExQs`&tWLU9ZD$)37i>Mg;ki0;sA~wv(i3j+d=NgKs6+`_{z4ZDcvm-wF7r zjV06ht?P$o7NsO|QALN$4xA}F)0G!-mG0_=Bj9XbA(o5uT11vxE3iY+j;yrpXNrD# z+{hM6KOqtGJkajrZ!^B4$!1sOU~b=X8fd>Xk#kdXY+Gr!piNYRm7l1BXuCr#Hr(6g zHDiHMtPRfPTuM-erwvx=s^D8cCbe^vpb=@y)&j)bjH&VkO32;3YP3iQIUxwtXKwPO z&$#U3p=(st=j({Ig9E{P$izZBhI6KA&Q@QM{5lqf#kp3z?f?8% zAYa;$=9ys*#H_fRX5*GWMD{&?Bz?WMFqK6_zz(XwgQ?&oh(IGTei^nou#9DY$D7D? zk%4iZ1-*$TRPvD4-u&HCNyXT~;=|K#z^fr4gvmHET_F zH#LKa%FS3TywV=iImQkeF2@qlnvkg#!P`r8sDQdtsYIVQ>Q)Po znBN-iJq;?e7}IFP8TmtU{Pk`l1yrDGYXs^B%gqnJjv-W$02_Zt3$Ugd>INJc!}~D4 zkDI2g!srgS}e`E$t6Mk1x%q&TX3{s*Y$BU%bUF`PmoDNPhqeo6RV8T@DM?N0<_W5 z9YeiKl<`1+$eX<|ck5GsEKvr8Q;*vMVr9AZWcoU+R!#Bs8BZ|l;vcJ#HHLx`fybKn zVy8nrUBwXhed7Ez9Dog+aU(>*Fu=ywQBzFad#@(L>4lrds%Gz=I94V|X4E4ICR-sV zmiX6-(IL6uT&q2tX2CI$<*$@AW$;o&*-=In73}cmcW!zLzf`7oB^T_ttKO^Hph^Vri zQKNBq)I<*!mB{Ex!?H!{{*hQHRyn`S>Z(EllbS?94$a@=ah{|lU3}~w=|BW=N}IJ_ z{S654-)utE5sp+)Y6f5IlY}ia%$n`=rdoBu?dc8s>|Vq$WD7$27xj%|JQGl2;b7RI8wvGdzox zf`GWGgk$OfvSqAKmL$|M4o64l5m9YSzZE0-GOJ5g@!cP8L7yOcH)DlTU$Z%sV{3A6au*Gp1X}r~zkshmrTxpU$|N zw&bkj0ket*#srMr1u86O4wVic^B6WiW&gfN8LUL*UC|s+ynIR@JYMSLO0#GxOkJ3q zHum+Ow3j@0MKa%b$1}mef)EYOuIPYKs=9Y^{ODSXN;)&=a)3xPjmXuOh7T_^LYyN> zFa5E78DG_W=iA=o%>|6QkUGENoZU4L8^pcDls;M}{kAq#?JBRuVfh`Xf7~u#Uw-zp zgn@XEQ?G2oT`CAMqW;YYD))3Kznan;Ct>e&i@=Up|BIsukH;i~Vi`Htb|}1>!;yjaEo~I@c3bxNs+X`5 zjX7MEW6tLTj@Lp1oXkd;uq(oB0>T4YCvoXN@Nrf68wxTqZnWr*6YGCs<2Kg+9vegJ z02W(FcrsaFzp$teO%%U+aS6lq&lV)S4{=$}Rn)o=)+t&`h6_=E;e4T$NJ0bl7kK^D zK<@MZ`&9xp>uhz)Ee@vY!#UYkSpw#!P_kH$6k*fT(~Q3ae{XjM%uXU_vR){njwbSc ztq*5>THYHsdfuvzKAIZvKUN?g{TH~-QA0M)$^I%i>Zmb?di$)^k6vz=3F`_ z+!y4gfU>rvS`!!ck&iyt%M}S(u`}y05RQ?=QSHD-MpV!2eZ11Pu|Y-tCiyICihAGx zWU7DtCenG2G*k=4xk2r4{;2n5{?w03t^QWQK*sDP~IfyAc2b`6gOr%OdyYPQvY2bx@2yhDZ2 zx#Q_c&jZcCJM`gV7gT#fuFoB!Qo%0~$Y9U#L9kcmIX4N} zH2OPS&G|m3y{YhjhRp*U%k&Ng055EwcXLjCP?Afwu;wYmkq4(Caj}-(#(M}i!i+GJ zx5;m&rf(+?+i6Unh-Ad@nnC^250mIZf_R$Y`&uQN_TQ-pLxWKjq}*cwe|0yHg+cK= z!rigfOPjU}+(f@*yua~vWlv}OHq4bLzJ6D79;LZ`qmMd}a?bu2H0(-i1I$tSG5SFO!9$V$1HlRaSYo4Y#zL*mqY~${x0^ar{{CbQ?gG&yIA^X?S$}L? z`S!w#XH|0;0FTBvK;vf!U37I_HkDiA8ELz^-UxR1tJ990XOmZA|4`&+b^KcC{$}7a z(t^sy=J3-U*1)#}he-GBPY&MewqnV5i*C;Yh(RCk_&-T}yGa_(#AnZe@I@W`d}|ii zDO!(}k;3b%gPPhRH4J@F3mZ9mD4C=}yg4~J1~+>nZl5u6{WqNt0s@H6UOp%z&*lo= zc=%@|{-f5(KSzEEs3G=mau5k)`Jm~~V+0rukQ!`}rS&T->bTEsKMKRbeottyb8bLz z*XGz($?HJxIEoiT6Zol*AYYdXjq}qwZ)(8J;UkQ3DvXcx0Rh$v$6vk-Wf8YQXo`c( zUpzmAga`KfeD00E7vW>$)xfYk@xLswjBg(w=AZ*ZWJvw7I;;W$B`$X&eNW`8GP-#f}Zmw6r6`FSeRrY?3aL1I%*iujIOjz1G1t=T&Yt2 zx0>Wnn2mv*J;IY|id(O!*0Kyp0wJ4Z*S1m>*Y;+W{5@U9tXC=K`L$4;uJv}^!PxvH zhc6%4iO;;|NPj-GJ4!Ze4bd1Unf#==0Wk|tl8${U@pLC?b6+??zic4lwT{4_J}g~p z4Ck0GBxS-;0BBPg+@ffvn6Eb7r}-46Mbao;LI$${L<*9arA5KzdR& z@^5_x)=C^?^nWo7i1j&P+OS zAE@yzsEksrLyWwE2Y2s|U=Vagd5sQ2Nr=LQlMT@WX9{0m*O!mR_xApN{S`Nj>&aZq zNY#rriiHnCumPW|>A)WUrAGLGgi1!F0Ox?e4wE|ScqF&){k$hRj`P5+Th22g-9gu2 z`K%x3o3;=WB5(QE8)Ll|i4yyTe%dkCbW zsvWU>SOjfaGZR)agAqA!Pn0gcie^7R<^9QV!9KQ}O0I`B;!4^9eN!KOOybv9@BcB6 zJB+qyo+N-hWU-`^OR#i8b(=#i$+6wt^$-x4RhqC~9ZOuEPfTc&Tzy?0oW-;b0~~B$ z%G&R+#&83WU>naBzNfdOhK_E6X(DoT?PG0vd7u)Q&eAUzi+6d)C*Z+u-5cBg{83`+ zQf&{!sQm6kqxqj%0AlrOXQ#nrw*aEDBBpv{w8AXv-}z24-K9xfjE<8gy-3t*YbC*L zt^s%7ix3{?a}dYl_bp$S(JBh;+fbJebQM)?1hKlPdmpgdY&S}Tj!9@%xk~*!nG#(k zIVhDMhRa+CQ=FK(cGFo?7N%5(RyHBCx^dgv7n|L5Ho>At7L4wF#x^AVvB}tdp#&GmM#=~U zF?nLl^O#+}O#c{_IwMrd%*{M=A>bO2V*~VXq{-|}hGa`L_<*SDSANo#p$B_*$?S&< z{vr)9$Ho7x3%kjKw)AhdrtdR#@a%d~$tS^Uofei6!MbDXz$)ar8vbZRr;s$pVycer-syL2 zZl{1|uS%-Ob|JgTvk_Ow=9nh$3NDy9%hiCAar-WM0G zxt2q!hm}Yk?x+>z#L;IMSEo)phO$g$2`I7YQb&n?>m^Xk_k2dc1T12$wZUdiccD>U z-cLgRd4o~4&KD}(cW<1qJ(;Wlw2-I>GVfC?C2G4pfGrNCcU2|{Z6N#hpP;nu$Xfe> zK4n4*(u+>^!vbm9MT$=MdxzdMshz&fNH?c`NB^%FfnC{L7@J9zp5_rIudPR%8z|T9 zMWqx!smT)Dq&ej*;K>Lz@Jw$^wrZ>K)Vh`X6LjWLFkOs99G)QCl#2z`)FdLAIIj$V3iPN?O2F=1>iBl780}qyTWpyvPwT0k&r0J z+k;0-z^b~If6*{fh{6Fn(QC#JW$aj0@i9_oii+4U`39%xY4N_%DEtR|S;pG;@( zqIbS#^R%obT^%-XX;e0kM2cq|@lJ)hsD6$|Lp^JUdWJmjn#3>xNb^9UEU>=-e83tv z@qc5pE}5H^PJ3B(wLPX z)31>u!v5GV5wbz$+fjX|9L!~4vp_o9g`Fe6O^81-vN{MOYSe%71&iYFKhyST_(6;9 zEN;H%t;Tolauruwo++NW$g8?VEWCCkGg-6py)}kaI1JUCKT3Ndq4tjL+xoz#aD{31 z(;1#subfG__{w+EkrBP_gt;OVgiWPxGU1rWL%g* zVB&Bl;FC~*rMw~Jts>#chV1#LC4bA1fj%C|Pqyt6=^b3d@}NcVQ3S_B33g?Tx)cK% z(|R^v!w)XJdJI~Hy!q)m19$E+O_rsnl12=AiL>^6B7;(#oQbMjDV$4+LI}GOwTBR3 zuH+3vUSgt9g$!qu42Q2!(Ltp7c`QyzqLIXE&ij)`dVWAa+|H~Kpo_Taj-+4AoSGYT=&oW~ z4p~qz-x8oV|KF7oMf;2izfDyErOf$?(@4zU5`U#C&^8ZuxAgw{eMYebM(ym=dJo|G zBU9AmyE`3r;JMf_+`#Kx$rF(U)^ai<|#Cqhx+E_WT6}| z3<@FMoRO7oH!1YT#hB00?5a%J55OV1;&a!iFG+fF#(~(-sq~-%YC`MD6@2L3xc?sA zh647dwb$;ei~ZO!(h!ID8~accp8o#n!pt+Qq4=^VI3+Z?ERL`b!~de}DC;I1@^4}t zN9hpP-TEE4ayD;WScN4BD!G<6Ne}1p&SBcQb1KV~EI5@#SLS3fvl>f&#w4_d zp{W%Nyw6o;vs+H8%8_j6g4`^c*hY}4dH_EO<1%L}cqU4_oipArWs>{6lhW@C)dZ4uK#Dg?6#$&H3_9M<<8c#;d-YNC*WrScidt*@& zK93$Ja~Qlz$q_><9h|D{p3CA82Li4j>L1QOJ#PM3Caao}8NiId7BYM>w{msRv52*75tnuQwYY!OEQT z0UYZTrf^#-F1qvxaiJu;GGs6Cm>{wd!|%y%*SXW9gUN}zf}VsVw&XE4)K98)TFztr zdne{C`XII{WPGs_-QtZ8WZSyLW{|`vol=pJYY6D%p_l2Mt>D>fKCFr_Vvp% z1Z@rMIKIdOmsSSvm<|_6RQ!dprBH_Xz_G;Pa{|NWPxjl3wB~F`{;8B7=?PBqFe2C3 zSX3;6fM5wk(^{nl>f-k$6~zfM-PYsRm z5>Gz&jdkO4o1GVVDb|q}O&2@#8HQwy6_O>Q)5Djjf#VvQnd` z`Q*8y*@=o5Uj6I0<8mu=I9QKqvmj>?APDFns?A`J) z>`6t={aZ|659d#5lMx~KvVt3@+q+KBlSYIW2vkh1D+@PvV7bka9hY_AES;#tqO zhZNO%T8Q;DK!t)AA2fzLTb~;FNfV*# z{$PW4^^f3*0hOh0gkxl3DSO7PL2s-(Xo!5QGoC-BwRN-bYl+=qob&VDuCRF&V zHN5_5(G2T0*Naoe@aFg%^Knl5+BulFgyqz{t}4@~vCBjIS_T@bRwvn@813O9Z1zwR z+VuKDS~3Sq1se}vo7nV_(e2OySVLlLD1G4|)0k>f<&! zNZBZ`e9&kn1pdcYuepKXzMXYy$>d?V3?W2u-~GXV_`Kikq_i*ybjtFo6We4`yR%l& zylGkc>*OJT4@u+>g!LG_cz6l-K{*Rl)uCR1Wc2%^AP^o_x$K3Or4@z#mZpM&@RNym zsG!t<{ggP9u0|%(s?Vp$)BteBKwcYg`{jL;;r85Ib=GtO<=mRUit$eJN! zRz+=El>J3h>H!Inr()*F+S}o-$#BvpNGOg9pN#gkoezFTTTFb)=H;IqB2?j#ow>iK zAPr`&k>$0-KZOU)Yn~z=a$Cz1rZLln_AycX8~nScwW(!jDP>8#dVJDeSJnR?HLlOL z1Y)m68HPp`HS+Q#{xgq``_o4 z)lRX(=qA%{6^X-G1lI#9UtV{rj~}#a#WwF7e^LWQ#@U;NV5(Vzal7&l-bav0H%oJo zc&luX)SKc=143syIpwb>cmzslWpV$dfDPD?BgK5>4R+0b*mHoVr^6U&^9)zvMIH7H zc(g>qiZ~FWxp+<$`)1iSPrDAl2YyA|M?XHEUR4L$y6H9l&eyG}b-fbLG*PRyv1f~S z^j8MNCDS{BiGB&uN3|hbb-oyafj1Mv(D=;__Au71ol~*z3}zjaJMj)78GMmBia?HZ zI_FNSgW-9Q@Fx@K!`ZW|DorFhZ9;n!ORLN9QlkG)niG6_(tI7)a`PpOYV6leI1R_! zdt(;nFji}!=VTM{Ez-pF7RGQeI8B+hAyLcEmHM^Y1$^P`F2@wFqg0#mo>zGvge}e2 zE5RC{m*x}JrU&W5s(FSkZUZ%Q?*dWBxxIa@f$c(uNw0_N01HM6bbH6zm($c%3!W3T zBKrd)W>hKcHHYYQ32SrB>xXyedb6cpPJ6r$Lwgf6LhBYH@Xrw^b!beDjn^0a3IM2) z61~&Cr$<^$LF<#H_-5}HDS`v)0J0Q_WzMqPol^p>kdEHgBu{zJRd+>pWVE4#2DT?< zH8)1sJKY7xdwG5c|125U6Zzk(FT;bX{F%rSQU4*ntl?avDp$HTOArtA+h0luiO)%v z_G$(fS}>)y5?3FTxnXx0gL8yEU6CUq?CtI03xU_vP>9*=+qdNfRRbJ`RQm0SWWW)o zO0Nx<*G%B*vS@sKL~|rJy^1wInU0s}03^crfZ7q|_yNTs!SuRnl45b` z`akKhQmN6^kQP$Xu=v0o69sjM@6mQP%gLpfzW`7a=8Un5f|#oEe$nC*QUa5l63dWC z0!B5L8>#5TkO`JPi9;iLE#n2>{q)vDGD&&bwklEkY4G?5BxulIMl5JeP}s&<5gIJM zTYH^X+h6xL8Q@n15fc!J$x71U>j*>MD0FKo$;=KbKELDoXF2>2j3HcS-{zf@}(*Aj9bjU3?jVnG{6;iA9^6KP%LmXeT2N^6wwm(MZXVpXa zZ6uWQ9hhxdz!f+h8>z7ns>zykg?`2Knvu$QKGs6L(jJsWcD^0OqPW~*ig<4`7MZS# z>lXZE9FD`9mT@e@?D5W|V__lt^w(Z$Bld2~jY5dGLnmpeTT;=RFe$xvJUC!ae7i@k z&25X41OX`t_UMLo6taI18oRX3{<0=)3W9Txr0U*G?@ceLm|lMjT_-B11d+xyRZ@J697KFZQ@rM3;vyy z&k>yzj4l-Fu)qdm?=W>7y)GjE_49-Lu;cC%jV#oS*l{eu=eaSNXQlc48$R+=+^TBI}~35-sly#r%vbp z4FT13aqyTc*`IZ86vZ%KE~kA2f1|N}A&j|WcYWT@;~FoDQ;nTFQw{{P_(pS|$|8hU zlW{KJgk??H)`ZiqP(vZj4O5@){->@;7V!3T-45(Yab!L*yFa#Rl(KNti~ykwpX7aV zOI>C@W^z_@Iczw5Lta9VCHTBzW18JrnQSioah#@G(Avh8a7sqf4b9 zk}fD}YwNk50htPTn{kvm`LdmXXtohj<5_u-j!xYZ8<3{}nR!=ZCrH$7g{NC%AFT}T z5`{9{iL6gw2UNM0C;1s^uvKvGxlxdWVZGBMOjnhw-W4zWGOZK&C3&!xHFRJzf;E?; z{`%zsJi#KU&OyO6(-cJ{f}6*0wnW{|vh*#ATy zdbmT(r<;)kGgp9Ze*4(3LZW{zpD=3Ub_>oy5xT3-rS`r%)V|Ce+;f`l zYIwbL=c0I)DogZPT^nt8^3GCb3Y+(Zy~H|yYXzd3c5&7pt;SLY?odULUg0E8VhN9p zMn+=_3RHqNu9k~BR*MFR0t3!on38*FO1E;yNEsNADl}nJm1+sf*o_H_a_`G!3jpR? z93HV(X`GZSSmX03x-(1i-$zosb!m3{beyi~P2zsV{jF?gsa;P!&!<__Saxv&P>P^g;eLWrlLa)PYL7ccqKR5VgwGgsW|b3vnb-Wv;2t{J?Hi#pSm| z2m!0MU!1LVs*Cw=W1fIR3iTRs9&`)Jj`nyK#lOL+EXk8RK!dyqIn;{x@We$o3jg6( z(Rg~V>tyyTaZwE?yb-c88@cm_eQciQKi-`#NL0coY`_` z4%0VZS(UTK<3c~p|C2jXufazB2SsFrVvbbYPXMa%6pE~5OnT4mQ=y;|9zNs^l{0~* zUEQ50Bd`Y14i3>I{r%fJ$G<`e9WxjKeaNA^RYC`wnzjO8?{^Y&oj;~30YJ=avHN)Q zg6Q~0|0B9kE%qz+43%S*5YUMxIcJoZSn^BwDu2ucjnZ@IjL=j{-$zx^a0dZE#w(NZ z_2dT)IoI+(NonNJ{r>}D>u1v`_5YeIajgEY$&!&{^M9h*MQJ{cCi=XOJc4cTdV89D z_T~Ilo{UT0cp`uDKo`L2_5n3y!F9_#4#;_A-jh@M*HPn=REbiEgtB%g5+>K?Vcpmt zJUmpZy9&0{ST>1!R!u2)$q7hMzQyG?m#-BYQWtufN?#AWy%whxkfh4RPivg8d}Ii}Bf1AVRd ziJloXjAZJ)UVwlb@_si#fb~X-Etk2 z^J;Ks&gdJygV%%-=|pnRCs-B!%R@Eq>31r!`+}G(A{9;?)_u#l)6=*B2yV&}Y+-|) zt+s_}F~rzP+@2zv+WICMO7V!XFajfx{C5#PE|k^O&mjgTJ&vIwSVytgNbsyoqQiAM zsmvP^-FBV~>1V}&^ZG`{SBZJR?62n%K7m#7}@s+qJSh( zer)5)hJ&}ykA&E0|1Ig;+c5(<+ADcB(AfN<-8RVOZ>Lj==9O{L;qTAfnjKX8y7m|w?q2a9 zPso{=km9Y^BwW0ME(f)@uI6K0+}^hIHl(0%n>p)zCcHJOCl+{#0w<+OZXacc;B*?3 z{U%?NREpC^eQKlA-Sp&KushYq7LxH+6q(aMoCo?pn&Ui`2P+=&3C}~>uSAeLOmPau zE@k8l@owTC>j0~IGN?n&xf%2pw-%ULXU^c8DjVE!`yWq4B(Pk^dIaFnDdk0*w)S|8}v&78EE?{!dVv-UhKyZyN zeoy0sGd!|c!z+PFwQyH3wm||*s$Dez2*UKee5%Hw|lhbXa#%6d)(atp$0Jt7pdam2Idea581WVuO) zY|-mxuE}UBo3VfPI5jIoyZsbPa>l6Z!tSMunkh}jBF>Mi!7;ad~q9 zpCo@z<9ZQvYh+p&mykBdM&vcRp)Xk>gWXh6u%C~|j({ioR%MBfmpkz4+gZOPzC3wG zMmyJAg2V9jnrPlrHXuUD-$@b$2Pjj!$zK|&i(;F!iLcW!lxy?W!n1;T?2_k>(bONd zA~aKvTPF>GC9XTWQP<{<#+rs2!LMeVmI_wG5hAsm3~}}GKTw5~o1zX@?G==@Ug>uF zpMM}uevXRr;KTNHfl`{Kg5AN#@Fup`oo>D(#gp=`pkOR3lkRwV7+nh`vPL3U8 zr>Y$X6%c7eTHC#%p-pA%FUA469V9o8)Fvx2oUMw@#9B*KFCgIp`id39U zjiLhEqx)3*N+K$V5B&gj5bdCu_T{lAD^T|g&`1T+whZ0Jv=HzHX4;N90Dgb^1S?!c9(Y4EGlDP2?0p zxa@GTG*X3Rw|crqheSsCWH2FiCk+J&Ja*ABPl$=Ne@)rY@GD!V%W~=#^n73@ec~j< zNL)-Vx!v7o>{+DU%-1SQSs4G_={w`I_^likk}>-|kC?zixQV8Ej#NmzztQ z|7NzRfUx{1o{~zuC52FI8LRSX26YwITt%q7`xNB*gc^t_{ZwHd##W6YerTat%u? zRl`3Uf#(_-ru6YiIo@H~iiLk3X;up%=4aGL1c)PYS8p2rELh z`WkX?21h(xlRjDpwasjfv~v|> ziL&2bvzDlG5|em5ozMF(cLRn^DOgykVA0I9?J?$g~m)3tmJTedEWanM>z(UpW>y+%s~SWaV4;OhGXN}!AS(XJY-N9hRb5@()#o|C_dU;pE4*w!S$HI)Rf2xLqKFHqSl`?FR9{!8s^e{{ zQF(UVuZzg?U1p9#^JmYpgLte&h3><*d6xu;Vn|AX!EOSM2>pnj@+cG^KM=&)J=P!m zUR>_sI>9FD=jjgLr>XO*!swR{{%y_)T>TO~=jg1Ih!Sg^iMkg)`-?K;0L9@}ItxxB zByPKNPaUCQqd^)W%plU~^DPO9M7d59#FlYwSpJ&!90x5%M? zMs5T!=qIl3ST0>ts~P>f!nr+`d?BA3Tz)n^eRR)2P<$^wjm@R~-ROdk{{oes*@hDJ zH-Sw|)z`(8iC>PN2DK%UHD}Gg$>Q$xoK6;d5gQvD2TMne*jRR9<6cda(J zBb(wxu&z&n;|vv2(u&w$dz6E7C^>%hcf6AzA~E9%?GOZmI{kpodAkoHbE1yq?K-z6 zwOe2f>UGM=FRC}BmzbHte|F*)@63P_I(Li4gM;}4IPb#CfL}@(HomLe?ZK91Oo=$I zPYmW}1hLZlRJhFN3z;*#Y*vCn_Sywu%$94d=_&ZYMvLM~ z2W6hQ6)q`{cJLOMmy0lC3$%gfOY@_mqMBDtr4G~$-C{jN>mOR4y*GQEbB|kF8#(S3 zHYGHd;7s%eZZR)Uufg%4ou*~kDL!mIPi+XYuf`Q-NHJVJQC;k8#Al^G;mfNmJz97i z{>?HgRTn|p5S?^R3O72RP;loifUNpVr%YBrmhOZAgekuU{}nFUkP6`f8-s_rH5iuct@#{8opi{R z%kG*$)r(PEJri*zMU;V5@(13An`ZEj@4~AQCd+^ z(H=_kI)H6M9Pc1g7yNga)s_ZB^490gO}gsp5||yDBrbV-5&okNsDHKCp#6r&;%}P3 zM|t|cE?;?SwAz0i!;*0a|9XFweC!>}7mRH>z3HD90H@c4zgqDucyp zf-c_U4MNEY@4^6;#Ho4gbUr1Cmb0#*15Z zZP2e$z*J*JDi#+zSsLVH8mI@Fi*Hc7D_BoLYoRrsLv}mAaz+A76cG4O#EF&T+?CE; zYNV?8HYTg-;vi?@d@|Cw8xGw@r*c!2n|DWZ4GwJEFR9%4sO7wcJ2C??tDi-acZ5*~ z=W@sD${NG<ol&CBRXv zU-6CL(_%rgbI2Uh<{k-rJ53kX%^IgDz=tojw3)-mdeTFYWIE%8B^|Permxh=`j72h zRaIH-8^F}g=yq;h$9mSUp|0QE<8ru@N?fMXCcM)b{j}3aXg|sfcJ8X=i6K@(c$pCv z>8b)6rZkf1a&3U>&1#bQy`a=K;B~KHhc_D*JxOAj2;(8a=#dt*Sw`R5%M5UQ+U-`Q zr_(jyKzGO3i|lO)X+L6tfPubx!UyZa%sr1~PWb;z0{>6hs}WfgjyP4YCw9m!?d=gn z`VgNB$xWU@cBvH%eR5n@UkAyW!_w;5$)5yIk6A&d^GV^{BIl#}g>!o!9A*LDfFf&V zbN#^HR3RT8lql9s%C$H-){UQ;Z^NZa=*(XBRqh@Mk_>tW3l#l!7Y$PF5J|&inDXpq zM$$7*7V}b}8%nTpk`reIWQ( za#kC6wu~IZ$xkmkD{EY_FDQtoPKjCnCjU~}=DRD{`>$O}RFMwlc2;(S>E#4(;CW4$ zNm*uK7uNkxQ;o)Jfz+<}wRiK}LBb$Dw>zG52VHf1m#y~x^gsH-x0u_Bj+SrsX=g+U z;U)s(?=hAc#M*B6t!-45~V$O1!McmabU}TEsd>ZA|a+X#8`VCp9`gU!Xp(jKU z>qP0NZ@`6a_XNOEAz0ERQz%iixI@wR*g&+wXjr0O+J#zDAmEfk{$F(*YV-cG%`CQ} zzm0gYJVodCSl#Gu*d?%Ai)hYsb=R2TxXYS`@cbG|RZ~ zA&tqp2<0NLY47M%=9>y#pJPXdltDL}4Rb;)u6&<7CF(g%^#t9cz(s3tzV+zb@31!| z8nU`Da(js0+4blCu9fURXSRcC2>K|YSXe*>mavs(6hm03YnJ<_ug5&>g8>;?q~tG# zotVnoHt&ti_26t<^?V71(#Iy|9eqXn-KaJd?q9kd#%o!E@*1d8s@w=oipbn`H-=*u z+O-54YI66T#`S3EbnNWWgxFlP^G%^Oe+x-ZZWcTJQW&SvF=XOB;Pcm|kGyDZp8Ph_ zTY(P2#B8UmJZ?FIt;vZbQAE_rU#9I{SDk8|+c{=<*|LntGO0L! zmyF`P+@F6*L=vyJclQk6Jw+x{Mc9$X-eN`|+Z^UfJ>7nOpw01WEXBAvOGtI3SIf?fkwTeiK^jO&FDG&FWM!Fsvc*%SS>8tdeP6=#8X6iUDu+kAwRZ7b z1pqD7Y~0~0M~~Y>M|#dj^J(}@$Zgw?qlNNi??zWPE)W0Mn2>EuW?NK}&Wr}Up2J^N z{S{#rcyDb;Shn9DnPcat9vOjBMY1Ls;2(69jevHErkq&zJD}vj1Wp_W zA42^J;e{Od-NCCiJR;&pjNq$vN9iR?pr4CE?caeuj$pV<;QCLQ5Rx{@^g4i&$L&)| znSUFAjkb>+t|)@#Bh3DXI57DhE%GtLJviGZV7*=_>rI>+RHOVg=j(1mtzH$v-p>%f zPKnWugpp{DN2<_|RLpvMXBdcKF&2|dK0hYKr5b5@6JwnpffKou`ZGVwsBW=MTf;WY zt7K01q%PK@U~`TTW!2`(E31sQ7I-s`Ho6)j)&PIt4z$D&a7U_A&5y0(Q_HCnIA1!< zP~|+&H8}AU*#DW9OmwKI&1yYN!nfv?(w9Y}dHAKO44Bh}xGLO0oSJMHupsNJ%vmh( zJn%tbJGVq2319rg`W?9N+%V%s&hiJH#SdsNoiZJmh4$ojIEE~|OmO02?R6XpFUm4M z+5x`DR=9izH(lBK52=m6lIT~F5#M+|Z+uzh=IyQg5aglPnCBW0Dn7I1->#e}=Jn@^ zBY2lNu)8;TjQ76t8B*gws_zzP_Omlt%Tuj{?0@N%VI6_*a;rS%s?W6qtKTC$CloB4W~6js8)q z5H4aN;DVQP$;EuTk=t~oB6b0uc)6@IuT0GexZj&EA-@R&I zMA|&IdT;5}&Gd-c0WqW74I6Yt$AEJ`kJr7JNWf$DR&#m-NZp8bhR7Jp9nJvWdZsry!f zz>4Kgi7aqQiTFs&dcjmQm!eGmrU*FH+LR9P;DlveVeZ-m9@FQ>Mr`C%zgRlDvbu`B zZJ-=sb6MS?YR}lTcl^0E8_r3$TXVW_rQW)2?fVwDc32jWhJdEU0}ZCPMostKWYmO- zct7e$PqzDbw0fNOiN$2czcr{2KQg$6wYo7vZ}~1{mQ>y&dK_JPABkq1b*Iq6M+3qq zE^*5IxqxJ-r|!@_7{+2i05Qwww9k= zef+rYWQnsxKc`eWt(e3WmxG%w2)r+Sat)&-0j+EMRty|7p>^nHFu?@MuSCv$CQ!C! zp4jvVD0{Z-Dt$`Q{%0fae?`OGghwXwo$Dh}TuAMBIh8H+jJujNax04e4f?*A zowB>{B(NJ)ok-Jja;XKUe((7u^))+5%A^!Y2G-|^{~b);8PkhL33PANP$H}w3PLhR4NPL-wb~4}(Zb=s)SMH7DteFFA`}km84v=Os z6b;+Br~>)>iK`KchOh#!huih}A}4DRL-s|eTxN9F1g7PWK-YOB!};#G|4)rvw&>ZY zZvyM9AcvIadz^Pq#mn5+T#a~niOC}Ci5}Bjh(0_&@b*L-L%B|*dPSOlf8OzItelT> zzby6@fY96riIUS|#OutXXq-1H(b~+Bl6LTiRf$^-%q&y=*{G+9z7p=sNbomdEmY;u z=rph+k`uKAB_HhpHknMY$#-1urBlSTyjDZNIq#jnpe+C()fU(f*x9sK=Zt-~pX|{R zytj2NI`9<4W0xLw4AkoQ2H*~2Ud93BNoL|cn-W>fDSr-NZ)EuDWIMBZk_%w@QNE$y z`BK1EJd@kUU-blCB3&#~@QyhWIxTsIW3@i~KdH3TNzt8ehANt}m>0|nnPgv6P_&L> z=9GGT(s>g5KLqgax6#-woZ9l7(OTJk+atb}Ik2=95cj14G$czQYoRln!^fScDwnO6 zBRTE3*uk@OqR}UDF6u`iRc4PL2vzq0G<(8TaV|6kQrrl`JA?M;8XejmbHgqv>pXV8 zEx2v`^0PKG|6b!^;}dNIShO57_c?Q;n!PbLJ~!ueK_Ae2Vs_}vRZH)~1AL}=dP$ax z1w`Ci7~L`gArk>amEu9Y-WFtwqqAnO`a@(oL{)Tb*r#Y+14BV$j*Mb-$#d3k_No&) z{LuRko&>`}k@b=BX5f~hH&pLXPFa^4ZAcB1W#K_`KA;iL^Hr=&~E4t-=h|^J>F)=Bw|H&*jU@%cWr25l6 zr*79?EB1I-H4kkzaio0=RSxWQ(h~Tdj9F-qepgFY)H?`B;CdYT$4Q-z+LZg?w$5VM zCI(cmCHs70?u{(8xwo&Px(QI6OZpll-!&8D+%MrOnZ38h^c5`Ax4j`EY6)lM6x0d` z`VTT*wH0)5@cZy#qJB0AOw#|Q@SN2X6J+YOKNW$Mt@{lz38Gh7iW^HIJOM;!p4S^} zLHDEW+i|ry42}jmIEWk3LrlrY8B}ZQn#l}5oN6gM-+J=2tj0jc%%E2MCj56i-llJ6 z>lF%N6vOXJ5=uR!KINK1?bt{v*0!%?#JT7HF{I~Wiw$%Rq8eT7)EV6?EHAlpgNI6ad6OX+TtI8v>l z?PEIxOauy$Rs14yqvRP@+}4Z(+gE_JGwcEk3;K=XUl8;beUqUAB%NxQ!S; zKZOnz^Xb&daba%Oq_fR=0CIW}^%Kfj$_?9c)#foJsPJ$B;Ok?$hAdE7qnEA@Dg87! zn_b^G*q7iZ$oNE~9Ns6PP6auKC`D<;dsak2Gje6}^k3Qc)PKup(Zl~)J;;~-)|Rkt zB@3!Lezmj*w}XF$@EW+^k-^+7?lZB}s5NT@zK&cj?I zpd9E9ozeap9COjH5?}l>5^31p+sKY3B8x*!@^>hcLGI7ZI8k8k4%UosMoXeg5ktK? z%92?%xz6BjSXF-M9r?Y8P&{*`A>+jksd-6zdK3s}W{iUuo!i=$5G zibzaqi_rj%`a((}%r4{Lkf$>=Oy{ajQ$b+G5AWK4p~WSIC)JeM!-$wmXExK(pD29z z>}@$C3mKT3X-NMG^lB8=!ssBv!mrvhdKxts*0&F0;==eSvn@Vr*^j1ahHX@RkiE0u zH$HK=^=w<;nu-hA&E&bhGJa)4FgYtBL&l{E@mu;_?ZM>xs8jnf{)uY4p(c-bTF+!< zFS4_YNt{x7t_8qwUq^2js16%cZ0&zDBIDI*No|0DxzGD#^wwfraANyQ?3^50dKn0Z zd>lvOhEcSsePb8jezUROJu%OfEK%64(DGw|fli`9`+K*r-4SscUiDUCI)Y#h!QBmWPJ3b4^W+y(;uMXf`$L+)bLbB;VoK%j2m zx?FLsNS6|t$0x=AymL;J{)@hYU%oidP*Y)wS5@_|9XDY4VLDv+x_fm>UzU)xyZBjg ze>#KeN5`bJ|A5e8SOW#<$&9!=vJ8#QMg&U|$DwQ`<`5O$-@<&FM75z%=ro8t=uZament_cmake_auto autoware_cmake + diagnostic_msgs fmt geometry_msgs rclcpp diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..9d88d8e6833ed --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 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 "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + addKeyValue(key_value); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index bda7af74b8489..1852d70bced71 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -25,11 +25,26 @@ #include #include +#include #include namespace autoware::gyro_odometer { +std::array transformCovariance(const std::array & cov) +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + + std::array cov_transformed; + cov_transformed.fill(0.); + cov_transformed[COV_IDX::X_X] = max_cov; + cov_transformed[COV_IDX::Y_Y] = max_cov; + cov_transformed[COV_IDX::Z_Z] = max_cov; + return cov_transformed; +} + GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) : Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), @@ -56,6 +71,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } @@ -63,186 +80,195 @@ GyroOdometerNode::~GyroOdometerNode() { } -std::array transformCovariance(const std::array & cov) +void GyroOdometerNode::callbackVehicleTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); - double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + vehicle_twist_arrived_ = true; + latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + concatGyroAndOdometer(); - std::array cov_transformed; - cov_transformed.fill(0.); - cov_transformed[COV_IDX::X_X] = max_cov; - cov_transformed[COV_IDX::Y_Y] = max_cov; - cov_transformed[COV_IDX::Z_Z] = max_cov; - return cov_transformed; + diagnostics_->publish(vehicle_twist_ptr->header.stamp); } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); - double vx_mean = 0; - geometry_msgs::msg::Vector3 gyro_mean{}; - double vx_covariance_original = 0; - geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { - vx_mean += vehicle_twist.twist.twist.linear.x; - vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; - } - vx_mean /= vehicle_twist_queue.size(); - vx_covariance_original /= vehicle_twist_queue.size(); - - for (const auto & gyro : gyro_queue) { - gyro_mean.x += gyro.angular_velocity.x; - gyro_mean.y += gyro.angular_velocity.y; - gyro_mean.z += gyro.angular_velocity.z; - gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; - gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; - gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; - } - gyro_mean.x /= gyro_queue.size(); - gyro_mean.y /= gyro_queue.size(); - gyro_mean.z /= gyro_queue.size(); - gyro_covariance_original.x /= gyro_queue.size(); - gyro_covariance_original.y /= gyro_queue.size(); - gyro_covariance_original.z /= gyro_queue.size(); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); - if (latest_vehicle_twist_stamp < latest_imu_stamp) { - twist_with_cov.header.stamp = latest_imu_stamp; - } else { - twist_with_cov.header.stamp = latest_vehicle_twist_stamp; - } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; - twist_with_cov.twist.twist.linear.x = vx_mean; - twist_with_cov.twist.twist.angular = gyro_mean; - - // From a statistical point of view, here we reduce the covariances according to the number of - // observed data - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); + imu_arrived_ = true; + latest_imu_ros_time_ = imu_msg_ptr->header.stamp; + gyro_queue_.push_back(*imu_msg_ptr); + concatGyroAndOdometer(); - return twist_with_cov; + diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::concatGyroAndOdometer() { - vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + // check arrive first topic + diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + if (!vehicle_twist_arrived_) { + std::stringstream message; + message << "Twist msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (!imu_arrived_) { + std::stringstream message; + message << "Imu msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + // check timeout + const double vehicle_twist_dt = + std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); + const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); + diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + if (vehicle_twist_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", + vehicle_twist_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (imu_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); + // check queue size + diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue return; } + // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); + transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); + + const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); + diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + if (!is_succeed_transform_imu) { + std::stringstream message; + message << "Please publish TF " << output_frame_ << " to " + << gyro_queue_.front().header.frame_id; + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + } + + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // calc mean, covariance + double vx_mean = 0; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue_) { + vx_mean += vehicle_twist.twist.twist.linear.x; + vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } + vx_mean /= vehicle_twist_queue_.size(); + vx_covariance_original /= vehicle_twist_queue_.size(); + + for (const auto & gyro : gyro_queue_) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; + gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; + } + gyro_mean.x /= gyro_queue_.size(); + gyro_mean.y /= gyro_queue_.size(); + gyro_mean.z /= gyro_queue_.size(); + gyro_covariance_original.x /= gyro_queue_.size(); + gyro_covariance_original.y /= gyro_queue_.size(); + gyro_covariance_original.z /= gyro_queue_.size(); + + // concat + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; + } + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // From a statistical point of view, here we reduce the covariances according to the number of + // observed data + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = + vx_covariance_original / vehicle_twist_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = + gyro_covariance_original.x / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / gyro_queue_.size(); + + publishData(twist_with_cov); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); vehicle_twist_queue_.clear(); gyro_queue_.clear(); }