forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(geography_utils): add projection in geography_utils (autowarefou…
…ndation#4833) * feat(gnss_poser): Subscribe map_projector_info Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * update readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * small fix Signed-off-by: kminoda <[email protected]> * update commetn Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * add local cartesian Signed-off-by: kminoda <[email protected]> * update launch file Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * fix Signed-off-by: kminoda <[email protected]> * create new function for conversion of height Signed-off-by: kminoda <[email protected]> * minor update Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * update Signed-off-by: kminoda <[email protected]> * rename Signed-off-by: kminoda <[email protected]> * remove unnecessary include Signed-off-by: kminoda <[email protected]> * add projection in geography_utils Signed-off-by: kminoda <[email protected]> * update projection.cpp Signed-off-by: kminoda <[email protected]> * use projection in default_ad_api Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * Feat/gnss poser/subscribe map projector info (#24) * fix map origin Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove config file Signed-off-by: kminoda <[email protected]> * rfix readme Signed-off-by: kminoda <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * use projection.cpp in gnss_poser Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary files Signed-off-by: kminoda <[email protected]> * remove unnecessary parts Signed-off-by: kminoda <[email protected]> * remove lanelet2 dependency from default_ad_api Signed-off-by: kminoda <[email protected]> * remove geography_utils.hpp Signed-off-by: kminoda <[email protected]> * add get_lanelet2_projector and use that in whole system Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary parts Signed-off-by: kminoda <[email protected]> * revert using Point Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary files Signed-off-by: kminoda <[email protected]> * update test Signed-off-by: kminoda <[email protected]> * update comment Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * style(pre-commit): autofix * use constant string instead Signed-off-by: kminoda <[email protected]> * use reference Signed-off-by: kminoda <[email protected]> * fix test name Signed-off-by: kminoda <[email protected]> * fix bug Signed-off-by: kminoda <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando <[email protected]>
- Loading branch information
1 parent
fa8da4c
commit d339c09
Showing
15 changed files
with
330 additions
and
270 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
33 changes: 33 additions & 0 deletions
33
common/geography_utils/include/geography_utils/projection.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,33 @@ | ||
// Copyright 2023 TIER IV, Inc. | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#ifndef GEOGRAPHY_UTILS__PROJECTION_HPP_ | ||
#define GEOGRAPHY_UTILS__PROJECTION_HPP_ | ||
|
||
#include <geographic_msgs/msg/geo_point.hpp> | ||
#include <geometry_msgs/msg/point.hpp> | ||
#include <tier4_map_msgs/msg/map_projector_info.hpp> | ||
|
||
namespace geography_utils | ||
{ | ||
using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; | ||
using GeoPoint = geographic_msgs::msg::GeoPoint; | ||
using LocalPoint = geometry_msgs::msg::Point; | ||
|
||
LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); | ||
GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); | ||
|
||
} // namespace geography_utils | ||
|
||
#endif // GEOGRAPHY_UTILS__PROJECTION_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,95 @@ | ||
// Copyright 2023 TIER IV, Inc. | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#include <GeographicLib/Geoid.hpp> | ||
#include <geography_utils/lanelet2_projector.hpp> | ||
#include <geography_utils/projection.hpp> | ||
#include <lanelet2_extension/projection/mgrs_projector.hpp> | ||
|
||
namespace geography_utils | ||
{ | ||
|
||
Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) | ||
{ | ||
Eigen::Vector3d dst; | ||
dst.x() = src.x; | ||
dst.y() = src.y; | ||
dst.z() = src.z; | ||
return dst; | ||
} | ||
|
||
LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) | ||
{ | ||
std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info); | ||
lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; | ||
|
||
lanelet::BasicPoint3d projected_local_point; | ||
if (projector_info.projector_type == MapProjectorInfo::MGRS) { | ||
const int mgrs_precision = 9; // set precision as 100 micro meter | ||
const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get()); | ||
|
||
// project x and y using projector | ||
// note that the altitude is ignored in MGRS projection conventionally | ||
projected_local_point = mgrs_projector->forward(position, mgrs_precision); | ||
} else { | ||
// project x and y using projector | ||
// note that the original projector such as UTM projector does not compensate for the altitude | ||
// offset | ||
projected_local_point = projector->forward(position); | ||
|
||
// correct z based on the map origin | ||
// note that the converted altitude in local point is in the same vertical datum as the geo | ||
// point | ||
projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; | ||
} | ||
|
||
LocalPoint local_point; | ||
local_point.x = projected_local_point.x(); | ||
local_point.y = projected_local_point.y(); | ||
local_point.z = projected_local_point.z(); | ||
|
||
return local_point; | ||
} | ||
|
||
GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) | ||
{ | ||
std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info); | ||
|
||
lanelet::GPSPoint projected_gps_point; | ||
if (projector_info.projector_type == MapProjectorInfo::MGRS) { | ||
const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get()); | ||
// project latitude and longitude using projector | ||
// note that the z is ignored in MGRS projection conventionally | ||
projected_gps_point = | ||
mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); | ||
} else { | ||
// project latitude and longitude using projector | ||
// note that the original projector such as UTM projector does not compensate for the altitude | ||
// offset | ||
projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); | ||
|
||
// correct altitude based on the map origin | ||
// note that the converted altitude in local point is in the same vertical datum as the geo | ||
// point | ||
projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; | ||
} | ||
|
||
GeoPoint geo_point; | ||
geo_point.latitude = projected_gps_point.lat; | ||
geo_point.longitude = projected_gps_point.lon; | ||
geo_point.altitude = projected_gps_point.ele; | ||
return geo_point; | ||
} | ||
|
||
} // namespace geography_utils |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.