提交 c076246c 编写于 作者: D Dong Li 提交者: Jiangtao Hu

migrate Point in map to pointENU

上级 7c1a8cc4
......@@ -21,7 +21,7 @@
namespace apollo {
namespace dreamview {
using ::apollo::hdmap::Point;
using ::apollo::common::PointENU;
using ::apollo::hdmap::Map;
using ::apollo::hdmap::Id;
using ::apollo::hdmap::LaneInfoConstPtr;
......@@ -124,7 +124,7 @@ MapService::MapService(const std::string &map_filename) {
AINFO << "HDMap loaded, Map: " << map_filename;
}
MapElementIds MapService::CollectMapElements(const Point &point,
MapElementIds MapService::CollectMapElements(const PointENU &point,
double radius) const {
MapElementIds result;
......
......@@ -62,7 +62,7 @@ struct MapElementIds {
class MapService {
public:
explicit MapService(const std::string &map_filename);
MapElementIds CollectMapElements(const apollo::hdmap::Point &point,
MapElementIds CollectMapElements(const apollo::common::PointENU &point,
double raidus) const;
// The returned value is of a ::apollo::hdmap::Map proto. This
......
......@@ -21,7 +21,7 @@
using ::apollo::hdmap::Id;
using ::apollo::hdmap::Map;
using ::apollo::hdmap::Point;
using ::apollo::common::PointENU;
using ::testing::UnorderedElementsAre;
namespace apollo {
......@@ -73,7 +73,9 @@ TEST_F(MapServiceTest, LoadMap) {
}
TEST_F(MapServiceTest, CollectMapElements) {
Point p;
PointENU p;
p.set_x(0.0);
p.set_y(0.0);
MapElementIds map_element_ids = map_service.CollectMapElements(p, 20000.0);
EXPECT_THAT(map_element_ids.lane, UnorderedElementsAre("l1"));
......
......@@ -297,7 +297,7 @@ Json SimulationWorldService::GetUpdateAsJson() const {
::google::protobuf::util::MessageToJsonString(world_, &sim_world_json);
// Gather required map element ids based on current location.
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(world_.auto_driving_car().position_x());
point.set_y(world_.auto_driving_car().position_y());
......
......@@ -39,7 +39,7 @@ typedef ::apollo::hdmap::ClearArea PbClearArea;
typedef ::apollo::hdmap::LineSegment PbLineSegment;
typedef ::apollo::hdmap::CurveSegment PbCurveSegment;
typedef ::apollo::hdmap::Curve PbCurve;
typedef ::apollo::hdmap::Point PbPoint3D;
typedef ::apollo::common::PointENU PbPoint3D;
typedef ::apollo::hdmap::Lane_LaneType PbLaneType;
typedef ::apollo::hdmap::Lane_LaneTurn PbTurnType;
typedef ::apollo::hdmap::Id PbID;
......
......@@ -58,55 +58,55 @@ RoadInfoConstPtr HDMap::get_road_by_id(const apollo::hdmap::Id& id) const {
return _impl.get_road_by_id(id);
}
int HDMap::get_lanes(const apollo::hdmap::Point& point,
int HDMap::get_lanes(const apollo::common::PointENU& point,
double distance,
std::vector<LaneInfoConstPtr>* lanes) const {
return _impl.get_lanes(point, distance, lanes);
}
int HDMap::get_junctions(const apollo::hdmap::Point& point,
int HDMap::get_junctions(const apollo::common::PointENU& point,
double distance,
std::vector<JunctionInfoConstPtr>* junctions) const {
return _impl.get_junctions(point, distance, junctions);
}
int HDMap::get_signals(const apollo::hdmap::Point& point,
int HDMap::get_signals(const apollo::common::PointENU& point,
double distance,
std::vector<SignalInfoConstPtr>* signals) const {
return _impl.get_signals(point, distance, signals);
}
int HDMap::get_crosswalks(const apollo::hdmap::Point& point,
int HDMap::get_crosswalks(const apollo::common::PointENU& point,
double distance,
std::vector<CrosswalkInfoConstPtr>* crosswalks) const {
return _impl.get_crosswalks(point, distance, crosswalks);
}
int HDMap::get_stop_signs(const apollo::hdmap::Point& point,
int HDMap::get_stop_signs(const apollo::common::PointENU& point,
double distance,
std::vector<StopSignInfoConstPtr>* stop_signs) const {
return _impl.get_stop_signs(point, distance, stop_signs);
}
int HDMap::get_yield_signs(const apollo::hdmap::Point& point,
int HDMap::get_yield_signs(const apollo::common::PointENU& point,
double distance,
std::vector<YieldSignInfoConstPtr>* yield_signs) const {
return _impl.get_yield_signs(point, distance, yield_signs);
}
int HDMap::get_roads(const apollo::hdmap::Point& point, double distance,
int HDMap::get_roads(const apollo::common::PointENU& point, double distance,
std::vector<RoadInfoConstPtr>* roads) const {
return _impl.get_roads(point, distance, roads);
}
int HDMap::get_nearest_lane(const ::apollo::hdmap::Point& point,
int HDMap::get_nearest_lane(const ::apollo::common::PointENU& point,
LaneInfoConstPtr* nearest_lane,
double* nearest_s,
double* nearest_l) const {
return _impl.get_nearest_lane(point, nearest_lane, nearest_s, nearest_l);
}
int HDMap::get_nearest_lane_with_heading(const apollo::hdmap::Point& point,
int HDMap::get_nearest_lane_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
......@@ -118,7 +118,7 @@ int HDMap::get_nearest_lane_with_heading(const apollo::hdmap::Point& point,
nearest_s, nearest_l);
}
int HDMap::get_lanes_with_heading(const apollo::hdmap::Point& point,
int HDMap::get_lanes_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
......@@ -127,7 +127,7 @@ int HDMap::get_lanes_with_heading(const apollo::hdmap::Point& point,
max_heading_difference, lanes);
}
int HDMap::get_road_boundaries(const apollo::hdmap::Point& point,
int HDMap::get_road_boundaries(const apollo::common::PointENU& point,
double radius,
std::vector<RoadROIBoundaryPtr>* road_boundaries,
std::vector<JunctionBoundaryPtr>* junctions) const {
......
......@@ -50,37 +50,37 @@ class HDMap {
OverlapInfoConstPtr get_overlap_by_id(const apollo::hdmap::Id& id) const;
RoadInfoConstPtr get_road_by_id(const apollo::hdmap::Id& id) const;
int get_lanes(const apollo::hdmap::Point& point, double distance,
int get_lanes(const apollo::common::PointENU& point, double distance,
std::vector<LaneInfoConstPtr>* lanes) const;
int get_junctions(const apollo::hdmap::Point& point, double distance,
int get_junctions(const apollo::common::PointENU& point, double distance,
std::vector<JunctionInfoConstPtr>* junctions) const;
int get_signals(const apollo::hdmap::Point& point, double distance,
int get_signals(const apollo::common::PointENU& point, double distance,
std::vector<SignalInfoConstPtr>* signals) const;
int get_crosswalks(const apollo::hdmap::Point& point, double distance,
int get_crosswalks(const apollo::common::PointENU& point, double distance,
std::vector<CrosswalkInfoConstPtr>* crosswalks) const;
int get_stop_signs(const apollo::hdmap::Point& point, double distance,
int get_stop_signs(const apollo::common::PointENU& point, double distance,
std::vector<StopSignInfoConstPtr>* stop_signs) const;
int get_yield_signs(const apollo::hdmap::Point& point, double distance,
int get_yield_signs(const apollo::common::PointENU& point, double distance,
std::vector<YieldSignInfoConstPtr>* yield_signs) const;
int get_roads(const apollo::hdmap::Point& point, double distance,
int get_roads(const apollo::common::PointENU& point, double distance,
std::vector<RoadInfoConstPtr>* roads) const;
int get_nearest_lane(const apollo::hdmap::Point& point,
int get_nearest_lane(const apollo::common::PointENU& point,
LaneInfoConstPtr* nearest_lane, double* nearest_s,
double* nearest_l) const;
int get_nearest_lane_with_heading(const apollo::hdmap::Point& point,
int get_nearest_lane_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
LaneInfoConstPtr* nearest_lane,
double* nearest_s,
double* nearest_l) const;
int get_lanes_with_heading(const apollo::hdmap::Point& point,
int get_lanes_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
std::vector<LaneInfoConstPtr>* lanes) const;
int get_road_boundaries(const apollo::hdmap::Point& point,
int get_road_boundaries(const apollo::common::PointENU& point,
double radius,
std::vector<RoadROIBoundaryPtr>* road_boundaries,
std::vector<JunctionBoundaryPtr>* junctions) const;
......
......@@ -83,15 +83,15 @@ void segments_from_curve(
}
}
apollo::hdmap::Point point_from_vec2d(
apollo::common::PointENU point_from_vec2d(
const apollo::common::math::Vec2d &point) {
apollo::hdmap::Point pt;
apollo::common::PointENU pt;
pt.set_x(point.x());
pt.set_y(point.y());
return pt;
}
apollo::common::math::Vec2d vec2d_from_point(
const apollo::hdmap::Point &point) {
const apollo::common::PointENU &point) {
return apollo::common::math::Vec2d(point.x(), point.y());
;
}
......@@ -220,7 +220,7 @@ bool LaneInfo::is_on_lane(const apollo::common::math::Box2d &box) const {
return true;
}
apollo::hdmap::Point LaneInfo::get_smooth_point(double s) const {
apollo::common::PointENU LaneInfo::get_smooth_point(double s) const {
CHECK_GE(_points.size(), 2);
if (s <= 0.0) {
return point_from_vec2d(_points[0]);
......@@ -261,7 +261,7 @@ double LaneInfo::distance_to(const apollo::common::math::Vec2d &point,
return distance;
}
apollo::hdmap::Point LaneInfo::get_nearest_point(
apollo::common::PointENU LaneInfo::get_nearest_point(
const apollo::common::math::Vec2d &point, double *distance) const {
const auto segment_box = _lane_segment_kdtree->GetNearestObject(point);
int index = segment_box->id();
......
......@@ -131,12 +131,12 @@ class LaneInfo {
bool is_on_lane(const apollo::common::math::Vec2d &point) const;
bool is_on_lane(const apollo::common::math::Box2d &box) const;
apollo::hdmap::Point get_smooth_point(double s) const;
apollo::common::PointENU get_smooth_point(double s) const;
double distance_to(const apollo::common::math::Vec2d &point) const;
double distance_to(const apollo::common::math::Vec2d &point,
apollo::common::math::Vec2d *map_point, double *s_offset,
int *s_offset_index) const;
apollo::hdmap::Point get_nearest_point(
apollo::common::PointENU get_nearest_point(
const apollo::common::math::Vec2d &point, double *distance) const;
bool get_projection(const apollo::common::math::Vec2d &point,
double *accumulate_s, double *lateral) const;
......
......@@ -45,7 +45,7 @@ void HDMapCommonTestSuite::init_lane_obj(apollo::hdmap::Lane* lane) {
lane->mutable_central_curve()->add_segment();
apollo::hdmap::LineSegment* line_segment =
curve_segment->mutable_line_segment();
apollo::hdmap::Point* pt = line_segment->add_point();
apollo::common::PointENU* pt = line_segment->add_point();
pt->set_x(1.0);
pt->set_y(1.0);
pt = line_segment->add_point();
......@@ -97,7 +97,7 @@ void HDMapCommonTestSuite::init_junction_obj(
apollo::hdmap::Junction* junction) {
junction->mutable_id()->set_id("junction_1");
apollo::hdmap::Polygon* polygon = junction->mutable_polygon();
apollo::hdmap::Point* pt = polygon->add_point();
apollo::common::PointENU* pt = polygon->add_point();
pt->set_x(1.0);
pt->set_y(1.0);
pt = polygon->add_point();
......@@ -129,7 +129,7 @@ void HDMapCommonTestSuite::init_junction_obj(
void HDMapCommonTestSuite::init_signal_obj(apollo::hdmap::Signal* signal) {
signal->mutable_id()->set_id("signal_1");
apollo::hdmap::Polygon* polygon = signal->mutable_boundary();
apollo::hdmap::Point* pt = polygon->add_point();
apollo::common::PointENU* pt = polygon->add_point();
pt->set_x(1.0);
pt->set_y(1.0);
pt->set_z(1.0);
......@@ -201,7 +201,7 @@ void HDMapCommonTestSuite::init_crosswalk_obj(
apollo::hdmap::Crosswalk* crosswalk) {
crosswalk->mutable_id()->set_id("crosswalk_1");
apollo::hdmap::Polygon* polygon = crosswalk->mutable_polygon();
apollo::hdmap::Point* pt = polygon->add_point();
apollo::common::PointENU* pt = polygon->add_point();
pt->set_x(0.0);
pt->set_y(0.0);
pt->set_z(0.0);
......@@ -225,7 +225,7 @@ void HDMapCommonTestSuite::init_stop_sign_obj(
stop_sign->mutable_stop_line()->add_segment();
apollo::hdmap::LineSegment* line_segment =
curve_segment->mutable_line_segment();
apollo::hdmap::Point* pt = line_segment->add_point();
apollo::common::PointENU* pt = line_segment->add_point();
pt->set_x(0.0);
pt->set_y(0.0);
pt->set_z(0.0);
......@@ -245,7 +245,7 @@ void HDMapCommonTestSuite::init_yield_sign_obj(
yield_sign->mutable_stop_line()->add_segment();
apollo::hdmap::LineSegment* line_segment =
curve_segment->mutable_line_segment();
apollo::hdmap::Point* pt = line_segment->add_point();
apollo::common::PointENU* pt = line_segment->add_point();
pt->set_x(0.0);
pt->set_y(0.0);
pt->set_z(0.0);
......
......@@ -138,7 +138,7 @@ RoadInfoConstPtr HDMapImpl::get_road_by_id(const apollo::hdmap::Id& id) const {
return it != _road_table.end() ? it->second : nullptr;
}
int HDMapImpl::get_lanes(const apollo::hdmap::Point& point,
int HDMapImpl::get_lanes(const apollo::common::PointENU& point,
double distance,
std::vector<LaneInfoConstPtr>* lanes) const {
return get_lanes({point.x(), point.y()}, distance, lanes);
......@@ -165,7 +165,7 @@ int HDMapImpl::get_lanes(const apollo::common::math::Vec2d &point,
return 0;
}
int HDMapImpl::get_roads(const apollo::hdmap::Point& point,
int HDMapImpl::get_roads(const apollo::common::PointENU& point,
double distance,
std::vector<RoadInfoConstPtr>* roads) const {
return get_roads({point.x(), point.y()}, distance, roads);
......@@ -192,7 +192,7 @@ int HDMapImpl::get_roads(const apollo::common::math::Vec2d& point,
return 0;
}
int HDMapImpl::get_junctions(const apollo::hdmap::Point& point,
int HDMapImpl::get_junctions(const apollo::common::PointENU& point,
double distance,
std::vector<JunctionInfoConstPtr>* junctions) const {
return get_junctions({point.x(), point.y()}, distance, junctions);
......@@ -217,7 +217,7 @@ int HDMapImpl::get_junctions(const apollo::common::math::Vec2d& point,
return 0;
}
int HDMapImpl::get_signals(const apollo::hdmap::Point& point,
int HDMapImpl::get_signals(const apollo::common::PointENU& point,
double distance,
std::vector<SignalInfoConstPtr>* signals) const {
return get_signals({point.x(), point.y()}, distance, signals);
......@@ -242,7 +242,7 @@ int HDMapImpl::get_signals(const apollo::common::math::Vec2d& point,
return 0;
}
int HDMapImpl::get_crosswalks(const apollo::hdmap::Point& point,
int HDMapImpl::get_crosswalks(const apollo::common::PointENU& point,
double distance,
std::vector<CrosswalkInfoConstPtr>* crosswalks) const {
return get_crosswalks({point.x(), point.y()}, distance, crosswalks);
......@@ -267,7 +267,7 @@ int HDMapImpl::get_crosswalks(const apollo::common::math::Vec2d& point,
return 0;
}
int HDMapImpl::get_stop_signs(const apollo::hdmap::Point& point,
int HDMapImpl::get_stop_signs(const apollo::common::PointENU& point,
double distance,
std::vector<StopSignInfoConstPtr>* stop_signs) const {
return get_stop_signs({point.x(), point.y()}, distance, stop_signs);
......@@ -292,7 +292,7 @@ int HDMapImpl::get_stop_signs(const apollo::common::math::Vec2d& point,
return 0;
}
int HDMapImpl::get_yield_signs(const apollo::hdmap::Point& point,
int HDMapImpl::get_yield_signs(const apollo::common::PointENU& point,
double distance,
std::vector<YieldSignInfoConstPtr>* yield_signs) const {
return get_yield_signs({point.x(), point.y()}, distance, yield_signs);
......@@ -415,7 +415,7 @@ int HDMapImpl::search_objects(const apollo::common::math::Vec2d& center,
return 0;
}
int HDMapImpl::get_nearest_lane(const apollo::hdmap::Point& point,
int HDMapImpl::get_nearest_lane(const apollo::common::PointENU& point,
LaneInfoConstPtr* nearest_lane,
double* nearest_s,
double* nearest_l) const {
......@@ -448,7 +448,7 @@ int HDMapImpl::get_nearest_lane(const apollo::common::math::Vec2d &point,
return 0;
}
int HDMapImpl::get_nearest_lane_with_heading(const apollo::hdmap::Point& point,
int HDMapImpl::get_nearest_lane_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
......@@ -509,7 +509,7 @@ int HDMapImpl::get_nearest_lane_with_heading(
return 0;
}
int HDMapImpl::get_lanes_with_heading(const apollo::hdmap::Point& point,
int HDMapImpl::get_lanes_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
......@@ -550,7 +550,7 @@ int HDMapImpl::get_lanes_with_heading(const apollo::common::math::Vec2d &point,
return 0;
}
int HDMapImpl::get_road_boundaries(const apollo::hdmap::Point& point,
int HDMapImpl::get_road_boundaries(const apollo::common::PointENU& point,
double radius,
std::vector<RoadROIBoundaryPtr>* road_boundaries,
std::vector<JunctionBoundaryPtr>* junctions) const {
......
......@@ -71,44 +71,44 @@ class HDMapImpl {
OverlapInfoConstPtr get_overlap_by_id(const Id& id) const;
RoadInfoConstPtr get_road_by_id(const apollo::hdmap::Id& id) const;
int get_lanes(const apollo::hdmap::Point& point,
int get_lanes(const apollo::common::PointENU& point,
double distance,
std::vector<LaneInfoConstPtr>* lanes) const;
int get_junctions(const apollo::hdmap::Point& point,
int get_junctions(const apollo::common::PointENU& point,
double distance,
std::vector<JunctionInfoConstPtr>* junctions) const;
int get_crosswalks(const apollo::hdmap::Point& point,
int get_crosswalks(const apollo::common::PointENU& point,
double distance,
std::vector<CrosswalkInfoConstPtr>* crosswalks) const;
int get_signals(const apollo::hdmap::Point& point,
int get_signals(const apollo::common::PointENU& point,
double distance,
std::vector<SignalInfoConstPtr>* signals) const;
int get_stop_signs(const apollo::hdmap::Point& point,
int get_stop_signs(const apollo::common::PointENU& point,
double distance,
std::vector<StopSignInfoConstPtr>* stop_signs) const;
int get_yield_signs(const apollo::hdmap::Point& point,
int get_yield_signs(const apollo::common::PointENU& point,
double distance,
std::vector<YieldSignInfoConstPtr>* yield_signs) const;
int get_nearest_lane(const apollo::hdmap::Point& point,
int get_nearest_lane(const apollo::common::PointENU& point,
LaneInfoConstPtr* nearest_lane,
double* nearest_s,
double* nearest_l) const;
int get_nearest_lane_with_heading(const apollo::hdmap::Point& point,
int get_nearest_lane_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
LaneInfoConstPtr* nearest_lane,
double* nearest_s,
double* nearest_l) const;
int get_lanes_with_heading(const apollo::hdmap::Point& point,
int get_lanes_with_heading(const apollo::common::PointENU& point,
const double distance,
const double central_heading,
const double max_heading_difference,
std::vector<LaneInfoConstPtr>* lanes) const;
int get_roads(const apollo::hdmap::Point& point, double distance,
int get_roads(const apollo::common::PointENU& point, double distance,
std::vector<RoadInfoConstPtr>* roads) const;
int get_road_boundaries(const apollo::hdmap::Point& point,
int get_road_boundaries(const apollo::common::PointENU& point,
double radius,
std::vector<RoadROIBoundaryPtr>* road_boundaries,
std::vector<JunctionBoundaryPtr>* junctions) const;
......
......@@ -158,7 +158,7 @@ TEST_F(HDMapImplTestSuite, get_road_by_id) {
TEST_F(HDMapImplTestSuite, get_lanes) {
initial_context();
std::vector<LaneInfoConstPtr> lanes;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(0.0);
point.set_y(0.0);
point.set_z(0.0);
......@@ -179,7 +179,7 @@ TEST_F(HDMapImplTestSuite, get_lanes) {
TEST_F(HDMapImplTestSuite, get_nearest_lane_with_heading) {
initial_context();
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(0.0);
point.set_y(0.0);
point.set_z(0.0);
......@@ -199,7 +199,7 @@ TEST_F(HDMapImplTestSuite, get_nearest_lane_with_heading) {
TEST_F(HDMapImplTestSuite, get_lanes_with_heading) {
initial_context();
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(0.0);
point.set_y(0.0);
point.set_z(0.0);
......@@ -217,7 +217,7 @@ TEST_F(HDMapImplTestSuite, get_lanes_with_heading) {
TEST_F(HDMapImplTestSuite, get_junctions) {
initial_context();
std::vector<JunctionInfoConstPtr> junctions;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(-36.0);
point.set_y(-28.0);
point.set_z(0.0);
......@@ -231,7 +231,7 @@ TEST_F(HDMapImplTestSuite, get_junctions) {
TEST_F(HDMapImplTestSuite, get_crosswalks) {
initial_context();
std::vector<CrosswalkInfoConstPtr> crosswalks;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(199.0);
point.set_y(-440.0);
point.set_z(0.0);
......@@ -246,7 +246,7 @@ TEST_F(HDMapImplTestSuite, get_signals) {
initial_context();
std::vector<SignalInfoConstPtr> signals;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(-250.0);
point.set_y(405.0);
point.set_z(0.0);
......@@ -263,7 +263,7 @@ TEST_F(HDMapImplTestSuite, get_stop_signs) {
initial_context();
std::vector<StopSignInfoConstPtr> stop_signs;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(0.0);
point.set_y(0.0);
point.set_z(0.0);
......@@ -276,7 +276,7 @@ TEST_F(HDMapImplTestSuite, get_yield_signs) {
initial_context();
std::vector<YieldSignInfoConstPtr> yield_signs;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(0.0);
point.set_y(0.0);
point.set_z(0.0);
......@@ -288,7 +288,7 @@ TEST_F(HDMapImplTestSuite, get_roads) {
initial_boundary_context();
std::vector<RoadInfoConstPtr> roads;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(435730.0);
point.set_y(4436777.0);
point.set_z(0.0);
......@@ -318,7 +318,7 @@ TEST_F(HDMapImplTestSuite, get_nearest_lane) {
double s = 0.0;
double l = 0.0;
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(2.5);
point.set_y(-20.0);
......@@ -339,7 +339,7 @@ TEST_F(HDMapImplTestSuite, get_nearest_lane) {
TEST_F(HDMapImplTestSuite, get_road_boundaries) {
initial_boundary_context();
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(435798.0);
point.set_y(4436768.0);
point.set_z(0.0);
......
......@@ -29,7 +29,7 @@
using Id = apollo::hdmap::Id;
using Lane = apollo::hdmap::Lane;
using LaneSampleAssociation = apollo::hdmap::LaneSampleAssociation;
using Point = apollo::hdmap::Point;
using Point = apollo::common::PointENU;
using RoutingRequest = apollo::hdmap::RoutingRequest;
using RoutingResult = apollo::hdmap::RoutingResult;
using MapPathPoint = apollo::hdmap::MapPathPoint;
......
syntax = "proto2";
package apollo.hdmap;
import "modules/common/proto/geometry.proto";
// 3D points.
// TODO: Use adu.common.PointENU instead of this.
message Point {
optional double x = 1; // in meters.
optional double y = 2; // in meters.
optional double z = 3; // height in meters.
}
package apollo.hdmap;
// Polygon, not necessary convex.
message Polygon {
repeated Point point = 1;
repeated apollo.common.PointENU point = 1;
}
// Straight line segment.
message LineSegment {
repeated Point point = 1;
repeated apollo.common.PointENU point = 1;
}
message Arc {
optional Point center = 1;
optional apollo.common.PointENU center = 1;
optional double radius = 2;
optional double start_angle = 3;
optional double end_angle = 4;
......@@ -37,7 +31,7 @@ message Spline {
optional int32 dimension = 1;
optional int32 degree = 2;
repeated double knot = 3 [packed = true];
repeated Point control = 4;
repeated apollo.common.PointENU control = 4;
}
message Poly3 {
......@@ -54,7 +48,7 @@ message CurveSegment {
Poly3 poly3 = 5;
}
optional double s = 6; // start position (s-coordinate)
optional Point start_position = 7;
optional apollo.common.PointENU start_position = 7;
optional double heading = 8; // start orientation
optional double length = 9;
}
......
......@@ -2,6 +2,7 @@ syntax = "proto2";
package apollo.hdmap;
import "modules/common/proto/geometry.proto";
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
......@@ -21,7 +22,7 @@ message Subsignal {
optional Type type = 2;
// Location of the center of the bulb. now no data support.
optional Point location = 3;
optional apollo.common.PointENU location = 3;
}
message Signal {
......
......@@ -2,6 +2,7 @@ syntax = "proto2";
package apollo.hdmap;
import "modules/common/proto/geometry.proto";
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
......@@ -22,7 +23,7 @@ message StopSign {
optional Curve stop_line = 2;
// Location of the stand point.
optional Point location = 3;
optional apollo.common.PointENU location = 3;
repeated Id overlap_id = 4;
......
......@@ -2,6 +2,7 @@ syntax = "proto2";
package apollo.hdmap;
import "modules/common/proto/geometry.proto";
import "modules/map/proto/map_geometry.proto";
import "modules/map/proto/map_id.proto";
......@@ -13,7 +14,7 @@ message YieldSign {
optional Curve stop_line = 2;
// Location of the stand point.
optional Point location = 3;
optional apollo.common.PointENU location = 3;
repeated Id overlap_id = 4;
}
......@@ -67,7 +67,7 @@ bool HDMapInput::GetROI(const PointD& pointd, HdmapStructPtr* mapptr) {
if (mapptr != NULL && *mapptr == nullptr) {
(*mapptr).reset(new HdmapStruct);
}
apollo::hdmap::Point point;
apollo::common::PointENU point;
point.set_x(pointd.x);
point.set_y(pointd.y);
point.set_z(pointd.z);
......
......@@ -77,7 +77,7 @@ const PublishableTrajectory &Frame::computed_trajectory() const {
}
bool Frame::CreateReferenceLineFromRouting() {
hdmap::Point vehicle_position;
common::PointENU vehicle_position;
vehicle_position.set_x(init_pose_.position().x());
vehicle_position.set_y(init_pose_.position().y());
vehicle_position.set_z(init_pose_.position().z());
......
......@@ -157,7 +157,7 @@ bool RoutingHelper::validate_routing(
}
bool RoutingHelper::CreatePathFromRouting(const hdmap::RoutingResult &routing,
const hdmap::Point &point,
const common::PointENU &point,
const double backward_length,
const double forward_length,
hdmap::Path *path) const {
......@@ -193,7 +193,7 @@ bool RoutingHelper::CreatePathFromRouting(const hdmap::RoutingResult &routing,
continue;
}
double distance = 0.0;
hdmap::Point map_point =
common::PointENU map_point =
lane->get_nearest_point({point.x(), point.y()}, &distance);
if (distance < min_distance) {
min_distance = distance;
......
......@@ -38,7 +38,7 @@ class RoutingHelper {
void SetMap(const hdmap::HDMap *map_ptr);
~RoutingHelper() = default;
bool CreatePathFromRouting(const hdmap::RoutingResult &routing,
const hdmap::Point &point,
const common::PointENU &point,
const double backward_length,
const double forward_length,
hdmap::Path *path) const;
......
......@@ -68,6 +68,7 @@ TEST_F(PathSamplerTest, sample_one_point) {
}
TEST_F(PathSamplerTest, sample_three_points) {
ReferenceLine ref = *reference_line_;
dp_poly_path_config_.set_sample_points_num_each_level(3);
PathSampler sampler(dp_poly_path_config_);
bool sample_result =
......
......@@ -89,7 +89,7 @@ Status StBoundaryMapper::get_speed_limits(
const double default_speed_limit, SpeedLimit* const speed_limit_data) {
const auto& adc_position = pose.position();
apollo::hdmap::Point adc_point;
apollo::common::PointENU adc_point;
adc_point.set_x(adc_position.x());
adc_point.set_y(adc_position.y());
adc_point.set_z(adc_position.z());
......
......@@ -55,7 +55,7 @@ Id PredictionMap::id(const std::string& str_id) {
Eigen::Vector2d PredictionMap::PositionOnLane(const LaneInfo* lane_info,
const double s) {
apollo::hdmap::Point point = lane_info->get_smooth_point(s);
apollo::common::PointENU point = lane_info->get_smooth_point(s);
return {point.x(), point.y()};
}
......@@ -115,7 +115,7 @@ bool PredictionMap::ProjectionFromLane(const LaneInfo* lane_info_ptr, double s,
if (lane_info_ptr == nullptr) {
return false;
}
apollo::hdmap::Point point = lane_info_ptr->get_smooth_point(s);
apollo::common::PointENU point = lane_info_ptr->get_smooth_point(s);
double heading = HeadingOnLane(lane_info_ptr, s);
path_point->set_x(point.x());
path_point->set_y(point.y());
......@@ -129,7 +129,7 @@ void PredictionMap::OnLane(const std::vector<const LaneInfo*>& prev_lanes,
std::vector<const LaneInfo*>* lanes) {
std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
// TODO(kechxu) clean the messy code of this function
apollo::hdmap::Point hdmap_point;
apollo::common::PointENU hdmap_point;
hdmap_point.set_x(point[0]);
hdmap_point.set_y(point[1]);
apollo::common::math::Vec2d vec_point;
......@@ -151,7 +151,7 @@ void PredictionMap::OnLane(const std::vector<const LaneInfo*>& prev_lanes,
continue;
} else {
double distance = 0.0;
apollo::hdmap::Point nearest_point =
apollo::common::PointENU nearest_point =
candidate_lane->get_nearest_point(vec_point, &distance);
double nearest_point_heading =
PathHeading(candidate_lane.get(), nearest_point);
......@@ -165,7 +165,7 @@ void PredictionMap::OnLane(const std::vector<const LaneInfo*>& prev_lanes,
}
double PredictionMap::PathHeading(const LaneInfo* lane_info_ptr,
const apollo::hdmap::Point& point) {
const apollo::common::PointENU& point) {
apollo::common::math::Vec2d vec_point;
vec_point.set_x(point.x());
vec_point.set_y(point.y());
......@@ -184,7 +184,7 @@ int PredictionMap::SmoothPointFromLane(const apollo::hdmap::Id& id,
return -1;
}
const LaneInfo* lane = LaneById(id);
apollo::hdmap::Point hdmap_point = lane->get_smooth_point(s);
apollo::common::PointENU hdmap_point = lane->get_smooth_point(s);
*heading = PathHeading(lane, hdmap_point);
point->operator[](0) = hdmap_point.x() - std::sin(*heading) * l;
point->operator[](1) = hdmap_point.y() + std::cos(*heading) * l;
......
......@@ -67,7 +67,7 @@ class PredictionMap {
std::vector<const apollo::hdmap::LaneInfo*>* lanes);
double PathHeading(const apollo::hdmap::LaneInfo* lane_info_ptr,
const apollo::hdmap::Point& point);
const apollo::common::PointENU& point);
int SmoothPointFromLane(const apollo::hdmap::Id& id, const double s,
const double l, Eigen::Vector2d* point,
......
......@@ -777,7 +777,7 @@ void Obstacle::SetCurrentLanes(Feature* feature) {
// TODO(kechxu) clean the follow code
apollo::common::math::Vec2d vec_point(point[0], point[1]);
double distance = 0.0;
apollo::hdmap::Point nearest_point =
apollo::common::PointENU nearest_point =
current_lane->get_nearest_point(vec_point, &distance);
double nearest_point_heading =
map->PathHeading(current_lane, nearest_point);
......
......@@ -91,7 +91,7 @@ class MapUtil {
return ret.get();
}
int point_to_sl(const ::apollo::hdmap::Point &point,
int point_to_sl(const ::apollo::common::PointENU &point,
std::string *lane_id, double *s, double *l) {
QUIT_IF(lane_id == nullptr, -1, ERROR, "arg lane id is null");
QUIT_IF(s == nullptr, -2, ERROR, "arg s is null");
......@@ -105,7 +105,7 @@ class MapUtil {
}
int sl_to_point(const std::string &lane_id, const double s,
const double l, ::apollo::hdmap::Point *point,
const double l, ::apollo::common::PointENU *point,
double *heading) {
QUIT_IF(point == nullptr, -1, ERROR, "arg point is null");
QUIT_IF(heading == nullptr, -2, ERROR, "arg heading is null");
......@@ -166,7 +166,7 @@ int main(int argc, char *argv[]) {
if (FLAGS_xy_to_sl) {
double x = FLAGS_x;
double y = FLAGS_y;
::apollo::hdmap::Point point;
::apollo::common::PointENU point;
point.set_x(x);
point.set_y(y);
point.set_z(0);
......@@ -180,7 +180,7 @@ int main(int argc, char *argv[]) {
lane_id.c_str(), s, l, heading);
}
if (FLAGS_sl_to_xy) {
::apollo::hdmap::Point point;
::apollo::common::PointENU point;
double heading = 0.0;
map_util.sl_to_point(FLAGS_lane, FLAGS_s, FLAGS_l, &point, &heading);
printf("x[%f] y[%f], heading[%f]\n", point.x(), point.y(), heading);
......@@ -198,7 +198,7 @@ int main(int argc, char *argv[]) {
printf("lane[%s] s[%f], l[%f]\n", FLAGS_lane.c_str(), s, l);
}
if (FLAGS_lane_to_lane) {
::apollo::hdmap::Point point;
::apollo::common::PointENU point;
double heading = 0.0;
map_util.sl_to_point(FLAGS_from_lane, FLAGS_s, 0.0, &point, &heading);
double target_s = 0.0;
......@@ -227,11 +227,11 @@ int main(int argc, char *argv[]) {
const auto *lane_ptr = map_util.get_lane(FLAGS_lane_info);
const auto &lane = lane_ptr->lane();
::apollo::hdmap::Point start_point;
::apollo::common::PointENU start_point;
double start_heading = 0.0;
map_util.sl_to_point(FLAGS_lane_info, 0, 0, &start_point, &start_heading);
::apollo::hdmap::Point end_point;
::apollo::common::PointENU end_point;
double end_heading = 0.0;
map_util.sl_to_point(FLAGS_lane_info, lane_ptr->total_length(), 0,
&end_point, &end_heading);
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册