Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 26b2dbd1 编写于 作者: U unacao 提交者： siyangy
浏览文件
操作

parse decision result in dreamview backend (#44)

上级 5faa7ad3
隐藏空白更改
内联 并排
Showing with 425 addition and 90 deletion
modules/dreamview/backend/simulation_world/simulation_world_service.cc
浏览文件 @ 26b2dbd1
......@@ -33,17 +33,14 @@
using apollo::common::Point3D;
using apollo::common::adapter::AdapterManager;
using apollo::common::adapter::MonitorAdapter;
using apollo::common::adapter::LocalizationAdapter;
using apollo::common::adapter::ChassisAdapter;
using apollo::common::adapter::PerceptionObstaclesAdapter;
using apollo::common::adapter::PlanningAdapter;
using apollo::common::config::VehicleConfigHelper;
using apollo::common::monitor::MonitorMessage;
using apollo::common::monitor::MonitorMessageItem;
using apollo::localization::LocalizationEstimate;
using apollo::planning::ADCTrajectory;
using apollo::common::TrajectoryPoint;
using apollo::planning::DecisionResult;
using apollo::planning::StopReasonCode;
using apollo::canbus::Chassis;
using apollo::perception::PerceptionObstacle;
using apollo::perception::PerceptionObstacles;
......@@ -146,6 +143,99 @@ void SetObstacleType(const PerceptionObstacle &obstacle, Object *world_object) {
}
}
void SetStopReason(const StopReasonCode &reason_code, Decision *decision) {
switch (reason_code) {
case StopReasonCode::STOP_REASON_HEAD_VEHICLE:
decision->set_stopreason(Decision::STOP_REASON_HEAD_VEHICLE);
break;
case StopReasonCode::STOP_REASON_DESTINATION:
decision->set_stopreason(Decision::STOP_REASON_DESTINATION);
break;
case StopReasonCode::STOP_REASON_PEDESTRIAN:
decision->set_stopreason(Decision::STOP_REASON_PEDESTRIAN);
break;
case StopReasonCode::STOP_REASON_OBSTACLE:
decision->set_stopreason(Decision::STOP_REASON_OBSTACLE);
break;
case StopReasonCode::STOP_REASON_SIGNAL:
decision->set_stopreason(Decision::STOP_REASON_SIGNAL);
break;
case StopReasonCode::STOP_REASON_STOP_SIGN:
decision->set_stopreason(Decision::STOP_REASON_STOP_SIGN);
break;
case StopReasonCode::STOP_REASON_YIELD_SIGN:
decision->set_stopreason(Decision::STOP_REASON_YIELD_SIGN);
break;
case StopReasonCode::STOP_REASON_CLEAR_ZONE:
decision->set_stopreason(Decision::STOP_REASON_CLEAR_ZONE);
break;
case StopReasonCode::STOP_REASON_CROSSWALK:
decision->set_stopreason(Decision::STOP_REASON_CROSSWALK);
break;
default:
AWARN<< "Unrecognizable stop reason code:" << reason_code;
}
}
void UpdateTurnSignal(const apollo::common::VehicleSignal &signal,
Object *auto_driving_car) {
if (signal.turn_signal() == apollo::common::VehicleSignal::TURN_LEFT) {
auto_driving_car->set_current_signal("LEFT");
} else if (signal.turn_signal()
== apollo::common::VehicleSignal::TURN_RIGHT) {
auto_driving_car->set_current_signal("RIGHT");
} else if (signal.emergency_light()) {
auto_driving_car->set_current_signal("EMERGENCY");
} else {
auto_driving_car->set_current_signal("");
}
}
bool LocateMarker(const apollo::planning::ObjectDecisionType& decision,
double heading, Decision* world_decision) {
apollo::common::PointENU fence_point;
if (decision.has_stop() && decision.stop().has_stop_point()) {
world_decision->set_type(Decision_Type_STOP);
fence_point = decision.stop().stop_point();
} else if (decision.has_follow() && decision.follow().has_follow_point()) {
world_decision->set_type(Decision_Type_FOLLOW);
fence_point = decision.follow().follow_point();
} else if (decision.has_yield() && decision.yield().has_yield_point()) {
world_decision->set_type(Decision_Type_YIELD);
fence_point = decision.yield().yield_point();
} else if (decision.has_overtake()
&& decision.overtake().has_overtake_point()) {
world_decision->set_type(Decision_Type_OVERTAKE);
fence_point = decision.overtake().overtake_point();
} else {
return false;
}
world_decision->set_position_x(fence_point.x());
world_decision->set_position_y(fence_point.y());
world_decision->set_heading(heading);
return true;
}
void FindNudgeRegion(const apollo::planning::ObjectDecisionType& decision,
const Object& world_obj, Decision* world_decision) {
std::vector<apollo::common::math::Vec2d> points;
for (auto &polygon_pt : world_obj.polygon_point()) {
points.emplace_back(polygon_pt.x(), polygon_pt.y());
}
const apollo::common::math::Polygon2d obj_polygon(points);
const apollo::common::math::Polygon2d &nudge_polygon = obj_polygon
.ExpandByDistance(fabs(decision.nudge().distance_l()));
const std::vector<apollo::common::math::Vec2d> &nudge_points = nudge_polygon
.points();
for (auto &nudge_pt : nudge_points) {
PolygonPoint* poly_pt = world_decision->add_polygon_point();
poly_pt->set_x(nudge_pt.x());
poly_pt->set_y(nudge_pt.y());
}
world_decision->set_type(Decision_Type_NUDGE);
}
} // namespace
constexpr int SimulationWorldService::kMaxMonitorItems;
......@@ -161,9 +251,14 @@ const SimulationWorld &SimulationWorldService::Update() {
AdapterManager::Observe();
UpdateWithLatestObserved("Chassis", AdapterManager::GetChassis());
UpdateWithLatestObserved("Localization", AdapterManager::GetLocalization());
obj_map_.clear();
UpdateWithLatestObserved("PerceptionObstacles",
AdapterManager::GetPerceptionObstacles());
UpdateWithLatestObserved("Planning", AdapterManager::GetPlanning());
world_.clear_object();
for (auto &kv : obj_map_) {
*world_.add_object() = kv.second;
}
return world_;
}
......@@ -260,17 +355,7 @@ void SimulationWorldService::UpdateSimulationWorld(const Chassis &chassis) {
}
auto_driving_car->set_steering_angle(angle_percentage);
if (chassis.signal().turn_signal() ==
::apollo::common::VehicleSignal::TURN_LEFT) {
auto_driving_car->set_current_signal("LEFT");
} else if (chassis.signal().turn_signal() ==
::apollo::common::VehicleSignal::TURN_RIGHT) {
auto_driving_car->set_current_signal("RIGHT");
} else if (chassis.signal().emergency_light()) {
auto_driving_car->set_current_signal("EMERGENCY");
} else {
auto_driving_car->set_current_signal("");
}
UpdateTurnSignal(chassis.signal(), auto_driving_car);
auto_driving_car->set_disengage_type(DeduceDisengageType(chassis));
......@@ -284,16 +369,38 @@ void SimulationWorldService::UpdateSimulationWorld(const Chassis &chassis) {
std::max(world_.timestamp_sec(), chassis.header().timestamp_sec()));
}
Object &SimulationWorldService::CreateWorldObject(
const PerceptionObstacle &obstacle) {
const std::string id = std::to_string(obstacle.id());
// Create a new world object if the id does not exists in the map yet.
if (obj_map_.find(id) == obj_map_.end()) {
Object &world_obj = obj_map_[id];
SetObstacleInfo(obstacle, &world_obj);
SetObstaclePolygon(obstacle, &world_obj);
SetObstacleType(obstacle, &world_obj);
}
return obj_map_[id];
}
template <>
void SimulationWorldService::UpdateSimulationWorld(
const PerceptionObstacles &obstacles) {
for (const auto &obstacle : obstacles.perception_obstacle()) {
CreateWorldObject(obstacle);
}
world_.set_timestamp_sec(
std::max(world_.timestamp_sec(), obstacles.header().timestamp_sec()));
}
void SimulationWorldService::UpdatePlanningTrajectory(
const ADCTrajectory &trajectory) {
const double cutoff_time = world_.auto_driving_car().timestamp_sec();
const double header_time = trajectory.header().timestamp_sec();
const size_t trajectory_length = trajectory.trajectory_point_size();
util::TrajectoryPointCollector collector(&world_);
size_t i = 0;
const size_t trajectory_length = trajectory.trajectory_point_size();
bool collecting_started = false;
while (i < trajectory_length) {
const TrajectoryPoint &point = trajectory.trajectory_point(i);
......@@ -302,8 +409,8 @@ void SimulationWorldService::UpdateSimulationWorld(
// localization/chassis message) will be dropped.
//
// Note that the last two points are always included.
if (collecting_started ||
point.relative_time() + header_time >= cutoff_time) {
if (collecting_started
|| point.relative_time() + header_time >= cutoff_time) {
collecting_started = true;
collector.Collect(point);
if (i == trajectory_length - 1) {
......@@ -327,21 +434,120 @@ void SimulationWorldService::UpdateSimulationWorld(
++i;
}
}
}
void SimulationWorldService::UpdateMainDecision(
const apollo::planning::MainDecision &main_decision,
double update_timestamp_sec, Object *world_main_stop) {
apollo::common::math::Vec2d stop_pt;
double stop_heading = 0.0;
if (main_decision.has_not_ready()) {
// The car is not ready!
// Use the current ADC pose since it is better not to self-drive.
stop_pt.set_x(world_.auto_driving_car().position_x());
stop_pt.set_y(world_.auto_driving_car().position_y());
stop_heading = world_.auto_driving_car().heading();
world_main_stop->add_decision()->set_stopreason(
Decision::STOP_REASON_NOT_READY);
} else if (main_decision.has_estop()) {
// Emergency stop.
// Use the current ADC pose since it is better to stop immediately.
stop_pt.set_x(world_.auto_driving_car().position_x());
stop_pt.set_y(world_.auto_driving_car().position_y());
stop_heading = world_.auto_driving_car().heading();
world_main_stop->add_decision()->set_stopreason(
Decision::STOP_REASON_EMERGENCY);
world_.mutable_auto_driving_car()->set_current_signal("EMERGENCY");
} else {
// Normal stop.
const apollo::planning::MainStop& stop = main_decision.stop();
stop_pt.set_x(stop.stop_point().x());
stop_pt.set_y(stop.stop_point().y());
stop_heading = stop.stop_heading();
if (stop.has_reason_code()) {
SetStopReason(stop.reason_code(), world_main_stop->add_decision());
}
}
world_main_stop->set_position_x(stop_pt.x());
world_main_stop->set_position_y(stop_pt.y());
world_main_stop->set_heading(stop_heading);
world_main_stop->set_timestamp_sec(update_timestamp_sec);
}
void SimulationWorldService::UpdateDecision(
const DecisionResult &decision_res, double header_time) {
// Update turn signal.
UpdateTurnSignal(decision_res.vehicle_signal(),
world_.mutable_auto_driving_car());
apollo::planning::MainDecision main_decision = decision_res.main_decision();
// Update speed limit.
if (main_decision.target_lane_size() > 0) {
world_.set_speed_limit(main_decision.target_lane(0).speed_limit());
}
world_.set_timestamp_sec(std::max(world_.timestamp_sec(), header_time));
// Update relevant main stop with reason.
world_.clear_main_stop();
if (main_decision.has_not_ready() || main_decision.has_estop()
|| main_decision.has_stop()) {
Object* world_main_stop = world_.mutable_main_stop();
UpdateMainDecision(main_decision, header_time, world_main_stop);
}
// Update obstacle decision.
for (const auto &obj_decision : decision_res.object_decision().decision()) {
if (obj_decision.has_prediction()) {
const auto &p_obj = obj_decision.prediction().perception_obstacle();
const std::string id = std::to_string(p_obj.id());
// If the object does not exist in the map yet, it could be extra virtual
// objects created by prediction/decision modules.
if (obj_map_.find(id) == obj_map_.end()) {
CreateWorldObject(p_obj);
}
Object &world_obj = obj_map_[id];
if (obj_decision.type()
== apollo::planning::ObjectDecision_ObjectType_VIRTUAL) {
world_obj.set_type(Object_Type_VIRTUAL);
}
for (const auto &decision : obj_decision.object_decision()) {
Decision* world_decision = world_obj.add_decision();
world_decision->set_type(Decision_Type_IGNORE);
if (decision.has_stop() || decision.has_follow() || decision.has_yield()
|| decision.has_overtake()) {
if (!LocateMarker(decision, world_.auto_driving_car().heading(),
world_decision)) {
AWARN<< "No decision marker position found for object id="
<< world_obj.id();
continue;
}
} else if (decision.has_nudge()) {
if (world_obj.polygon_point_size() == 0) {
AWARN << "No polygon points found for object id=" << world_obj.id();
continue;
}
FindNudgeRegion(decision, world_obj, world_decision);
} else if (decision.has_sidepass()) {
world_decision->set_type(Decision_Type_SIDEPASS);
}
}
world_obj.set_timestamp_sec(
std::max(world_obj.timestamp_sec(), header_time));
}
}
}
template <>
void SimulationWorldService::UpdateSimulationWorld(
const PerceptionObstacles &obstacles) {
for (const auto &obstacle : obstacles.perception_obstacle()) {
Object *world_obj = world_.add_object();
SetObstacleInfo(obstacle, world_obj);
SetObstaclePolygon(obstacle, world_obj);
SetObstacleType(obstacle, world_obj);
}
world_.set_timestamp_sec(
std::max(world_.timestamp_sec(), obstacles.header().timestamp_sec()));
const ADCTrajectory &trajectory) {
const double header_time = trajectory.header().timestamp_sec();
UpdatePlanningTrajectory(trajectory);
UpdateDecision(trajectory.decision(), header_time);
world_.set_timestamp_sec(std::max(world_.timestamp_sec(), header_time));
}
void SimulationWorldService::RegisterMonitorCallback() {
......
modules/dreamview/backend/simulation_world/simulation_world_service.h
浏览文件 @ 26b2dbd1
......@@ -106,6 +106,15 @@ class SimulationWorldService {
template <typename DataType>
void UpdateSimulationWorld(const DataType &data);
Object &CreateWorldObject(
const apollo::perception::PerceptionObstacle &obstacle);
void UpdatePlanningTrajectory(
const apollo::planning::ADCTrajectory &trajectory);
void UpdateDecision(
const apollo::planning::DecisionResult &decision_res, double header_time);
void UpdateMainDecision(const apollo::planning::MainDecision &main_decision,
double update_timestamp_sec, Object *world_main_stop);
/**
* @brief Check whether a particular adapter has been initialized correctly.
*/
......@@ -152,6 +161,7 @@ class SimulationWorldService {
// The handle of MapService, not owned by SimulationWorldService.
MapService *map_service_;
// The map holding obstacle string id to the actual object
std::unordered_map<std::string, Object> obj_map_;
FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorSuccess);
......@@ -161,6 +171,7 @@ class SimulationWorldService {
FRIEND_TEST(SimulationWorldServiceTest, UpdateLocalization);
FRIEND_TEST(SimulationWorldServiceTest, UpdatePerceptionObstacles);
FRIEND_TEST(SimulationWorldServiceTest, UpdatePlanningTrajectory);
FRIEND_TEST(SimulationWorldServiceTest, UpdateDecision);
};
} // namespace dreamview
......
modules/dreamview/backend/simulation_world/simulation_world_service_test.cc
浏览文件 @ 26b2dbd1
......@@ -28,6 +28,7 @@ using apollo::common::monitor::MonitorMessage;
using apollo::localization::LocalizationEstimate;
using apollo::planning::ADCTrajectory;
using apollo::common::TrajectoryPoint;
using apollo::planning::DecisionResult;
using apollo::perception::PerceptionObstacle;
using apollo::perception::PerceptionObstacles;
......@@ -63,11 +64,11 @@ TEST_F(SimulationWorldServiceTest, UpdateMonitorSuccess) {
"I am the previous message.");
sim_world_service_->UpdateSimulationWorld(monitor);
EXPECT_EQ(2, sim_world_service_->world_.monitor().item_size());
EXPECT_EQ("I am the latest message.",
sim_world_service_->world_.monitor().item(0).msg());
EXPECT_EQ("I am the previous message.",
sim_world_service_->world_.monitor().item(1).msg());
const SimulationWorld &world = sim_world_service_->world();
EXPECT_EQ(2, world.monitor().item_size());
EXPECT_EQ("I am the latest message.", world.monitor().item(0).msg());
EXPECT_EQ("I am the previous message.", world.monitor().item(1).msg());
}
TEST_F(SimulationWorldServiceTest, UpdateMonitorRemove) {
......@@ -88,14 +89,14 @@ TEST_F(SimulationWorldServiceTest, UpdateMonitorRemove) {
sim_world_service_->world_.monitor().item(last).msg());
sim_world_service_->UpdateSimulationWorld(monitor);
const SimulationWorld &world = sim_world_service_->world();
EXPECT_EQ(SimulationWorldService::kMaxMonitorItems,
sim_world_service_->world_.monitor().item_size());
EXPECT_EQ("I am message -2",
sim_world_service_->world_.monitor().item(0).msg());
EXPECT_EQ("I am message -1",
sim_world_service_->world_.monitor().item(1).msg());
world.monitor().item_size());
EXPECT_EQ("I am message -2", world.monitor().item(0).msg());
EXPECT_EQ("I am message -1", world.monitor().item(1).msg());
EXPECT_EQ("I am message " + std::to_string(last - monitor.item_size()),
sim_world_service_->world_.monitor().item(last).msg());
world.monitor().item(last).msg());
}
TEST_F(SimulationWorldServiceTest, UpdateMonitorTruncate) {
......@@ -113,19 +114,20 @@ TEST_F(SimulationWorldServiceTest, UpdateMonitorTruncate) {
->set_timestamp_sec(1990);
sim_world_service_->UpdateSimulationWorld(monitor);
const SimulationWorld &world = sim_world_service_->world();
int last = SimulationWorldService::kMaxMonitorItems - 1;
EXPECT_EQ(SimulationWorldService::kMaxMonitorItems,
sim_world_service_->world_.monitor().item_size());
EXPECT_EQ("I am message 0",
sim_world_service_->world_.monitor().item(0).msg());
world.monitor().item_size());
EXPECT_EQ("I am message 0", world.monitor().item(0).msg());
EXPECT_EQ("I am message " + std::to_string(last),
sim_world_service_->world_.monitor().item(last).msg());
world.monitor().item(last).msg());
}
TEST_F(SimulationWorldServiceTest, UpdateChassisInfo) {
// Prepare the chassis message that will be used to update the
// SimulationWorld object.
::apollo::canbus::Chassis chassis;
Chassis chassis;
chassis.set_speed_mps(25);
chassis.set_throttle_percentage(50);
chassis.set_brake_percentage(10);
......@@ -151,7 +153,7 @@ TEST_F(SimulationWorldServiceTest, UpdateChassisInfo) {
TEST_F(SimulationWorldServiceTest, UpdateLocalization) {
// Prepare the localization message that will be used to update the
// SimulationWorld object.
::apollo::localization::LocalizationEstimate localization;
LocalizationEstimate localization;
localization.mutable_pose()->mutable_position()->set_x(1.0);
localization.mutable_pose()->mutable_position()->set_y(1.5);
localization.mutable_pose()->mutable_orientation()->set_qx(0.0);
......@@ -177,41 +179,6 @@ TEST_F(SimulationWorldServiceTest, UpdateLocalization) {
car.heading());
}
TEST_F(SimulationWorldServiceTest, UpdatePlanningTrajectory) {
// Prepare the trajectory message that will be used to update the
// SimulationWorld object.
ADCTrajectory planning_trajectory;
for (int i = 0; i < 30; ++i) {
TrajectoryPoint* point = planning_trajectory.add_trajectory_point();
point->mutable_path_point()->set_x(i * 10);
point->mutable_path_point()->set_y(i * 10 + 10);
}
// Commit the update.
sim_world_service_->UpdateSimulationWorld(planning_trajectory);
// Check the update result.
EXPECT_EQ(sim_world_service_->world_.planning_trajectory_size(), 4);
// Check first point.
{
const Object point = sim_world_service_->world_.planning_trajectory(0);
EXPECT_DOUBLE_EQ(0.0, point.position_x());
EXPECT_DOUBLE_EQ(10.0, point.position_y());
EXPECT_DOUBLE_EQ(atan2(100.0, 100.0), point.heading());
EXPECT_EQ(point.polygon_point_size(), 4);
}
// Check last point.
{
const Object point = sim_world_service_->world_.planning_trajectory(3);
EXPECT_DOUBLE_EQ(280.0, point.position_x());
EXPECT_DOUBLE_EQ(290.0, point.position_y());
EXPECT_DOUBLE_EQ(atan2(100.0, 100.0), point.heading());
EXPECT_EQ(point.polygon_point_size(), 4);
}
}
TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
PerceptionObstacles obstacles;
PerceptionObstacle* obstacle1 = obstacles.add_perception_obstacle();
......@@ -227,7 +194,7 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
point3->set_y(0.0);
obstacle1->set_timestamp(1489794020.123);
obstacle1->set_type(apollo::perception::PerceptionObstacle_Type_UNKNOWN);
apollo::perception::PerceptionObstacle* obstacle2 =
PerceptionObstacle* obstacle2 =
obstacles.add_perception_obstacle();
obstacle2->set_id(2);
apollo::perception::Point* point = obstacle2->mutable_position();
......@@ -240,20 +207,24 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
obstacle2->set_type(apollo::perception::PerceptionObstacle_Type_VEHICLE);
sim_world_service_->UpdateSimulationWorld(obstacles);
sim_world_service_->world_.clear_object();
for (auto &kv : sim_world_service_->obj_map_) {
*sim_world_service_->world_.add_object() = kv.second;
}
EXPECT_EQ(2, sim_world_service_->world_.object_size());
for (const auto& object : sim_world_service_->world_.object()) {
for (const auto &object : sim_world_service_->world_.object()) {
if (object.id() == "1") {
EXPECT_NEAR(1489794020.123, object.timestamp_sec(), kEpsilon);
EXPECT_DOUBLE_EQ(1489794020.123, object.timestamp_sec());
EXPECT_EQ(3, object.polygon_point_size());
EXPECT_EQ(Object_Type_UNKNOWN, object.type());
} else if (object.id() == "2") {
EXPECT_NEAR(1.0, object.position_x(), kEpsilon);
EXPECT_NEAR(2.0, object.position_y(), kEpsilon);
EXPECT_NEAR(3.0, object.heading(), kEpsilon);
EXPECT_NEAR(4.0, object.length(), kEpsilon);
EXPECT_NEAR(5.0, object.width(), kEpsilon);
EXPECT_NEAR(6.0, object.height(), kEpsilon);
EXPECT_DOUBLE_EQ(1.0, object.position_x());
EXPECT_DOUBLE_EQ(2.0, object.position_y());
EXPECT_DOUBLE_EQ(3.0, object.heading());
EXPECT_DOUBLE_EQ(4.0, object.length());
EXPECT_DOUBLE_EQ(5.0, object.width());
EXPECT_DOUBLE_EQ(6.0, object.height());
EXPECT_EQ(0, object.polygon_point_size());
EXPECT_EQ(Object_Type_VEHICLE, object.type());
} else {
......@@ -262,5 +233,152 @@ TEST_F(SimulationWorldServiceTest, UpdatePerceptionObstacles) {
}
}
TEST_F(SimulationWorldServiceTest, UpdatePlanningTrajectory) {
// Prepare the trajectory message that will be used to update the
// SimulationWorld object.
ADCTrajectory planning_trajectory;
for (int i = 0; i < 30; ++i) {
TrajectoryPoint* point = planning_trajectory.add_trajectory_point();
point->mutable_path_point()->set_x(i * 10);
point->mutable_path_point()->set_y(i * 10 + 10);
}
// Commit the update.
sim_world_service_->UpdatePlanningTrajectory(planning_trajectory);
// Check the update result.
const SimulationWorld &world = sim_world_service_->world();
EXPECT_EQ(4, world.planning_trajectory_size());
// Check first point.
{
const Object point = world.planning_trajectory(0);
EXPECT_DOUBLE_EQ(0.0, point.position_x());
EXPECT_DOUBLE_EQ(10.0, point.position_y());
EXPECT_DOUBLE_EQ(atan2(100.0, 100.0), point.heading());
EXPECT_EQ(4, point.polygon_point_size());
}
// Check last point.
{
const Object point = world.planning_trajectory(3);
EXPECT_DOUBLE_EQ(280.0, point.position_x());
EXPECT_DOUBLE_EQ(290.0, point.position_y());
EXPECT_DOUBLE_EQ(atan2(100.0, 100.0), point.heading());
EXPECT_EQ(4, point.polygon_point_size());
}
}
TEST_F(SimulationWorldServiceTest, UpdateDecision) {
DecisionResult decision_res;
decision_res.mutable_vehicle_signal()->set_turn_signal(
apollo::common::VehicleSignal::TURN_RIGHT);
apollo::planning::MainDecision* main_decision = decision_res
.mutable_main_decision();
main_decision->add_target_lane()->set_speed_limit(35);
apollo::planning::MainStop* main_stop = main_decision->mutable_stop();
main_stop->mutable_stop_point()->set_x(45678.9);
main_stop->mutable_stop_point()->set_y(1234567.8);
main_stop->set_stop_heading(1.234);
main_stop->set_reason_code(
apollo::planning::StopReasonCode::STOP_REASON_CROSSWALK);
apollo::planning::ObjectDecisions *obj_decisions = decision_res
.mutable_object_decision();
// The 1st obstacle has two decisions: nudge or sidepass.
apollo::planning::ObjectDecision *obj_decision1 =
obj_decisions->add_decision();
obj_decision1->set_type(apollo::planning::ObjectDecision_ObjectType_VIRTUAL);
PerceptionObstacle* perception1 = obj_decision1->mutable_prediction()
->mutable_perception_obstacle();
perception1->set_id(1);
perception1->mutable_position()->set_x(1.0);
perception1->mutable_position()->set_y(2.0);
perception1->set_theta(3.0);
perception1->set_length(4.0);
perception1->set_width(5.0);
perception1->set_height(6.0);
for (int i = 0; i < 3; ++i) {
apollo::perception::Point* perception_pt = perception1->add_polygon_point();
if (i < 2) {
perception_pt->set_x(10.0 * (i + 1));
perception_pt->set_y(20.0 * (i + 1));
} else {
perception_pt->set_y(10.0 * (i + 1));
perception_pt->set_x(20.0 * (i + 1));
}
}
obj_decision1->add_object_decision()->mutable_nudge()->set_distance_l(1.8);
obj_decision1->add_object_decision()->mutable_sidepass();
// The 2nd obstacle has one decision: follow.
apollo::planning::ObjectDecision* obj_decision2 =
obj_decisions->add_decision();
obj_decision2->set_type(
apollo::planning::ObjectDecision_ObjectType_PERCEPTION);
PerceptionObstacle* perception2 = obj_decision2->mutable_prediction()
->mutable_perception_obstacle();
perception2->set_id(2);
perception2->mutable_position()->set_x(-1860.48981629632);
perception2->mutable_position()->set_y(-3001.9591838696506);
apollo::planning::ObjectFollow* follow = obj_decision2->add_object_decision()
->mutable_follow();
follow->set_distance_s(2.0);
apollo::common::PointENU* follow_point = follow->mutable_follow_point();
follow_point->set_x(-1859.98);
follow_point->set_y(-3000.03);
sim_world_service_->UpdateDecision(decision_res, 1501095053);
const SimulationWorld &world = sim_world_service_->world();
EXPECT_EQ("RIGHT", world.auto_driving_car().current_signal());
EXPECT_EQ(35, world.speed_limit());
const Object &world_main_stop = world.main_stop();
EXPECT_EQ(world_main_stop.decision(0).stopreason(),
Decision::STOP_REASON_CROSSWALK);
EXPECT_DOUBLE_EQ(45678.9, world_main_stop.position_x());
EXPECT_DOUBLE_EQ(1234567.8, world_main_stop.position_y());
EXPECT_DOUBLE_EQ(1.234, world_main_stop.heading());
sim_world_service_->world_.clear_object();
for (auto &kv : sim_world_service_->obj_map_) {
*sim_world_service_->world_.add_object() = kv.second;
}
EXPECT_EQ(world.object_size(), 2);
for (int i = 0; i < 2; ++i) {
const Object &obj_dec = world.object(i);
if (obj_dec.id() == "1") {
EXPECT_EQ(Object_Type_VIRTUAL, obj_dec.type());
EXPECT_DOUBLE_EQ(1.0, obj_dec.position_x());
EXPECT_DOUBLE_EQ(2.0, obj_dec.position_y());
EXPECT_DOUBLE_EQ(3.0, obj_dec.heading());
EXPECT_DOUBLE_EQ(4.0, obj_dec.length());
EXPECT_DOUBLE_EQ(5.0, obj_dec.width());
EXPECT_DOUBLE_EQ(6.0, obj_dec.height());
EXPECT_EQ(obj_dec.decision_size(), 2);
for (int j = 0; j < obj_dec.decision_size(); ++j) {
const Decision& decision = obj_dec.decision(j);
if (j == 0) {
EXPECT_EQ(decision.type(), Decision_Type_NUDGE);
EXPECT_EQ(decision.polygon_point_size(), 67);
} else {
EXPECT_EQ(decision.type(), Decision_Type_SIDEPASS);
}
}
} else {
EXPECT_NE(Object_Type_VIRTUAL, obj_dec.type());
EXPECT_EQ(1, obj_dec.decision_size());
const Decision &decision = obj_dec.decision(0);
EXPECT_EQ(Decision_Type_FOLLOW, decision.type());
EXPECT_DOUBLE_EQ(-1859.98, decision.position_x());
EXPECT_DOUBLE_EQ(-3000.03, decision.position_y());
EXPECT_DOUBLE_EQ(0.0, decision.heading());
}
}
}
} // namespace dreamview
} // namespace apollo
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或