From d24a2be5943a90810e7a00a760fa1d96cc73f3ed Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 12 Apr 2023 10:27:27 +0900 Subject: [PATCH 01/12] fix(static_centerline_optimizer): add mission_planner's param for launch (#3360) Signed-off-by: Takayuki Murooka --- .../launch/static_centerline_optimizer.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index 7b315adbf29e8..f727c0e58d327 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -21,6 +21,7 @@ name="obstacle_avoidance_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml" /> + @@ -61,6 +62,7 @@ + From dc8895cd7ee99b065c97c02542ac4234ce3b63c3 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 12 Apr 2023 11:47:17 +0900 Subject: [PATCH 02/12] feat(route_handler): use pointer for route msgs (#3362) * feat(route_handler): use pointer for route msgs Signed-off-by: yutaka * add warning Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../include/route_handler/route_handler.hpp | 3 +- planning/route_handler/src/route_handler.cpp | 51 +++++++++++-------- 2 files changed, 32 insertions(+), 22 deletions(-) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7e690e2443215..369b71993bd1b 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -328,11 +328,10 @@ class RouteHandler lanelet::ConstLanelets start_lanelets_; lanelet::ConstLanelets goal_lanelets_; lanelet::ConstLanelets shoulder_lanelets_; - LaneletRoute route_msg_; + std::shared_ptr route_ptr_{nullptr}; rclcpp::Logger logger_{rclcpp::get_logger("route_handler")}; - bool is_route_msg_ready_{false}; bool is_map_msg_ready_{false}; bool is_handler_ready_{false}; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ef56e47999bd5..6a40048105120 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -126,6 +126,7 @@ namespace route_handler RouteHandler::RouteHandler(const HADMapBin & map_msg) { setMap(map_msg); + route_ptr_ = nullptr; } void RouteHandler::setMap(const HADMapBin & map_msg) @@ -172,8 +173,7 @@ bool RouteHandler::isRouteLooped(const RouteSections & route_sections) void RouteHandler::setRoute(const LaneletRoute & route_msg) { if (!isRouteLooped(route_msg.segments)) { - route_msg_ = route_msg; - is_route_msg_ready_ = true; + route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; setLaneletsFromRouteMsg(); } else { @@ -294,30 +294,29 @@ void RouteHandler::clearRoute() preferred_lanelets_.clear(); start_lanelets_.clear(); goal_lanelets_.clear(); - route_msg_ = LaneletRoute(); - is_route_msg_ready_ = false; + route_ptr_ = nullptr; is_handler_ready_ = false; } void RouteHandler::setLaneletsFromRouteMsg() { - if (!is_route_msg_ready_ || !is_map_msg_ready_) { + if (!route_ptr_ || !is_map_msg_ready_) { return; } route_lanelets_.clear(); preferred_lanelets_.clear(); - const bool is_route_valid = lanelet::utils::route::isRouteValid(route_msg_, lanelet_map_ptr_); + const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_); if (!is_route_valid) { return; } size_t primitive_size{0}; - for (const auto & route_section : route_msg_.segments) { + for (const auto & route_section : route_ptr_->segments) { primitive_size += route_section.primitives.size(); } route_lanelets_.reserve(primitive_size); - for (const auto & route_section : route_msg_.segments) { + for (const auto & route_section : route_ptr_->segments) { for (const auto & primitive : route_section.primitives) { const auto id = primitive.id; const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); @@ -329,15 +328,15 @@ void RouteHandler::setLaneletsFromRouteMsg() } goal_lanelets_.clear(); start_lanelets_.clear(); - if (!route_msg_.segments.empty()) { - goal_lanelets_.reserve(route_msg_.segments.back().primitives.size()); - for (const auto & primitive : route_msg_.segments.back().primitives) { + if (!route_ptr_->segments.empty()) { + goal_lanelets_.reserve(route_ptr_->segments.back().primitives.size()); + for (const auto & primitive : route_ptr_->segments.back().primitives) { const auto id = primitive.id; const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); goal_lanelets_.push_back(llt); } - start_lanelets_.reserve(route_msg_.segments.front().primitives.size()); - for (const auto & primitive : route_msg_.segments.front().primitives) { + start_lanelets_.reserve(route_ptr_->segments.front().primitives.size()); + for (const auto & primitive : route_ptr_->segments.front().primitives) { const auto id = primitive.id; const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); start_lanelets_.push_back(llt); @@ -353,12 +352,20 @@ lanelet::ConstPolygon3d RouteHandler::getIntersectionAreaById(const lanelet::Id Header RouteHandler::getRouteHeader() const { - return route_msg_.header; + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getRouteHeader: Route has not been set yet"); + return Header(); + } + return route_ptr_->header; } UUID RouteHandler::getRouteUuid() const { - return route_msg_.uuid; + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); + UUID(); + } + return route_ptr_->uuid; } std::vector RouteHandler::getLanesBeforePose( @@ -404,16 +411,20 @@ lanelet::ConstLanelets RouteHandler::getRouteLanelets() const Pose RouteHandler::getGoalPose() const { - return route_msg_.goal_pose; + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); + Pose(); + } + return route_ptr_->goal_pose; } lanelet::Id RouteHandler::getGoalLaneId() const { - if (route_msg_.segments.empty()) { + if (!route_ptr_ || route_ptr_->segments.empty()) { return lanelet::InvalId; } - return route_msg_.segments.back().preferred_primitive.id; + return route_ptr_->segments.back().preferred_primitive.id; } bool RouteHandler::getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const @@ -430,10 +441,10 @@ bool RouteHandler::getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const bool RouteHandler::isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const { - if (route_msg_.segments.empty()) { + if (!route_ptr_ || route_ptr_->segments.empty()) { return false; } - return exists(route_msg_.segments.back().primitives, lanelet.id()); + return exists(route_ptr_->segments.back().primitives, lanelet.id()); } lanelet::ConstLanelets RouteHandler::getLaneletsFromIds(const lanelet::Ids & ids) const From 6b6fe15f1db3369ab469cae2a9fd96f5fce0cc1a Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 12 Apr 2023 12:24:41 +0900 Subject: [PATCH 03/12] feat(route_handler): add lateral distance calculator (#3361) * feat(route_handler): add lateral distance calculator Signed-off-by: yutaka * Update planning/route_handler/src/route_handler.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * udpate Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../include/route_handler/route_handler.hpp | 12 +++++ planning/route_handler/src/route_handler.cpp | 54 +++++++++++++++++++ 2 files changed, 66 insertions(+) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 369b71993bd1b..1d24a7d2ff688 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -270,6 +270,18 @@ class RouteHandler int getNumLaneToPreferredLane( const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + /** + * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return + * the distance to the preferred lane from the give lane. + * The distance is computed from the front point of the centerline of the given lane to + * the front point of the preferred lane. + * @param Desired lanelet to query + * @param lane change direction + * @return number of lanes from input to the preferred lane + */ + double getLateralDistanceToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getClosestLaneletWithConstrainsWithinRoute( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 6a40048105120..09ff9606ec32a 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1330,6 +1330,60 @@ int RouteHandler::getNumLaneToPreferredLane( return 0; // TODO(Horibe) check if return 0 is appropriate. } +double RouteHandler::getLateralDistanceToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction) const +{ + if (exists(preferred_lanelets_, lanelet)) { + return 0.0; + } + + if ((direction == Direction::NONE) || (direction == Direction::RIGHT)) { + double accumulated_distance = 0.0; + lanelet::ConstLanelet current_lanelet = lanelet; + const auto & right_lanes = + lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); + for (const auto & right : right_lanes) { + const auto & current_centerline = current_lanelet.centerline(); + const auto & next_centerline = right.centerline(); + if (current_centerline.empty() || next_centerline.empty()) { + return -accumulated_distance; + } + const auto & curr_pt = current_centerline.front(); + const auto & next_pt = next_centerline.front(); + accumulated_distance += lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt)); + + if (exists(preferred_lanelets_, right)) { + return -accumulated_distance; + } + current_lanelet = right; + } + } + + if ((direction == Direction::NONE) || (direction == Direction::LEFT)) { + double accumulated_distance = 0.0; + lanelet::ConstLanelet current_lanelet = lanelet; + const auto & left_lanes = + lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); + for (const auto & left : left_lanes) { + const auto & current_centerline = current_lanelet.centerline(); + const auto & next_centerline = left.centerline(); + if (current_centerline.empty() || next_centerline.empty()) { + return accumulated_distance; + } + const auto & curr_pt = current_centerline.front(); + const auto & next_pt = next_centerline.front(); + accumulated_distance += lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt)); + + if (exists(preferred_lanelets_, left)) { + return accumulated_distance; + } + current_lanelet = left; + } + } + + return 0.0; +} + bool RouteHandler::isInPreferredLane(const PoseStamped & pose) const { lanelet::ConstLanelet lanelet; From 22d561d99c3679492658183fb5d940bae54f0413 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 12 Apr 2023 14:27:31 +0900 Subject: [PATCH 04/12] refactor(avoidance): remove duplicated utils function (#3350) * refactor(avoidance): move to utils Signed-off-by: satoshi-ota * refactor(avoidance): move to utils Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 16 +- .../scene_module/avoidance_by_lc/module.hpp | 9 +- .../util/avoidance/avoidance_module_data.hpp | 4 +- .../util/avoidance/util.hpp | 24 + .../avoidance/avoidance_module.cpp | 574 ++---------------- .../scene_module/avoidance_by_lc/module.cpp | 442 ++------------ .../src/util/avoidance/util.cpp | 450 ++++++++++++++ 7 files changed, 579 insertions(+), 940 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 8e23aa1d81158..d3d7687d94803 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -172,17 +172,10 @@ class AvoidanceModule : public SceneModuleInterface } } - /** - * object pre-process - */ - void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; - - void fillObjectEnvelopePolygon(const Pose & closest_pose, ObjectData & object_data) const; + ObjectData createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const; - void fillObjectMovingTime(ObjectData & object_data) const; - - void compensateDetectionLost( - ObjectDataArray & target_objects, ObjectDataArray & other_objects) const; + void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; @@ -209,7 +202,6 @@ class AvoidanceModule : public SceneModuleInterface AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; ObjectDataArray registered_objects_; - void updateRegisteredObject(const ObjectDataArray & objects); // ========= shift line generator ====== @@ -276,8 +268,6 @@ class AvoidanceModule : public SceneModuleInterface // intersection (old) boost::optional calcIntersectionShiftLine(const AvoidancePlanningData & data) const; - bool isTargetObjectType(const PredictedObject & object) const; - // debug mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp index 0b598f9388d91..9776305ab79c9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp @@ -170,13 +170,10 @@ class AvoidanceByLCModule : public SceneModuleInterface ObjectDataArray registered_objects_; mutable ObjectDataArray stopped_objects_; + ObjectData createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const; + void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; - void fillObjectEnvelopePolygon(const Pose & closest_pose, ObjectData & object_data) const; - void fillObjectMovingTime(ObjectData & object_data) const; - void updateRegisteredObject(const ObjectDataArray & objects); - void compensateDetectionLost( - ObjectDataArray & target_objects, ObjectDataArray & other_objects) const; - bool isTargetObjectType(const PredictedObject & object) const; lanelet::ConstLanelets get_original_lanes() const; PathWithLaneId getReferencePath() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp index 553145e0b1394..aa9c1086e3b40 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/avoidance_module_data.hpp @@ -483,7 +483,8 @@ struct DebugData { std::shared_ptr expanded_lanelets; std::shared_ptr current_lanelets; - std::shared_ptr farthest_linestring_from_overhang; + + lanelet::ConstLineStrings3d bounds; AvoidLineArray current_shift_lines; // in path shifter AvoidLineArray new_shift_lines; // in path shifter @@ -525,6 +526,7 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + AvoidanceDebugMsgArray avoidance_debug_msg_array; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp index 68cebbb34f33e..fd6b15965c12a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/avoidance/util.hpp @@ -101,9 +101,33 @@ lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); +bool isTargetObjectType( + const PredictedObject & object, const std::shared_ptr & parameters); + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out); + +void fillObjectEnvelopePolygon( + ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose, + const std::shared_ptr & parameters); + +void fillObjectMovingTime( + ObjectData & object_data, ObjectDataArray & stopped_objects, + const std::shared_ptr & parameters); + +void updateRegisteredObject( + ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, + const std::shared_ptr & parameters); + +void compensateDetectionLost( + const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, + ObjectDataArray & other_objects); + +void filterTargetObjects( + ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__UTIL__AVOIDANCE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 175bd93ba0936..4b8fe6db112c8 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -260,19 +260,6 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using boost::geometry::return_centroid; - using boost::geometry::within; - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - using lanelet::utils::getId; - using lanelet::utils::to2D; - - const auto & path_points = data.reference_path.points; - const auto & ego_pos = getEgoPosition(); - - // detection area filter - // when expanding lanelets, right_offset must be minus. - // This is because y axis is positive on the left. const auto expanded_lanelets = getTargetLanelets( planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, parameters_->detection_area_right_expand_dist * (-1.0)); @@ -287,409 +274,79 @@ void AvoidanceModule::fillAvoidanceTargetObjects( data.other_objects.push_back(other_object); } - DEBUG_PRINT("dynamic_objects size = %lu", planner_data_->dynamic_object->objects.size()); - DEBUG_PRINT("lane_filtered_objects size = %lu", object_within_target_lane.objects.size()); - - // for goal - const auto & rh = planner_data_->route_handler; - const auto dist_to_goal = - rh->isInGoalRouteSection(expanded_lanelets.back()) - ? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position) - : std::numeric_limits::max(); - - lanelet::ConstLineStrings3d debug_linestring; - debug_linestring.clear(); - // for filtered objects - ObjectDataArray target_objects; - std::vector avoidance_debug_msg_array; + ObjectDataArray objects; for (const auto & object : object_within_target_lane.objects) { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - AvoidanceDebugMsg avoidance_debug_msg; - const auto avoidance_debug_array_false_and_push_back = - [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) { - avoidance_debug_msg.allow_avoidance = false; - avoidance_debug_msg.failed_reason = failed_reason; - avoidance_debug_msg_array.push_back(avoidance_debug_msg); - }; - - ObjectData object_data; - object_data.object = object; - avoidance_debug_msg.object_id = toHexString(object_data.object.object_id); - - if (!isTargetObjectType(object)) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); - object_data.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; - data.other_objects.push_back(object_data); - continue; - } - - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - - // Calc envelop polygon. - fillObjectEnvelopePolygon(object_closest_pose, object_data); - - // calc object centroid. - object_data.centroid = return_centroid(object_data.envelope_poly); - - // calc longitudinal distance from ego to closest target object footprint point. - fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path, ego_pos, object_data); - avoidance_debug_msg.longitudinal_distance = object_data.longitudinal; - - // Calc moving time. - fillObjectMovingTime(object_data); - - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto safety_margin = 0.5 * vehicle_width + parameters_->lateral_passable_safety_buffer; - object_data.avoid_required = - (isOnRight(object_data) && std::abs(object_data.overhang_dist) < safety_margin) || - (!isOnRight(object_data) && object_data.overhang_dist < safety_margin); - - if (object_data.move_time > parameters_->threshold_time_object_is_moving) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT); - object_data.reason = AvoidanceDebugFactor::MOVING_OBJECT; - data.other_objects.push_back(object_data); - continue; - } - - // object is behind ego or too far. - if (object_data.longitudinal < -parameters_->object_check_backward_distance) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); - object_data.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; - data.other_objects.push_back(object_data); - continue; - } - if (object_data.longitudinal > parameters_->object_check_forward_distance) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); - object_data.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; - data.other_objects.push_back(object_data); - continue; - } - - // Target object is behind the path goal -> ignore. - if (object_data.longitudinal > dist_to_goal) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); - object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; - data.other_objects.push_back(object_data); - continue; - } - - if ( - object_data.longitudinal + object_data.length / 2 + parameters_->object_check_goal_distance > - dist_to_goal) { - avoidance_debug_array_false_and_push_back("TooNearToGoal"); - object_data.reason = "TooNearToGoal"; - data.other_objects.push_back(object_data); - continue; - } - - lanelet::ConstLanelet overhang_lanelet; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { - continue; - } - - if (overhang_lanelet.id()) { - object_data.overhang_lanelet = overhang_lanelet; - lanelet::BasicPoint3d overhang_basic_pose( - object_data.overhang_pose.position.x, object_data.overhang_pose.position.y, - object_data.overhang_pose.position.z); - const bool get_left = - isOnRight(object_data) && parameters_->enable_avoidance_over_same_direction; - const bool get_right = - !isOnRight(object_data) && parameters_->enable_avoidance_over_same_direction; - - const auto target_lines = rh->getFurthestLinestring( - overhang_lanelet, get_right, get_left, - parameters_->enable_avoidance_over_opposite_direction); - - if (isOnRight(object_data)) { - object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); - debug_linestring.push_back(target_lines.back()); - } else { - object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); - debug_linestring.push_back(target_lines.front()); - } - } - - // calculate avoid_margin dynamically - // NOTE: This calculation must be after calculating to_road_shoulder_distance. - const double max_avoid_margin = parameters_->lateral_collision_safety_buffer + - parameters_->lateral_collision_margin + 0.5 * vehicle_width; - const double min_safety_lateral_distance = - parameters_->lateral_collision_safety_buffer + 0.5 * vehicle_width; - const auto max_allowable_lateral_distance = object_data.to_road_shoulder_distance - - parameters_->road_shoulder_safety_margin - - 0.5 * vehicle_width; - - const auto avoid_margin = [&]() -> boost::optional { - if (max_allowable_lateral_distance < min_safety_lateral_distance) { - return boost::none; - } - return std::min(max_allowable_lateral_distance, max_avoid_margin); - }(); - - // force avoidance for stopped vehicle - { - const auto to_traffic_light = - util::getDistanceToNextTrafficLight(object_pose, data.current_lanelets); - - object_data.to_stop_factor_distance = - std::min(to_traffic_light, object_data.to_stop_factor_distance); - } - - if ( - object_data.stop_time > parameters_->threshold_time_force_avoidance_for_stopped_vehicle && - parameters_->enable_force_avoidance_for_stopped_vehicle) { - if ( - object_data.to_stop_factor_distance > parameters_->object_check_force_avoidance_clearance) { - object_data.last_seen = clock_->now(); - object_data.avoid_margin = avoid_margin; - data.target_objects.push_back(object_data); - continue; - } - } - - DEBUG_PRINT( - "set object_data: longitudinal = %f, lateral = %f, largest_overhang = %f," - "to_road_shoulder_distance = %f", - object_data.longitudinal, object_data.lateral, object_data.overhang_dist, - object_data.to_road_shoulder_distance); - - // Object is on center line -> ignore. - avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; - if (std::abs(object_data.lateral) < parameters_->threshold_distance_object_is_on_center) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); - object_data.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; - data.other_objects.push_back(object_data); - continue; - } - - lanelet::ConstLanelet object_closest_lanelet; - const auto lanelet_map = rh->getLaneletMapPtr(); - if (!lanelet::utils::query::getClosestLanelet( - lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) { - continue; - } - - lanelet::BasicPoint2d object_centroid(object_data.centroid.x(), object_data.centroid.y()); - - /** - * Is not object in adjacent lane? - * - Yes -> Is parking object? - * - Yes -> the object is avoidance target. - * - No -> ignore this object. - * - No -> the object is avoidance target no matter whether it is parking object or not. - */ - const auto is_in_ego_lane = - within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); - if (is_in_ego_lane) { - /** - * TODO(Satoshi Ota) use intersection area - * under the assumption that there is no parking vehicle inside intersection, - * ignore all objects that is in the ego lane as not parking objects. - */ - std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); - object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - data.other_objects.push_back(object_data); - continue; - } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); - - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - bool is_left_side_parked_vehicle = false; - { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); - const auto most_left_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - - const auto center_to_left_boundary = distance2d( - to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_left_boundary - 0.5 * object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); - object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; - - is_left_side_parked_vehicle = - object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; - } - - bool is_right_side_parked_vehicle = false; - { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_right_road_lanelet = rh->getMostRightLanelet(object_closest_lanelet); - const auto most_right_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - - const auto center_to_right_boundary = distance2d( - to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_right_boundary - 0.5 * object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); - object_data.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; - - is_right_side_parked_vehicle = - object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; - } - - if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); - object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - data.other_objects.push_back(object_data); - continue; - } - } - - object_data.last_seen = clock_->now(); - object_data.avoid_margin = avoid_margin; - - // set data - data.target_objects.push_back(object_data); + objects.push_back(createObjectData(data, object)); } + filterTargetObjects(objects, data, debug, planner_data_, parameters_); + // debug { - updateAvoidanceDebugData(avoidance_debug_msg_array); - debug.farthest_linestring_from_overhang = - std::make_shared(debug_linestring); debug.current_lanelets = std::make_shared(data.current_lanelets); debug.expanded_lanelets = std::make_shared(expanded_lanelets); + + std::vector debug_info_array; + const auto append = [&](const auto & o) { + AvoidanceDebugMsg debug_info; + debug_info.object_id = toHexString(o.object.object_id); + debug_info.longitudinal_distance = o.longitudinal; + debug_info.lateral_distance_from_centerline = o.lateral; + debug_info.allow_avoidance = o.reason == ""; + debug_info.failed_reason = o.reason; + debug_info_array.push_back(debug_info); + }; + + for (const auto & o : objects) { + append(o); + } + + updateAvoidanceDebugData(debug_info_array); } } -void AvoidanceModule::fillObjectEnvelopePolygon( - const Pose & closest_pose, ObjectData & object_data) const +ObjectData AvoidanceModule::createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const { - using boost::geometry::within; + using boost::geometry::return_centroid; - const auto id = object_data.object.object_id; - const auto same_id_obj = std::find_if( - registered_objects_.begin(), registered_objects_.end(), - [&id](const auto & o) { return o.object.object_id == id; }); + const auto & path_points = data.reference_path.points; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - if (same_id_obj == registered_objects_.end()) { - object_data.envelope_poly = - createEnvelopePolygon(object_data, closest_pose, parameters_->object_envelope_buffer); - return; - } + ObjectData object_data{}; - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); - if (!within(object_polygon, same_id_obj->envelope_poly)) { - object_data.envelope_poly = - createEnvelopePolygon(object_data, closest_pose, parameters_->object_envelope_buffer); - return; - } + object_data.object = object; - object_data.envelope_poly = same_id_obj->envelope_poly; -} + // Calc envelop polygon. + fillObjectEnvelopePolygon(object_data, registered_objects_, object_closest_pose, parameters_); -void AvoidanceModule::fillObjectMovingTime(ObjectData & object_data) const -{ - const auto & object_vel = - object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto is_faster_than_threshold = object_vel > parameters_->threshold_speed_object_is_stopped; + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); - const auto id = object_data.object.object_id; - const auto same_id_obj = std::find_if( - stopped_objects_.begin(), stopped_objects_.end(), - [&id](const auto & o) { return o.object.object_id == id; }); + // calc longitudinal distance from ego to closest target object footprint point. + fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path, getEgoPosition(), object_data); - const auto is_new_object = same_id_obj == stopped_objects_.end(); + // Calc moving time. + fillObjectMovingTime(object_data, stopped_objects_, parameters_); - if (!is_faster_than_threshold) { - object_data.last_stop = clock_->now(); - object_data.move_time = 0.0; - if (is_new_object) { - object_data.stop_time = 0.0; - object_data.last_move = clock_->now(); - stopped_objects_.push_back(object_data); - } else { - same_id_obj->stop_time = (clock_->now() - same_id_obj->last_move).seconds(); - same_id_obj->last_stop = clock_->now(); - same_id_obj->move_time = 0.0; - object_data.stop_time = same_id_obj->stop_time; - } - return; - } + // Calc lateral deviation from path to target object. + object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - if (is_new_object) { - object_data.move_time = std::numeric_limits::max(); - object_data.stop_time = 0.0; - object_data.last_move = clock_->now(); - return; - } + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = calcEnvelopeOverhangDistance( + object_data, object_closest_pose, object_data.overhang_pose.position); - object_data.last_stop = same_id_obj->last_stop; - object_data.move_time = (clock_->now() - same_id_obj->last_stop).seconds(); - object_data.stop_time = 0.0; + // Check whether the the ego should avoid the object. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto safety_margin = 0.5 * vehicle_width + parameters_->lateral_passable_safety_buffer; + object_data.avoid_required = + (isOnRight(object_data) && std::abs(object_data.overhang_dist) < safety_margin) || + (!isOnRight(object_data) && object_data.overhang_dist < safety_margin); - if (object_data.move_time > parameters_->threshold_time_object_is_moving) { - stopped_objects_.erase(same_id_obj); - } + return object_data; } void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const @@ -2403,7 +2060,7 @@ bool AvoidanceModule::isEnoughMargin( const auto & v_ego_lon = getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - if (!isTargetObjectType(object.object)) { + if (!isTargetObjectType(object.object, parameters_)) { return true; } @@ -3454,9 +3111,9 @@ void AvoidanceModule::updateData() debug_data_ = DebugData(); avoidance_data_ = calcAvoidancePlanningData(debug_data_); - // TODO(Horibe): this is not tested yet, disable now. - updateRegisteredObject(avoidance_data_.target_objects); - compensateDetectionLost(avoidance_data_.target_objects, avoidance_data_.other_objects); + updateRegisteredObject(registered_objects_, avoidance_data_.target_objects, parameters_); + compensateDetectionLost( + registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); std::sort( avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), @@ -3486,109 +3143,6 @@ void AvoidanceModule::updateData() fillShiftLine(avoidance_data_, debug_data_); } -/* - * updateRegisteredObject - * - * Same object is observed this time -> update registered object with the new one. - * Not observed -> increment the lost_count. if it exceeds the threshold, remove it. - * How to check if it is same object? - * - it has same ID - * - it has different id, but sn object is found around similar position - */ -void AvoidanceModule::updateRegisteredObject(const ObjectDataArray & now_objects) -{ - const auto updateIfDetectedNow = [&now_objects, this](auto & registered_object) { - const auto & n = now_objects; - const auto r_id = registered_object.object.object_id; - const auto same_id_obj = std::find_if( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - - // same id object is detected. update registered. - if (same_id_obj != n.end()) { - registered_object = *same_id_obj; - return true; - } - - constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose; - const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR; - }); - - // same id object is not detected, but object is found around registered. update registered. - if (similar_pos_obj != n.end()) { - registered_object = *similar_pos_obj; - return true; - } - - // Same ID nor similar position object does not found. - return false; - }; - - // -- check registered_objects, remove if lost_count exceeds limit. -- - for (int i = static_cast(registered_objects_.size()) - 1; i >= 0; --i) { - auto & r = registered_objects_.at(i); - - // registered object is not detected this time. lost count up. - if (!updateIfDetectedNow(r)) { - r.lost_time = (clock_->now() - r.last_seen).seconds(); - } else { - r.last_seen = clock_->now(); - r.lost_time = 0.0; - } - - // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters_->object_last_seen_threshold) { - registered_objects_.erase(registered_objects_.begin() + i); - } - } - - const auto isAlreadyRegistered = [this](const auto & n_id) { - const auto & r = registered_objects_; - return std::any_of( - r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); - }; - - // -- check now_objects, add it if it has new object id -- - for (const auto & now_obj : now_objects) { - if (!isAlreadyRegistered(now_obj.object.object_id)) { - registered_objects_.push_back(now_obj); - } - } -} - -/* - * CompensateDetectionLost - * - * add registered object if the now_objects does not contain the same object_id. - * - */ -void AvoidanceModule::compensateDetectionLost( - ObjectDataArray & now_objects, ObjectDataArray & other_objects) const -{ - const auto old_size = now_objects.size(); // for debug - - const auto isDetectedNow = [&](const auto & r_id) { - const auto & n = now_objects; - return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - }; - - const auto isIgnoreObject = [&](const auto & r_id) { - const auto & n = other_objects; - return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - }; - - for (const auto & registered : registered_objects_) { - if ( - !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { - now_objects.push_back(registered); - } - } - DEBUG_PRINT("object size: %lu -> %lu", old_size, now_objects.size()); -} - void AvoidanceModule::processOnEntry() { initVariables(); @@ -3618,22 +3172,6 @@ void AvoidanceModule::initVariables() is_avoidance_maneuver_starts = false; } -bool AvoidanceModule::isTargetObjectType(const PredictedObject & object) const -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - const auto t = util::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && parameters_->avoid_car) || - (t == ObjectClassification::TRUCK && parameters_->avoid_truck) || - (t == ObjectClassification::BUS && parameters_->avoid_bus) || - (t == ObjectClassification::TRAILER && parameters_->avoid_trailer) || - (t == ObjectClassification::UNKNOWN && parameters_->avoid_unknown) || - (t == ObjectClassification::BICYCLE && parameters_->avoid_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameters_->avoid_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameters_->avoid_pedestrian)); - return is_object_type; -} - TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const { const auto shift_lines = path_shifter_.getShiftLines(); @@ -3811,7 +3349,7 @@ void AvoidanceModule::setDebugData( add(createOtherObjectsMarkerArray(data.other_objects, "other_objects")); add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray( - *debug.farthest_linestring_from_overhang, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); + debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); add(createUnavoidableObjectsMarkerArray(debug.unavoidable_objects, "unavoidable_objects")); add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp index 7f0fbdd27e3fe..e715f9d589403 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp @@ -143,8 +143,10 @@ void AvoidanceByLCModule::updateData() debug_data_ = DebugData(); avoidance_data_ = calcAvoidancePlanningData(debug_data_); - updateRegisteredObject(avoidance_data_.target_objects); - compensateDetectionLost(avoidance_data_.target_objects, avoidance_data_.other_objects); + updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, parameters_->avoidance); + compensateDetectionLost( + registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); std::sort( avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), @@ -183,27 +185,12 @@ AvoidancePlanningData AvoidanceByLCModule::calcAvoidancePlanningData(DebugData & // target objects for avoidance fillAvoidanceTargetObjects(data, debug); - // DEBUG_PRINT("target object size = %lu", data.target_objects.size()); - return data; } void AvoidanceByLCModule::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const + AvoidancePlanningData & data, DebugData & debug) const { - using boost::geometry::return_centroid; - using boost::geometry::within; - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - using lanelet::utils::getId; - using lanelet::utils::to2D; - - const auto & path_points = data.reference_path.points; - const auto & ego_pos = getEgoPosition(); - - // detection area filter - // when expanding lanelets, right_offset must be minus. - // This is because y axis is positive on the left. const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( data.current_lanelets, parameters_->avoidance->detection_area_left_expand_dist, parameters_->avoidance->detection_area_right_expand_dist * (-1.0)); @@ -218,391 +205,58 @@ void AvoidanceByLCModule::fillAvoidanceTargetObjects( data.other_objects.push_back(other_object); } - // for goal - const auto & rh = planner_data_->route_handler; - const auto dist_to_goal = - rh->isInGoalRouteSection(expanded_lanelets.back()) - ? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position) - : std::numeric_limits::max(); - - lanelet::ConstLineStrings3d debug_linestring; - debug_linestring.clear(); - // for filtered objects - ObjectDataArray target_objects; + ObjectDataArray objects; for (const auto & object : object_within_target_lane.objects) { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - - ObjectData object_data; - object_data.object = object; - - if (!isTargetObjectType(object)) { - data.other_objects.push_back(object_data); - continue; - } - - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - - // Calc envelop polygon. - fillObjectEnvelopePolygon(object_closest_pose, object_data); - - // calc object centroid. - object_data.centroid = return_centroid(object_data.envelope_poly); - - // calc longitudinal distance from ego to closest target object footprint point. - fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path, ego_pos, object_data); - - // Calc moving time. - fillObjectMovingTime(object_data); - - if (object_data.move_time > parameters_->avoidance->threshold_time_object_is_moving) { - data.other_objects.push_back(object_data); - continue; - } - - // object is behind ego or too far. - if (object_data.longitudinal < -parameters_->avoidance->object_check_backward_distance) { - data.other_objects.push_back(object_data); - continue; - } - if (object_data.longitudinal > parameters_->avoidance->object_check_forward_distance) { - data.other_objects.push_back(object_data); - continue; - } - - // Target object is behind the path goal -> ignore. - if (object_data.longitudinal > dist_to_goal) { - data.other_objects.push_back(object_data); - continue; - } - - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); - - lanelet::ConstLanelet overhang_lanelet; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { - continue; - } - - if (overhang_lanelet.id()) { - object_data.overhang_lanelet = overhang_lanelet; - lanelet::BasicPoint3d overhang_basic_pose( - object_data.overhang_pose.position.x, object_data.overhang_pose.position.y, - object_data.overhang_pose.position.z); - const bool get_left = - isOnRight(object_data) && parameters_->avoidance->enable_avoidance_over_same_direction; - const bool get_right = - !isOnRight(object_data) && parameters_->avoidance->enable_avoidance_over_same_direction; - - const auto target_lines = rh->getFurthestLinestring( - overhang_lanelet, get_right, get_left, - parameters_->avoidance->enable_avoidance_over_opposite_direction); - - if (isOnRight(object_data)) { - object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); - debug_linestring.push_back(target_lines.back()); - } else { - object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); - debug_linestring.push_back(target_lines.front()); - } - } - - // Object is on center line -> ignore. - // avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; - if ( - std::abs(object_data.lateral) < - parameters_->avoidance->threshold_distance_object_is_on_center) { - data.other_objects.push_back(object_data); - continue; - } - - lanelet::ConstLanelet object_closest_lanelet; - const auto lanelet_map = rh->getLaneletMapPtr(); - if (!lanelet::utils::query::getClosestLanelet( - lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) { - continue; - } - - lanelet::BasicPoint2d object_centroid(object_data.centroid.x(), object_data.centroid.y()); - - /** - * Is not object in adjacent lane? - * - Yes -> Is parking object? - * - Yes -> the object is avoidance target. - * - No -> ignore this object. - * - No -> the object is avoidance target no matter whether it is parking object or not. - */ - const auto is_in_ego_lane = - within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); - if (is_in_ego_lane) { - /** - * TODO(Satoshi Ota) use intersection area - * under the assumption that there is no parking vehicle inside intersection, - * ignore all objects that is in the ego lane as not parking objects. - */ - std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - data.other_objects.push_back(object_data); - continue; - } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); - - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - bool is_left_side_parked_vehicle = false; - { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); - const auto most_left_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - - const auto center_to_left_boundary = distance2d( - to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_left_boundary - 0.5 * object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters_->avoidance->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); - object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; - - is_left_side_parked_vehicle = - object_data.shiftable_ratio > parameters_->avoidance->object_check_shiftable_ratio; - } - - bool is_right_side_parked_vehicle = false; - { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_right_road_lanelet = rh->getMostRightLanelet(object_closest_lanelet); - const auto most_right_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - - const auto center_to_right_boundary = distance2d( - to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_right_boundary - 0.5 * object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters_->avoidance->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = toArcCoordinates( - to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); - object_data.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; - - is_right_side_parked_vehicle = - object_data.shiftable_ratio > parameters_->avoidance->object_check_shiftable_ratio; - } - - if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { - data.other_objects.push_back(object_data); - continue; - } - } - - object_data.last_seen = clock_->now(); - - // set data - data.target_objects.push_back(object_data); + objects.push_back(createObjectData(data, object)); } -} - -void AvoidanceByLCModule::fillObjectEnvelopePolygon( - const Pose & closest_pose, ObjectData & object_data) const -{ - using boost::geometry::within; - const auto id = object_data.object.object_id; - const auto same_id_obj = std::find_if( - registered_objects_.begin(), registered_objects_.end(), - [&id](const auto & o) { return o.object.object_id == id; }); - - if (same_id_obj == registered_objects_.end()) { - object_data.envelope_poly = createEnvelopePolygon( - object_data, closest_pose, parameters_->avoidance->object_envelope_buffer); - return; - } - - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); - - if (!within(object_polygon, same_id_obj->envelope_poly)) { - object_data.envelope_poly = createEnvelopePolygon( - object_data, closest_pose, parameters_->avoidance->object_envelope_buffer); - return; - } - - object_data.envelope_poly = same_id_obj->envelope_poly; + filterTargetObjects(objects, data, debug, planner_data_, parameters_->avoidance); } -void AvoidanceByLCModule::fillObjectMovingTime(ObjectData & object_data) const +ObjectData AvoidanceByLCModule::createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const { - const auto & object_vel = - object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto is_faster_than_threshold = - object_vel > parameters_->avoidance->threshold_speed_object_is_stopped; - - const auto id = object_data.object.object_id; - const auto same_id_obj = std::find_if( - stopped_objects_.begin(), stopped_objects_.end(), - [&id](const auto & o) { return o.object.object_id == id; }); - - const auto is_new_object = same_id_obj == stopped_objects_.end(); - - if (!is_faster_than_threshold) { - object_data.last_stop = clock_->now(); - object_data.move_time = 0.0; - if (is_new_object) { - stopped_objects_.push_back(object_data); - } else { - same_id_obj->last_stop = clock_->now(); - same_id_obj->move_time = 0.0; - } - return; - } - - if (is_new_object) { - object_data.move_time = std::numeric_limits::max(); - return; - } - - object_data.last_stop = same_id_obj->last_stop; - object_data.move_time = (clock_->now() - same_id_obj->last_stop).seconds(); - - if (object_data.move_time > parameters_->avoidance->threshold_time_object_is_moving) { - stopped_objects_.erase(same_id_obj); - } -} - -void AvoidanceByLCModule::updateRegisteredObject(const ObjectDataArray & now_objects) -{ - const auto updateIfDetectedNow = [&now_objects, this](auto & registered_object) { - const auto & n = now_objects; - const auto r_id = registered_object.object.object_id; - const auto same_id_obj = std::find_if( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - - // same id object is detected. update registered. - if (same_id_obj != n.end()) { - registered_object = *same_id_obj; - return true; - } + using boost::geometry::return_centroid; - constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose; - const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR; - }); + const auto & path_points = data.reference_path.points; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - // same id object is not detected, but object is found around registered. update registered. - if (similar_pos_obj != n.end()) { - registered_object = *similar_pos_obj; - return true; - } + ObjectData object_data{}; - // Same ID nor similar position object does not found. - return false; - }; + object_data.object = object; - // -- check registered_objects, remove if lost_count exceeds limit. -- - for (int i = static_cast(registered_objects_.size()) - 1; i >= 0; --i) { - auto & r = registered_objects_.at(i); + // Calc envelop polygon. + fillObjectEnvelopePolygon( + object_data, registered_objects_, object_closest_pose, parameters_->avoidance); - // registered object is not detected this time. lost count up. - if (!updateIfDetectedNow(r)) { - r.lost_time = (clock_->now() - r.last_seen).seconds(); - } else { - r.last_seen = clock_->now(); - r.lost_time = 0.0; - } + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); - // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters_->avoidance->object_last_seen_threshold) { - registered_objects_.erase(registered_objects_.begin() + i); - } - } + // calc longitudinal distance from ego to closest target object footprint point. + fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path, getEgoPosition(), object_data); - const auto isAlreadyRegistered = [this](const auto & n_id) { - const auto & r = registered_objects_; - return std::any_of( - r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); - }; + // Calc moving time. + fillObjectMovingTime(object_data, stopped_objects_, parameters_->avoidance); - // -- check now_objects, add it if it has new object id -- - for (const auto & now_obj : now_objects) { - if (!isAlreadyRegistered(now_obj.object.object_id)) { - registered_objects_.push_back(now_obj); - } - } -} + // Calc lateral deviation from path to target object. + object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); -void AvoidanceByLCModule::compensateDetectionLost( - ObjectDataArray & now_objects, ObjectDataArray & other_objects) const -{ - const auto isDetectedNow = [&](const auto & r_id) { - const auto & n = now_objects; - return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - }; + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = calcEnvelopeOverhangDistance( + object_data, object_closest_pose, object_data.overhang_pose.position); - const auto isIgnoreObject = [&](const auto & r_id) { - const auto & n = other_objects; - return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - }; + // Check whether the the ego should avoid the object. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto safety_margin = + 0.5 * vehicle_width + parameters_->avoidance->lateral_passable_safety_buffer; + object_data.avoid_required = + (isOnRight(object_data) && std::abs(object_data.overhang_dist) < safety_margin) || + (!isOnRight(object_data) && object_data.overhang_dist < safety_margin); - for (const auto & registered : registered_objects_) { - if ( - !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { - now_objects.push_back(registered); - } - } + return object_data; } ModuleStatus AvoidanceByLCModule::updateState() @@ -1331,22 +985,6 @@ void AvoidanceByLCModule::updateOutputTurnSignal(BehaviorModuleOutput & output) util::lane_change::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); } -bool AvoidanceByLCModule::isTargetObjectType(const PredictedObject & object) const -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - const auto t = util::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && parameters_->avoidance->avoid_car) || - (t == ObjectClassification::TRUCK && parameters_->avoidance->avoid_truck) || - (t == ObjectClassification::BUS && parameters_->avoidance->avoid_bus) || - (t == ObjectClassification::TRAILER && parameters_->avoidance->avoid_trailer) || - (t == ObjectClassification::UNKNOWN && parameters_->avoidance->avoid_unknown) || - (t == ObjectClassification::BICYCLE && parameters_->avoidance->avoid_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameters_->avoidance->avoid_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameters_->avoidance->avoid_pedestrian)); - return is_object_type; -} - void AvoidanceByLCModule::resetParameters() { is_abort_path_approved_ = false; diff --git a/planning/behavior_path_planner/src/util/avoidance/util.cpp b/planning/behavior_path_planner/src/util/avoidance/util.cpp index 1fe0a8d22ede6..e76080b10a983 100644 --- a/planning/behavior_path_planner/src/util/avoidance/util.cpp +++ b/planning/behavior_path_planner/src/util/avoidance/util.cpp @@ -22,6 +22,8 @@ #include #include +#include + #include #include #include @@ -33,14 +35,20 @@ namespace behavior_path_planner { using motion_utils::calcLongitudinalOffsetPoint; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcLateralDeviation; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; +using tier4_autoware_utils::toHexString; +using tier4_planning_msgs::msg::AvoidanceDebugFactor; +using tier4_planning_msgs::msg::AvoidanceDebugMsg; namespace { @@ -699,6 +707,23 @@ lanelet::ConstLanelets getTargetLanelets( return target_lanelets; } +bool isTargetObjectType( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + const auto t = util::getHighestProbLabel(object.classification); + const auto is_object_type = + ((t == ObjectClassification::CAR && parameters->avoid_car) || + (t == ObjectClassification::TRUCK && parameters->avoid_truck) || + (t == ObjectClassification::BUS && parameters->avoid_bus) || + (t == ObjectClassification::TRAILER && parameters->avoid_trailer) || + (t == ObjectClassification::UNKNOWN && parameters->avoid_unknown) || + (t == ObjectClassification::BICYCLE && parameters->avoid_bicycle) || + (t == ObjectClassification::MOTORCYCLE && parameters->avoid_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && parameters->avoid_pedestrian)); + return is_object_type; +} + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, boost::optional & p_out) @@ -729,4 +754,429 @@ void insertDecelPoint( p_out = getPose(path.points.at(insert_idx.get())); } + +void fillObjectEnvelopePolygon( + ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose, + const std::shared_ptr & parameters) +{ + using boost::geometry::within; + + const auto id = object_data.object.object_id; + const auto same_id_obj = std::find_if( + registered_objects.begin(), registered_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + if (same_id_obj == registered_objects.end()) { + object_data.envelope_poly = + createEnvelopePolygon(object_data, closest_pose, parameters->object_envelope_buffer); + return; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + + if (!within(object_polygon, same_id_obj->envelope_poly)) { + object_data.envelope_poly = + createEnvelopePolygon(object_data, closest_pose, parameters->object_envelope_buffer); + return; + } + + object_data.envelope_poly = same_id_obj->envelope_poly; +} + +void fillObjectMovingTime( + ObjectData & object_data, ObjectDataArray & stopped_objects, + const std::shared_ptr & parameters) +{ + const auto & object_vel = + object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto is_faster_than_threshold = object_vel > parameters->threshold_speed_object_is_stopped; + + const auto id = object_data.object.object_id; + const auto same_id_obj = std::find_if( + stopped_objects.begin(), stopped_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + const auto is_new_object = same_id_obj == stopped_objects.end(); + const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + + if (!is_faster_than_threshold) { + object_data.last_stop = now; + object_data.move_time = 0.0; + if (is_new_object) { + stopped_objects.push_back(object_data); + } else { + same_id_obj->last_stop = now; + same_id_obj->move_time = 0.0; + } + return; + } + + if (is_new_object) { + object_data.move_time = std::numeric_limits::max(); + return; + } + + object_data.last_stop = same_id_obj->last_stop; + object_data.move_time = (now - same_id_obj->last_stop).seconds(); + + if (object_data.move_time > parameters->threshold_time_object_is_moving) { + stopped_objects.erase(same_id_obj); + } +} + +void updateRegisteredObject( + ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, + const std::shared_ptr & parameters) +{ + const auto updateIfDetectedNow = [&now_objects](auto & registered_object) { + const auto & n = now_objects; + const auto r_id = registered_object.object.object_id; + const auto same_id_obj = std::find_if( + n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + + // same id object is detected. update registered. + if (same_id_obj != n.end()) { + registered_object = *same_id_obj; + return true; + } + + constexpr auto POS_THR = 1.5; + const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose; + const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { + return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR; + }); + + // same id object is not detected, but object is found around registered. update registered. + if (similar_pos_obj != n.end()) { + registered_object = *similar_pos_obj; + return true; + } + + // Same ID nor similar position object does not found. + return false; + }; + + const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + + // -- check registered_objects, remove if lost_count exceeds limit. -- + for (int i = static_cast(registered_objects.size()) - 1; i >= 0; --i) { + auto & r = registered_objects.at(i); + + // registered object is not detected this time. lost count up. + if (!updateIfDetectedNow(r)) { + r.lost_time = (now - r.last_seen).seconds(); + } else { + r.last_seen = now; + r.lost_time = 0.0; + } + + // lost count exceeds threshold. remove object from register. + if (r.lost_time > parameters->object_last_seen_threshold) { + registered_objects.erase(registered_objects.begin() + i); + } + } + + const auto isAlreadyRegistered = [&](const auto & n_id) { + const auto & r = registered_objects; + return std::any_of( + r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); + }; + + // -- check now_objects, add it if it has new object id -- + for (const auto & now_obj : now_objects) { + if (!isAlreadyRegistered(now_obj.object.object_id)) { + registered_objects.push_back(now_obj); + } + } +} + +void compensateDetectionLost( + const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, + ObjectDataArray & other_objects) +{ + const auto isDetectedNow = [&](const auto & r_id) { + const auto & n = now_objects; + return std::any_of( + n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + }; + + const auto isIgnoreObject = [&](const auto & r_id) { + const auto & n = other_objects; + return std::any_of( + n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + }; + + for (const auto & registered : registered_objects) { + if ( + !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { + now_objects.push_back(registered); + } + } +} + +void filterTargetObjects( + ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + using boost::geometry::return_centroid; + using boost::geometry::within; + using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; + using lanelet::utils::to2D; + + const auto & rh = planner_data->route_handler; + const auto & path_points = data.reference_path.points; + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + const auto & vehicle_width = planner_data->parameters.vehicle_width; + const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + + // for goal + const auto dist_to_goal = + rh->isInGoalRouteSection(data.current_lanelets.back()) + ? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position) + : std::numeric_limits::max(); + + for (auto & o : objects) { + const auto & object_pose = o.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; + + if (!isTargetObjectType(o.object, parameters)) { + data.other_objects.push_back(o); + continue; + } + + if (o.move_time > parameters->threshold_time_object_is_moving) { + o.reason = AvoidanceDebugFactor::MOVING_OBJECT; + data.other_objects.push_back(o); + continue; + } + + // object is behind ego or too far. + if (o.longitudinal < -parameters->object_check_backward_distance) { + o.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; + data.other_objects.push_back(o); + continue; + } + if (o.longitudinal > parameters->object_check_forward_distance) { + o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; + data.other_objects.push_back(o); + continue; + } + + // Target object is behind the path goal -> ignore. + if (o.longitudinal > dist_to_goal) { + o.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + data.other_objects.push_back(o); + continue; + } + + if (o.longitudinal + o.length / 2 + parameters->object_check_goal_distance > dist_to_goal) { + o.reason = "TooNearToGoal"; + data.other_objects.push_back(o); + continue; + } + + lanelet::ConstLanelet overhang_lanelet; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { + continue; + } + + if (overhang_lanelet.id()) { + o.overhang_lanelet = overhang_lanelet; + lanelet::BasicPoint3d overhang_basic_pose( + o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z); + const bool get_left = isOnRight(o) && parameters->enable_avoidance_over_same_direction; + const bool get_right = !isOnRight(o) && parameters->enable_avoidance_over_same_direction; + + const auto target_lines = rh->getFurthestLinestring( + overhang_lanelet, get_right, get_left, + parameters->enable_avoidance_over_opposite_direction); + + if (isOnRight(o)) { + o.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); + debug.bounds.push_back(target_lines.back()); + } else { + o.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); + debug.bounds.push_back(target_lines.front()); + } + } + + // calculate avoid_margin dynamically + // NOTE: This calculation must be after calculating to_road_shoulder_distance. + const double max_avoid_margin = parameters->lateral_collision_safety_buffer + + parameters->lateral_collision_margin + 0.5 * vehicle_width; + const double min_safety_lateral_distance = + parameters->lateral_collision_safety_buffer + 0.5 * vehicle_width; + const auto max_allowable_lateral_distance = + o.to_road_shoulder_distance - parameters->road_shoulder_safety_margin - 0.5 * vehicle_width; + + const auto avoid_margin = [&]() -> boost::optional { + if (max_allowable_lateral_distance < min_safety_lateral_distance) { + return boost::none; + } + return std::min(max_allowable_lateral_distance, max_avoid_margin); + }(); + + // force avoidance for stopped vehicle + { + const auto to_traffic_light = + util::getDistanceToNextTrafficLight(object_pose, data.current_lanelets); + + o.to_stop_factor_distance = std::min(to_traffic_light, o.to_stop_factor_distance); + } + + if ( + o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle && + parameters->enable_force_avoidance_for_stopped_vehicle) { + if (o.to_stop_factor_distance > parameters->object_check_force_avoidance_clearance) { + o.last_seen = now; + o.avoid_margin = avoid_margin; + data.target_objects.push_back(o); + continue; + } + } + + // Object is on center line -> ignore. + if (std::abs(o.lateral) < parameters->threshold_distance_object_is_on_center) { + o.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + data.other_objects.push_back(o); + continue; + } + + lanelet::ConstLanelet object_closest_lanelet; + const auto lanelet_map = rh->getLaneletMapPtr(); + if (!lanelet::utils::query::getClosestLanelet( + lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) { + continue; + } + + lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y()); + + /** + * Is not object in adjacent lane? + * - Yes -> Is parking object? + * - Yes -> the object is avoidance target. + * - No -> ignore this object. + * - No -> the object is avoidance target no matter whether it is parking object or not. + */ + const auto is_in_ego_lane = + within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); + if (is_in_ego_lane) { + /** + * TODO(Satoshi Ota) use intersection area + * under the assumption that there is no parking vehicle inside intersection, + * ignore all objects that is in the ego lane as not parking objects. + */ + std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + data.other_objects.push_back(o); + continue; + } + + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position); + lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + bool is_left_side_parked_vehicle = false; + { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); + const auto most_left_lanelet_candidates = + rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + + const auto center_to_left_boundary = distance2d( + to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_left_boundary - 0.5 * o.object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + o.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; + + is_left_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; + } + + bool is_right_side_parked_vehicle = false; + { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_right_road_lanelet = rh->getMostRightLanelet(object_closest_lanelet); + const auto most_right_lanelet_candidates = + rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); + + lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; + const lanelet::Attribute sub_type = + most_right_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_right_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_right_lanelet = ll; + } + } + + const auto center_to_right_boundary = distance2d( + to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_right_boundary - 0.5 * o.object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + o.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; + + is_right_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; + } + + if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { + o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + data.other_objects.push_back(o); + continue; + } + } + + o.last_seen = now; + o.avoid_margin = avoid_margin; + + // set data + data.target_objects.push_back(o); + } +} } // namespace behavior_path_planner From 3aead0d1d39a542da9e71337d6ca2b53e127d4f6 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 12 Apr 2023 15:15:05 +0900 Subject: [PATCH 05/12] fix(lane change): change Logger level (#3369) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/util/lane_change/util.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 4dc62fcf6eaff..b38dc17a56371 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -349,7 +349,7 @@ bool getLaneChangePaths( minimum_prepare_length); if (prepare_length < target_length) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "prepare length is shorter than distance to target lane!!"); break; @@ -366,7 +366,7 @@ bool getLaneChangePaths( #endif if (prepare_segment.points.empty()) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "prepare segment is empty!!"); continue; @@ -381,7 +381,7 @@ bool getLaneChangePaths( // target lanelet, even if the condition prepare_length > target_length is satisfied. In // that case, the lane change shouldn't be executed. if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "[only new arch] lane change start pose is behind target lanelet!!"); break; @@ -396,7 +396,7 @@ bool getLaneChangePaths( calcLaneChangingLength(lane_changing_velocity, shift_length, common_parameter, parameter); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "lane changing path too long"); continue; @@ -411,7 +411,7 @@ bool getLaneChangePaths( s_start + lane_changing_length + parameter.lane_change_finish_judge_buffer + required_total_min_length > s_goal) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "length of lane changing path is longer than length to goal!!"); continue; @@ -423,7 +423,7 @@ bool getLaneChangePaths( target_lane_length, lane_changing_length, lane_changing_velocity, required_total_min_length); if (target_segment.points.empty()) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "target segment is empty!! something wrong..."); continue; @@ -438,7 +438,7 @@ bool getLaneChangePaths( lc_length.lane_changing, forward_path_length, resample_interval, is_goal_in_route); if (target_lane_reference_path.points.empty()) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "target_lane_reference_path is empty!!"); continue; @@ -454,7 +454,7 @@ bool getLaneChangePaths( target_lanelets, sorted_lane_ids, acceleration, lc_length, lc_velocity, parameter); if (!candidate_path) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "no candidate path!!"); continue; @@ -471,7 +471,7 @@ bool getLaneChangePaths( #endif if (!is_valid) { - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), "invalid candidate path!!"); continue; From ce43f0de946fae6249cdc5123f2b7747972c9715 Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Wed, 12 Apr 2023 16:59:30 +0900 Subject: [PATCH 06/12] feat(elevation_map_loader): add support for seleceted_map_loader (#3344) * feat(elevation_map_loader): add support for sequential_map_loading Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix(elevation_map_loader): fix bug Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(elevation_map_loader): make it possible to adjust the number of PCD maps loaded at once when using sequential map loading Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(elevation_map_loader): change default value of use_lane_filter as false Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix(elevation_map_loader): fix typo Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * refactor(elevation_map_loader): Add a range of param. And refactor receiveMap. Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(elevation_map_loader): Change info level log into debug level log with throttle. And remove abbreviation Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --------- Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --- .../ground_segmentation.launch.py | 4 + perception/elevation_map_loader/README.md | 39 +++-- .../elevation_map_loader_node.hpp | 22 ++- .../launch/elevation_map_loader.launch.xml | 6 + perception/elevation_map_loader/package.xml | 1 + .../src/elevation_map_loader_node.cpp | 150 +++++++++++++++++- 6 files changed, 203 insertions(+), 19 deletions(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 105bcb0e76e93..7ed5860612601 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -321,10 +321,14 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con ("output/elevation_map", "map"), ("input/pointcloud_map", "/map/pointcloud_map"), ("input/vector_map", "/map/vector_map"), + ("input/pointcloud_map_metadata", "/map/pointcloud_map_metadata"), + ("service/get_selected_pointcloud_map", "/map/get_selected_pointcloud_map"), ], parameters=[ { "use_lane_filter": False, + "use_sequential_load": False, + "sequential_map_load_num": 1, "use_inpaint": True, "inpaint_radius": 1.0, "lane_margin": 2.0, diff --git a/perception/elevation_map_loader/README.md b/perception/elevation_map_loader/README.md index 2cabc2803c8ec..0fa644cbb304d 100644 --- a/perception/elevation_map_loader/README.md +++ b/perception/elevation_map_loader/README.md @@ -20,10 +20,11 @@ Cells with No elevation value can be inpainted using the values of neighboring c ### Input -| Name | Type | Description | -| ---------------------- | -------------------------------------------- | ------------------------------------------ | -| `input/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | The point cloud map | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | (Optional) The binary data of lanelet2 map | +| Name | Type | Description | +| ------------------------------- | ----------------------------------------------- | ------------------------------------------ | +| `input/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | The point cloud map | +| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | (Optional) The binary data of lanelet2 map | +| `input/pointcloud_map_metadata` | `autoware_map_msgs::msg::PointCloudMapMetaData` | (Optional) The metadata of point cloud map | ### Output @@ -32,21 +33,29 @@ Cells with No elevation value can be inpainted using the values of neighboring c | `output/elevation_map` | `grid_map_msgs::msg::GridMap` | The elevation map | | `output/elevation_map_cloud` | `sensor_msgs::msg::PointCloud2` | (Optional) The point cloud generated from the value of elevation map | +### Service + +| Name | Type | Description | +| ------------------------------ | -------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------- | +| `service/get_selected_pcd_map` | `autoware_map_msgs::srv::GetSelectedPointCloudMap` | (Optional) service to request point cloud map. If pointcloud_map_loader uses selected pointcloud map loading via ROS 2 service, use this. | + ## Parameters ### Node parameters -| Name | Type | Description | Default value | -| :-------------------------------- | :---------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| map_layer_name | std::string | elevation_map layer name | elevation | -| param_file_path | std::string | GridMap parameters config | path_default | -| elevation_map_directory | std::string | elevation_map file (bag2) | path_default | -| map_frame | std::string | map_frame when loading elevation_map file | map | -| use_inpaint | bool | Whether to inpaint empty cells | true | -| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | -| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | -| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | -| lane_margin | float | Margin distance from the lane polygon of the area to be included in the inpainting mask [m]. Used only when use_lane_filter=True. | 0.0 | +| Name | Type | Description | Default value | +| :-------------------------------- | :---------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| map_layer_name | std::string | elevation_map layer name | elevation | +| param_file_path | std::string | GridMap parameters config | path_default | +| elevation_map_directory | std::string | elevation_map file (bag2) | path_default | +| map_frame | std::string | map_frame when loading elevation_map file | map | +| use_inpaint | bool | Whether to inpaint empty cells | true | +| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | +| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | +| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | +| lane_margin | float | Margin distance from the lane polygon of the area to be included in the inpainting mask [m]. Used only when use_lane_filter=True. | 0.0 | +| use_sequential_load | bool | Whether to get point cloud map by service | false | +| sequential_map_load_num | int | The number of point cloud maps to load at once (only used when use_sequential_load is set true). This should not be larger than number of all point cloud map cells. | 1 | ### GridMap parameters diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 1e45fde199f3c..3f383ce3d6d0e 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -26,6 +26,8 @@ #include "tier4_external_api_msgs/msg/map_hash.hpp" #include +#include +#include #include #include @@ -53,6 +55,7 @@ class DataManager pcl::PointCloud::Ptr map_pcl_ptr_; lanelet::LaneletMapPtr lanelet_map_ptr_; bool use_lane_filter_ = false; + std::vector pointcloud_map_ids_; }; class ElevationMapLoaderNode : public rclcpp::Node @@ -64,12 +67,25 @@ class ElevationMapLoaderNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_pointcloud_map_; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_map_hash_; + rclcpp::Subscription::SharedPtr + sub_pointcloud_metadata_; rclcpp::Publisher::SharedPtr pub_elevation_map_; rclcpp::Publisher::SharedPtr pub_elevation_map_cloud_; + rclcpp::Client::SharedPtr pcd_loader_client_; + rclcpp::CallbackGroup::SharedPtr group_; + rclcpp::TimerBase::SharedPtr timer_; void onPointcloudMap(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map); void onMapHash(const tier4_external_api_msgs::msg::MapHash::ConstSharedPtr map_hash); + void timerCallback(); void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map); - + void onPointCloudMapMetaData( + const autoware_map_msgs::msg::PointCloudMapMetaData pointcloud_map_metadata); + void receiveMap(); + void concatenatePointCloudMaps( + sensor_msgs::msg::PointCloud2 & pointcloud_map, + const std::vector & new_pointcloud_with_ids) + const; + std::vector getRequestIDs(const unsigned int map_id_counter) const; void publish(); void createElevationMap(); void setVerbosityLevelToDebugIfFlagSet(); @@ -85,8 +101,12 @@ class ElevationMapLoaderNode : public rclcpp::Node std::string elevation_map_directory_; bool use_inpaint_; float inpaint_radius_; + unsigned int sequential_map_load_num_; bool use_elevation_map_cloud_publisher_; std::string param_file_path_; + bool is_map_metadata_received_ = false; + bool is_map_received_ = false; + bool is_elevation_map_published_ = false; DataManager data_manager_; struct LaneFilter diff --git a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml index 7b415f5b2e770..b222d11c634a7 100644 --- a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml +++ b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml @@ -2,16 +2,22 @@ + + + + + + diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index e809298e4c415..0e2ea81f9f28e 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_auto_mapping_msgs + autoware_map_msgs grid_map_cv grid_map_pcl grid_map_ros diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index c57934b960906..351493d24bf61 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -54,6 +54,13 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); param_file_path_ = this->declare_parameter("param_file_path", "path_default"); map_frame_ = this->declare_parameter("map_frame", "map"); + bool use_sequential_load = this->declare_parameter("use_sequential_load", true); + int sequential_map_load_num_int = this->declare_parameter("sequential_map_load_num", 1); + if (sequential_map_load_num_int > 0) { + sequential_map_load_num_ = (unsigned int)sequential_map_load_num_int; + } else { + throw std::runtime_error("sequential_map_load_num should be larger than 0."); + } use_inpaint_ = this->declare_parameter("use_inpaint", true); inpaint_radius_ = this->declare_parameter("inpaint_radius", 0.3); use_elevation_map_cloud_publisher_ = @@ -79,11 +86,38 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio sub_map_hash_ = create_subscription( "/api/autoware/get/map/info/hash", durable_qos, std::bind(&ElevationMapLoaderNode::onMapHash, this, _1)); - sub_pointcloud_map_ = this->create_subscription( - "input/pointcloud_map", durable_qos, - std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); sub_vector_map_ = this->create_subscription( "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1)); + if (use_sequential_load) { + { + sub_pointcloud_metadata_ = + this->create_subscription( + "input/pointcloud_map_metadata", durable_qos, + std::bind(&ElevationMapLoaderNode::onPointCloudMapMetaData, this, _1)); + constexpr auto period_ns = + std::chrono::duration_cast(std::chrono::duration(1.0)); + group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + pcd_loader_client_ = create_client( + "service/get_selected_pointcloud_map", rmw_qos_profile_services_default, group_); + + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_DEBUG_THROTTLE( + this->get_logger(), *get_clock(), 5000, + "Waiting for pcd map loader service. Check if the enable_selected_load in " + "pointcloud_map_loader is set `true`."); + } + timer_ = + this->create_wall_timer(period_ns, std::bind(&ElevationMapLoaderNode::timerCallback, this)); + } + + if (data_manager_.isInitialized()) { + publish(); + } + } else { + sub_pointcloud_map_ = this->create_subscription( + "input/pointcloud_map", durable_qos, + std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1)); + } } void ElevationMapLoaderNode::publish() @@ -128,6 +162,20 @@ void ElevationMapLoaderNode::publish() pcl::toROSMsg(*elevation_map_cloud_ptr, elevation_map_cloud_msg); pub_elevation_map_cloud_->publish(elevation_map_cloud_msg); } + is_elevation_map_published_ = true; +} + +void ElevationMapLoaderNode::timerCallback() +{ + if (!is_map_received_ && is_map_metadata_received_) { + ElevationMapLoaderNode::receiveMap(); + // flag to make receiveMap() called only once. + is_map_received_ = true; + RCLCPP_DEBUG(this->get_logger(), "receive service with pointcloud_map"); + } + if (data_manager_.isInitialized() && !is_elevation_map_published_) { + publish(); + } } void ElevationMapLoaderNode::onMapHash( @@ -156,6 +204,30 @@ void ElevationMapLoaderNode::onPointcloudMap( } } +void ElevationMapLoaderNode::onPointCloudMapMetaData( + const autoware_map_msgs::msg::PointCloudMapMetaData pointcloud_map_metadata) +{ + RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map metadata"); + { + if (pointcloud_map_metadata.metadata_list.size() < 1) { + RCLCPP_ERROR( + this->get_logger(), "PCD metadata size: %lu", pointcloud_map_metadata.metadata_list.size()); + throw std::runtime_error("PCD metadata is invalid"); + } + if (sequential_map_load_num_ > pointcloud_map_metadata.metadata_list.size()) { + RCLCPP_WARN( + this->get_logger(), + "The following range is not met: sequential_map_load_num <= %lu (number of all point cloud " + "map cells)", + pointcloud_map_metadata.metadata_list.size()); + } + for (const auto & pointcloud_map_cell_metadata : pointcloud_map_metadata.metadata_list) { + data_manager_.pointcloud_map_ids_.push_back(pointcloud_map_cell_metadata.cell_id); + } + } + is_map_metadata_received_ = true; +} + void ElevationMapLoaderNode::onVectorMap( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr vector_map) { @@ -170,6 +242,78 @@ void ElevationMapLoaderNode::onVectorMap( } } +void ElevationMapLoaderNode::receiveMap() +{ + sensor_msgs::msg::PointCloud2 pointcloud_map; + // create a loading request with mode = 1 + auto request = std::make_shared(); + if (!pcd_loader_client_->service_is_ready()) { + RCLCPP_DEBUG_THROTTLE( + this->get_logger(), *get_clock(), 5000, + "Waiting for pcd map loader service. Check if the enable_selected_load in " + "pointcloud_map_loader is set `true`."); + } + + // request PCD maps in batches of sequential_map_load_num + for (unsigned int map_id_counter = 0; map_id_counter < data_manager_.pointcloud_map_ids_.size(); + map_id_counter += sequential_map_load_num_) { + // get ids to request + request->cell_ids = getRequestIDs(map_id_counter); + + // send a request to map_loader + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *get_clock(), 5000, "send a request to map_loader"); + auto result{pcd_loader_client_->async_send_request( + request, + [](rclcpp::Client::SharedFuture) {})}; + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *get_clock(), 5000, "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + + // concatenate maps + concatenatePointCloudMaps(pointcloud_map, result.get()->new_pointcloud_with_ids); + } + RCLCPP_DEBUG(this->get_logger(), "finish receiving"); + pcl::PointCloud map_pcl; + pcl::fromROSMsg(pointcloud_map, map_pcl); + data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); +} + +void ElevationMapLoaderNode::concatenatePointCloudMaps( + sensor_msgs::msg::PointCloud2 & pointcloud_map, + const std::vector & new_pointcloud_with_ids) + const +{ + for (const auto & new_pointcloud_with_id : new_pointcloud_with_ids) { + if (pointcloud_map.width == 0) { + pointcloud_map = new_pointcloud_with_id.pointcloud; + } else { + pointcloud_map.width += new_pointcloud_with_id.pointcloud.width; + pointcloud_map.row_step += new_pointcloud_with_id.pointcloud.row_step; + pointcloud_map.data.insert( + pointcloud_map.data.end(), new_pointcloud_with_id.pointcloud.data.begin(), + new_pointcloud_with_id.pointcloud.data.end()); + } + } +} + +std::vector ElevationMapLoaderNode::getRequestIDs( + const unsigned int map_id_counter) const +{ + std::vector pointcloud_map_ids = { + data_manager_.pointcloud_map_ids_.at(map_id_counter)}; + for (unsigned int i = 1; i < sequential_map_load_num_; i++) { + if (map_id_counter + i < data_manager_.pointcloud_map_ids_.size()) { + pointcloud_map_ids.push_back(data_manager_.pointcloud_map_ids_.at(map_id_counter + i)); + } + } + return pointcloud_map_ids; +} + void ElevationMapLoaderNode::createElevationMap() { auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); From 03ae154385b7fef72b0a87d936d91a33d55fc6c3 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 12 Apr 2023 17:21:48 +0900 Subject: [PATCH 07/12] feat(lidar_apollo_instance_segmentation, tensorrt_common): use onnx instead of caffe for apollo cnnseg (#3320) * feat: use onnx instead of caffe for apollo cnnseg Signed-off-by: wep21 * style(pre-commit): autofix * diable some lint Signed-off-by: wep21 * add include guard Signed-off-by: wep21 * style(pre-commit): autofix * fix company name in license Signed-off-by: wep21 * remove unrelated file Signed-off-by: wep21 * change find order Signed-off-by: wep21 * update year in license Signed-off-by: wep21 * fix year in license Signed-off-by: wep21 --------- Signed-off-by: wep21 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/tensorrt_common/CMakeLists.txt | 70 +- common/tensorrt_common/package.xml | 6 +- .../.gitignore | 2 +- .../CMakeLists.txt | 262 +++---- .../data/hdl-64.prototxt | 685 ----------------- .../data/vlp-16.prototxt | 685 ----------------- .../data/vls-128.prototxt | 686 ------------------ .../cluster2d.hpp | 17 +- .../debugger.hpp | 10 +- .../detector.hpp | 35 +- .../disjoint_set.hpp | 3 + .../feature_generator.hpp | 10 +- .../feature_map.hpp | 12 +- .../log_table.hpp | 11 +- .../node.hpp | 12 +- .../util.hpp | 3 + ...ar_apollo_instance_segmentation.launch.xml | 11 +- .../lib/LICENSE | 21 - .../lib/README.md | 28 - .../lib/include/TrtNet.hpp | 115 --- .../lib/include/Utils.hpp | 134 ---- .../lib/src/TrtNet.cpp | 152 ---- .../package.xml | 12 +- .../src/cluster2d.cpp | 35 +- .../src/debugger.cpp | 5 +- .../src/detector.cpp | 100 ++- .../src/feature_generator.cpp | 5 +- .../src/feature_map.cpp | 5 +- .../src/log_table.cpp | 5 +- .../src/node.cpp | 7 +- 30 files changed, 341 insertions(+), 2803 deletions(-) delete mode 100644 perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt delete mode 100644 perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt delete mode 100644 perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt delete mode 100644 perception/lidar_apollo_instance_segmentation/lib/LICENSE delete mode 100644 perception/lidar_apollo_instance_segmentation/lib/README.md delete mode 100644 perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp delete mode 100644 perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp delete mode 100644 perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp diff --git a/common/tensorrt_common/CMakeLists.txt b/common/tensorrt_common/CMakeLists.txt index 395a5ee4f1941..36071c9ae14f8 100644 --- a/common/tensorrt_common/CMakeLists.txt +++ b/common/tensorrt_common/CMakeLists.txt @@ -1,57 +1,81 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.17) project(tensorrt_common) -find_package(autoware_cmake REQUIRED) -autoware_package() - -# TODO(tensorrt_common): Remove once upgrading to TensorRT 8.5 is complete -add_compile_options(-Wno-deprecated-declarations) +find_package(ament_cmake REQUIRED) +find_package(cudnn_cmake_module REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tensorrt_cmake_module REQUIRED) find_package(CUDA) +find_package(CUDAToolkit) find_package(CUDNN) find_package(TENSORRT) -if(NOT (${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND})) +if(NOT (CUDAToolkit_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND)) message(WARNING "cuda, cudnn, tensorrt libraries are not found") return() endif() -cuda_add_library(${PROJECT_NAME} SHARED +add_library(${PROJECT_NAME} SHARED src/tensorrt_common.cpp ) -ament_target_dependencies(${PROJECT_NAME} - rclcpp +target_link_libraries(${PROJECT_NAME} + CUDA::cudart + rclcpp::rclcpp + stdc++fs + ${TENSORRT_LIBRARIES} ) target_include_directories(${PROJECT_NAME} PUBLIC $ - $ + $ ${TENSORRT_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} - ${TENSORRT_LIBRARIES} - stdc++fs +set_target_properties(${PROJECT_NAME} + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + CXX_EXTENSIONS NO +) + +# TODO(tensorrt_common): Remove -Wno-deprecated-declarations once upgrading to TensorRT 8.5 is complete +target_compile_options(${PROJECT_NAME} PRIVATE + -Wall -Wextra -Wpedantic -Werror -Wno-deprecated-declarations ) target_compile_definitions(${PROJECT_NAME} PRIVATE TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR} ) -list(APPEND ${PROJECT_NAME}_LIBRARIES "${PROJECT_NAME}") - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + + # These don't pass yet, disable them for now + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_pep257_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(include) -ament_export_dependencies(CUDA) -ament_export_dependencies(cudnn_cmake_module) -ament_export_dependencies(CUDNN) -ament_export_dependencies(tensorrt_cmake_module) -ament_export_dependencies(TENSORRT) +install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_${PROJECT_NAME}) + +ament_export_dependencies( + "CUDA" + "CUDAToolkit" + "cudnn_cmake_module" + "CUDNN" + "rclcpp" + "tensorrt_cmake_module" + "TENSORRT" +) -ament_auto_package() +ament_package() diff --git a/common/tensorrt_common/package.xml b/common/tensorrt_common/package.xml index 6e8a0b042dd70..915f3899badd8 100644 --- a/common/tensorrt_common/package.xml +++ b/common/tensorrt_common/package.xml @@ -11,16 +11,14 @@ Apache License 2.0 - ament_cmake_auto + ament_cmake cudnn_cmake_module tensorrt_cmake_module - autoware_cmake - rclcpp ament_lint_auto - autoware_lint_common + ament_lint_common ament_cmake diff --git a/perception/lidar_apollo_instance_segmentation/.gitignore b/perception/lidar_apollo_instance_segmentation/.gitignore index 356b207498044..cdac399b8df3f 100644 --- a/perception/lidar_apollo_instance_segmentation/.gitignore +++ b/perception/lidar_apollo_instance_segmentation/.gitignore @@ -1,2 +1,2 @@ -data/*.caffemodel +data/*.onnx data/*.engine diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 53b0d6ba6a352..24f97e4201e00 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -1,173 +1,135 @@ -cmake_minimum_required(VERSION 3.14) +cmake_minimum_required(VERSION 3.17) project(lidar_apollo_instance_segmentation) -find_package(autoware_cmake REQUIRED) -autoware_package() - -option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) - -# set flags for CUDA availability -option(CUDA_AVAIL "CUDA available" OFF) -find_package(CUDA) -if(CUDA_FOUND) - find_library(CUBLAS_LIBRARIES cublas HINTS - ${CUDA_TOOLKIT_ROOT_DIR}/lib64 - ${CUDA_TOOLKIT_ROOT_DIR}/lib +find_package(ament_cmake REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tier4_autoware_utils REQUIRED) +find_package(tier4_debug_msgs REQUIRED) +find_package(tier4_perception_msgs REQUIRED) + +find_package(tensorrt_common) +if(NOT ${tensorrt_common_FOUND}) + message(WARNING "The tensorrt_common package is not found. Please check its dependencies.") + # to avoid launch file missing without a gpu + ament_package() + install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) - if(CUDA_VERBOSE) - message("CUDA is available!") - message("CUDA Libs: ${CUDA_LIBRARIES}") - message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") - endif() - set(CUDA_AVAIL ON) -else() - message("CUDA NOT FOUND") - set(CUDA_AVAIL OFF) + return() endif() +find_package(cuda_utils REQUIRED) -# set flags for TensorRT availability -option(TRT_AVAIL "TensorRT available" OFF) -# try to find the tensorRT modules -find_library(NVINFER NAMES nvinfer) -find_library(NVPARSERS NAMES nvparsers) -find_library(NVCAFFE_PARSER NAMES nvcaffe_parser) -find_library(NVINFER_PLUGIN NAMES nvinfer_plugin) -if(NVINFER AND NVPARSERS AND NVCAFFE_PARSER AND NVINFER_PLUGIN) - if(CUDA_VERBOSE) - message("TensorRT is available!") - message("NVINFER: ${NVINFER}") - message("NVPARSERS: ${NVPARSERS}") - message("NVCAFFE_PARSER: ${NVCAFFE_PARSER}") - endif() - set(TRT_AVAIL ON) -else() - message("TensorRT is NOT Available") - set(TRT_AVAIL OFF) -endif() -# set flags for CUDNN availability -option(CUDNN_AVAIL "CUDNN available" OFF) -# try to find the CUDNN module -find_library(CUDNN_LIBRARY - NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} - PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} - PATH_SUFFIXES lib lib64 bin - DOC "CUDNN library." -) -if(CUDNN_LIBRARY) - if(CUDA_VERBOSE) - message("CUDNN is available!") - message("CUDNN_LIBRARY: ${CUDNN_LIBRARY}") - endif() - set(CUDNN_AVAIL ON) -else() - message("CUDNN is NOT Available") - set(CUDNN_AVAIL OFF) +# download weight files +set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") +if(NOT EXISTS "${DATA_PATH}") + execute_process(COMMAND mkdir -p ${DATA_PATH}) endif() -if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) - # download weight files - set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") - if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) - endif() - - function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() +function(download FILE_NAME FILE_HASH) + message(STATUS "Checking and downloading ${FILE_NAME}") + set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) + set(STATUS_CODE 0) + message(STATUS "start ${FILE_NAME}") + if(EXISTS ${FILE_PATH}) + message(STATUS "found ${FILE_NAME}") + file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) + if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) + message(STATUS "same ${FILE_NAME}") + message(STATUS "File already exists.") else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) + message(STATUS "diff ${FILE_NAME}") + message(STATUS "File hash changes. Downloading now ...") + file(DOWNLOAD + https://awf.ml.dev.web.auto/perception/models/lidar_apollo_instance_segmentation/${FILE_NAME} ${FILE_PATH} + STATUS DOWNLOAD_STATUS TIMEOUT 300 + ) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() - endfunction() - - download(vlp-16.caffemodel f79f56a835893eb5289182dd06ce9905) - download(hdl-64.caffemodel f79f56a835893eb5289182dd06ce9905) - download(vls-128.caffemodel e5864f65c42d7d69a58fa7b01970d078) - - find_package(PCL REQUIRED) - - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_MWAITXINTRIN_H_INCLUDED") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_FORCE_INLINES") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__STRICT_ANSI__") + else() + message(STATUS "not found ${FILE_NAME}") + message(STATUS "File doesn't exists. Downloading now ...") + file(DOWNLOAD + https://awf.ml.dev.web.auto/perception/models/lidar_apollo_instance_segmentation/${FILE_NAME} ${FILE_PATH} + STATUS DOWNLOAD_STATUS TIMEOUT 300 + ) + list(GET DOWNLOAD_STATUS 0 STATUS_CODE) + list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) + endif() + if(${STATUS_CODE} EQUAL 0) + message(STATUS "Download completed successfully!") + else() + message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") + endif() +endfunction() + +download(vlp-16.onnx 63a5a1bb73f7dbb64cf70d04eca45fb4) +download(hdl-64.onnx 009745e33b1df44b68296431cc384cd2) +download(vls-128.onnx b2d709f56f73ae2518c9bf4d0214468f) + +add_library(${PROJECT_NAME} SHARED + src/node.cpp + src/detector.cpp + src/log_table.cpp + src/feature_generator.cpp + src/feature_map.cpp + src/cluster2d.cpp + src/debugger.cpp +) - include_directories( - lib/include - ${CUDA_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ) +target_include_directories(${PROJECT_NAME} PRIVATE + "$" + "$" + ${cuda_utils_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} + ${tier4_autoware_utils_INCLUDE_DIRS} +) - ament_auto_add_library(tensorrt_apollo_cnn_lib SHARED - lib/src/TrtNet.cpp - ) +target_link_libraries(${PROJECT_NAME} + pcl_common + rclcpp::rclcpp + rclcpp_components::component + tensorrt_common::tensorrt_common + tf2_eigen::tf2_eigen + ${tier4_debug_msgs_TARGETS} + ${tier4_perception_msgs_TARGETS} +) - target_link_libraries(tensorrt_apollo_cnn_lib - ${NVINFER} - ${NVCAFFE_PARSER} - ${NVINFER_PLUGIN} - ${CUDA_LIBRARIES} - ${CUBLAS_LIBRARIES} - ${CUDNN_LIBRARY} - ${PCL_LIBRARIES} - ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) - target_compile_options(tensorrt_apollo_cnn_lib - PUBLIC - -Wno-deprecated-declarations - ) + # These don't pass yet, disable them for now + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_pep257_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) - ament_auto_add_library(lidar_apollo_instance_segmentation SHARED - src/node.cpp - src/detector.cpp - src/log_table.cpp - src/feature_generator.cpp - src/feature_map.cpp - src/cluster2d.cpp - src/debugger.cpp - ) + ament_lint_auto_find_test_dependencies() +endif() - target_link_libraries(lidar_apollo_instance_segmentation - tensorrt_apollo_cnn_lib - ) +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) - rclcpp_components_register_node(lidar_apollo_instance_segmentation - PLUGIN "LidarInstanceSegmentationNode" - EXECUTABLE lidar_apollo_instance_segmentation_node - ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "lidar_apollo_instance_segmentation::LidarInstanceSegmentationNode" + EXECUTABLE ${PROJECT_NAME}_node +) - ament_auto_package(INSTALL_TO_SHARE - launch +ament_package() +install( + DIRECTORY config data - ) - -else() - # to avoid launch file missing without a gpu - ament_auto_package(INSTALL_TO_SHARE launch - ) - -endif() + DESTINATION share/${PROJECT_NAME} +) diff --git a/perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt b/perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt deleted file mode 100644 index 80c62b001c367..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt +++ /dev/null @@ -1,685 +0,0 @@ -name: "pcd_parsing" -layer { - name: "input" - type: "Input" - top: "data" - input_param { - shape { - dim: 1 - dim: 6 - dim: 672 - dim: 672 - } - } -} -layer { - name: "conv0_1" - type: "Convolution" - bottom: "data" - top: "conv0_1" - convolution_param { - num_output: 24 - bias_term: true - pad: 0 - kernel_size: 1 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv0_1" - type: "ReLU" - bottom: "conv0_1" - top: "conv0_1" -} -layer { - name: "conv0" - type: "Convolution" - bottom: "conv0_1" - top: "conv0" - convolution_param { - num_output: 24 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv0" - type: "ReLU" - bottom: "conv0" - top: "conv0" -} -layer { - name: "conv1_1" - type: "Convolution" - bottom: "conv0" - top: "conv1_1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv1_1" - type: "ReLU" - bottom: "conv1_1" - top: "conv1_1" -} -layer { - name: "conv1" - type: "Convolution" - bottom: "conv1_1" - top: "conv1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv1" - type: "ReLU" - bottom: "conv1" - top: "conv1" -} -layer { - name: "conv2_1" - type: "Convolution" - bottom: "conv1" - top: "conv2_1" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2_1" - type: "ReLU" - bottom: "conv2_1" - top: "conv2_1" -} -layer { - name: "conv2_2" - type: "Convolution" - bottom: "conv2_1" - top: "conv2_2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2_2" - type: "ReLU" - bottom: "conv2_2" - top: "conv2_2" -} -layer { - name: "conv2" - type: "Convolution" - bottom: "conv2_2" - top: "conv2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2" - type: "ReLU" - bottom: "conv2" - top: "conv2" -} -layer { - name: "conv3_1" - type: "Convolution" - bottom: "conv2" - top: "conv3_1" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3_1" - type: "ReLU" - bottom: "conv3_1" - top: "conv3_1" -} -layer { - name: "conv3_2" - type: "Convolution" - bottom: "conv3_1" - top: "conv3_2" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3_2" - type: "ReLU" - bottom: "conv3_2" - top: "conv3_2" -} -layer { - name: "conv3" - type: "Convolution" - bottom: "conv3_2" - top: "conv3" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3" - type: "ReLU" - bottom: "conv3" - top: "conv3" -} -layer { - name: "conv4_1" - type: "Convolution" - bottom: "conv3" - top: "conv4_1" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4_1" - type: "ReLU" - bottom: "conv4_1" - top: "conv4_1" -} -layer { - name: "conv4_2" - type: "Convolution" - bottom: "conv4_1" - top: "conv4_2" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4_2" - type: "ReLU" - bottom: "conv4_2" - top: "conv4_2" -} -layer { - name: "conv4" - type: "Convolution" - bottom: "conv4_2" - top: "conv4" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4" - type: "ReLU" - bottom: "conv4" - top: "conv4" -} -layer { - name: "conv5_1" - type: "Convolution" - bottom: "conv4" - top: "conv5_1" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv5_1" - type: "ReLU" - bottom: "conv5_1" - top: "conv5_1" -} -layer { - name: "conv5" - type: "Convolution" - bottom: "conv5_1" - top: "conv5" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv5" - type: "ReLU" - bottom: "conv5" - top: "conv5" -} -layer { - name: "deconv5_1" - type: "Convolution" - bottom: "conv5" - top: "deconv5_1" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv5_1" - type: "ReLU" - bottom: "deconv5_1" - top: "deconv5_1" -} -layer { - name: "deconv4" - type: "Deconvolution" - bottom: "deconv5_1" - top: "deconv4" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv4" - type: "ReLU" - bottom: "deconv4" - top: "deconv4" -} -layer { - name: "Concat_concat4" - type: "Concat" - bottom: "conv4" - bottom: "deconv4" - top: "concat4" -} -layer { - name: "deconv4_1" - type: "Convolution" - bottom: "concat4" - top: "deconv4_1" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv4_1" - type: "ReLU" - bottom: "deconv4_1" - top: "deconv4_1" -} -layer { - name: "deconv3" - type: "Deconvolution" - bottom: "deconv4_1" - top: "deconv3" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv3" - type: "ReLU" - bottom: "deconv3" - top: "deconv3" -} -layer { - name: "Concat_concat3" - type: "Concat" - bottom: "conv3" - bottom: "deconv3" - top: "concat3" -} -layer { - name: "deconv3_1" - type: "Convolution" - bottom: "concat3" - top: "deconv3_1" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv3_1" - type: "ReLU" - bottom: "deconv3_1" - top: "deconv3_1" -} -layer { - name: "deconv2" - type: "Deconvolution" - bottom: "deconv3_1" - top: "deconv2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv2" - type: "ReLU" - bottom: "deconv2" - top: "deconv2" -} -layer { - name: "Concat_concat2" - type: "Concat" - bottom: "conv2" - bottom: "deconv2" - top: "concat2" -} -layer { - name: "deconv2_1" - type: "Convolution" - bottom: "concat2" - top: "deconv2_1" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv2_1" - type: "ReLU" - bottom: "deconv2_1" - top: "deconv2_1" -} -layer { - name: "deconv1" - type: "Deconvolution" - bottom: "deconv2_1" - top: "deconv1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv1" - type: "ReLU" - bottom: "deconv1" - top: "deconv1" -} -layer { - name: "Concat_concat1" - type: "Concat" - bottom: "conv1" - bottom: "deconv1" - top: "concat1" -} -layer { - name: "deconv1_1" - type: "Convolution" - bottom: "concat1" - top: "deconv1_1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv1_1" - type: "ReLU" - bottom: "deconv1_1" - top: "deconv1_1" -} -layer { - name: "deconv0" - type: "Deconvolution" - bottom: "deconv1_1" - top: "deconv0" - convolution_param { - num_output: 12 - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} diff --git a/perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt b/perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt deleted file mode 100644 index 80c62b001c367..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt +++ /dev/null @@ -1,685 +0,0 @@ -name: "pcd_parsing" -layer { - name: "input" - type: "Input" - top: "data" - input_param { - shape { - dim: 1 - dim: 6 - dim: 672 - dim: 672 - } - } -} -layer { - name: "conv0_1" - type: "Convolution" - bottom: "data" - top: "conv0_1" - convolution_param { - num_output: 24 - bias_term: true - pad: 0 - kernel_size: 1 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv0_1" - type: "ReLU" - bottom: "conv0_1" - top: "conv0_1" -} -layer { - name: "conv0" - type: "Convolution" - bottom: "conv0_1" - top: "conv0" - convolution_param { - num_output: 24 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv0" - type: "ReLU" - bottom: "conv0" - top: "conv0" -} -layer { - name: "conv1_1" - type: "Convolution" - bottom: "conv0" - top: "conv1_1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv1_1" - type: "ReLU" - bottom: "conv1_1" - top: "conv1_1" -} -layer { - name: "conv1" - type: "Convolution" - bottom: "conv1_1" - top: "conv1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv1" - type: "ReLU" - bottom: "conv1" - top: "conv1" -} -layer { - name: "conv2_1" - type: "Convolution" - bottom: "conv1" - top: "conv2_1" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2_1" - type: "ReLU" - bottom: "conv2_1" - top: "conv2_1" -} -layer { - name: "conv2_2" - type: "Convolution" - bottom: "conv2_1" - top: "conv2_2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2_2" - type: "ReLU" - bottom: "conv2_2" - top: "conv2_2" -} -layer { - name: "conv2" - type: "Convolution" - bottom: "conv2_2" - top: "conv2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2" - type: "ReLU" - bottom: "conv2" - top: "conv2" -} -layer { - name: "conv3_1" - type: "Convolution" - bottom: "conv2" - top: "conv3_1" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3_1" - type: "ReLU" - bottom: "conv3_1" - top: "conv3_1" -} -layer { - name: "conv3_2" - type: "Convolution" - bottom: "conv3_1" - top: "conv3_2" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3_2" - type: "ReLU" - bottom: "conv3_2" - top: "conv3_2" -} -layer { - name: "conv3" - type: "Convolution" - bottom: "conv3_2" - top: "conv3" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3" - type: "ReLU" - bottom: "conv3" - top: "conv3" -} -layer { - name: "conv4_1" - type: "Convolution" - bottom: "conv3" - top: "conv4_1" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4_1" - type: "ReLU" - bottom: "conv4_1" - top: "conv4_1" -} -layer { - name: "conv4_2" - type: "Convolution" - bottom: "conv4_1" - top: "conv4_2" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4_2" - type: "ReLU" - bottom: "conv4_2" - top: "conv4_2" -} -layer { - name: "conv4" - type: "Convolution" - bottom: "conv4_2" - top: "conv4" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4" - type: "ReLU" - bottom: "conv4" - top: "conv4" -} -layer { - name: "conv5_1" - type: "Convolution" - bottom: "conv4" - top: "conv5_1" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv5_1" - type: "ReLU" - bottom: "conv5_1" - top: "conv5_1" -} -layer { - name: "conv5" - type: "Convolution" - bottom: "conv5_1" - top: "conv5" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv5" - type: "ReLU" - bottom: "conv5" - top: "conv5" -} -layer { - name: "deconv5_1" - type: "Convolution" - bottom: "conv5" - top: "deconv5_1" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv5_1" - type: "ReLU" - bottom: "deconv5_1" - top: "deconv5_1" -} -layer { - name: "deconv4" - type: "Deconvolution" - bottom: "deconv5_1" - top: "deconv4" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv4" - type: "ReLU" - bottom: "deconv4" - top: "deconv4" -} -layer { - name: "Concat_concat4" - type: "Concat" - bottom: "conv4" - bottom: "deconv4" - top: "concat4" -} -layer { - name: "deconv4_1" - type: "Convolution" - bottom: "concat4" - top: "deconv4_1" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv4_1" - type: "ReLU" - bottom: "deconv4_1" - top: "deconv4_1" -} -layer { - name: "deconv3" - type: "Deconvolution" - bottom: "deconv4_1" - top: "deconv3" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv3" - type: "ReLU" - bottom: "deconv3" - top: "deconv3" -} -layer { - name: "Concat_concat3" - type: "Concat" - bottom: "conv3" - bottom: "deconv3" - top: "concat3" -} -layer { - name: "deconv3_1" - type: "Convolution" - bottom: "concat3" - top: "deconv3_1" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv3_1" - type: "ReLU" - bottom: "deconv3_1" - top: "deconv3_1" -} -layer { - name: "deconv2" - type: "Deconvolution" - bottom: "deconv3_1" - top: "deconv2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv2" - type: "ReLU" - bottom: "deconv2" - top: "deconv2" -} -layer { - name: "Concat_concat2" - type: "Concat" - bottom: "conv2" - bottom: "deconv2" - top: "concat2" -} -layer { - name: "deconv2_1" - type: "Convolution" - bottom: "concat2" - top: "deconv2_1" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv2_1" - type: "ReLU" - bottom: "deconv2_1" - top: "deconv2_1" -} -layer { - name: "deconv1" - type: "Deconvolution" - bottom: "deconv2_1" - top: "deconv1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv1" - type: "ReLU" - bottom: "deconv1" - top: "deconv1" -} -layer { - name: "Concat_concat1" - type: "Concat" - bottom: "conv1" - bottom: "deconv1" - top: "concat1" -} -layer { - name: "deconv1_1" - type: "Convolution" - bottom: "concat1" - top: "deconv1_1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv1_1" - type: "ReLU" - bottom: "deconv1_1" - top: "deconv1_1" -} -layer { - name: "deconv0" - type: "Deconvolution" - bottom: "deconv1_1" - top: "deconv0" - convolution_param { - num_output: 12 - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} diff --git a/perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt b/perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt deleted file mode 100644 index a2d918b760626..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt +++ /dev/null @@ -1,686 +0,0 @@ -name: "pcd_parsing" -layer { - name: "input" - type: "Input" - top: "data" - input_param { - shape { - dim: 1 - dim: 4 - dim: 864 - dim: 864 - } - } -} - -layer { - name: "conv0_1" - type: "Convolution" - bottom: "data" - top: "conv0_1" - convolution_param { - num_output: 24 - bias_term: true - pad: 0 - kernel_size: 1 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv0_1" - type: "ReLU" - bottom: "conv0_1" - top: "conv0_1" -} -layer { - name: "conv0" - type: "Convolution" - bottom: "conv0_1" - top: "conv0" - convolution_param { - num_output: 24 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv0" - type: "ReLU" - bottom: "conv0" - top: "conv0" -} -layer { - name: "conv1_1" - type: "Convolution" - bottom: "conv0" - top: "conv1_1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv1_1" - type: "ReLU" - bottom: "conv1_1" - top: "conv1_1" -} -layer { - name: "conv1" - type: "Convolution" - bottom: "conv1_1" - top: "conv1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv1" - type: "ReLU" - bottom: "conv1" - top: "conv1" -} -layer { - name: "conv2_1" - type: "Convolution" - bottom: "conv1" - top: "conv2_1" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2_1" - type: "ReLU" - bottom: "conv2_1" - top: "conv2_1" -} -layer { - name: "conv2_2" - type: "Convolution" - bottom: "conv2_1" - top: "conv2_2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2_2" - type: "ReLU" - bottom: "conv2_2" - top: "conv2_2" -} -layer { - name: "conv2" - type: "Convolution" - bottom: "conv2_2" - top: "conv2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv2" - type: "ReLU" - bottom: "conv2" - top: "conv2" -} -layer { - name: "conv3_1" - type: "Convolution" - bottom: "conv2" - top: "conv3_1" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3_1" - type: "ReLU" - bottom: "conv3_1" - top: "conv3_1" -} -layer { - name: "conv3_2" - type: "Convolution" - bottom: "conv3_1" - top: "conv3_2" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3_2" - type: "ReLU" - bottom: "conv3_2" - top: "conv3_2" -} -layer { - name: "conv3" - type: "Convolution" - bottom: "conv3_2" - top: "conv3" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv3" - type: "ReLU" - bottom: "conv3" - top: "conv3" -} -layer { - name: "conv4_1" - type: "Convolution" - bottom: "conv3" - top: "conv4_1" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4_1" - type: "ReLU" - bottom: "conv4_1" - top: "conv4_1" -} -layer { - name: "conv4_2" - type: "Convolution" - bottom: "conv4_1" - top: "conv4_2" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4_2" - type: "ReLU" - bottom: "conv4_2" - top: "conv4_2" -} -layer { - name: "conv4" - type: "Convolution" - bottom: "conv4_2" - top: "conv4" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv4" - type: "ReLU" - bottom: "conv4" - top: "conv4" -} -layer { - name: "conv5_1" - type: "Convolution" - bottom: "conv4" - top: "conv5_1" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv5_1" - type: "ReLU" - bottom: "conv5_1" - top: "conv5_1" -} -layer { - name: "conv5" - type: "Convolution" - bottom: "conv5_1" - top: "conv5" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_conv5" - type: "ReLU" - bottom: "conv5" - top: "conv5" -} -layer { - name: "deconv5_1" - type: "Convolution" - bottom: "conv5" - top: "deconv5_1" - convolution_param { - num_output: 192 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv5_1" - type: "ReLU" - bottom: "deconv5_1" - top: "deconv5_1" -} -layer { - name: "deconv4" - type: "Deconvolution" - bottom: "deconv5_1" - top: "deconv4" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv4" - type: "ReLU" - bottom: "deconv4" - top: "deconv4" -} -layer { - name: "Concat_concat4" - type: "Concat" - bottom: "conv4" - bottom: "deconv4" - top: "concat4" -} -layer { - name: "deconv4_1" - type: "Convolution" - bottom: "concat4" - top: "deconv4_1" - convolution_param { - num_output: 128 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv4_1" - type: "ReLU" - bottom: "deconv4_1" - top: "deconv4_1" -} -layer { - name: "deconv3" - type: "Deconvolution" - bottom: "deconv4_1" - top: "deconv3" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv3" - type: "ReLU" - bottom: "deconv3" - top: "deconv3" -} -layer { - name: "Concat_concat3" - type: "Concat" - bottom: "conv3" - bottom: "deconv3" - top: "concat3" -} -layer { - name: "deconv3_1" - type: "Convolution" - bottom: "concat3" - top: "deconv3_1" - convolution_param { - num_output: 96 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv3_1" - type: "ReLU" - bottom: "deconv3_1" - top: "deconv3_1" -} -layer { - name: "deconv2" - type: "Deconvolution" - bottom: "deconv3_1" - top: "deconv2" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv2" - type: "ReLU" - bottom: "deconv2" - top: "deconv2" -} -layer { - name: "Concat_concat2" - type: "Concat" - bottom: "conv2" - bottom: "deconv2" - top: "concat2" -} -layer { - name: "deconv2_1" - type: "Convolution" - bottom: "concat2" - top: "deconv2_1" - convolution_param { - num_output: 64 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv2_1" - type: "ReLU" - bottom: "deconv2_1" - top: "deconv2_1" -} -layer { - name: "deconv1" - type: "Deconvolution" - bottom: "deconv2_1" - top: "deconv1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv1" - type: "ReLU" - bottom: "deconv1" - top: "deconv1" -} -layer { - name: "Concat_concat1" - type: "Concat" - bottom: "conv1" - bottom: "deconv1" - top: "concat1" -} -layer { - name: "deconv1_1" - type: "Convolution" - bottom: "concat1" - top: "deconv1_1" - convolution_param { - num_output: 48 - bias_term: true - pad: 1 - kernel_size: 3 - stride: 1 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} -layer { - name: "relu_deconv1_1" - type: "ReLU" - bottom: "deconv1_1" - top: "deconv1_1" -} -layer { - name: "deconv0" - type: "Deconvolution" - bottom: "deconv1_1" - top: "deconv0" - convolution_param { - num_output: 12 - pad: 1 - kernel_size: 4 - stride: 2 - weight_filler { - type: "xavier" - } - bias_filler { - type: "constant" - value: 0.0 - } - } -} diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp index e8ce50b28b1a4..53cab7409578e 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -60,6 +60,8 @@ #include #include +namespace lidar_apollo_instance_segmentation +{ enum MetaType { META_UNKNOWN, META_SMALL_MOT, @@ -94,12 +96,12 @@ class Cluster2D ~Cluster2D() {} void cluster( - const std::shared_ptr & inferred_data, - const pcl::PointCloud::Ptr & pc_ptr, const pcl::PointIndices & valid_indices, - float objectness_thresh, bool use_all_grids_for_clustering); + const float * inferred_data, const pcl::PointCloud::Ptr & pc_ptr, + const pcl::PointIndices & valid_indices, float objectness_thresh, + bool use_all_grids_for_clustering); - void filter(const std::shared_ptr & inferred_data); - void classify(const std::shared_ptr & inferred_data); + void filter(const float * inferred_data); + void classify(const float * inferred_data); void getObjects( const float confidence_thresh, const float height_thresh, const int min_pts_num, @@ -112,7 +114,7 @@ class Cluster2D private: int rows_; int cols_; - int siz_; + int size_; float range_; float scale_; float inv_res_x_; @@ -158,5 +160,6 @@ class Cluster2D void traverse(Node * x); }; +} // namespace lidar_apollo_instance_segmentation #endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp index 87ab2234d61e3..954d7c5cd1582 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ #include #include +namespace lidar_apollo_instance_segmentation +{ class Debugger { public: @@ -29,3 +32,6 @@ class Debugger private: rclcpp::Publisher::SharedPtr instance_pointcloud_pub_; }; +} // namespace lidar_apollo_instance_segmentation + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp index d2956a0a9823e..5686ce5cec34f 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -12,26 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ #include "cluster2d.hpp" #include "feature_generator.hpp" #include "lidar_apollo_instance_segmentation/node.hpp" -#include +#include +#include +#include +#include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif #include #include #include #include +namespace lidar_apollo_instance_segmentation +{ +using cuda_utils::CudaUniquePtr; +using cuda_utils::CudaUniquePtrHost; +using cuda_utils::makeCudaStream; +using cuda_utils::StreamUniquePtr; + class LidarApolloInstanceSegmentation : public LidarInstanceSegmentationInterface { public: @@ -46,8 +52,9 @@ class LidarApolloInstanceSegmentation : public LidarInstanceSegmentationInterfac const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, float z_offset); + std::unique_ptr trt_common_; + rclcpp::Node * node_; - std::unique_ptr net_ptr_; std::shared_ptr cluster2d_; std::shared_ptr feature_generator_; float score_threshold_; @@ -56,4 +63,14 @@ class LidarApolloInstanceSegmentation : public LidarInstanceSegmentationInterfac tf2_ros::TransformListener tf_listener_; std::string target_frame_; float z_offset_; + + size_t output_size_; + CudaUniquePtr input_d_; + CudaUniquePtr output_d_; + CudaUniquePtrHost output_h_; + + StreamUniquePtr stream_{makeCudaStream()}; }; +} // namespace lidar_apollo_instance_segmentation + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp index 9c1c0405b36d1..023dd3b1c3515 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp @@ -17,6 +17,8 @@ #ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ #define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ +namespace lidar_apollo_instance_segmentation +{ template void DisjointSetMakeSet(T * x) { @@ -71,5 +73,6 @@ void DisjointSetUnion(T * x, T * y) DisjointSetMerge(x, y); } } +} // namespace lidar_apollo_instance_segmentation #endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp index ec9f200994396..13ee9647cbea4 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ #include "lidar_apollo_instance_segmentation/feature_map.hpp" #include "util.hpp" @@ -22,6 +23,8 @@ #include +namespace lidar_apollo_instance_segmentation +{ class FeatureGenerator { private: @@ -40,3 +43,6 @@ class FeatureGenerator std::shared_ptr generate( const pcl::PointCloud::Ptr & pc_ptr); }; +} // namespace lidar_apollo_instance_segmentation + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp index 2f54bbe0c960e..5cbfbd16c1c3a 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -11,10 +11,15 @@ // 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. -#pragma once + +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ + #include #include +namespace lidar_apollo_instance_segmentation +{ struct FeatureMapInterface { public: @@ -63,3 +68,6 @@ struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface void initializeMap(std::vector & map) override; void resetMap(std::vector & map) override; }; +} // namespace lidar_apollo_instance_segmentation + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp index 73c29de6cb613..b1aa49d579928 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -11,6 +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. -#pragma once +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ + +namespace lidar_apollo_instance_segmentation +{ float calcApproximateLog(float num); +} // namespace lidar_apollo_instance_segmentation + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp index bfea09983cbc3..0ed5f37b6fa14 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -11,7 +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. -#pragma once + +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ + #include "lidar_apollo_instance_segmentation/debugger.hpp" #include @@ -23,6 +26,8 @@ #include +namespace lidar_apollo_instance_segmentation +{ class LidarInstanceSegmentationInterface { public: @@ -48,3 +53,6 @@ class LidarInstanceSegmentationNode : public rclcpp::Node public: explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); }; +} // namespace lidar_apollo_instance_segmentation + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp index 9ae382bd51cec..8837919436dbe 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp @@ -34,6 +34,8 @@ #include #include +namespace lidar_apollo_instance_segmentation +{ // project point cloud to 2d map. calc in which grid point is. // pointcloud to pixel inline int F2I(float val, float ori, float scale) @@ -55,5 +57,6 @@ inline float Pixel2Pc(int in_pixel, float in_size, float out_range) float res = 2.0 * out_range / in_size; return out_range - (static_cast(in_pixel) + 0.5f) * res; } +} // namespace lidar_apollo_instance_segmentation #endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index 78127590685fb..bceb928dd6692 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -7,21 +7,20 @@ - - - + + + - - - + + diff --git a/perception/lidar_apollo_instance_segmentation/lib/LICENSE b/perception/lidar_apollo_instance_segmentation/lib/LICENSE deleted file mode 100644 index 4527cae1a769f..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/lib/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2018 lewes6369 - -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. diff --git a/perception/lidar_apollo_instance_segmentation/lib/README.md b/perception/lidar_apollo_instance_segmentation/lib/README.md deleted file mode 100644 index 38cb4cafd6c4c..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/lib/README.md +++ /dev/null @@ -1,28 +0,0 @@ -### Note - -This library is customized. -Original repository : - -```txt -MIT License - -Copyright (c) 2018 lewes6369 - -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. -``` diff --git a/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp b/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp deleted file mode 100644 index 14c1ff6d44c63..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2018 lewes6369 - * - * 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 TRTNET_HPP_ -#define TRTNET_HPP_ - -#include "Utils.hpp" - -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace Tn -{ -enum class RUN_MODE { FLOAT32 = 0, FLOAT16 = 1, INT8 = 2 }; - -class trtNet -{ -public: - // Load from engine file - explicit trtNet(const std::string & engineFile); - - ~trtNet() - { - // Release the stream and the buffers - cudaStreamSynchronize(mTrtCudaStream); - cudaStreamDestroy(mTrtCudaStream); - for (auto & item : mTrtCudaBuffer) { - cudaFree(item); - } - - if (!mTrtRunTime) { - delete mTrtRunTime; - } - if (!mTrtContext) { - delete mTrtContext; - } - if (!mTrtEngine) { - delete mTrtEngine; - } - } - - void saveEngine(std::string fileName) - { - if (mTrtEngine) { - nvinfer1::IHostMemory * data = mTrtEngine->serialize(); - std::ofstream file; - file.open(fileName, std::ios::binary | std::ios::out); - if (!file.is_open()) { - std::cout << "read create engine file" << fileName << " failed" << std::endl; - return; - } - - file.write((const char *)data->data(), data->size()); - file.close(); - } - } - - void doInference(const void * inputData, void * outputData); - - inline size_t getInputSize() - { - return std::accumulate( - mTrtBindBufferSize.begin(), mTrtBindBufferSize.begin() + mTrtInputCount, 0); - } - - inline size_t getOutputSize() - { - return std::accumulate( - mTrtBindBufferSize.begin() + mTrtInputCount, mTrtBindBufferSize.end(), 0); - } - -private: - void InitEngine(); - - nvinfer1::IExecutionContext * mTrtContext; - nvinfer1::ICudaEngine * mTrtEngine; - nvinfer1::IRuntime * mTrtRunTime; - cudaStream_t mTrtCudaStream; - Profiler mTrtProfiler; - RUN_MODE mTrtRunMode; - - std::vector mTrtCudaBuffer; - std::vector mTrtBindBufferSize; - int mTrtInputCount; -}; -} // namespace Tn - -#endif // TRTNET_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp b/perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp deleted file mode 100644 index 93c90740934b9..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2018 lewes6369 - * - * 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 UTILS_HPP_ -#define UTILS_HPP_ - -#include -#include - -#include -#include -#include -#include -#include - -#ifndef CUDA_CHECK - -#define CUDA_CHECK(callstr) \ - { \ - cudaError_t error_code = callstr; \ - if (error_code != cudaSuccess) { \ - std::cerr << "CUDA error " << error_code << " at " << __FILE__ << ":" << __LINE__; \ - assert(0); \ - } \ - } - -#endif - -namespace Tn -{ -class Profiler : public nvinfer1::IProfiler -{ -public: - void printLayerTimes(int iterationsTimes) - { - float totalTime = 0; - for (size_t i = 0; i < mProfile.size(); i++) { - printf("%-40.40s %4.3fms\n", mProfile[i].first.c_str(), mProfile[i].second / iterationsTimes); - totalTime += mProfile[i].second; - } - printf("Time over all layers: %4.3f\n", totalTime / iterationsTimes); - } - -private: - typedef std::pair Record; - std::vector mProfile; - - void reportLayerTime(const char * layerName, float ms) noexcept override - { - auto record = std::find_if( - mProfile.begin(), mProfile.end(), [&](const Record & r) { return r.first == layerName; }); - if (record == mProfile.end()) { - mProfile.push_back(std::make_pair(layerName, ms)); - } else { - record->second += ms; - } - } -}; - -// Logger for TensorRT info/warning/errors -class Logger : public nvinfer1::ILogger -{ -public: - Logger() : Logger(Severity::kWARNING) {} - - explicit Logger(Severity severity) : reportableSeverity(severity) {} - - void log(Severity severity, const char * msg) noexcept override - { - // suppress messages with severity enum value greater than the reportable - if (severity > reportableSeverity) { - return; - } - - switch (severity) { - case Severity::kINTERNAL_ERROR: - std::cerr << "INTERNAL_ERROR: "; - break; - case Severity::kERROR: - std::cerr << "ERROR: "; - break; - case Severity::kWARNING: - std::cerr << "WARNING: "; - break; - case Severity::kINFO: - std::cerr << "INFO: "; - break; - default: - std::cerr << "UNKNOWN: "; - break; - } - std::cerr << msg << std::endl; - } - - Severity reportableSeverity{Severity::kWARNING}; -}; - -template -void write(char *& buffer, const T & val) -{ - *reinterpret_cast(buffer) = val; - buffer += sizeof(T); -} - -template -void read(const char *& buffer, T & val) -{ - val = *reinterpret_cast(buffer); - buffer += sizeof(T); -} -} // namespace Tn - -#endif // UTILS_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp b/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp deleted file mode 100644 index 0f1da4bdac8a6..0000000000000 --- a/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2018 lewes6369 - * - * 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 - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -using namespace nvinfer1; // NOLINT -using namespace nvcaffeparser1; // NOLINT -using namespace plugin; // NOLINT - -static Tn::Logger gLogger; - -inline void * safeCudaMalloc(size_t memSize) -{ - void * deviceMem; - CUDA_CHECK(cudaMalloc(&deviceMem, memSize)); - if (deviceMem == nullptr) { - std::cerr << "Out of memory" << std::endl; - exit(1); - } - return deviceMem; -} - -inline int64_t volume(const nvinfer1::Dims & d) -{ - return std::accumulate(d.d, d.d + d.nbDims, 1, std::multiplies()); -} - -inline unsigned int getElementSize(nvinfer1::DataType t) -{ - switch (t) { - case nvinfer1::DataType::kINT32: - return 4; - case nvinfer1::DataType::kFLOAT: - return 4; - case nvinfer1::DataType::kHALF: - return 2; - case nvinfer1::DataType::kINT8: - return 1; - default: - throw std::runtime_error("Invalid DataType."); - return 0; - } -} - -namespace Tn -{ -trtNet::trtNet(const std::string & engineFile) -: mTrtContext(nullptr), - mTrtEngine(nullptr), - mTrtRunTime(nullptr), - mTrtRunMode(RUN_MODE::FLOAT32), - mTrtInputCount(0) -{ - using namespace std; // NOLINT - fstream file; - - file.open(engineFile, ios::binary | ios::in); - if (!file.is_open()) { - cout << "read engine file" << engineFile << " failed" << endl; - return; - } - file.seekg(0, ios::end); - int length = file.tellg(); - file.seekg(0, ios::beg); - std::unique_ptr data(new char[length]); - file.read(data.get(), length); - file.close(); - - mTrtRunTime = createInferRuntime(gLogger); - assert(mTrtRunTime != nullptr); - mTrtEngine = mTrtRunTime->deserializeCudaEngine(data.get(), length); - assert(mTrtEngine != nullptr); - - InitEngine(); -} - -void trtNet::InitEngine() -{ - const int maxBatchSize = 1; - mTrtContext = mTrtEngine->createExecutionContext(); - assert(mTrtContext != nullptr); - mTrtContext->setProfiler(&mTrtProfiler); - - int nbBindings = mTrtEngine->getNbBindings(); - - mTrtCudaBuffer.resize(nbBindings); - mTrtBindBufferSize.resize(nbBindings); - for (int i = 0; i < nbBindings; ++i) { - Dims dims = mTrtEngine->getBindingDimensions(i); - DataType dtype = mTrtEngine->getBindingDataType(i); - int64_t totalSize = volume(dims) * maxBatchSize * getElementSize(dtype); - mTrtBindBufferSize[i] = totalSize; - mTrtCudaBuffer[i] = safeCudaMalloc(totalSize); - if (mTrtEngine->bindingIsInput(i)) { - mTrtInputCount++; - } - } - - CUDA_CHECK(cudaStreamCreate(&mTrtCudaStream)); -} - -void trtNet::doInference(const void * inputData, void * outputData) -{ - static const int batchSize = 1; - assert(mTrtInputCount == 1); - - int inputIndex = 0; - CUDA_CHECK(cudaMemcpyAsync( - mTrtCudaBuffer[inputIndex], inputData, mTrtBindBufferSize[inputIndex], cudaMemcpyHostToDevice, - mTrtCudaStream)); - - mTrtContext->execute(batchSize, &mTrtCudaBuffer[inputIndex]); - - for (size_t bindingIdx = mTrtInputCount; bindingIdx < mTrtBindBufferSize.size(); ++bindingIdx) { - auto size = mTrtBindBufferSize[bindingIdx]; - CUDA_CHECK(cudaMemcpyAsync( - outputData, mTrtCudaBuffer[bindingIdx], size, cudaMemcpyDeviceToHost, mTrtCudaStream)); - } -} -} // namespace Tn diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml index 21cdf36c401e1..9cb4d975cfddd 100755 --- a/perception/lidar_apollo_instance_segmentation/package.xml +++ b/perception/lidar_apollo_instance_segmentation/package.xml @@ -10,24 +10,22 @@ Kosuke Takeuchi Yukihiro Saito - ament_cmake_auto - - autoware_cmake + ament_cmake autoware_auto_perception_msgs + cuda_utils libpcl-all-dev pcl_conversions rclcpp rclcpp_components - tf2 + tensorrt_common tf2_eigen - tf2_geometry_msgs - tf2_ros tier4_autoware_utils + tier4_debug_msgs tier4_perception_msgs - ament_cmake_cppcheck ament_lint_auto + ament_lint_common ament_cmake diff --git a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp index 510f6e7ddd8f8..a7b1bee69aebb 100644 --- a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -56,6 +56,8 @@ #include #endif +namespace lidar_apollo_instance_segmentation +{ geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) { tf2::Quaternion q; @@ -67,13 +69,13 @@ Cluster2D::Cluster2D(const int rows, const int cols, const float range) { rows_ = rows; cols_ = cols; - siz_ = rows * cols; + size_ = rows * cols; range_ = range; scale_ = 0.5 * static_cast(rows_) / range_; inv_res_x_ = 0.5 * static_cast(cols_) / range_; inv_res_y_ = 0.5 * static_cast(rows_) / range_; point2grid_.clear(); - id_img_.assign(siz_, -1); + id_img_.assign(size_, -1); pc_ptr_.reset(); valid_indices_in_pc_ = nullptr; } @@ -102,13 +104,13 @@ void Cluster2D::traverse(Node * x) } void Cluster2D::cluster( - const std::shared_ptr & inferred_data, const pcl::PointCloud::Ptr & pc_ptr, + const float * inferred_data, const pcl::PointCloud::Ptr & pc_ptr, const pcl::PointIndices & valid_indices, float objectness_thresh, bool use_all_grids_for_clustering) { - const float * category_pt_data = inferred_data.get(); - const float * instance_pt_x_data = inferred_data.get() + siz_; - const float * instance_pt_y_data = inferred_data.get() + siz_ * 2; + const float * category_pt_data = inferred_data; + const float * instance_pt_x_data = inferred_data + size_; + const float * instance_pt_y_data = inferred_data + size_ * 2; pc_ptr_ = pc_ptr; @@ -176,7 +178,7 @@ void Cluster2D::cluster( int count_obstacles = 0; obstacles_.clear(); - id_img_.assign(siz_, -1); + id_img_.assign(size_, -1); for (int row = 0; row < rows_; ++row) { for (int col = 0; col < cols_; ++col) { Node * node = &nodes[row][col]; @@ -197,12 +199,12 @@ void Cluster2D::cluster( classify(inferred_data); } -void Cluster2D::filter(const std::shared_ptr & inferred_data) +void Cluster2D::filter(const float * inferred_data) { - const float * confidence_pt_data = inferred_data.get() + siz_ * 3; - const float * heading_pt_x_data = inferred_data.get() + siz_ * 9; - const float * heading_pt_y_data = inferred_data.get() + siz_ * 10; - const float * height_pt_data = inferred_data.get() + siz_ * 11; + const float * confidence_pt_data = inferred_data + size_ * 3; + const float * heading_pt_x_data = inferred_data + size_ * 9; + const float * heading_pt_y_data = inferred_data + size_ * 10; + const float * height_pt_data = inferred_data + size_ * 11; for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { Obstacle * obs = &obstacles_[obstacle_id]; @@ -223,9 +225,9 @@ void Cluster2D::filter(const std::shared_ptr & inferred_data) } } -void Cluster2D::classify(const std::shared_ptr & inferred_data) +void Cluster2D::classify(const float * inferred_data) { - const float * classify_pt_data = inferred_data.get() + siz_ * 4; + const float * classify_pt_data = inferred_data + size_ * 4; int num_classes = 5; for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { Obstacle * obs = &obstacles_[obs_id]; @@ -233,7 +235,7 @@ void Cluster2D::classify(const std::shared_ptr & inferred_data) for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { int grid = obs->grids[grid_id]; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid]; + obs->meta_type_probs[k] += classify_pt_data[k * size_ + grid]; } } int meta_type_id = 0; @@ -374,3 +376,4 @@ void Cluster2D::getObjects( } objects.header = in_header; } +} // namespace lidar_apollo_instance_segmentation diff --git a/perception/lidar_apollo_instance_segmentation/src/debugger.cpp b/perception/lidar_apollo_instance_segmentation/src/debugger.cpp index 2ba6715444e05..2cbb160006423 100644 --- a/perception/lidar_apollo_instance_segmentation/src/debugger.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/debugger.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -21,6 +21,8 @@ #include #include +namespace lidar_apollo_instance_segmentation +{ Debugger::Debugger(rclcpp::Node * node) { instance_pointcloud_pub_ = @@ -98,3 +100,4 @@ void Debugger::publishColoredPointCloud( output_msg.header = input.header; instance_pointcloud_pub_->publish(output_msg); } +} // namespace lidar_apollo_instance_segmentation diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 6f486c9fb8ce1..e28b2a941a95a 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -20,74 +20,44 @@ #include #include +namespace lidar_apollo_instance_segmentation +{ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * node) : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) { int range, width, height; bool use_intensity_feature, use_constant_feature; - std::string engine_file; - std::string prototxt_file; - std::string caffemodel_file; + std::string onnx_file; score_threshold_ = node_->declare_parameter("score_threshold", 0.8); range = node_->declare_parameter("range", 60); width = node_->declare_parameter("width", 640); height = node_->declare_parameter("height", 640); - engine_file = node_->declare_parameter("engine_file", "vls-128.engine"); - prototxt_file = node_->declare_parameter("prototxt_file", "vls-128.prototxt"); - caffemodel_file = node_->declare_parameter("caffemodel_file", "vls-128.caffemodel"); + onnx_file = node_->declare_parameter("onnx_file", "vls-128.onnx"); use_intensity_feature = node_->declare_parameter("use_intensity_feature", true); use_constant_feature = node_->declare_parameter("use_constant_feature", true); target_frame_ = node_->declare_parameter("target_frame", "base_link"); z_offset_ = node_->declare_parameter("z_offset", -2.0); + const auto precision = node_->declare_parameter("precision", "fp32"); - // load weight file - std::ifstream fs(engine_file); - if (!fs.is_open()) { - RCLCPP_INFO( - node_->get_logger(), - "Could not find %s. try making TensorRT engine from caffemodel and prototxt", - engine_file.c_str()); - Tn::Logger logger; - nvinfer1::IBuilder * builder = nvinfer1::createInferBuilder(logger); - nvinfer1::INetworkDefinition * network = builder->createNetworkV2(0U); - nvcaffeparser1::ICaffeParser * parser = nvcaffeparser1::createCaffeParser(); - nvinfer1::IBuilderConfig * config = builder->createBuilderConfig(); - const nvcaffeparser1::IBlobNameToTensor * blob_name2tensor = parser->parse( - prototxt_file.c_str(), caffemodel_file.c_str(), *network, nvinfer1::DataType::kFLOAT); - std::string output_node = "deconv0"; - auto output = blob_name2tensor->find(output_node.c_str()); - if (output == nullptr) { - RCLCPP_ERROR(node_->get_logger(), "can not find output named %s", output_node.c_str()); - } - network->markOutput(*output); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, 1 << 30); -#else - config->setMaxWorkspaceSize(1 << 30); -#endif - nvinfer1::IHostMemory * plan = builder->buildSerializedNetwork(*network, *config); - assert(plan != nullptr); - std::ofstream outfile(engine_file, std::ofstream::binary); - assert(!outfile.fail()); - outfile.write(reinterpret_cast(plan->data()), plan->size()); - outfile.close(); - if (network) { - delete network; - } - if (parser) { - delete parser; - } - if (builder) { - delete builder; - } - if (config) { - delete config; - } - if (plan) { - delete plan; - } + trt_common_ = std::make_unique( + onnx_file, precision, nullptr, tensorrt_common::BatchConfig{1, 1, 1}, 1 << 30); + trt_common_->setup(); + + if (!trt_common_->isInitialized()) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "failed to create tensorrt engine file."); + return; } - net_ptr_.reset(new Tn::trtNet(engine_file)); + + // GPU memory allocation + const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_size = + std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); + input_d_ = cuda_utils::make_unique(input_size); + const auto output_dims = trt_common_->getBindingDimensions(1); + output_size_ = std::accumulate( + output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); + output_d_ = cuda_utils::make_unique(output_size_); + output_h_ = cuda_utils::make_unique_host(output_size_, cudaHostAllocPortable); // feature map generator: pre process feature_generator_ = std::make_shared( @@ -139,6 +109,10 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( const sensor_msgs::msg::PointCloud2 & input, tier4_perception_msgs::msg::DetectedObjectsWithFeature & output) { + if (!trt_common_->isInitialized()) { + return false; + } + // move up pointcloud z_offset in z axis sensor_msgs::msg::PointCloud2 transformed_cloud; transformCloud(input, transformed_cloud, z_offset_); @@ -151,9 +125,18 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( std::shared_ptr feature_map_ptr = feature_generator_->generate(pcl_pointcloud_raw_ptr); - // inference - std::shared_ptr inferred_data(new float[net_ptr_->getOutputSize() / sizeof(float)]); - net_ptr_->doInference(feature_map_ptr->map_data.data(), inferred_data.get()); + CHECK_CUDA_ERROR(cudaMemcpy( + input_d_.get(), feature_map_ptr->map_data.data(), + feature_map_ptr->map_data.size() * sizeof(float), cudaMemcpyHostToDevice)); + + std::vector buffers = {input_d_.get(), output_d_.get()}; + + trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + output_h_.get(), output_d_.get(), sizeof(float) * output_size_, cudaMemcpyDeviceToHost, + *stream_)); + cudaStreamSynchronize(*stream_); // post process const float objectness_thresh = 0.5; @@ -161,7 +144,7 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( valid_idx.indices.resize(pcl_pointcloud_raw_ptr->size()); std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0); cluster2d_->cluster( - inferred_data, pcl_pointcloud_raw_ptr, valid_idx, objectness_thresh, + output_h_.get(), pcl_pointcloud_raw_ptr, valid_idx, objectness_thresh, true /*use all grids for clustering*/); const float height_thresh = 0.5; const int min_pts_num = 3; @@ -178,3 +161,4 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( output.header = transformed_cloud.header; return true; } +} // namespace lidar_apollo_instance_segmentation diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp b/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp index eb563f0abba8c..56291e086e41f 100644 --- a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -24,6 +24,8 @@ inline float normalizeIntensity(float intensity) } } // namespace +namespace lidar_apollo_instance_segmentation +{ FeatureGenerator::FeatureGenerator( const int width, const int height, const int range, const bool use_intensity_feature, const bool use_constant_feature) @@ -96,3 +98,4 @@ std::shared_ptr FeatureGenerator::generate( } return map_ptr_; } +} // namespace lidar_apollo_instance_segmentation diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp b/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp index 7c097f1fc519d..024a62ed2a08e 100644 --- a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -18,6 +18,8 @@ #include +namespace lidar_apollo_instance_segmentation +{ FeatureMapInterface::FeatureMapInterface( const int _channels, const int _width, const int _height, const int _range) : channels(_channels), @@ -166,3 +168,4 @@ void FeatureMapWithConstantAndIntensity::resetMap([[maybe_unused]] std::vector(num * 10.0); @@ -43,3 +45,4 @@ float calcApproximateLog(float num) } return std::log(static_cast(1.0 + num)); } +} // namespace lidar_apollo_instance_segmentation diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp index 246bb49e5946e..5bf8ed0d027e5 100644 --- a/perception/lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 TierIV +// Copyright 2020-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. @@ -19,6 +19,8 @@ #include #include +namespace lidar_apollo_instance_segmentation +{ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( const rclcpp::NodeOptions & node_options) : Node("lidar_apollo_instance_segmentation_node", node_options) @@ -62,6 +64,7 @@ void LidarInstanceSegmentationNode::pointCloudCallback( "debug/processing_time_ms", processing_time_ms); } } +} // namespace lidar_apollo_instance_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(LidarInstanceSegmentationNode) +RCLCPP_COMPONENTS_REGISTER_NODE(lidar_apollo_instance_segmentation::LidarInstanceSegmentationNode) From 50d97bc58dfc6a6ad3cd4f886bec1b526ad19c7b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 12 Apr 2023 18:04:10 +0900 Subject: [PATCH 08/12] fix(avoidance): fix clip forward length and backward length (#3347) Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 4b8fe6db112c8..d6eb9cc67f0df 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2827,8 +2827,8 @@ CandidateOutput AvoidanceModule::planCandidate() const if (!data.safe_new_sl.empty()) { // clip from shift start index for visualize util::clipPathLength( - shifted_path.path, data.safe_new_sl.front().start_idx, 0.0, - std::numeric_limits::max()); + shifted_path.path, data.safe_new_sl.front().start_idx, std::numeric_limits::max(), + 0.0); const auto sl = getNonStraightShiftLine(data.safe_new_sl); const auto sl_front = data.safe_new_sl.front(); From b8d9790537758487bb0252cc668693007d6164f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Wed, 12 Apr 2023 12:54:42 +0300 Subject: [PATCH 09/12] fix(mpc_lateral_controller): use contant instead of macros (#3358) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(mpc_lateral_controller): use contant instead of macros Signed-off-by: Kaan Colak --------- Signed-off-by: Kaan Çolak Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- control/mpc_lateral_controller/src/mpc.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 107bca1274e48..986af8c6888f5 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -24,9 +24,6 @@ #include #include -#define DEG2RAD 3.1415926535 / 180.0 -#define RAD2DEG 180.0 / 3.1415926535 - namespace autoware::motion::control::mpc_lateral_controller { using namespace std::literals::chrono_literals; @@ -297,7 +294,8 @@ bool MPC::getData( if (std::fabs(data->yaw_err) > m_admissible_yaw_error_rad) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, duration, "yaw error is over limit. error = %f deg, limit %f deg", - RAD2DEG * data->yaw_err, RAD2DEG * m_admissible_yaw_error_rad); + tier4_autoware_utils::rad2deg(data->yaw_err), + tier4_autoware_utils::rad2deg(m_admissible_yaw_error_rad)); return false; } @@ -594,7 +592,7 @@ MPCMatrix MPC::generateMPCMatrix( /* get reference input (feed-forward) */ m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); - if (std::fabs(Uref(0, 0)) < DEG2RAD * m_param.zero_ff_steer_deg) { + if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) { Uref(0, 0) = 0.0; // ignore curvature noise } m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; From baa7197bb14f2680fdb33f0c8566da95b9d0f195 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 12 Apr 2023 23:46:34 +0900 Subject: [PATCH 10/12] feat(route_handler): add lateral intervals calculator (#3371) --- .../include/route_handler/route_handler.hpp | 19 +++++++++--- planning/route_handler/src/route_handler.cpp | 29 ++++++++++++------- 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 1d24a7d2ff688..514c147f26d8f 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -273,13 +273,24 @@ class RouteHandler /** * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return * the distance to the preferred lane from the give lane. - * The distance is computed from the front point of the centerline of the given lane to + * The total distance is computed from the front point of the centerline of the given lane to * the front point of the preferred lane. - * @param Desired lanelet to query - * @param lane change direction + * @param lanelet lanelet to query + * @param direction change direction + * @return number of lanes from input to the preferred lane + */ + double getTotalLateralDistanceToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + + /** + * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return + * the distance to the preferred lane from the give lane. + * This computes each lateral interval to the preferred lane from the given lanelet + * @param lanelet lanelet to query + * @param direction change direction * @return number of lanes from input to the preferred lane */ - double getLateralDistanceToPreferredLane( + std::vector getLateralIntervalsToPreferredLane( const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; bool getClosestLaneletWithinRoute( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 09ff9606ec32a..014b0973bd1e2 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1330,15 +1330,22 @@ int RouteHandler::getNumLaneToPreferredLane( return 0; // TODO(Horibe) check if return 0 is appropriate. } -double RouteHandler::getLateralDistanceToPreferredLane( +double RouteHandler::getTotalLateralDistanceToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction) const +{ + const auto intervals = getLateralIntervalsToPreferredLane(lanelet, direction); + return std::accumulate(intervals.begin(), intervals.end(), 0); +} + +std::vector RouteHandler::getLateralIntervalsToPreferredLane( const lanelet::ConstLanelet & lanelet, const Direction direction) const { if (exists(preferred_lanelets_, lanelet)) { - return 0.0; + return {}; } if ((direction == Direction::NONE) || (direction == Direction::RIGHT)) { - double accumulated_distance = 0.0; + std::vector intervals; lanelet::ConstLanelet current_lanelet = lanelet; const auto & right_lanes = lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); @@ -1346,21 +1353,21 @@ double RouteHandler::getLateralDistanceToPreferredLane( const auto & current_centerline = current_lanelet.centerline(); const auto & next_centerline = right.centerline(); if (current_centerline.empty() || next_centerline.empty()) { - return -accumulated_distance; + return intervals; } const auto & curr_pt = current_centerline.front(); const auto & next_pt = next_centerline.front(); - accumulated_distance += lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt)); + intervals.push_back(-lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt))); if (exists(preferred_lanelets_, right)) { - return -accumulated_distance; + return intervals; } current_lanelet = right; } } if ((direction == Direction::NONE) || (direction == Direction::LEFT)) { - double accumulated_distance = 0.0; + std::vector intervals; lanelet::ConstLanelet current_lanelet = lanelet; const auto & left_lanes = lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); @@ -1368,20 +1375,20 @@ double RouteHandler::getLateralDistanceToPreferredLane( const auto & current_centerline = current_lanelet.centerline(); const auto & next_centerline = left.centerline(); if (current_centerline.empty() || next_centerline.empty()) { - return accumulated_distance; + return intervals; } const auto & curr_pt = current_centerline.front(); const auto & next_pt = next_centerline.front(); - accumulated_distance += lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt)); + intervals.push_back(lanelet::geometry::distance2d(to2D(curr_pt), to2D(next_pt))); if (exists(preferred_lanelets_, left)) { - return accumulated_distance; + return intervals; } current_lanelet = left; } } - return 0.0; + return {}; } bool RouteHandler::isInPreferredLane(const PoseStamped & pose) const From 1b1650d632e3b8f3da3f9218c41ae8cd9e9d7fff Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 13 Apr 2023 01:42:11 +0900 Subject: [PATCH 11/12] chore(lidar_apollo_instance_segmentation): add ignore directory (#3381) Signed-off-by: satoshi-ota --- perception/lidar_apollo_instance_segmentation/.gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/perception/lidar_apollo_instance_segmentation/.gitignore b/perception/lidar_apollo_instance_segmentation/.gitignore index cdac399b8df3f..8fce603003c1e 100644 --- a/perception/lidar_apollo_instance_segmentation/.gitignore +++ b/perception/lidar_apollo_instance_segmentation/.gitignore @@ -1,2 +1 @@ -data/*.onnx -data/*.engine +data/ From a2b2fcb571195de3062aaadf9121ae1960f05e90 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 13 Apr 2023 07:36:43 +0900 Subject: [PATCH 12/12] fix(avoidance): fix bug in yield judge logic (#3322) * fix(avoidance): fix bug in yield judge logic Signed-off-by: satoshi-ota * fix(avoidance): don't generate raw shift line for objects behind unavoidable object Signed-off-by: satoshi-ota * chore(avoidance): add comments Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.cpp | 29 +++++++++++++++---- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index d6eb9cc67f0df..478f5d1632f3c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -409,9 +409,12 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * if the both of following two conditions are satisfied, the module surely avoid the object. * Condition1: there is risk to collide with object without avoidance. * Condition2: there is enough space to avoid. + * In TOO_LARGE_JERK condition, it is possible to avoid object by deceleration even if the flag + * is_avoidable is FALSE. So, the module inserts stop point for such a object. */ for (const auto & o : data.target_objects) { - if (o.avoid_required && o.is_avoidable) { + const auto enough_space = o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + if (o.avoid_required && enough_space) { data.avoid_required = true; data.stop_target_object = o; break; @@ -732,7 +735,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; debug.unavoidable_objects.push_back(o); - continue; + if (o.avoid_required) { + break; + } else { + continue; + } } const auto is_object_on_right = isOnRight(o); @@ -741,7 +748,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; debug.unavoidable_objects.push_back(o); - continue; + if (o.avoid_required) { + break; + } else { + continue; + } } const auto avoiding_shift = shift_length - current_ego_shift; @@ -771,7 +782,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( if (!data.avoiding_now) { o.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; debug.unavoidable_objects.push_back(o); - continue; + if (o.avoid_required) { + break; + } else { + continue; + } } } @@ -785,7 +800,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( if (!data.avoiding_now) { o.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; debug.unavoidable_objects.push_back(o); - continue; + if (o.avoid_required) { + break; + } else { + continue; + } } } }