Skip to content

Commit

Permalink
make nest shallower
Browse files Browse the repository at this point in the history
  • Loading branch information
mitukou1109 committed Jun 27, 2024
1 parent eec09a7 commit 61ee56a
Show file tree
Hide file tree
Showing 3 changed files with 311 additions and 258 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,6 @@ struct Obstacle
: stamp(arg_stamp),
ego_to_obstacle_distance(ego_to_obstacle_distance),
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
source_type(SourceType::PREDICTED_OBJECT),
pose(arg_pose),
orientation_reliable(true),
twist(object.kinematics.initial_twist_with_covariance.twist),
Expand All @@ -84,22 +83,15 @@ struct Obstacle
: stamp(arg_stamp),
ego_to_obstacle_distance(ego_to_obstacle_distance),
lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj),
source_type(SourceType::POINTCLOUD),
stop_collision_point(stop_collision_point),
slow_down_front_collision_point(slow_down_front_collision_point),
slow_down_back_collision_point(slow_down_back_collision_point)
{
}

enum class SourceType {
PREDICTED_OBJECT,
POINTCLOUD,
};

rclcpp::Time stamp; // This is not the current stamp, but when the object was observed.
double ego_to_obstacle_distance;
double lat_dist_from_obstacle_to_traj;
SourceType source_type;

// for PredictedObject
geometry_msgs::msg::Pose pose; // interpolated with the current stamp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,22 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
const std::vector<TrajectoryPoint> & traj_points,
const std_msgs::msg::Header & traj_header) const;
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
determineEgoBehaviorAgainstObstacles(
const Odometry & odometry, const std::optional<PredictedObjects> & objects,
determineEgoBehaviorAgainstPredictedObjectObstacles(
const Odometry & odometry, const PredictedObjects & objects,
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Obstacle> & obstacles);
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
determineEgoBehaviorAgainstPointCloudObstacles(
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
const std::vector<Obstacle> & obstacles);
std::vector<TrajectoryPoint> decimateTrajectoryPoints(
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points) const;
std::optional<StopObstacle> createStopObstacle(
std::optional<StopObstacle> createStopObstacleForPredictedObject(
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
const std::vector<Polygon2d> & traj_polys, const Obstacle & obstacle,
const double precise_lateral_dist) const;
std::optional<StopObstacle> createStopObstacleForPointCloud(
const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
const double precise_lateral_dist) const;
bool isStopObstacle(const uint8_t label) const;
bool isInsideCruiseObstacle(const uint8_t label) const;
bool isOutsideCruiseObstacle(const uint8_t label) const;
Expand All @@ -101,9 +108,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
double calcCollisionTimeMargin(
const Odometry & odometry, const std::vector<PointWithStamp> & collision_points,
const std::vector<TrajectoryPoint> & traj_points, const bool is_driving_forward) const;
std::optional<SlowDownObstacle> createSlowDownObstacle(
std::optional<SlowDownObstacle> createSlowDownObstacleForPredictedObject(
const Odometry & odometry, const std::vector<TrajectoryPoint> & traj_points,
const Obstacle & obstacle, const double precise_lat_dist);
std::optional<SlowDownObstacle> createSlowDownObstacleForPointCloud(
const Obstacle & obstacle, const double precise_lat_dist);
PlannerData createPlannerData(
const Odometry & odometry, const AccelWithCovarianceStamped & acc,
const std::vector<TrajectoryPoint> & traj_points) const;
Expand Down
Loading

0 comments on commit 61ee56a

Please sign in to comment.