From eae0bdeea108f217a43856096b3c5fd75475258c Mon Sep 17 00:00:00 2001 From: siyangy Date: Wed, 30 Aug 2017 18:04:02 -0700 Subject: [PATCH] Dreamview: Add a dummy start point for SimControl (#1364) --- modules/dreamview/backend/map/map_service.cc | 44 +++++++++++++------ modules/dreamview/backend/map/map_service.h | 8 +++- .../dreamview/backend/map/map_service_test.cc | 8 ++++ .../backend/sim_control/sim_control.cc | 38 +++++++++------- .../backend/sim_control/sim_control.h | 7 +-- .../backend/sim_control/sim_control_test.cc | 8 +--- 6 files changed, 71 insertions(+), 42 deletions(-) diff --git a/modules/dreamview/backend/map/map_service.cc b/modules/dreamview/backend/map/map_service.cc index 24509a3c2b..1cd36b1dbf 100644 --- a/modules/dreamview/backend/map/map_service.cc +++ b/modules/dreamview/backend/map/map_service.cc @@ -233,6 +233,19 @@ Map MapService::RetrieveMapElements(const MapElementIds &ids) const { return result; } +bool MapService::GetNearestLane(const double x, const double y, + LaneInfoConstPtr *nearest_lane, + double *nearest_s, double *nearest_l) const { + PointENU point; + point.set_x(x); + point.set_y(y); + if (BaseMap().GetNearestLane(point, nearest_lane, nearest_s, nearest_l) < 0) { + AERROR << "Failed to get nearest lane!"; + return false; + } + return true; +} + bool MapService::GetPointsFromRouting(const RoutingResponse &routing, std::vector *points) const { Path path; @@ -253,13 +266,9 @@ bool MapService::GetPointsFromRouting(const RoutingResponse &routing, bool MapService::GetPoseWithRegardToLane(const double x, const double y, double *theta, double *s) const { - apollo::common::PointENU point; - point.set_x(x); - point.set_y(y); double l; LaneInfoConstPtr nearest_lane; - if (BaseMap().GetNearestLane(point, &nearest_lane, s, &l) < 0) { - AERROR << "Failed to get nearest lane!"; + if (!GetNearestLane(x, y, &nearest_lane, s, &l)) { return false; } @@ -269,26 +278,33 @@ bool MapService::GetPoseWithRegardToLane(const double x, const double y, bool MapService::ConstructLaneWayPoint( const double x, const double y, - RoutingRequest::LaneWaypoint* laneWayPoint) const { - - apollo::common::PointENU point; - point.set_x(x); - point.set_y(y); - + RoutingRequest::LaneWaypoint *laneWayPoint) const { double s, l; LaneInfoConstPtr lane; - if (BaseMap().GetNearestLane(point, &lane, &s, &l) < 0) { - AERROR << "Failed to get nearest lane!"; + if (!GetNearestLane(x, y, &lane, &s, &l)) { return false; } laneWayPoint->set_id(lane->id().id()); laneWayPoint->set_s(s); - auto* pose = laneWayPoint->mutable_pose(); + auto *pose = laneWayPoint->mutable_pose(); pose->set_x(x); pose->set_y(y); return true; } + +bool MapService::GetStartPoint(apollo::common::PointENU *start_point) const { + // Start from origin to find a lane from the map. + double s, l; + LaneInfoConstPtr lane; + if (!GetNearestLane(0.0, 0.0, &lane, &s, &l)) { + return false; + } + + *start_point = lane->GetSmoothPoint(0.0); + return true; +} + } // namespace dreamview } // namespace apollo diff --git a/modules/dreamview/backend/map/map_service.h b/modules/dreamview/backend/map/map_service.h index ce315c88c9..fbc63e4d74 100644 --- a/modules/dreamview/backend/map/map_service.h +++ b/modules/dreamview/backend/map/map_service.h @@ -80,6 +80,8 @@ class MapService { bool GetPoseWithRegardToLane(const double x, const double y, double *theta, double *s) const; + // Get a point on the map to serve as dummy start point of SimControl + bool GetStartPoint(apollo::common::PointENU *start_point) const; /** * @brief The function fills out proper routing lane waypoint @@ -91,13 +93,17 @@ class MapService { */ bool ConstructLaneWayPoint( const double x, const double y, - ::apollo::routing::RoutingRequest::LaneWaypoint* laneWayPoint) const; + ::apollo::routing::RoutingRequest::LaneWaypoint *laneWayPoint) const; private: const ::apollo::hdmap::HDMap &BaseMap() const { return pnc_map_.HDMap(); } + bool GetNearestLane(const double x, const double y, + apollo::hdmap::LaneInfoConstPtr *nearest_lane, + double *nearest_s, double *nearest_l) const; + // A wrapper around HDMap to provide some convenient utils dreamview needs. const ::apollo::hdmap::PncMap pnc_map_; // A downsampled map for dreamview frontend display. diff --git a/modules/dreamview/backend/map/map_service_test.cc b/modules/dreamview/backend/map/map_service_test.cc index 1c78112772..1bcd661ef7 100644 --- a/modules/dreamview/backend/map/map_service_test.cc +++ b/modules/dreamview/backend/map/map_service_test.cc @@ -89,5 +89,13 @@ TEST_F(MapServiceTest, RetrieveMapElements) { EXPECT_EQ("l1", map.lane(0).id().id()); } +TEST_F(MapServiceTest, GetStartPoint) { + PointENU start_point; + EXPECT_TRUE(map_service.GetStartPoint(&start_point)); + EXPECT_DOUBLE_EQ(-1826.4050789145094, start_point.x()); + EXPECT_DOUBLE_EQ(-3027.5187874953263, start_point.y()); + EXPECT_DOUBLE_EQ(0.0, start_point.z()); +} + } // namespace dreamview } // namespace apollo diff --git a/modules/dreamview/backend/sim_control/sim_control.cc b/modules/dreamview/backend/sim_control/sim_control.cc index 8e4a251a62..ba7e1f0d53 100644 --- a/modules/dreamview/backend/sim_control/sim_control.cc +++ b/modules/dreamview/backend/sim_control/sim_control.cc @@ -61,34 +61,33 @@ SimControl::SimControl(const MapService* map_service) initial_start_(true), enabled_(FLAGS_enable_sim_control) { if (enabled_) { - RoutingResponse routing; - if (!GetProtoFromFile(FLAGS_routing_response_file, &routing)) { - AWARN << "Unable to read start point from file: " - << FLAGS_routing_response_file; + apollo::common::PointENU start_point; + if (!map_service_->GetStartPoint(&start_point)) { + AWARN << "Failed to get a dummy start point from map!"; return; } - SetStartPoint(routing); + SetStartPoint(start_point.x(), start_point.y()); } } -void SimControl::SetStartPoint(const RoutingResponse& routing) { +void SimControl::SetStartPoint(const double x, const double y) { next_point_.set_v(0.0); next_point_.set_a(0.0); - auto* p = next_point_.mutable_path_point(); - - p->set_x(routing.routing_request().start().pose().x()); - p->set_y(routing.routing_request().start().pose().y()); - p->set_z(0.0); + auto* next_point = next_point_.mutable_path_point(); + next_point->set_x(x); + next_point->set_y(y); + next_point->set_z(0.0); double theta = 0.0; double s = 0.0; - if (!map_service_->GetPoseWithRegardToLane(p->x(), p->y(), &theta, &s)) { - AERROR << "Failed to get heading!"; + if (!map_service_->GetPoseWithRegardToLane(next_point->x(), next_point->y(), + &theta, &s)) { + AERROR << "Failed to get heading from map! Treat theta and s as 0.0!"; } - p->set_theta(theta); - p->set_s(s); - p->set_kappa(0.0); + next_point->set_theta(theta); + next_point->set_s(s); + next_point->set_kappa(0.0); prev_point_index_ = next_point_index_ = 0; received_planning_ = false; @@ -98,11 +97,16 @@ void SimControl::SetStartPoint(const RoutingResponse& routing) { } } +void SimControl::OnRoutingResponse(const RoutingResponse& routing) { + const auto& start_pose = routing.routing_request().start().pose(); + SetStartPoint(start_pose.x(), start_pose.y()); +} + void SimControl::Start() { if (initial_start_) { // Setup planning and routing result data callback. AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, this); - AdapterManager::AddRoutingResponseCallback(&SimControl::SetStartPoint, + AdapterManager::AddRoutingResponseCallback(&SimControl::OnRoutingResponse, this); // Start timer to publish localiztion and chassis messages. diff --git a/modules/dreamview/backend/sim_control/sim_control.h b/modules/dreamview/backend/sim_control/sim_control.h index 44656d4796..2634db54e8 100644 --- a/modules/dreamview/backend/sim_control/sim_control.h +++ b/modules/dreamview/backend/sim_control/sim_control.h @@ -62,10 +62,11 @@ class SimControl { private: void OnPlanning(const apollo::planning::ADCTrajectory &trajectory); + void OnRoutingResponse(const apollo::routing::RoutingResponse &routing); - // Reset the start point according to the RoutingResponse, which can be read - // from file or received from a publisher. - void SetStartPoint(const apollo::routing::RoutingResponse &routing); + // Reset the start point, which can be a dummy point on the map or received + // from the routing module. + void SetStartPoint(const double x, const double y); void Freeze(); diff --git a/modules/dreamview/backend/sim_control/sim_control_test.cc b/modules/dreamview/backend/sim_control/sim_control_test.cc index 1bddeca2e5..ff10cd1cb6 100644 --- a/modules/dreamview/backend/sim_control/sim_control_test.cc +++ b/modules/dreamview/backend/sim_control/sim_control_test.cc @@ -116,13 +116,7 @@ TEST_F(SimControlTest, Test) { const double timestamp = 100.0; adc_trajectory.mutable_header()->set_timestamp_sec(timestamp); - RoutingResponse routing; - routing.mutable_routing_request()->mutable_start()->mutable_pose()->set_x( - 1.0); - routing.mutable_routing_request()->mutable_start()->mutable_pose()->set_y( - 1.0); - - sim_control_.SetStartPoint(routing); + sim_control_.SetStartPoint(1.0, 1.0); AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, &sim_control_); AdapterManager::GetPlanning()->OnReceive(adc_trajectory); -- GitLab