32 {
33 EnvironmentFeatures environment_features;
34
35 auto ego_state_container = container_manager->GetContainer<PoseContainer>(
37
38 if (ego_state_container == nullptr ||
39 ego_state_container->ToPerceptionObstacle() == nullptr) {
40 AERROR <<
"Null ego state container found or "
41 "the container pointer is nullptr";
42 return environment_features;
43 }
44
45 auto ptr_ego_state = ego_state_container->ToPerceptionObstacle();
46 if (!ptr_ego_state->has_position() || !ptr_ego_state->position().has_x() ||
47 !ptr_ego_state->position().has_y() || !ptr_ego_state->has_theta() ||
48 !ptr_ego_state->has_velocity() || !ptr_ego_state->velocity().has_x() ||
49 !ptr_ego_state->velocity().has_y()) {
50 AERROR <<
"Incomplete ego pose information.";
51 return environment_features;
52 }
53
54 if (std::isnan(ptr_ego_state->position().x()) ||
55 std::isnan(ptr_ego_state->position().y()) ||
56 std::isnan(ptr_ego_state->theta()) ||
57 std::isnan(ptr_ego_state->velocity().x()) ||
58 std::isnan(ptr_ego_state->velocity().y())) {
59 AERROR <<
"nan found in ego state";
60 return environment_features;
61 }
62
63 Vec2d ego_position(ptr_ego_state->position().x(),
64 ptr_ego_state->position().y());
65
66 Vec2d ego_velocity(ptr_ego_state->velocity().x(),
67 ptr_ego_state->velocity().y());
68
69 environment_features.set_ego_speed(ego_velocity.Length());
70 environment_features.set_ego_heading(ptr_ego_state->theta());
71
72 auto ptr_ego_lane =
73 GetEgoLane(ptr_ego_state->position(), ptr_ego_state->theta());
74
75 ExtractEgoLaneFeatures(&environment_features, ptr_ego_lane, ego_position);
76
77 ExtractNeighborLaneFeatures(&environment_features, ptr_ego_lane,
78 ego_position);
79
80 ExtractFrontJunctionFeatures(&environment_features, container_manager);
81
82 return environment_features;
83}