From 6742e057758f7da446d04437e86a7511896641c7 Mon Sep 17 00:00:00 2001 From: Dong Li Date: Mon, 14 Aug 2017 15:06:17 -0700 Subject: [PATCH] added decision category: use the merged longitutional or lateral decision --- modules/planning/common/path_obstacle.cc | 20 +- modules/planning/common/path_obstacle.h | 12 +- .../optimizer/dp_st_speed/dp_st_graph.cc | 5 +- .../qp_spline_path/qp_frenet_frame.cc | 201 +++++++++--------- .../optimizer/st_graph/st_boundary_mapper.cc | 87 ++++---- modules/planning/planner/em/decider.cc | 89 ++++---- 6 files changed, 214 insertions(+), 200 deletions(-) diff --git a/modules/planning/common/path_obstacle.cc b/modules/planning/common/path_obstacle.cc index 44cace53f9..35478b0d06 100644 --- a/modules/planning/common/path_obstacle.cc +++ b/modules/planning/common/path_obstacle.cc @@ -131,6 +131,14 @@ ObjectDecisionType PathObstacle::MergeLongitutionalDecision( << " and decision: " << rhs.ShortDebugString(); } +const ObjectDecisionType& PathObstacle::LongitutionalDecision() const { + return longitutional_decision_; +} + +const ObjectDecisionType& PathObstacle::LateralDecision() const { + return lateral_decision_; +} + ObjectDecisionType PathObstacle::MergeLateralDecision( const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) { auto lhs_iter = s_lateral_decision_safety_sorter_.find(lhs.object_tag_case()); @@ -167,6 +175,12 @@ ObjectDecisionType PathObstacle::MergeLateralDecision( << " and decision: " << rhs.ShortDebugString(); } +bool PathObstacle::HasLateralDecision() const { return has_lateral_decision_; } + +bool PathObstacle::HasLongitutionalDecision() const { + return has_longitutional_decision_; +} + const planning::Obstacle* PathObstacle::Obstacle() const { return obstacle_; } void PathObstacle::AddDecision(const std::string& decider_tag, @@ -179,10 +193,12 @@ void PathObstacle::AddDecision(const std::string& decider_tag, MergeLongitutionalDecision(longitutional_decision_, merged_decision); } else { lateral_decision_ = merged_decision; + has_lateral_decision_ = true; } } else if (IsLongitutionalDecision(decision)) { longitutional_decision_ = MergeLongitutionalDecision(longitutional_decision_, decision); + has_longitutional_decision_ = true; } else { DCHECK(false) << "Unkown decision type: " << decision.DebugString(); } @@ -190,10 +206,6 @@ void PathObstacle::AddDecision(const std::string& decider_tag, decider_tags_.push_back(decider_tag); } -const std::vector& PathObstacle::Decisions() const { - return decisions_; -} - const std::string PathObstacle::DebugString() const { std::stringstream ss; for (std::size_t i = 0; i < decisions_.size(); ++i) { diff --git a/modules/planning/common/path_obstacle.h b/modules/planning/common/path_obstacle.h index dc01247b5a..ca9b786370 100644 --- a/modules/planning/common/path_obstacle.h +++ b/modules/planning/common/path_obstacle.h @@ -70,8 +70,6 @@ class PathObstacle { const planning::Obstacle* Obstacle() const; - bool MergeObstacle() const; - /** * @class Add decision to this obstacle. This function will * also calculate the merged decision. i.e., if there are multiple lateral @@ -87,19 +85,19 @@ class PathObstacle { void AddDecision(const std::string& decider_tag, const ObjectDecisionType& decision); - const std::vector& Decisions() const; - + bool HasLateralDecision() const; + bool HasLongitutionalDecision() const; /** * return the merged lateral decision * Lateral decision is one of {Nudge, Ignore} **/ - const ObjectDecisionType& MergedLateralDecision() const; + const ObjectDecisionType& LateralDecision() const; /** * @brief return the merged longitutional decision * Longitutional decision is one of {Stop, Yield, Follow, Overtake, Ignore} **/ - const ObjectDecisionType& MergedLongitutionalDecision() const; + const ObjectDecisionType& LongitutionalDecision() const; const std::string DebugString() const; @@ -137,7 +135,9 @@ class PathObstacle { // StGraphBoundary st_boundary_; ObjectDecisionType lateral_decision_; + bool has_lateral_decision_ = false; ObjectDecisionType longitutional_decision_; + bool has_longitutional_decision_ = false; struct ObjectTagCaseHash { std::size_t operator()( diff --git a/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc b/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc index 1259e8bc0e..0e65e9722b 100644 --- a/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc +++ b/modules/planning/optimizer/dp_st_speed/dp_st_graph.cc @@ -409,7 +409,7 @@ Status DpStGraph::GetObjectDecision(const StGraphData& st_graph_data, } } for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { - if (path_obstacle->Decisions().empty()) { + if (!path_obstacle->HasLongitutionalDecision()) { ObjectDecisionType ignore_decision; ignore_decision.mutable_ignore(); path_decision->AddDecision("dp_st_graph", path_obstacle->Id(), @@ -428,8 +428,7 @@ bool DpStGraph::CreateFollowDecision( const double follow_distance_s = -FLAGS_follow_min_distance; follow->set_distance_s(follow_distance_s); - const double reference_line_fence_s = - boundary.min_s() + follow_distance_s; + const double reference_line_fence_s = boundary.min_s() + follow_distance_s; common::PathPoint path_point; if (!path_data_.GetPathPointWithRefS(reference_line_fence_s, &path_point)) { AERROR << "Failed to get path point from reference line s " diff --git a/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc b/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc index b4fb3afe11..709e09bd5b 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc +++ b/modules/planning/optimizer/qp_spline_path/qp_frenet_frame.cc @@ -112,9 +112,8 @@ bool QpFrenetFrame::GetOverallBound( return false; } - std::pair obs_bound( - -std::numeric_limits::infinity(), - std::numeric_limits::infinity()); + std::pair obs_bound(-std::numeric_limits::infinity(), + std::numeric_limits::infinity()); if (!GetStaticObstacleBound(s, &obs_bound)) { return false; @@ -168,75 +167,77 @@ bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() { bool QpFrenetFrame::MapDynamicObstacleWithDecision( const PathObstacle& path_obstacle) { - const std::vector& decisions = path_obstacle.Decisions(); const Obstacle* ptr_obstacle = path_obstacle.Obstacle(); + if (!path_obstacle.HasLateralDecision()) { + AERROR << "object has no lateral decision"; + return false; + } + const auto& decision = path_obstacle.LateralDecision(); + if (!decision.has_nudge()) { + AWARN << "only support nudge now"; + return true; + } + const auto& nudge = decision.nudge(); + double buffer = std::fabs(nudge.distance_l()); + + int nudge_side = (nudge.type() == ObjectNudge::RIGHT_NUDGE) ? 1 : -1; + + for (const SpeedPoint& veh_point : discretized_vehicle_location_) { + double time = veh_point.t(); + common::TrajectoryPoint trajectory_point = + ptr_obstacle->GetPointAtTime(time); + common::math::Box2d obs_box = + ptr_obstacle->GetBoundingBox(trajectory_point); + // project obs_box on reference line + std::vector corners; + obs_box.GetAllCorners(&corners); + std::vector sl_corners; + + for (uint32_t i = 0; i < corners.size(); ++i) { + common::SLPoint cur_point; + if (!reference_line_.get_point_in_frenet_frame( + {corners[i].x(), corners[i].y()}, &cur_point)) { + AERROR << "Fail to map xy point " << corners[i].DebugString() << " to " + << cur_point.ShortDebugString(); - for (const auto& decision : decisions) { - if (!decision.has_nudge()) { - continue; + return false; + } + // shift box base on buffer + cur_point.set_l(cur_point.l() - buffer * nudge_side); + sl_corners.push_back(std::move(cur_point)); } - const auto& nudge = decision.nudge(); - double buffer = std::fabs(nudge.distance_l()); - - int nudge_side = (nudge.type() == ObjectNudge::RIGHT_NUDGE) ? 1 : -1; - - for (const SpeedPoint& veh_point : discretized_vehicle_location_) { - double time = veh_point.t(); - common::TrajectoryPoint trajectory_point = - ptr_obstacle->GetPointAtTime(time); - common::math::Box2d obs_box = - ptr_obstacle->GetBoundingBox(trajectory_point); - // project obs_box on reference line - std::vector corners; - obs_box.GetAllCorners(&corners); - std::vector sl_corners; - - for (uint32_t i = 0; i < corners.size(); ++i) { - common::SLPoint cur_point; - if (!reference_line_.get_point_in_frenet_frame( - {corners[i].x(), corners[i].y()}, &cur_point)) { - AERROR << "Fail to map xy point " << corners[i].DebugString() - << " to " << cur_point.ShortDebugString(); - - return false; - } - // shift box base on buffer - cur_point.set_l(cur_point.l() - buffer * nudge_side); - sl_corners.push_back(std::move(cur_point)); + + for (uint32_t i = 0; i < sl_corners.size(); ++i) { + common::SLPoint sl_first = sl_corners[i % sl_corners.size()]; + common::SLPoint sl_second = sl_corners[(i + 1) % sl_corners.size()]; + if (sl_first.s() < sl_second.s()) { + std::swap(sl_first, sl_second); } - for (uint32_t i = 0; i < sl_corners.size(); ++i) { - common::SLPoint sl_first = sl_corners[i % sl_corners.size()]; - common::SLPoint sl_second = sl_corners[(i + 1) % sl_corners.size()]; - if (sl_first.s() < sl_second.s()) { - std::swap(sl_first, sl_second); - } - - std::pair bound = MapLateralConstraint( - sl_first, sl_second, nudge_side, - veh_point.s() - vehicle_param_.back_edge_to_center(), - veh_point.s() + vehicle_param_.front_edge_to_center()); - - // update bound map - double s_resolution = std::fabs(veh_point.v() * time_resolution_); - double updated_start_s = - init_frenet_point_.s() + veh_point.s() - s_resolution; - double updated_end_s = - init_frenet_point_.s() + veh_point.s() + s_resolution; - if (updated_end_s > evaluated_knots_.back() || - updated_start_s < evaluated_knots_.front()) { - continue; - } - std::pair update_index_range = - FindInterval(updated_start_s, updated_end_s); - - for (uint32_t j = update_index_range.first; - j < update_index_range.second; ++j) { - dynamic_obstacle_bound_[j].first = - std::max(bound.first, dynamic_obstacle_bound_[j].first); - dynamic_obstacle_bound_[j].second = - std::min(bound.second, dynamic_obstacle_bound_[j].second); - } + std::pair bound = MapLateralConstraint( + sl_first, sl_second, nudge_side, + veh_point.s() - vehicle_param_.back_edge_to_center(), + veh_point.s() + vehicle_param_.front_edge_to_center()); + + // update bound map + double s_resolution = std::fabs(veh_point.v() * time_resolution_); + double updated_start_s = + init_frenet_point_.s() + veh_point.s() - s_resolution; + double updated_end_s = + init_frenet_point_.s() + veh_point.s() + s_resolution; + if (updated_end_s > evaluated_knots_.back() || + updated_start_s < evaluated_knots_.front()) { + continue; + } + std::pair update_index_range = + FindInterval(updated_start_s, updated_end_s); + + for (uint32_t j = update_index_range.first; j < update_index_range.second; + ++j) { + dynamic_obstacle_bound_[j].first = + std::max(bound.first, dynamic_obstacle_bound_[j].first); + dynamic_obstacle_bound_[j].second = + std::min(bound.second, dynamic_obstacle_bound_[j].second); } } } @@ -245,42 +246,42 @@ bool QpFrenetFrame::MapDynamicObstacleWithDecision( bool QpFrenetFrame::MapStaticObstacleWithDecision( const PathObstacle& path_obstacle) { - const std::vector& object_decisions = - path_obstacle.Decisions(); const auto ptr_obstacle = path_obstacle.Obstacle(); - - for (const auto& decision : object_decisions) { - if (!decision.has_nudge()) { - continue; + if (!path_obstacle.HasLateralDecision()) { + AERROR << "obstacle has no lateral decision"; + return false; + } + const auto& decision = path_obstacle.LateralDecision(); + if (!decision.has_nudge()) { + AWARN << "only support nudge decision now"; + return true; + } + const auto& nudge = decision.nudge(); + const double buffer = std::fabs(nudge.distance_l()); + if (nudge.type() == ObjectNudge::RIGHT_NUDGE) { + const auto& box = ptr_obstacle->PerceptionBoundingBox(); + std::vector corners; + box.GetAllCorners(&corners); + if (!MapPolygon(corners, buffer, -1, &static_obstacle_bound_)) { + AERROR << "fail to map polygon with id " << path_obstacle.Id() + << " in qp frenet frame"; + return false; } - const auto& nudge = decision.nudge(); - const double buffer = std::fabs(nudge.distance_l()); - if (nudge.type() == ObjectNudge::RIGHT_NUDGE) { - const auto& box = ptr_obstacle->PerceptionBoundingBox(); - std::vector corners; - box.GetAllCorners(&corners); - if (!MapPolygon(corners, buffer, -1, &static_obstacle_bound_)) { - AERROR << "fail to map polygon with id " << path_obstacle.Id() - << " in qp frenet frame"; - return false; - } - } else { - const auto& box = ptr_obstacle->PerceptionBoundingBox(); - std::vector corners; - box.GetAllCorners(&corners); - if (!MapPolygon(corners, buffer, 1, &static_obstacle_bound_)) { - AERROR << "fail to map polygon with id " << path_obstacle.Id() - << "in qp frenet frame"; - return false; - } + } else { + const auto& box = ptr_obstacle->PerceptionBoundingBox(); + std::vector corners; + box.GetAllCorners(&corners); + if (!MapPolygon(corners, buffer, 1, &static_obstacle_bound_)) { + AERROR << "fail to map polygon with id " << path_obstacle.Id() + << "in qp frenet frame"; + return false; } } return true; } bool QpFrenetFrame::MapPolygon( - const std::vector& corners, - const double buffer, + const std::vector& corners, const double buffer, const int nudge_side, std::vector>* const bound_map) { std::vector sl_corners; @@ -434,6 +435,9 @@ void QpFrenetFrame::CalculateHDMapBound() { bool QpFrenetFrame::CalculateObstacleBound() { for (const auto ptr_path_obstacle : path_obstacles_) { + if (!ptr_path_obstacle->HasLateralDecision()) { + continue; + } if (ptr_path_obstacle->Obstacle()->IsStatic()) { if (!MapStaticObstacleWithDecision(*ptr_path_obstacle)) { AERROR << "mapping obstacle with id [" << ptr_path_obstacle->Id() @@ -467,8 +471,8 @@ bool QpFrenetFrame::GetBound( const double s_low = evaluated_knots_[lower_index]; const double s_high = evaluated_knots_[lower_index + 1]; double weight = Double::Compare(s_low, s_high, 1e-8) < 0 - ? (s - s_low) / (s_high - s_low) - : 0.0; + ? (s - s_low) / (s_high - s_low) + : 0.0; double low_first = map_bound[lower_index].first; double low_second = map_bound[lower_index].second; @@ -501,7 +505,8 @@ uint32_t QpFrenetFrame::FindIndex(const double s) const { std::lower_bound(evaluated_knots_.begin() + 1, evaluated_knots_.end(), s); return std::min( static_cast(evaluated_knots_.size() - 1), - static_cast(lower_bound - evaluated_knots_.begin())) - 1; + static_cast(lower_bound - evaluated_knots_.begin())) - + 1; } } // namespace planning diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc index 0cf91c8bfc..9e684f4332 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc @@ -103,7 +103,7 @@ Status StBoundaryMapper::GetGraphBoundary( double min_stop_s = std::numeric_limits::max(); for (const auto* path_obstacle : path_obstacles.Items()) { - if (path_obstacle->Decisions().empty()) { + if (!path_obstacle->HasLongitutionalDecision()) { const auto ret = MapObstacleWithoutDecision(*path_obstacle, st_graph_boundaries); if (ret.code() == ErrorCode::PLANNING_ERROR) { @@ -113,52 +113,49 @@ Status StBoundaryMapper::GetGraphBoundary( return Status(ErrorCode::PLANNING_ERROR, msg); } } - for (const auto& decision : path_obstacle->Decisions()) { - StGraphBoundary boundary(path_obstacle); - if (decision.has_follow()) { - const auto ret = MapFollowDecision(*path_obstacle, decision, &boundary); - if (!ret.ok()) { - AERROR << "Fail to map obstacle " << path_obstacle->Id() - << " with follow decision: " << decision.DebugString(); - return Status(ErrorCode::PLANNING_ERROR, - "Fail to map follow decision"); - } - } else if (decision.has_stop()) { - // TODO(all) change start_s() to st_boundary.min_s() - const double stop_s = - path_obstacle->perception_sl_boundary().start_s() + - decision.stop().distance_s(); - if (stop_s < adc_front_s_) { - AERROR << "Invalid stop decision. not stop at ahead of current " - "position. stop_s : " - << stop_s << ", and current adc_s is; " << adc_front_s_; - return Status(ErrorCode::PLANNING_ERROR, "invalid decision"); - } - if (!stop_obstacle) { - stop_obstacle = path_obstacle; - stop_decision = decision; - min_stop_s = stop_s; - } else if (stop_s < min_stop_s) { - stop_obstacle = path_obstacle; - min_stop_s = stop_s; - stop_decision = decision; - } - } else if (decision.has_overtake() || decision.has_yield()) { - const auto ret = MapObstacleWithPredictionTrajectory( - *path_obstacle, decision, st_graph_boundaries); - if (!ret.ok()) { - AERROR << "Fail to map obstacle " << path_obstacle->Id() - << " with decision: " << decision.DebugString(); - return Status(ErrorCode::PLANNING_ERROR, - "Fail to map overtake/yield decision"); - } - } else { - std::string msg = common::util::StrCat("No mapping for decision: ", - decision.DebugString()); - return Status(ErrorCode::PLANNING_SKIP, msg); + const auto& decision = path_obstacle->LongitutionalDecision(); + StGraphBoundary boundary(path_obstacle); + if (decision.has_follow()) { + const auto ret = MapFollowDecision(*path_obstacle, decision, &boundary); + if (!ret.ok()) { + AERROR << "Fail to map obstacle " << path_obstacle->Id() + << " with follow decision: " << decision.DebugString(); + return Status(ErrorCode::PLANNING_ERROR, "Fail to map follow decision"); + } + } else if (decision.has_stop()) { + // TODO(all) change start_s() to st_boundary.min_s() + const double stop_s = path_obstacle->perception_sl_boundary().start_s() + + decision.stop().distance_s(); + if (stop_s < adc_front_s_) { + AERROR << "Invalid stop decision. not stop at ahead of current " + "position. stop_s : " + << stop_s << ", and current adc_s is; " << adc_front_s_; + return Status(ErrorCode::PLANNING_ERROR, "invalid decision"); + } + if (!stop_obstacle) { + stop_obstacle = path_obstacle; + stop_decision = decision; + min_stop_s = stop_s; + } else if (stop_s < min_stop_s) { + stop_obstacle = path_obstacle; + min_stop_s = stop_s; + stop_decision = decision; + } + } else if (decision.has_overtake() || decision.has_yield()) { + const auto ret = MapObstacleWithPredictionTrajectory( + *path_obstacle, decision, st_graph_boundaries); + if (!ret.ok()) { + AERROR << "Fail to map obstacle " << path_obstacle->Id() + << " with decision: " << decision.DebugString(); + return Status(ErrorCode::PLANNING_ERROR, + "Fail to map overtake/yield decision"); } - boundary.set_id(path_obstacle->Id()); + } else { + std::string msg = common::util::StrCat("No mapping for decision: ", + decision.DebugString()); + return Status(ErrorCode::PLANNING_SKIP, msg); } + boundary.set_id(path_obstacle->Id()); } if (stop_obstacle) { diff --git a/modules/planning/planner/em/decider.cc b/modules/planning/planner/em/decider.cc index 6b7877e6d6..76194c3928 100644 --- a/modules/planning/planner/em/decider.cc +++ b/modules/planning/planner/em/decider.cc @@ -18,8 +18,8 @@ * @file decider.cpp **/ -#include "modules/common/log.h" #include "modules/common/configs/vehicle_config_helper.h" +#include "modules/common/log.h" #include "modules/common/vehicle_state/vehicle_state.h" #include "modules/planning/planner/em/decider.h" @@ -31,8 +31,7 @@ using common::Status; using common::VehicleState; Decider::Decider(DecisionResult* decision_result) -: decision_(decision_result) { -} + : decision_(decision_result) {} const DecisionResult& Decider::Decision() const { return *decision_; } @@ -79,45 +78,42 @@ int Decider::MakeMainStopDecision(Frame* frame, const auto& path_obstacles = path_decision->path_obstacles(); for (const auto path_obstacle : path_obstacles.Items()) { const auto& obstacle = path_obstacle->Obstacle(); - for (const auto& object_decision : path_obstacle->Decisions()) { - if (not object_decision.has_stop()) { - continue; - } - - const auto& reference_line = frame->reference_line(); - apollo::common::PointENU stop_point = object_decision.stop().stop_point(); - common::SLPoint stop_line_sl; - reference_line.get_point_in_frenet_frame({stop_point.x(), stop_point.y()}, - &stop_line_sl); - - double stop_line_s = stop_line_sl.s(); - if (stop_line_s < 0 || stop_line_s > reference_line.length()) { - AERROR << "Ignore object:" << obstacle->Id() << " fence route_s[" - << stop_line_s << "] not in range[0, " << reference_line.length() - << "]"; - continue; - } - - // check stop_line_s vs adc_s - common::SLPoint adc_sl; - auto& adc_position = VehicleState::instance()->pose().position(); - reference_line.get_point_in_frenet_frame( - {adc_position.x(), adc_position.y()}, - &adc_sl); - const auto &vehicle_param = - common::VehicleConfigHelper::instance()->GetConfig().vehicle_param(); - if (stop_line_s <= adc_sl.s() + vehicle_param.front_edge_to_center()) { - AERROR << "object:" << obstacle->Id() << " fence route_s[" - << stop_line_s << "] behind adc route_s[" << adc_sl.s() - << "]"; - continue; - } - - if (stop_line_s < min_stop_line_s) { - min_stop_line_s = stop_line_s; - stop_obstacle = obstacle; - stop_decision = &(object_decision.stop()); - } + const auto& object_decision = path_obstacle->LongitutionalDecision(); + if (not object_decision.has_stop()) { + continue; + } + + const auto& reference_line = frame->reference_line(); + apollo::common::PointENU stop_point = object_decision.stop().stop_point(); + common::SLPoint stop_line_sl; + reference_line.get_point_in_frenet_frame({stop_point.x(), stop_point.y()}, + &stop_line_sl); + + double stop_line_s = stop_line_sl.s(); + if (stop_line_s < 0 || stop_line_s > reference_line.length()) { + AERROR << "Ignore object:" << obstacle->Id() << " fence route_s[" + << stop_line_s << "] not in range[0, " << reference_line.length() + << "]"; + continue; + } + + // check stop_line_s vs adc_s + common::SLPoint adc_sl; + auto& adc_position = VehicleState::instance()->pose().position(); + reference_line.get_point_in_frenet_frame( + {adc_position.x(), adc_position.y()}, &adc_sl); + const auto& vehicle_param = + common::VehicleConfigHelper::instance()->GetConfig().vehicle_param(); + if (stop_line_s <= adc_sl.s() + vehicle_param.front_edge_to_center()) { + AERROR << "object:" << obstacle->Id() << " fence route_s[" << stop_line_s + << "] behind adc route_s[" << adc_sl.s() << "]"; + continue; + } + + if (stop_line_s < min_stop_line_s) { + min_stop_line_s = stop_line_s; + stop_obstacle = obstacle; + stop_decision = &(object_decision.stop()); } } @@ -150,8 +146,13 @@ int Decider::SetObjectDecisions(PathDecision* const path_decision) { const auto& obstacle = path_obstacle->Obstacle(); object_decision->set_id(obstacle->Id()); object_decision->set_perception_id(obstacle->PerceptionId()); - for (const auto& decision : path_obstacle->Decisions()) { - object_decision->add_object_decision()->CopyFrom(decision); + if (path_obstacle->HasLateralDecision()) { + object_decision->add_object_decision()->CopyFrom( + path_obstacle->LateralDecision()); + } + if (path_obstacle->HasLongitutionalDecision()) { + object_decision->add_object_decision()->CopyFrom( + path_obstacle->LongitutionalDecision()); } } return 0; -- GitLab