diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt index feb49c42306fe..b705e819c059b 100644 --- a/common/geography_utils/CMakeLists.txt +++ b/common/geography_utils/CMakeLists.txt @@ -14,6 +14,7 @@ find_library(GeographicLib_LIBRARIES NAMES Geographic) ament_auto_add_library(geography_utils SHARED src/height.cpp + src/projection.cpp src/lanelet2_projector.cpp ) diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp index 1921e79699970..8bcda41f84cda 100644 --- a/common/geography_utils/include/geography_utils/height.hpp +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -15,10 +15,7 @@ #ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_ #define GEOGRAPHY_UTILS__HEIGHT_HPP_ -#include -#include #include -#include namespace geography_utils { diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp new file mode 100644 index 0000000000000..be616d854a466 --- /dev/null +++ b/common/geography_utils/include/geography_utils/projection.hpp @@ -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 +#include +#include + +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_ diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml index b9b31b0086646..414364e36ad29 100644 --- a/common/geography_utils/package.xml +++ b/common/geography_utils/package.xml @@ -10,7 +10,9 @@ ament_cmake_auto autoware_cmake + geographic_msgs geographiclib + geometry_msgs lanelet2_extension lanelet2_io tier4_map_msgs diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp new file mode 100644 index 0000000000000..ac9ce19052702 --- /dev/null +++ b/common/geography_utils/src/projection.cpp @@ -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 +#include +#include +#include + +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 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(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 projector = get_lanelet2_projector(projector_info); + + lanelet::GPSPoint projected_gps_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const auto mgrs_projector = dynamic_cast(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 diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/common/geography_utils/test/test_geography_utils.cpp similarity index 51% rename from sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp rename to common/geography_utils/test/test_geography_utils.cpp index acd21a83d96e4..480e17b8f49a4 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/common/geography_utils/test/test_geography_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -11,38 +11,16 @@ // 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 GNSS_POSER__GNSS_STAT_HPP_ -#define GNSS_POSER__GNSS_STAT_HPP_ -#include +#include "geography_utils/height.hpp" +#include "geography_utils/lanelet2_projector.hpp" +#include "geography_utils/projection.hpp" -namespace gnss_poser -{ -struct GNSSStat -{ - GNSSStat() - : east_north_up(true), - zone(0), - mgrs_zone(""), - x(0), - y(0), - z(0), - latitude(0), - longitude(0), - altitude(0) - { - } +#include - bool east_north_up; - int zone; - std::string mgrs_zone; - double x; - double y; - double z; - double latitude; - double longitude; - double altitude; -}; -} // namespace gnss_poser - -#endif // GNSS_POSER__GNSS_STAT_HPP_ +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp index 3d8a4c9c203fe..15297089ee131 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/geography_utils/test/test_height.cpp @@ -20,7 +20,7 @@ #include // Test case to verify if same source and target datums return original height -TEST(Tier4GeographyUtils, SameSourceTargetDatum) +TEST(GeographyUtils, SameSourceTargetDatum) { const double height = 10.0; const double latitude = 35.0; @@ -34,7 +34,7 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum) } // Test case to verify valid source and target datums -TEST(Tier4GeographyUtils, ValidSourceTargetDatum) +TEST(GeographyUtils, ValidSourceTargetDatum) { // Calculated with // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html @@ -50,7 +50,7 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum) } // Test case to verify invalid source and target datums -TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) +TEST(GeographyUtils, InvalidSourceTargetDatum) { const double height = 10.0; const double latitude = 35.0; @@ -62,7 +62,7 @@ TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) } // Test case to verify invalid source datums -TEST(Tier4GeographyUtils, InvalidSourceDatum) +TEST(GeographyUtils, InvalidSourceDatum) { const double height = 10.0; const double latitude = 35.0; @@ -74,7 +74,7 @@ TEST(Tier4GeographyUtils, InvalidSourceDatum) } // Test case to verify invalid target datums -TEST(Tier4GeographyUtils, InvalidTargetDatum) +TEST(GeographyUtils, InvalidTargetDatum) { const double height = 10.0; const double latitude = 35.0; @@ -84,9 +84,3 @@ TEST(Tier4GeographyUtils, InvalidTargetDatum) geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), std::invalid_argument); } - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/geography_utils/test/test_projection.cpp b/common/geography_utils/test/test_projection.cpp new file mode 100644 index 0000000000000..74ffb616cde6f --- /dev/null +++ b/common/geography_utils/test/test_projection.cpp @@ -0,0 +1,161 @@ +// 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 + +#include + +#include +#include + +TEST(GeographyUtilsProjection, ProjectForwardToMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) +{ + // source point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // target point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geographic_msgs::msg::GeoPoint converted_point = + geography_utils::project_reverse(local_point, projector_info); + + EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62406; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 0.0; + local_point.y = -22.18; + local_point.z = 20.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = -10.0; + + // conversion + const geometry_msgs::msg::Point converted_point = + geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + tier4_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.0; + projector_info.map_origin.longitude = 139.0; + projector_info.map_origin.altitude = 0.0; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index bd9886d376cad..167f2c51337ce 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -16,9 +16,7 @@ find_library(GeographicLib_LIBRARIES ) set(GNSS_POSER_HEADERS - include/gnss_poser/convert.hpp include/gnss_poser/gnss_poser_core.hpp - include/gnss_poser/gnss_stat.hpp ) ament_auto_add_library(gnss_poser_node SHARED diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp deleted file mode 100644 index 5cd0767f28130..0000000000000 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ /dev/null @@ -1,145 +0,0 @@ -// Copyright 2020 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 GNSS_POSER__CONVERT_HPP_ -#define GNSS_POSER__CONVERT_HPP_ - -#include "gnss_poser/gnss_stat.hpp" - -#include -#include -#include -#include -#include - -#include -#include - -#include - -namespace gnss_poser -{ -enum class MGRSPrecision { - _10_KILO_METER = 1, - _1_KILO_METER = 2, - _100_METER = 3, - _10_METER = 4, - _1_METER = 5, - _100_MIllI_METER = 6, - _10_MIllI_METER = 7, - _1_MIllI_METER = 8, - _100MICRO_METER = 9, -}; - -GNSSStat NavSatFix2UTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - const std::string & target_vertical_datum) -{ - GNSSStat utm; - - try { - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, - utm.y); - utm.z = geography_utils::convert_height( - nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", - target_vertical_datum); - utm.latitude = nav_sat_fix_msg.latitude; - utm.longitude = nav_sat_fix_msg.longitude; - utm.altitude = nav_sat_fix_msg.altitude; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); - } - return utm; -} - -GNSSStat NavSatFix2LocalCartesianUTM( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger, - const std::string & target_vertical_datum) -{ - GNSSStat utm_local; - try { - // origin of the local coordinate system in global frame - GNSSStat utm_origin; - GeographicLib::UTMUPS::Forward( - geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone, - utm_origin.east_north_up, utm_origin.x, utm_origin.y); - - utm_origin.z = geography_utils::convert_height( - geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84", - target_vertical_datum); - - // individual coordinates of global coordinate system - double global_x = 0.0; - double global_y = 0.0; - GeographicLib::UTMUPS::Forward( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, - utm_origin.east_north_up, global_x, global_y); - utm_local.latitude = nav_sat_fix_msg.latitude; - utm_local.longitude = nav_sat_fix_msg.longitude; - utm_local.altitude = nav_sat_fix_msg.altitude; - // individual coordinates of local coordinate system - utm_local.x = global_x - utm_origin.x; - utm_local.y = global_y - utm_origin.y; - utm_local.z = geography_utils::convert_height( - nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, - "WGS84", target_vertical_datum) - - utm_origin.z; - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); - } - return utm_local; -} - -GNSSStat UTM2MGRS( - const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) -{ - constexpr int GZD_ID_size = 5; // size of header like "53SPU" - - GNSSStat mgrs = utm; - try { - std::string mgrs_code; - GeographicLib::MGRS::Forward( - utm.zone, utm.east_north_up, utm.x, utm.y, utm.latitude, static_cast(precision), - mgrs_code); - mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); - mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.y = std::stod(mgrs_code.substr( - GZD_ID_size + static_cast(precision), static_cast(precision))) * - std::pow( - 10, static_cast(MGRSPrecision::_1_METER) - - static_cast(precision)); // set unit as [m] - mgrs.z = utm.z; // TODO(ryo.watanabe) - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); - } - return mgrs; -} - -GNSSStat NavSatFix2MGRS( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, const std::string & vertical_datum) -{ - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, vertical_datum); - const auto mgrs = UTM2MGRS(utm, precision, logger); - return mgrs; -} - -} // namespace gnss_poser - -#endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 05acfa967eb96..2118d33bc4b30 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -14,9 +14,6 @@ #ifndef GNSS_POSER__GNSS_POSER_CORE_HPP_ #define GNSS_POSER__GNSS_POSER_CORE_HPP_ -#include "gnss_poser/convert.hpp" -#include "gnss_poser/gnss_stat.hpp" - #include #include #include @@ -59,10 +56,6 @@ class GNSSPoser : public rclcpp::Node bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - GNSSStat convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - const MapProjectorInfo::Message & projector_info); - geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); geometry_msgs::msg::Point getMedianPosition( const boost::circular_buffer & position_buffer); geometry_msgs::msg::Point getAveragePosition( diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 9cd5a3ad34098..1ae8bce2ed7f2 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,6 +14,9 @@ #include "gnss_poser/gnss_poser_core.hpp" +#include +#include + #include #include @@ -93,8 +96,14 @@ void GNSSPoser::callbackNavSatFix( } // get position - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_); - const auto position = getPosition(gnss_stat); + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = nav_sat_fix_msg_ptr->latitude; + gps_point.longitude = nav_sat_fix_msg_ptr->longitude; + gps_point.altitude = nav_sat_fix_msg_ptr->altitude; + geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); + position.z = geography_utils::convert_height( + position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, + projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -197,36 +206,6 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - const MapProjectorInfo::Message & map_projector_info) -{ - GNSSStat gnss_stat; - if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { - gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(), - map_projector_info.vertical_datum); - } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) { - gnss_stat = NavSatFix2MGRS( - nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), - map_projector_info.vertical_datum); - } else { - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Unknown Projector type"); - } - return gnss_stat; -} - -geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) -{ - geometry_msgs::msg::Point point; - point.x = gnss_stat.x; - point.y = gnss_stat.y; - point.z = gnss_stat.z; - return point; -} - geometry_msgs::msg::Point GNSSPoser::getMedianPosition( const boost::circular_buffer & position_buffer) { diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index fd61d3c62e397..6a9a22f39cdc4 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,8 +23,8 @@ autoware_planning_msgs component_interface_specs component_interface_utils + geographic_msgs geography_utils - lanelet2_extension motion_utils nav_msgs rclcpp diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index c8f7df315c487..e25933e6ca2ad 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -15,6 +15,9 @@ #include "vehicle.hpp" #include +#include + +#include #include @@ -91,15 +94,6 @@ void VehicleNode::kinematic_state( kinematic_state_msgs_ = msg_ptr; } -Eigen::Vector3d VehicleNode::toBasicPoint3dPt(const geometry_msgs::msg::Point src) -{ - Eigen::Vector3d dst; - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - return dst; -} - void VehicleNode::acceleration_status( const localization_interface::Acceleration::Message::ConstSharedPtr msg_ptr) { @@ -148,30 +142,16 @@ 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 == 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 = geography_utils::convert_height( - projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, - map_projector_info_->vertical_datum, "WGS84"); - } 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}; - lanelet::projection::UtmProjector projector{origin}; - lanelet::GPSPoint projected_gps_point = - projector.reverse(toBasicPoint3dPt(kinematic_state_msgs_->pose.pose.position)); + if (map_projector_info_->projector_type != MapProjectorInfo::LOCAL) { + const geographic_msgs::msg::GeoPoint projected_gps_point = geography_utils::project_reverse( + kinematic_state_msgs_->pose.pose.position, *map_projector_info_); 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.latitude = projected_gps_point.latitude; + vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( - projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, - map_projector_info_->vertical_datum, "WGS84"); + projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, + map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = diff --git a/system/default_ad_api/src/vehicle.hpp b/system/default_ad_api/src/vehicle.hpp index 90f8b686206c5..56012152a132b 100644 --- a/system/default_ad_api/src/vehicle.hpp +++ b/system/default_ad_api/src/vehicle.hpp @@ -15,8 +15,6 @@ #ifndef VEHICLE_HPP_ #define VEHICLE_HPP_ -#include "lanelet2_extension/projection/mgrs_projector.hpp" - #include #include #include @@ -27,9 +25,6 @@ #include #include -#include -#include - #include // This file should be included after messages. @@ -83,7 +78,6 @@ class VehicleNode : public rclcpp::Node void publish_kinematics(); void publish_status(); void on_timer(); - Eigen::Vector3d toBasicPoint3dPt(const geometry_msgs::msg::Point src); }; } // namespace default_ad_api