Skip to content

Commit

Permalink
feat: use constant string for map_projector_info (autowarefoundation#…
Browse files Browse the repository at this point in the history
…4789)

* feat: use constant string for map_projector_info

Signed-off-by: kminoda <[email protected]>

* style(pre-commit): autofix

* update

Signed-off-by: kminoda <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: kminoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
kminoda and pre-commit-ci[bot] authored Aug 29, 2023
1 parent c75a8eb commit 66c6e15
Show file tree
Hide file tree
Showing 4 changed files with 12 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -92,21 +92,22 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
const double & map_origin_lat, const double & map_origin_lon)
{
lanelet::ErrorMessages errors{};
if (lanelet2_map_projector_type == "MGRS") {
if (lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) {
lanelet::projection::MGRSProjector projector{};
const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
if (errors.empty()) {
return map;
}
} else if (lanelet2_map_projector_type == "LocalCartesianUTM") {
} else if (
lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
lanelet::Origin origin{position};
lanelet::projection::UtmProjector projector{origin};
const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
if (errors.empty()) {
return map;
}
} else if (lanelet2_map_projector_type == "local") {
} else if (lanelet2_map_projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
// Use MGRSProjector as parser
lanelet::projection::MGRSProjector projector{};
const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
Expand Down
6 changes: 3 additions & 3 deletions map/map_projection_loader/src/load_info_from_lanelet2_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,15 +46,15 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str

tier4_map_msgs::msg::MapProjectorInfo msg;
if (is_local) {
msg.projector_type = "local";
msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL;
} else {
msg.projector_type = "MGRS";
msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS;
msg.mgrs_grid = projector.getProjectedMGRSGrid();
}

// We assume that the vertical datum of the map is WGS84 when using lanelet2 map.
// However, do note that this is not always true, and may cause problems in the future.
// Thus, please consider using the map_projector_info.yaml instead of this deprecated function.
msg.vertical_datum = "WGS84";
msg.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84;
return msg;
}
6 changes: 3 additions & 3 deletions map/map_projection_loader/src/map_projection_loader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,14 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi

tier4_map_msgs::msg::MapProjectorInfo msg;
msg.projector_type = data["projector_type"].as<std::string>();
if (msg.projector_type == "MGRS") {
if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) {
msg.vertical_datum = data["vertical_datum"].as<std::string>();
msg.mgrs_grid = data["mgrs_grid"].as<std::string>();
} else if (msg.projector_type == "LocalCartesianUTM") {
} else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
msg.vertical_datum = data["vertical_datum"].as<std::string>();
msg.map_origin.latitude = data["map_origin"]["latitude"].as<double>();
msg.map_origin.longitude = data["map_origin"]["longitude"].as<double>();
} else if (msg.projector_type == "local") {
} else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
; // do nothing
} else {
throw std::runtime_error(
Expand Down
4 changes: 2 additions & 2 deletions system/default_ad_api/src/vehicle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,15 +146,15 @@ void VehicleNode::publish_kinematics()
vehicle_kinematics.twist.header = kinematic_state_msgs_->header;
vehicle_kinematics.twist.header.frame_id = kinematic_state_msgs_->child_frame_id;
vehicle_kinematics.twist.twist = kinematic_state_msgs_->twist;
if (map_projector_info_->projector_type == "MGRS") {
if (map_projector_info_->projector_type == MapProjectorInfo::MGRS) {
lanelet::GPSPoint projected_gps_point = lanelet::projection::MGRSProjector::reverse(
toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position), map_projector_info_->mgrs_grid);
vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header;
vehicle_kinematics.geographic_pose.header.frame_id = "global";
vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat;
vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon;
vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele;
} else if (map_projector_info_->projector_type == "LocalCartesianUTM") {
} else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
lanelet::GPSPoint position{
map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude};
lanelet::Origin origin{position};
Expand Down

0 comments on commit 66c6e15

Please sign in to comment.