diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index caeebe87193cf..652d9b0597cd6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,6 +1,7 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_geography_utils/** koji.minoda@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai @@ -17,7 +18,6 @@ common/component_interface_tools/** isamu.takagi@tier4.jp common/component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/geography_utils/** koji.minoda@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp diff --git a/common/geography_utils/CMakeLists.txt b/common/autoware_geography_utils/CMakeLists.txt similarity index 70% rename from common/geography_utils/CMakeLists.txt rename to common/autoware_geography_utils/CMakeLists.txt index b705e819c059b..b4ab5c2f74494 100644 --- a/common/geography_utils/CMakeLists.txt +++ b/common/autoware_geography_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(geography_utils) +project(autoware_geography_utils) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,13 +12,13 @@ find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) find_library(GeographicLib_LIBRARIES NAMES Geographic) -ament_auto_add_library(geography_utils SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/height.cpp src/projection.cpp src/lanelet2_projector.cpp ) -target_link_libraries(geography_utils +target_link_libraries(${PROJECT_NAME} ${GeographicLib_LIBRARIES} ) @@ -27,10 +27,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_geography_utils ${test_files}) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - target_link_libraries(test_geography_utils - geography_utils + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} ) endif() diff --git a/common/geography_utils/README.md b/common/autoware_geography_utils/README.md similarity index 100% rename from common/geography_utils/README.md rename to common/autoware_geography_utils/README.md diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp similarity index 83% rename from common/geography_utils/include/geography_utils/height.hpp rename to common/autoware_geography_utils/include/autoware/geography_utils/height.hpp index 8bcda41f84cda..1f205eb8f8b18 100644 --- a/common/geography_utils/include/geography_utils/height.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_ -#define GEOGRAPHY_UTILS__HEIGHT_HPP_ +#ifndef AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ #include -namespace geography_utils +namespace autoware::geography_utils { typedef double (*HeightConversionFunction)( @@ -28,6 +28,6 @@ double convert_height( const double height, const double latitude, const double longitude, const std::string & source_vertical_datum, const std::string & target_vertical_datum); -} // namespace geography_utils +} // namespace autoware::geography_utils -#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ +#endif // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp similarity index 77% rename from common/geography_utils/include/geography_utils/lanelet2_projector.hpp rename to common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp index 739abe7019023..7f6b2d78701d2 100644 --- a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ -#define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#ifndef AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ #include @@ -21,12 +21,12 @@ #include -namespace geography_utils +namespace autoware::geography_utils { using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); -} // namespace geography_utils +} // namespace autoware::geography_utils -#endif // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#endif // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp similarity index 82% rename from common/geography_utils/include/geography_utils/projection.hpp rename to common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp index be616d854a466..5ad605f95f65b 100644 --- a/common/geography_utils/include/geography_utils/projection.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GEOGRAPHY_UTILS__PROJECTION_HPP_ -#define GEOGRAPHY_UTILS__PROJECTION_HPP_ +#ifndef AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ #include #include #include -namespace geography_utils +namespace autoware::geography_utils { using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; using GeoPoint = geographic_msgs::msg::GeoPoint; @@ -28,6 +28,6 @@ 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 +} // namespace autoware::geography_utils -#endif // GEOGRAPHY_UTILS__PROJECTION_HPP_ +#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/geography_utils/package.xml b/common/autoware_geography_utils/package.xml similarity index 88% rename from common/geography_utils/package.xml rename to common/autoware_geography_utils/package.xml index bf2356a7fabf5..bd3aa79bde8c2 100644 --- a/common/geography_utils/package.xml +++ b/common/autoware_geography_utils/package.xml @@ -1,9 +1,9 @@ - geography_utils + autoware_geography_utils 0.1.0 - The geography_utils package + The autoware_geography_utils package Koji Minoda Apache License 2.0 diff --git a/common/geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp similarity index 94% rename from common/geography_utils/src/height.cpp rename to common/autoware_geography_utils/src/height.cpp index fe69557f25a76..745dbf5b22cfc 100644 --- a/common/geography_utils/src/height.cpp +++ b/common/autoware_geography_utils/src/height.cpp @@ -13,14 +13,14 @@ // limitations under the License. #include -#include +#include #include #include #include #include -namespace geography_utils +namespace autoware::geography_utils { double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) @@ -60,4 +60,4 @@ double convert_height( } } -} // namespace geography_utils +} // namespace autoware::geography_utils diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp similarity index 94% rename from common/geography_utils/src/lanelet2_projector.cpp rename to common/autoware_geography_utils/src/lanelet2_projector.cpp index ea32e08cd157a..7de0935a3aa4e 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/autoware_geography_utils/src/lanelet2_projector.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include +#include #include #include -#include #include -namespace geography_utils +namespace autoware::geography_utils { std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) @@ -51,4 +51,4 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf throw std::invalid_argument(error_msg); } -} // namespace geography_utils +} // namespace autoware::geography_utils diff --git a/common/geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp similarity index 95% rename from common/geography_utils/src/projection.cpp rename to common/autoware_geography_utils/src/projection.cpp index d32252c28f5c0..3ab18b1d31698 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/autoware_geography_utils/src/projection.cpp @@ -13,11 +13,11 @@ // limitations under the License. #include +#include +#include #include -#include -#include -namespace geography_utils +namespace autoware::geography_utils { Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) @@ -92,4 +92,4 @@ GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo return geo_point; } -} // namespace geography_utils +} // namespace autoware::geography_utils diff --git a/common/geography_utils/test/test_geography_utils.cpp b/common/autoware_geography_utils/test/test_geography_utils.cpp similarity index 82% rename from common/geography_utils/test/test_geography_utils.cpp rename to common/autoware_geography_utils/test/test_geography_utils.cpp index 480e17b8f49a4..ee0e7428db2f2 100644 --- a/common/geography_utils/test/test_geography_utils.cpp +++ b/common/autoware_geography_utils/test/test_geography_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "geography_utils/height.hpp" -#include "geography_utils/lanelet2_projector.hpp" -#include "geography_utils/projection.hpp" +#include "autoware/geography_utils/height.hpp" +#include "autoware/geography_utils/lanelet2_projector.hpp" +#include "autoware/geography_utils/projection.hpp" #include diff --git a/common/geography_utils/test/test_height.cpp b/common/autoware_geography_utils/test/test_height.cpp similarity index 80% rename from common/geography_utils/test/test_height.cpp rename to common/autoware_geography_utils/test/test_height.cpp index 15297089ee131..f624f6c3ff9e3 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/autoware_geography_utils/test/test_height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -28,7 +28,7 @@ TEST(GeographyUtils, SameSourceTargetDatum) const std::string datum = "WGS84"; double converted_height = - geography_utils::convert_height(height, latitude, longitude, datum, datum); + autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum); EXPECT_DOUBLE_EQ(height, converted_height); } @@ -44,7 +44,7 @@ TEST(GeographyUtils, ValidSourceTargetDatum) const double target_height = -30.18; double converted_height = - geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); EXPECT_NEAR(target_height, converted_height, 0.1); } @@ -57,7 +57,7 @@ TEST(GeographyUtils, InvalidSourceTargetDatum) const double longitude = 139.0; EXPECT_THROW( - geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), std::invalid_argument); } @@ -69,7 +69,7 @@ TEST(GeographyUtils, InvalidSourceDatum) const double longitude = 139.0; EXPECT_THROW( - geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), std::invalid_argument); } @@ -81,6 +81,6 @@ TEST(GeographyUtils, InvalidTargetDatum) const double longitude = 139.0; EXPECT_THROW( - geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), std::invalid_argument); } diff --git a/common/geography_utils/test/test_projection.cpp b/common/autoware_geography_utils/test/test_projection.cpp similarity index 89% rename from common/geography_utils/test/test_projection.cpp rename to common/autoware_geography_utils/test/test_projection.cpp index 74ffb616cde6f..3355dbf8a50ea 100644 --- a/common/geography_utils/test/test_projection.cpp +++ b/common/autoware_geography_utils/test/test_projection.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -41,7 +41,7 @@ TEST(GeographyUtilsProjection, ProjectForwardToMGRS) // conversion const geometry_msgs::msg::Point converted_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::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); @@ -70,7 +70,7 @@ TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) // conversion const geographic_msgs::msg::GeoPoint converted_point = - geography_utils::project_reverse(local_point, projector_info); + autoware::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); @@ -93,9 +93,9 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) // conversion const geometry_msgs::msg::Point converted_local_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::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); + autoware::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); @@ -126,7 +126,7 @@ TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) // conversion const geometry_msgs::msg::Point converted_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::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); @@ -151,9 +151,9 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) // conversion const geometry_msgs::msg::Point converted_local_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::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); + autoware::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); diff --git a/localization/autoware_geo_pose_projector/package.xml b/localization/autoware_geo_pose_projector/package.xml index a56a53d609370..2d7c09f9e2ce2 100644 --- a/localization/autoware_geo_pose_projector/package.xml +++ b/localization/autoware_geo_pose_projector/package.xml @@ -17,10 +17,10 @@ ament_cmake_auto autoware_cmake + autoware_geography_utils component_interface_specs component_interface_utils geographic_msgs - geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp b/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp index e7fda37e07acd..f44780c140127 100644 --- a/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp @@ -14,8 +14,8 @@ #include "geo_pose_projector.hpp" -#include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -66,8 +66,8 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m gps_point.longitude = msg->pose.pose.position.longitude; gps_point.altitude = msg->pose.pose.position.altitude; geometry_msgs::msg::Point position = - geography_utils::project_forward(gps_point, projector_info_.value()); - position.z = geography_utils::convert_height( + autoware::geography_utils::project_forward(gps_point, projector_info_.value()); + position.z = autoware::geography_utils::convert_height( position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, projector_info_.value().vertical_datum); diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 6cceff98a8f9a..b8f92504a8b99 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,12 +19,12 @@ ament_cmake_auto autoware_cmake + autoware_geography_utils autoware_lanelet2_extension autoware_map_msgs component_interface_specs component_interface_utils fmt - geography_utils geometry_msgs libpcl-all-dev pcl_conversions diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 437c9e44cb7c1..9704b2ec68150 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -36,12 +36,12 @@ #include "lanelet2_local_projector.hpp" #include +#include #include #include #include #include #include -#include #include #include @@ -139,7 +139,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( lanelet::ErrorMessages errors{}; if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { std::unique_ptr projector = - geography_utils::get_lanelet2_projector(projector_info); + autoware::geography_utils::get_lanelet2_projector(projector_info); lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 6473c3150dcab..2480c4bd02753 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -16,6 +16,7 @@ rosidl_default_generators autoware_behavior_path_planner_common + autoware_geography_utils autoware_lanelet2_extension autoware_map_msgs autoware_mission_planner @@ -27,7 +28,6 @@ autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils - geography_utils geometry_msgs global_parameter_loader interpolation diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 4b9c5a73e81bc..e9958b74059a5 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -30,9 +30,9 @@ #include "type_alias.hpp" #include "utils.hpp" +#include #include #include -#include #include #include "std_msgs/msg/empty.hpp" @@ -871,7 +871,8 @@ void StaticCenterlineGeneratorNode::save_map() // save map with modified center line std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); - const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_); + const auto map_projector = + autoware::geography_utils::get_lanelet2_projector(*map_projector_info_); lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); RCLCPP_INFO( get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index eef6d690b6e72..e43013cfc37ec 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -19,12 +19,12 @@ libboost-dev + autoware_geography_utils autoware_sensing_msgs component_interface_specs component_interface_utils geographic_msgs geographiclib - geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 40a60d17ea7db..4cd3661edeb50 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,8 +14,8 @@ #include "gnss_poser/gnss_poser_core.hpp" -#include -#include +#include +#include #include @@ -116,8 +116,9 @@ void GNSSPoser::callback_nav_sat_fix( 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( + geometry_msgs::msg::Point position = + autoware::geography_utils::project_forward(gps_point, projector_info_); + position.z = autoware::geography_utils::convert_height( position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, projector_info_.vertical_datum); diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 95ced648f1326..c2751c96e995c 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,6 +15,7 @@ autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs + autoware_geography_utils autoware_motion_utils autoware_planning_msgs autoware_system_msgs @@ -24,7 +25,6 @@ component_interface_utils diagnostic_graph_utils geographic_msgs - geography_utils nav_msgs rclcpp rclcpp_components diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 47932f969c172..053e1258c7bef 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,8 +14,8 @@ #include "vehicle.hpp" -#include -#include +#include +#include #include @@ -143,15 +143,17 @@ void VehicleNode::publish_kinematics() 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::LOCAL) { - const geographic_msgs::msg::GeoPoint projected_gps_point = geography_utils::project_reverse( - kinematic_state_msgs_->pose.pose.position, *map_projector_info_); + const geographic_msgs::msg::GeoPoint projected_gps_point = + autoware::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.latitude; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; - vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( - projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, - map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); + vehicle_kinematics.geographic_pose.position.altitude = + autoware::geography_utils::convert_height( + 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 =