提交 2b8f79b3 编写于 作者: J Jiangtao Hu 提交者: Kecheng Xu

perception: fix build breakage for new perception pb change.

上级 5af1e3b4
......@@ -26,7 +26,6 @@ cc_library(
"//modules/common/math",
"//modules/common/proto:pnc_point_proto",
"//modules/common/time",
"//modules/perception/proto:perception_proto",
],
)
......
......@@ -54,9 +54,9 @@ PointENU MakePointENU(const math::Vec2d& xy) {
return point_enu;
}
apollo::perception::Point MakePerceptionPoint(const double x, const double y,
const double z) {
perception::Point point3d;
apollo::common::Point3D MakePoint3D(const double x, const double y,
const double z) {
common::Point3D point3d;
point3d.set_x(x);
point3d.set_y(y);
point3d.set_z(z);
......
......@@ -34,7 +34,6 @@
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/common/math/vec2d.h"
......@@ -127,8 +126,8 @@ PointENU operator+(const PointENU enu, const math::Vec2d& xy);
PointENU MakePointENU(const math::Vec2d& xy);
apollo::perception::Point MakePerceptionPoint(const double x, const double y,
const double z);
apollo::common::Point3D MakePoint3D(const double x, const double y,
const double z);
SpeedPoint MakeSpeedPoint(const double s, const double t, const double v,
const double a, const double da);
......
......@@ -125,22 +125,22 @@ void SetObstacleType(const PerceptionObstacle &obstacle, Object *world_object) {
}
switch (obstacle.type()) {
case PerceptionObstacle::UNKNOWN:
case perception::UNKNOWN:
world_object->set_type(Object_Type_UNKNOWN);
break;
case PerceptionObstacle::UNKNOWN_MOVABLE:
case perception::UNKNOWN_MOVABLE:
world_object->set_type(Object_Type_UNKNOWN_MOVABLE);
break;
case PerceptionObstacle::UNKNOWN_UNMOVABLE:
case perception::UNKNOWN_UNMOVABLE:
world_object->set_type(Object_Type_UNKNOWN_UNMOVABLE);
break;
case PerceptionObstacle::PEDESTRIAN:
case perception::PEDESTRIAN:
world_object->set_type(Object_Type_PEDESTRIAN);
break;
case PerceptionObstacle::BICYCLE:
case perception::BICYCLE:
world_object->set_type(Object_Type_BICYCLE);
break;
case PerceptionObstacle::VEHICLE:
case perception::VEHICLE:
world_object->set_type(Object_Type_VEHICLE);
break;
default:
......@@ -505,7 +505,8 @@ void SimulationWorldService::SetObstacleInfo(const PerceptionObstacle &obstacle,
world_object->set_speed_heading(
std::atan2(obstacle.velocity().y(), obstacle.velocity().x()));
world_object->set_timestamp_sec(obstacle.timestamp());
world_object->set_confidence(obstacle.confidence());
// FIXME(all): adjust to new perception pb
// world_object->set_confidence(obstacle.confidence());
}
void SimulationWorldService::SetObstaclePolygon(
......
......@@ -208,23 +208,23 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
PerceptionObstacles obstacles;
PerceptionObstacle* obstacle1 = obstacles.add_perception_obstacle();
obstacle1->set_id(1);
apollo::perception::Point* point1 = obstacle1->add_polygon_point();
apollo::common::Point3D* point1 = obstacle1->add_polygon_point();
point1->set_x(0.0);
point1->set_y(0.0);
apollo::perception::Point* point2 = obstacle1->add_polygon_point();
apollo::common::Point3D* point2 = obstacle1->add_polygon_point();
point2->set_x(0.0);
point2->set_y(1.0);
apollo::perception::Point* point3 = obstacle1->add_polygon_point();
apollo::common::Point3D* point3 = obstacle1->add_polygon_point();
point3->set_x(-1.0);
point3->set_y(0.0);
apollo::perception::Point* point4 = obstacle1->add_polygon_point();
apollo::common::Point3D* point4 = obstacle1->add_polygon_point();
point4->set_x(-1.0);
point4->set_y(0.0);
obstacle1->set_timestamp(1489794020.123);
obstacle1->set_type(apollo::perception::PerceptionObstacle_Type_UNKNOWN);
obstacle1->set_type(apollo::perception::UNKNOWN);
PerceptionObstacle* obstacle2 = obstacles.add_perception_obstacle();
obstacle2->set_id(2);
apollo::perception::Point* point = obstacle2->mutable_position();
apollo::common::Point3D* point = obstacle2->mutable_position();
point->set_x(1.0);
point->set_y(2.0);
obstacle2->set_theta(3.0);
......@@ -233,7 +233,7 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
obstacle2->set_height(6.0);
obstacle2->mutable_velocity()->set_x(3.0);
obstacle2->mutable_velocity()->set_y(4.0);
obstacle2->set_type(apollo::perception::PerceptionObstacle_Type_VEHICLE);
obstacle2->set_type(apollo::perception::VEHICLE);
sim_world_service_->UpdateSimulationWorld(obstacles);
sim_world_service_->world_.clear_object();
......
......@@ -146,7 +146,7 @@ void Object::Serialize(PerceptionObstacle* pb_obj) const {
pb_obj->set_confidence(score);
pb_obj->set_confidence_type(score_type);
pb_obj->set_tracking_time(tracking_time);
pb_obj->set_type(static_cast<PerceptionObstacle::Type>(type));
pb_obj->set_type(static_cast<perception::Type>(type));
pb_obj->set_timestamp(latest_tracked_time); // in seconds.
}
......
......@@ -160,7 +160,7 @@ TEST(ObjectTest, test_Deserialize) {
pb_obj.set_width(1.0);
pb_obj.set_height(1.5);
pb_obj.set_tracking_time(20.1);
pb_obj.set_type(PerceptionObstacle::BICYCLE);
pb_obj.set_type(perception::BICYCLE);
pb_obj.set_timestamp(1147012345.678);
PolygonDType polygon;
......
......@@ -114,9 +114,9 @@ TEST_F(LidarProcessTest, test_GeneratePbMsg) {
lidar_process_.GeneratePbMsg(&obstacles);
EXPECT_EQ(obstacles.perception_obstacle_size(), 2);
EXPECT_EQ(obstacles.perception_obstacle(0).type(),
PerceptionObstacle::VEHICLE);
perception::VEHICLE);
EXPECT_EQ(obstacles.perception_obstacle(1).type(),
PerceptionObstacle::PEDESTRIAN);
perception::PEDESTRIAN);
}
} // namespace perception
......
......@@ -65,7 +65,7 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
for (const auto& obstacle : latest_prediction->prediction_obstacle()) {
const auto& perception = obstacle.perception_obstacle();
if (perception.confidence() < FLAGS_perception_confidence_threshold &&
perception.type() != PerceptionObstacle::VEHICLE) {
perception.type() != perception::VEHICLE) {
continue;
}
double distance =
......@@ -84,7 +84,7 @@ void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {
const auto& perception = obstacle.perception_obstacle();
auto id = perception.id();
if (perception.confidence() < FLAGS_perception_confidence_threshold &&
perception.type() != PerceptionObstacle::VEHICLE) {
perception.type() != perception::VEHICLE) {
continue;
}
if (protected_obstacles.count(id) > 0) {
......
......@@ -110,7 +110,7 @@ common::TrajectoryPoint* Obstacle::AddTrajectoryPoint() {
}
bool Obstacle::IsStaticObstacle(const PerceptionObstacle& perception_obstacle) {
if (perception_obstacle.type() == PerceptionObstacle::UNKNOWN_UNMOVABLE) {
if (perception_obstacle.type() == perception::UNKNOWN_UNMOVABLE) {
return true;
}
auto moving_speed = std::hypot(perception_obstacle.velocity().x(),
......@@ -249,7 +249,7 @@ std::unique_ptr<Obstacle> Obstacle::CreateStaticVirtualObstacles(
perception_obstacle.set_width(obstacle_box.width());
perception_obstacle.set_height(FLAGS_virtual_stop_wall_height);
perception_obstacle.set_type(
perception::PerceptionObstacle::UNKNOWN_UNMOVABLE);
perception::UNKNOWN_UNMOVABLE);
perception_obstacle.set_tracking_time(1.0);
std::vector<common::math::Vec2d> corner_points;
......
......@@ -45,22 +45,22 @@ TEST(Obstacle, IsStaticObstacle) {
perception_obstacle.mutable_velocity()->set_y(0.5);
EXPECT_FALSE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.set_type(PerceptionObstacle::UNKNOWN);
perception_obstacle.set_type(perception::UNKNOWN);
EXPECT_FALSE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.set_type(PerceptionObstacle::UNKNOWN_UNMOVABLE);
perception_obstacle.set_type(perception::UNKNOWN_UNMOVABLE);
EXPECT_TRUE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.set_type(PerceptionObstacle::UNKNOWN_MOVABLE);
perception_obstacle.set_type(perception::UNKNOWN_MOVABLE);
EXPECT_FALSE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.set_type(PerceptionObstacle::PEDESTRIAN);
perception_obstacle.set_type(perception::PEDESTRIAN);
EXPECT_FALSE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.set_type(PerceptionObstacle::BICYCLE);
perception_obstacle.set_type(perception::BICYCLE);
EXPECT_FALSE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.set_type(PerceptionObstacle::VEHICLE);
perception_obstacle.set_type(perception::VEHICLE);
EXPECT_FALSE(Obstacle::IsStaticObstacle(perception_obstacle));
perception_obstacle.mutable_velocity()->set_x(0.5);
......
......@@ -125,15 +125,15 @@ void Crosswalk::MakeDecisions(Frame* const frame,
const PerceptionObstacle& perception_obstacle =
path_obstacle->obstacle()->Perception();
const std::string& obstacle_id = path_obstacle->Id();
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
perception::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name =
PerceptionObstacle_Type_Name(obstacle_type);
perception::Type_Name(obstacle_type);
// check type
if (obstacle_type != PerceptionObstacle::PEDESTRIAN &&
obstacle_type != PerceptionObstacle::BICYCLE &&
obstacle_type != PerceptionObstacle::UNKNOWN_MOVABLE &&
obstacle_type != PerceptionObstacle::UNKNOWN) {
if (obstacle_type != perception::PEDESTRIAN &&
obstacle_type != perception::BICYCLE &&
obstacle_type != perception::UNKNOWN_MOVABLE &&
obstacle_type != perception::UNKNOWN) {
ADEBUG << "obstacle_id[" << obstacle_id << "] type["
<< obstacle_type_name << "]. skip";
continue;
......
......@@ -235,9 +235,9 @@ std::string FrontVehicle::FindPassableObstacle(
const PerceptionObstacle& perception_obstacle =
path_obstacle->obstacle()->Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
perception::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name =
PerceptionObstacle_Type_Name(obstacle_type);
perception::Type_Name(obstacle_type);
if (path_obstacle->obstacle()->IsVirtual() ||
!path_obstacle->obstacle()->IsStatic()) {
......@@ -313,9 +313,9 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) {
const PerceptionObstacle& perception_obstacle =
path_obstacle->obstacle()->Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
perception::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name =
PerceptionObstacle_Type_Name(obstacle_type);
perception::Type_Name(obstacle_type);
if (path_obstacle->obstacle()->IsVirtual() ||
!path_obstacle->obstacle()->IsStatic()) {
......@@ -388,9 +388,9 @@ void FrontVehicle::MakeStopDecision(ReferenceLineInfo* reference_line_info) {
ObjectDecisionType stop;
auto stop_decision = stop.mutable_stop();
if (obstacle_type == PerceptionObstacle::UNKNOWN_MOVABLE ||
obstacle_type == PerceptionObstacle::BICYCLE ||
obstacle_type == PerceptionObstacle::VEHICLE) {
if (obstacle_type == perception::UNKNOWN_MOVABLE ||
obstacle_type == perception::BICYCLE ||
obstacle_type == perception::VEHICLE) {
stop_decision->set_reason_code(
StopReasonCode::STOP_REASON_HEAD_VEHICLE);
} else {
......
......@@ -148,9 +148,9 @@ PullOver::ValidateStopPointCode PullOver::IsValidStop(
const PerceptionObstacle& perception_obstacle =
path_obstacle->obstacle()->Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
perception::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name =
PerceptionObstacle_Type_Name(obstacle_type);
perception::Type_Name(obstacle_type);
if (path_obstacle->obstacle()->IsVirtual() ||
!path_obstacle->obstacle()->IsStatic()) {
......
......@@ -474,14 +474,14 @@ int StopSign::AddWatchVehicle(const PathObstacle& path_obstacle,
const PerceptionObstacle& perception_obstacle =
path_obstacle.obstacle()->Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle_type);
perception::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name = perception::Type_Name(obstacle_type);
// check type
if (obstacle_type != PerceptionObstacle::UNKNOWN &&
obstacle_type != PerceptionObstacle::UNKNOWN_MOVABLE &&
obstacle_type != PerceptionObstacle::BICYCLE &&
obstacle_type != PerceptionObstacle::VEHICLE) {
if (obstacle_type != perception::UNKNOWN &&
obstacle_type != perception::UNKNOWN_MOVABLE &&
obstacle_type != perception::BICYCLE &&
obstacle_type != perception::VEHICLE) {
ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name
<< "]. skip";
return 0;
......@@ -572,8 +572,8 @@ int StopSign::RemoveWatchVehicle(
const PerceptionObstacle& perception_obstacle =
path_obstacle.obstacle()->Perception();
const std::string& obstacle_id = std::to_string(perception_obstacle.id());
PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name = PerceptionObstacle_Type_Name(obstacle_type);
perception::Type obstacle_type = perception_obstacle.type();
std::string obstacle_type_name = perception::Type_Name(obstacle_type);
// check if being watched
if (std::find(watch_vehicle_ids.begin(), watch_vehicle_ids.end(),
......@@ -584,10 +584,10 @@ int StopSign::RemoveWatchVehicle(
}
// check type
if (obstacle_type != PerceptionObstacle::UNKNOWN &&
obstacle_type != PerceptionObstacle::UNKNOWN_MOVABLE &&
obstacle_type != PerceptionObstacle::BICYCLE &&
obstacle_type != PerceptionObstacle::VEHICLE) {
if (obstacle_type != perception::UNKNOWN &&
obstacle_type != perception::UNKNOWN_MOVABLE &&
obstacle_type != perception::BICYCLE &&
obstacle_type != perception::VEHICLE) {
ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name
<< "]. skip";
return 0;
......
......@@ -90,9 +90,9 @@ bool PathDecider::MakeStaticObstacleDecision(
const auto &obstacle = *path_obstacle->obstacle();
bool is_bycycle_or_pedestrain =
(obstacle.Perception().type() ==
perception::PerceptionObstacle::BICYCLE ||
perception::BICYCLE ||
obstacle.Perception().type() ==
perception::PerceptionObstacle::PEDESTRIAN);
perception::PEDESTRIAN);
if (!is_bycycle_or_pedestrain && !obstacle.IsStatic()) {
continue;
......
......@@ -79,9 +79,9 @@ TrajectoryCost::TrajectoryCost(
const auto *ptr_obstacle = ptr_path_obstacle->obstacle();
bool is_bycycle_or_pedestrian =
(ptr_obstacle->Perception().type() ==
perception::PerceptionObstacle::BICYCLE ||
perception::BICYCLE ||
ptr_obstacle->Perception().type() ==
perception::PerceptionObstacle::PEDESTRIAN);
perception::PEDESTRIAN);
if (Obstacle::IsVirtualObstacle(ptr_obstacle->Perception())) {
// Virtual obstacle
......
......@@ -290,10 +290,10 @@ bool SpeedDecider::CreateStopDecision(const PathObstacle& path_obstacle,
stop->set_reason_code(StopReasonCode::STOP_REASON_CLEAR_ZONE);
}
PerceptionObstacle::Type obstacle_type =
perception::Type obstacle_type =
path_obstacle.obstacle()->Perception().type();
ADEBUG << "STOP: obstacle_id[" << path_obstacle.obstacle()->Id()
<< "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type)
<< "] obstacle_type[" << perception::Type_Name(obstacle_type)
<< "]";
return true;
......@@ -329,10 +329,10 @@ bool SpeedDecider::CreateFollowDecision(
fence_point->set_z(0.0);
follow->set_fence_heading(ref_point.heading());
PerceptionObstacle::Type obstacle_type =
perception::Type obstacle_type =
path_obstacle.obstacle()->Perception().type();
ADEBUG << "FOLLOW: obstacle_id[" << path_obstacle.obstacle()->Id()
<< "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type)
<< "] obstacle_type[" << perception::Type_Name(obstacle_type)
<< "]";
return true;
......@@ -343,12 +343,12 @@ bool SpeedDecider::CreateYieldDecision(
ObjectDecisionType* const yield_decision) const {
DCHECK_NOTNULL(yield_decision);
PerceptionObstacle::Type obstacle_type =
perception::Type obstacle_type =
path_obstacle.obstacle()->Perception().type();
double yield_distance = FLAGS_yield_distance;
switch (obstacle_type) {
case PerceptionObstacle::PEDESTRIAN:
case PerceptionObstacle::BICYCLE:
case perception::PEDESTRIAN:
case perception::BICYCLE:
yield_distance = FLAGS_yield_distance_pedestrian_bycicle;
break;
default:
......@@ -380,7 +380,7 @@ bool SpeedDecider::CreateYieldDecision(
yield->set_fence_heading(ref_point.heading());
ADEBUG << "YIELD: obstacle_id[" << path_obstacle.obstacle()->Id()
<< "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type)
<< "] obstacle_type[" << perception::Type_Name(obstacle_type)
<< "]";
return true;
......@@ -423,10 +423,10 @@ bool SpeedDecider::CreateOvertakeDecision(
overtake->mutable_fence_point()->set_z(0.0);
overtake->set_fence_heading(ref_point.heading());
PerceptionObstacle::Type obstacle_type =
perception::Type obstacle_type =
path_obstacle.obstacle()->Perception().type();
ADEBUG << "OVERTAKE: obstacle_id[" << path_obstacle.obstacle()->Id()
<< "] obstacle_type[" << PerceptionObstacle_Type_Name(obstacle_type)
<< "] obstacle_type[" << perception::Type_Name(obstacle_type)
<< "]";
return true;
......
......@@ -61,7 +61,7 @@ Obstacle::Obstacle() {
{heading_filter_param}};
}
PerceptionObstacle::Type Obstacle::type() const { return type_; }
perception::Type Obstacle::type() const { return type_; }
int Obstacle::id() const { return id_; }
......@@ -161,7 +161,7 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
InitKFMotionTracker(feature);
}
UpdateKFMotionTracker(feature);
if (type_ == PerceptionObstacle::PEDESTRIAN) {
if (type_ == perception::PEDESTRIAN) {
if (!kf_pedestrian_tracker_.IsInitialized()) {
InitKFPedestrianTracker(feature);
}
......@@ -179,7 +179,7 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
SetNearbyLanes(&feature);
if (FLAGS_adjust_vehicle_heading_by_lane &&
type_ == PerceptionObstacle::VEHICLE) {
type_ == perception::VEHICLE) {
AdjustHeadingByLane(&feature);
}
......@@ -262,7 +262,7 @@ void Obstacle::UpdateStatus(Feature* feature) {
<< "].";
// Update pedestrian motion belief
if (type_ == PerceptionObstacle::PEDESTRIAN) {
if (type_ == perception::PEDESTRIAN) {
if (!kf_pedestrian_tracker_.IsInitialized()) {
ADEBUG << "Obstacle [" << id_
<< "] has not initialized pedestrian tracker.";
......@@ -408,8 +408,8 @@ void Obstacle::SetVelocity(const PerceptionObstacle& perception_obstacle,
}
}
double filtered_heading = heading_filter_.Filter(velocity_heading);
if (type_ == PerceptionObstacle::BICYCLE ||
type_ == PerceptionObstacle::PEDESTRIAN) {
if (type_ == perception::BICYCLE ||
type_ == perception::PEDESTRIAN) {
velocity_heading = filtered_heading;
}
velocity_x = speed * std::cos(velocity_heading);
......@@ -795,7 +795,7 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
}
// Ignore bike and sidewalk lanes for vehicles
if (type_ == PerceptionObstacle::VEHICLE &&
if (type_ == perception::VEHICLE &&
nearby_lane->lane().has_type() &&
(nearby_lane->lane().type() == ::apollo::hdmap::Lane::BIKING ||
nearby_lane->lane().type() == ::apollo::hdmap::Lane::SIDEWALK)) {
......@@ -1038,8 +1038,8 @@ void Obstacle::SetMotionStatus() {
double std = FLAGS_still_obstacle_position_std;
double speed_threshold = FLAGS_still_obstacle_speed_threshold;
if (type_ == PerceptionObstacle::PEDESTRIAN ||
type_ == PerceptionObstacle::BICYCLE) {
if (type_ == perception::PEDESTRIAN ||
type_ == perception::BICYCLE) {
speed_threshold = FLAGS_still_pedestrian_speed_threshold;
std = FLAGS_still_pedestrian_position_std;
}
......
......@@ -73,7 +73,7 @@ class Obstacle {
* @brief Get the type of perception obstacle's type.
* @return The type pf perception obstacle.
*/
perception::PerceptionObstacle::Type type() const;
perception::Type type() const;
/**
* @brief Get the obstacle's ID.
......@@ -244,8 +244,8 @@ class Obstacle {
private:
int id_ = -1;
perception::PerceptionObstacle::Type type_ =
perception::PerceptionObstacle::UNKNOWN_UNMOVABLE;
perception::Type type_ =
perception::UNKNOWN_UNMOVABLE;
std::deque<Feature> feature_history_;
common::math::KalmanFilter<double, 6, 2, 0> kf_motion_tracker_;
common::math::KalmanFilter<double, 2, 2, 4> kf_pedestrian_tracker_;
......
......@@ -64,7 +64,7 @@ TEST_F(ObstacleTest, VehicleBasic) {
Obstacle* obstacle_ptr = container_.GetObstacle(1);
EXPECT_TRUE(obstacle_ptr != nullptr);
EXPECT_EQ(obstacle_ptr->id(), 1);
EXPECT_EQ(obstacle_ptr->type(), perception::PerceptionObstacle::VEHICLE);
EXPECT_EQ(obstacle_ptr->type(), perception::VEHICLE);
EXPECT_TRUE(obstacle_ptr->IsOnLane());
EXPECT_EQ(obstacle_ptr->history_size(), 3);
EXPECT_DOUBLE_EQ(obstacle_ptr->timestamp(), 0.2);
......@@ -143,7 +143,7 @@ TEST_F(ObstacleTest, PedestrianBasic) {
Obstacle* obstacle_ptr = container_.GetObstacle(101);
EXPECT_TRUE(obstacle_ptr != nullptr);
EXPECT_EQ(obstacle_ptr->id(), 101);
EXPECT_EQ(obstacle_ptr->type(), perception::PerceptionObstacle::PEDESTRIAN);
EXPECT_EQ(obstacle_ptr->type(), perception::PEDESTRIAN);
EXPECT_EQ(obstacle_ptr->history_size(), 3);
EXPECT_DOUBLE_EQ(obstacle_ptr->timestamp(), 0.2);
}
......
......@@ -123,7 +123,7 @@ void ObstaclesContainer::PrioritizeObstacles() {
bool ObstaclesContainer::IsPredictable(
const PerceptionObstacle& perception_obstacle) {
if (!perception_obstacle.has_type() ||
perception_obstacle.type() == PerceptionObstacle::UNKNOWN_UNMOVABLE) {
perception_obstacle.type() == perception::UNKNOWN_UNMOVABLE) {
return false;
}
return true;
......
......@@ -48,19 +48,19 @@ TEST_F(ObstaclesContainerTest, Vehicles) {
Obstacle* obstacle_ptr0 = container_.GetObstacle(0);
EXPECT_TRUE(obstacle_ptr0 != nullptr);
EXPECT_EQ(obstacle_ptr0->id(), 0);
EXPECT_EQ(obstacle_ptr0->type(), perception::PerceptionObstacle::VEHICLE);
EXPECT_EQ(obstacle_ptr0->type(), perception::VEHICLE);
Obstacle* obstacle_ptr1 = container_.GetObstacle(1);
EXPECT_TRUE(obstacle_ptr1 != nullptr);
EXPECT_EQ(obstacle_ptr1->id(), 1);
EXPECT_EQ(obstacle_ptr1->type(), perception::PerceptionObstacle::VEHICLE);
EXPECT_EQ(obstacle_ptr1->type(), perception::VEHICLE);
Obstacle* obstacle_ptr2 = container_.GetObstacle(2);
EXPECT_TRUE(obstacle_ptr2 != nullptr);
EXPECT_EQ(obstacle_ptr2->id(), 2);
EXPECT_EQ(obstacle_ptr2->type(), perception::PerceptionObstacle::VEHICLE);
EXPECT_EQ(obstacle_ptr2->type(), perception::VEHICLE);
Obstacle* obstacle_ptr3 = container_.GetObstacle(3);
EXPECT_TRUE(obstacle_ptr3 != nullptr);
EXPECT_EQ(obstacle_ptr3->id(), 3);
EXPECT_EQ(obstacle_ptr3->type(), perception::PerceptionObstacle::VEHICLE);
EXPECT_EQ(obstacle_ptr3->type(), perception::VEHICLE);
Obstacle* obstacle_ptr4 = container_.GetObstacle(4);
EXPECT_TRUE(obstacle_ptr4 == nullptr);
}
......@@ -70,12 +70,12 @@ TEST_F(ObstaclesContainerTest, Pedestrian) {
EXPECT_TRUE(obstacle_ptr101 != nullptr);
EXPECT_EQ(obstacle_ptr101->id(), 101);
EXPECT_EQ(obstacle_ptr101->type(),
perception::PerceptionObstacle::PEDESTRIAN);
perception::PEDESTRIAN);
Obstacle* obstacle_ptr102 = container_.GetObstacle(102);
EXPECT_TRUE(obstacle_ptr102 != nullptr);
EXPECT_EQ(obstacle_ptr102->id(), 102);
EXPECT_EQ(obstacle_ptr102->type(),
perception::PerceptionObstacle::PEDESTRIAN);
perception::PEDESTRIAN);
Obstacle* obstacle_ptr103 = container_.GetObstacle(103);
EXPECT_TRUE(obstacle_ptr103 == nullptr);
}
......
......@@ -24,7 +24,7 @@ namespace prediction {
using apollo::localization::LocalizationEstimate;
using apollo::perception::PerceptionObstacle;
using apollo::perception::Point;
using apollo::common::Point3D;
void PoseContainer::Insert(const ::google::protobuf::Message& message) {
localization::LocalizationEstimate localization;
......@@ -56,7 +56,7 @@ void PoseContainer::Update(
obstacle_ptr_->Clear();
obstacle_ptr_->set_id(ID);
Point position;
Point3D position;
position.set_x(localization.pose().position().x());
position.set_y(localization.pose().position().y());
position.set_z(localization.pose().position().z());
......@@ -76,7 +76,7 @@ void PoseContainer::Update(
}
obstacle_ptr_->set_theta(theta);
Point velocity;
Point3D velocity;
velocity.set_x(localization.pose().linear_velocity().x());
velocity.set_y(localization.pose().linear_velocity().y());
velocity.set_z(localization.pose().linear_velocity().z());
......@@ -110,7 +110,7 @@ double PoseContainer::GetTheta() const {
return obstacle_ptr_->theta();
}
Point PoseContainer::GetPosition() const {
common::Point3D PoseContainer::GetPosition() const {
return obstacle_ptr_->position();
}
......
......@@ -77,7 +77,7 @@ class PoseContainer : public Container {
* @brief Get adc position
* @return adc position
*/
apollo::perception::Point GetPosition() const;
apollo::common::Point3D GetPosition() const;
private:
/**
......@@ -88,8 +88,8 @@ class PoseContainer : public Container {
public:
static const int ID = -1;
static const perception::PerceptionObstacle::Type type_ =
perception::PerceptionObstacle::VEHICLE;
static const perception::Type type_ =
perception::VEHICLE;
private:
std::unique_ptr<perception::PerceptionObstacle> obstacle_ptr_;
......
......@@ -55,18 +55,18 @@ void EvaluatorManager::Init(const PredictionConf& config) {
if (obstacle_conf.has_obstacle_status() &&
obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
switch (obstacle_conf.obstacle_type()) {
case PerceptionObstacle::VEHICLE: {
case perception::VEHICLE: {
vehicle_on_lane_evaluator_ = obstacle_conf.evaluator_type();
break;
}
case PerceptionObstacle::BICYCLE: {
case perception::BICYCLE: {
cyclist_on_lane_evaluator_ = obstacle_conf.evaluator_type();
break;
}
case PerceptionObstacle::PEDESTRIAN: {
case perception::PEDESTRIAN: {
break;
}
case PerceptionObstacle::UNKNOWN: {
case perception::UNKNOWN: {
default_on_lane_evaluator_ = obstacle_conf.evaluator_type();
break;
}
......@@ -116,21 +116,21 @@ void EvaluatorManager::Run(
}
switch (perception_obstacle.type()) {
case PerceptionObstacle::VEHICLE: {
case perception::VEHICLE: {
if (obstacle->IsOnLane()) {
evaluator = GetEvaluator(vehicle_on_lane_evaluator_);
CHECK_NOTNULL(evaluator);
}
break;
}
case PerceptionObstacle::BICYCLE: {
case perception::BICYCLE: {
if (obstacle->IsOnLane()) {
evaluator = GetEvaluator(cyclist_on_lane_evaluator_);
CHECK_NOTNULL(evaluator);
}
break;
}
case PerceptionObstacle::PEDESTRIAN: {
case perception::PEDESTRIAN: {
break;
}
default: {
......
......@@ -57,7 +57,7 @@ void FreeMovePredictor::Predict(Obstacle* obstacle) {
std::vector<TrajectoryPoint> points(0);
double prediction_total_time = FLAGS_prediction_duration;
if (obstacle->type() == PerceptionObstacle::PEDESTRIAN) {
if (obstacle->type() == perception::PEDESTRIAN) {
prediction_total_time = FLAGS_prediction_pedestrian_total_time;
}
DrawFreeMoveTrajectoryPoints(position, velocity, acc, theta,
......
......@@ -71,7 +71,7 @@ TEST_F(FreeMovePredictorTest, General) {
TEST_F(FreeMovePredictorTest, Pedestrian) {
perception_obstacles_.mutable_perception_obstacle(0)->set_type(
::apollo::perception::PerceptionObstacle::PEDESTRIAN);
::apollo::perception::PEDESTRIAN);
apollo::perception::PerceptionObstacle perception_obstacle =
perception_obstacles_.perception_obstacle(0);
ObstaclesContainer container;
......
......@@ -63,7 +63,7 @@ void PredictorManager::Init(const PredictionConf& config) {
}
switch (obstacle_conf.obstacle_type()) {
case PerceptionObstacle::VEHICLE: {
case perception::VEHICLE: {
if (obstacle_conf.has_obstacle_status()) {
if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
vehicle_on_lane_predictor_ = obstacle_conf.predictor_type();
......@@ -74,7 +74,7 @@ void PredictorManager::Init(const PredictionConf& config) {
}
break;
}
case PerceptionObstacle::BICYCLE: {
case perception::BICYCLE: {
if (obstacle_conf.has_obstacle_status()) {
if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
cyclist_on_lane_predictor_ = obstacle_conf.predictor_type();
......@@ -85,11 +85,11 @@ void PredictorManager::Init(const PredictionConf& config) {
}
break;
}
case PerceptionObstacle::PEDESTRIAN: {
case perception::PEDESTRIAN: {
pedestrian_predictor_ = obstacle_conf.predictor_type();
break;
}
case PerceptionObstacle::UNKNOWN: {
case perception::UNKNOWN: {
if (obstacle_conf.has_obstacle_status()) {
if (obstacle_conf.obstacle_status() == ObstacleConf::ON_LANE) {
default_on_lane_predictor_ = obstacle_conf.predictor_type();
......@@ -160,7 +160,7 @@ void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
predictor = GetPredictor(ObstacleConf::EMPTY_PREDICTOR);
} else {
switch (perception_obstacle.type()) {
case PerceptionObstacle::VEHICLE: {
case perception::VEHICLE: {
if (obstacle->IsOnLane()) {
predictor = GetPredictor(vehicle_on_lane_predictor_);
} else {
......@@ -168,11 +168,11 @@ void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
}
break;
}
case PerceptionObstacle::PEDESTRIAN: {
case perception::PEDESTRIAN: {
predictor = GetPredictor(pedestrian_predictor_);
break;
}
case PerceptionObstacle::BICYCLE: {
case perception::BICYCLE: {
if (obstacle->IsOnLane() && !obstacle->IsNearJunction()) {
predictor = GetPredictor(cyclist_on_lane_predictor_);
} else {
......@@ -194,7 +194,7 @@ void PredictorManager::Run(const PerceptionObstacles& perception_obstacles) {
if (predictor != nullptr) {
predictor->Predict(obstacle);
if (FLAGS_enable_trim_prediction_trajectory &&
obstacle->type() == PerceptionObstacle::VEHICLE) {
obstacle->type() == perception::VEHICLE) {
CHECK_NOTNULL(adc_trajectory_container);
predictor->TrimTrajectories(obstacle, adc_trajectory_container);
}
......
......@@ -64,7 +64,7 @@ message Feature {
optional double t_acc = 21 [deprecated = true];
optional bool is_still = 22 [default = false];
optional apollo.perception.PerceptionObstacle.Type type = 23;
optional apollo.perception.Type type = 23;
optional double label_update_time_delta = 24;
enum Priority {
......
......@@ -27,7 +27,7 @@ message ObstacleConf {
SINGLE_LANE_PREDICTOR = 5;
}
optional apollo.perception.PerceptionObstacle.Type obstacle_type = 1;
optional apollo.perception.Type obstacle_type = 1;
optional ObstacleStatus obstacle_status = 2;
optional EvaluatorType evaluator_type = 3;
optional PredictorType predictor_type = 4;
......
......@@ -38,7 +38,7 @@ using apollo::common::PointENU;
using apollo::common::Quaternion;
using apollo::hdmap::HDMapUtil;
using apollo::perception::PerceptionObstacle;
using apollo::perception::Point;
using apollo::common::Point3D;
double GetAngleFromQuaternion(const Quaternion quaternion) {
double theta = std::atan2(2.0 * quaternion.qw() * quaternion.qz() +
......@@ -123,18 +123,18 @@ double GetDefaultObjectWidth(const int object_type) {
return default_object_width;
}
Point SLtoXY(const Point& point, const double theta) {
Point3D SLtoXY(const Point3D& point, const double theta) {
return SLtoXY(point.x(), point.y(), theta);
}
Point SLtoXY(const double x, const double y, const double theta) {
Point converted_point;
Point3D SLtoXY(const double x, const double y, const double theta) {
Point3D converted_point;
converted_point.set_x(x * std::cos(theta) + y * std::sin(theta));
converted_point.set_y(x * std::sin(theta) - y * std::cos(theta));
return converted_point;
}
double Distance(const Point& point1, const Point& point2) {
double Distance(const Point3D& point1, const Point3D& point2) {
double distance =
std::sqrt((point1.x() - point2.x()) * (point1.x() - point2.x()) +
(point1.y() - point2.y()) * (point1.y() - point2.y()));
......@@ -166,7 +166,7 @@ double GetNearestLaneHeading(const PointENU& point_enu) {
return lane_heading;
}
double GetNearestLaneHeading(const Point& point) {
double GetNearestLaneHeading(const Point3D& point) {
auto* hdmap = HDMapUtil::BaseMapPtr();
if (hdmap == nullptr) {
AERROR << "Failed to get nearest lane for point " << point.DebugString();
......@@ -197,7 +197,7 @@ double GetNearestLaneHeading(const double x, const double y, const double z) {
return GetNearestLaneHeading(point_enu);
}
double GetLateralDistanceToNearestLane(const Point& point) {
double GetLateralDistanceToNearestLane(const Point3D& point) {
auto* hdmap = HDMapUtil::BaseMapPtr();
if (hdmap == nullptr) {
AERROR << "Failed to get nearest lane for point " << point.DebugString();
......@@ -224,7 +224,7 @@ double GetLateralDistanceToNearestLane(const Point& point) {
return nearest_l;
}
double Speed(const Point& point) {
double Speed(const Point3D& point) {
return std::sqrt(point.x() * point.x() + point.y() + point.y());
}
......
......@@ -42,7 +42,7 @@ void FillPerceptionPolygon(
const double length, const double width, const double height,
const double heading);
// TODO(lizh): change it to PerceptionObstacle::VEHICLE or so
// TODO(lizh): change it to perception::VEHICLE or so
// when perception obstacle type is extended.
// object type | int
// car | 0
......@@ -54,26 +54,26 @@ double GetDefaultObjectLength(const int object_type);
double GetDefaultObjectWidth(const int object_type);
apollo::perception::Point SLtoXY(const double x, const double y,
apollo::common::Point3D SLtoXY(const double x, const double y,
const double theta);
apollo::perception::Point SLtoXY(const apollo::perception::Point& point,
apollo::common::Point3D SLtoXY(const apollo::common::Point3D& point,
const double theta);
double Distance(const apollo::perception::Point& point1,
const apollo::perception::Point& point2);
double Distance(const apollo::common::Point3D& point1,
const apollo::common::Point3D& point2);
double Speed(const apollo::perception::Point& point);
double Speed(const apollo::common::Point3D& point);
double Speed(const double vx, const double vy);
double GetNearestLaneHeading(const apollo::common::PointENU& point_enu);
double GetNearestLaneHeading(const apollo::perception::Point& point);
double GetNearestLaneHeading(const apollo::common::Point3D& point);
double GetNearestLaneHeading(const double x, const double y, const double z);
double GetLateralDistanceToNearestLane(const apollo::perception::Point& point);
double GetLateralDistanceToNearestLane(const apollo::common::Point3D& point);
double HeadingDifference(const double theta1, const double theta2);
......
......@@ -44,7 +44,7 @@ using apollo::drivers::Mobileye;
using apollo::localization::LocalizationEstimate;
using apollo::perception::PerceptionObstacle;
using apollo::perception::PerceptionObstacles;
using apollo::perception::Point;
using apollo::common::Point3D;
std::map<std::int32_t, ::apollo::hdmap::LaneBoundaryType_Type>
lane_conversion_map = {{0, apollo::hdmap::LaneBoundaryType::DOTTED_YELLOW},
......@@ -157,7 +157,7 @@ PerceptionObstacles MobileyeToPerceptionObstacles(
mob_x += FLAGS_mobileye_pos_adjust; // offset: imu <-> mobileye
mob_x += mob_l / 2.0; // make x the middle point of the vehicle.
Point xy_point = SLtoXY(mob_x, mob_y, adc_theta);
Point3D xy_point = SLtoXY(mob_x, mob_y, adc_theta);
// TODO(QiL) : Clean this up after data collection and validation
double converted_x = 0.0;
......@@ -225,20 +225,20 @@ PerceptionObstacles MobileyeToPerceptionObstacles(
switch (mob_type) {
case 0:
case 1: {
mob->set_type(PerceptionObstacle::VEHICLE); // VEHICLE
mob->set_type(perception::VEHICLE); // VEHICLE
break;
}
case 2:
case 4: {
mob->set_type(PerceptionObstacle::BICYCLE); // BIKE
mob->set_type(perception::BICYCLE); // BIKE
break;
}
case 3: {
mob->set_type(PerceptionObstacle::PEDESTRIAN); // PED
mob->set_type(perception::PEDESTRIAN); // PED
break;
}
default: {
mob->set_type(PerceptionObstacle::UNKNOWN); // UNKNOWN
mob->set_type(perception::UNKNOWN); // UNKNOWN
break;
}
}
......@@ -256,7 +256,8 @@ PerceptionObstacles MobileyeToPerceptionObstacles(
mob->position().z(), mob->length(), mob->width(),
mob->height(), mob->theta());
mob->set_confidence(0.5);
// FIXME(all): adjust based on new perception pb
// mob->set_confidence(0.5);
}
return obstacles;
......@@ -287,15 +288,15 @@ RadarObstacles ContiToRadarObstacles(
rob.set_width(GetDefaultObjectWidth(4));
rob.set_height(3.0);
Point relative_pos_sl;
Point3D relative_pos_sl;
// TODO(QiL): load the radar configs here
relative_pos_sl.set_x(contiobs.longitude_dist());
relative_pos_sl.set_y(contiobs.lateral_dist());
rob.mutable_relative_position()->CopyFrom(relative_pos_sl);
Point relative_pos_xy = SLtoXY(relative_pos_sl, adc_theta);
Point absolute_pos;
Point3D relative_pos_xy = SLtoXY(relative_pos_sl, adc_theta);
Point3D absolute_pos;
absolute_pos.set_x(adc_pos.x() + relative_pos_xy.x());
absolute_pos.set_y(adc_pos.y() + relative_pos_xy.y());
absolute_pos.set_z(adc_pos.z());
......@@ -305,7 +306,7 @@ RadarObstacles ContiToRadarObstacles(
rob.mutable_relative_velocity()->set_y(contiobs.lateral_vel());
const auto iter = last_radar_obstacles.radar_obstacle().find(index);
Point absolute_vel;
Point3D absolute_vel;
if (iter == last_radar_obstacles.radar_obstacle().end()) {
rob.set_count(0);
rob.set_movable(false);
......@@ -422,7 +423,7 @@ RadarObstacles DelphiToRadarObstacles(
const double range = data_500.can_tx_track_range();
const double angle = data_500.can_tx_track_angle() * M_PI / 180.0;
Point relative_pos_sl;
Point3D relative_pos_sl;
relative_pos_sl.set_x(range * std::cos(angle) +
FLAGS_radar_pos_adjust + // offset: imu <-> mobileye
rob.length() /
......@@ -430,8 +431,8 @@ RadarObstacles DelphiToRadarObstacles(
relative_pos_sl.set_y(range * std::sin(angle));
rob.mutable_relative_position()->CopyFrom(relative_pos_sl);
Point relative_pos_xy = SLtoXY(relative_pos_sl, adc_theta);
Point absolute_pos;
Point3D relative_pos_xy = SLtoXY(relative_pos_sl, adc_theta);
Point3D absolute_pos;
absolute_pos.set_x(adc_pos.x() + relative_pos_xy.x());
absolute_pos.set_y(adc_pos.y() + relative_pos_xy.y());
absolute_pos.set_z(adc_pos.z());
......@@ -448,7 +449,7 @@ RadarObstacles DelphiToRadarObstacles(
lateral_vel * std::cos(angle));
const auto iter = last_radar_obstacles.radar_obstacle().find(index);
Point absolute_vel;
Point3D absolute_vel;
if (iter == last_radar_obstacles.radar_obstacle().end()) {
rob.set_count(0);
rob.set_movable(false);
......@@ -499,7 +500,7 @@ PerceptionObstacles RadarObstaclesToPerceptionObstacles(
pob->set_id(radar_obstacle.id() + FLAGS_radar_id_offset);
pob->set_type(PerceptionObstacle::VEHICLE);
pob->set_type(perception::VEHICLE);
pob->set_length(radar_obstacle.length());
pob->set_width(radar_obstacle.width());
pob->set_height(radar_obstacle.height());
......@@ -518,7 +519,8 @@ PerceptionObstacles RadarObstaclesToPerceptionObstacles(
FillPerceptionPolygon(pob, pob->position().x(), pob->position().y(),
pob->position().z(), pob->length(), pob->width(),
pob->height(), pob->theta());
pob->set_confidence(0.01);
// FIXME(all): adjust based on new perception pb
// pob->set_confidence(0.01);
}
obstacles.mutable_header()->CopyFrom(radar_obstacles.header());
......
......@@ -78,7 +78,8 @@ PerceptionObstacles MobileyeRadarFusion(
for (auto& radar_obstacle :
*(radar_obstacles_fusion.mutable_perception_obstacle())) {
if (HasOverlap(mobileye_obstacle, radar_obstacle)) {
mobileye_obstacle.set_confidence(0.99);
// FIXME(all): adjust based on new perception pb.
// mobileye_obstacle.set_confidence(0.99);
mobileye_obstacle.mutable_velocity()->CopyFrom(
radar_obstacle.velocity());
}
......
......@@ -16,6 +16,5 @@ proto_library(
"//modules/common/proto:geometry_proto_lib",
"//modules/common/proto:error_code_proto_lib",
"//modules/common/proto:header_proto_lib",
"//modules/perception/proto:perception_proto_lib",
],
)
......@@ -3,14 +3,15 @@ syntax = "proto2";
package apollo.third_party_perception;
import "modules/common/proto/error_code.proto";
import "modules/common/proto/geometry.proto";
import "modules/common/proto/header.proto";
import "modules/perception/proto/perception_obstacle.proto";
// next ID: 14
message RadarObstacle {
optional int32 id = 1; // obstacle ID.
optional apollo.perception.Point relative_position =
optional apollo.common.Point3D relative_position =
2; // obstacle position in the sl coordinate system.
optional apollo.perception.Point relative_velocity =
optional apollo.common.Point3D relative_velocity =
3; // obstacle relative velocity.
optional double rcs = 4; // radar signal intensity.
optional bool movable = 5; // whether this obstacle is able to move.
......@@ -18,8 +19,8 @@ message RadarObstacle {
optional double length = 7;
optional double height = 8;
optional double theta = 9;
optional apollo.perception.Point absolute_position = 10;
optional apollo.perception.Point absolute_velocity = 11;
optional apollo.common.Point3D absolute_position = 10;
optional apollo.common.Point3D absolute_velocity = 11;
optional int32 count = 12;
optional int32 moving_frames_count = 13;
}
......
......@@ -39,7 +39,7 @@ using apollo::drivers::Mobileye;
using apollo::localization::LocalizationEstimate;
using apollo::perception::PerceptionObstacle;
using apollo::perception::PerceptionObstacles;
using apollo::perception::Point;
using apollo::common::Point3D;
std::string ThirdPartyPerception::Name() const {
return FLAGS_third_party_perception_node_name;
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册