Skip to content

Commit

Permalink
resolve errors
Browse files Browse the repository at this point in the history
Signed-off-by: Y.Hisaki <[email protected]>
  • Loading branch information
yhisaki committed Jul 19, 2024
1 parent b49bb5a commit 843add6
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 27 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -155,17 +155,11 @@ class LaneChangeBase

bool isValidPath() const { return status_.is_valid_path; }

void setData(const std::shared_ptr<const PlannerData> & data)
void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }

void setTimeKeeper(const std::shared_ptr<universe_utils::TimeKeeper> & time_keeper)
{
planner_data_ = data;
if (!common_data_ptr_->bpp_param_ptr) {
common_data_ptr_->bpp_param_ptr =
std::make_shared<BehaviorPathPlannerParameters>(data->parameters);
}
common_data_ptr_->self_odometry_ptr = data->self_odometry;
common_data_ptr_->route_handler_ptr = data->route_handler;
common_data_ptr_->lc_param_ptr = lane_change_parameters_;
common_data_ptr_->direction = direction_;
time_keeper_ = time_keeper;
}

void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -310,6 +310,8 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
AvoidancePlanningData & data, DebugData & debug) const
{
using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
using utils::static_obstacle_avoidance::fillObjectStoppableJudge;
using utils::static_obstacle_avoidance::filterTargetObjects;
using utils::static_obstacle_avoidance::separateObjectsByPath;
using utils::static_obstacle_avoidance::updateRoadShoulderDistance;
Expand Down Expand Up @@ -371,21 +373,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
}
}

void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const
{
using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
using utils::static_obstacle_avoidance::fillObjectStoppableJudge;

// Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
std::for_each(objects.begin(), objects.end(), [&, this](auto & o) {
fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
o.to_stop_line = calcDistanceToStopLine(o);
fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
});
}

ObjectData StaticObstacleAvoidanceModule::createObjectData(
const AvoidancePlanningData & data, const PredictedObject & object) const
{
Expand Down Expand Up @@ -1405,8 +1392,6 @@ void StaticObstacleAvoidanceModule::updateRTCData()

void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
{
using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray;
using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray;
using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray;

info_marker_.markers.clear();
Expand Down

0 comments on commit 843add6

Please sign in to comment.