From 35de0a3e292bc62da1c557d928530757f7040c12 Mon Sep 17 00:00:00 2001 From: liuXinGangChina Date: Wed, 15 Jan 2025 09:40:40 +0800 Subject: [PATCH] feat(autoware_gnss_poser): porting from universe to core, autoware_gnss_poser, remove package dependency to which is under universe repo: v0.2 Signed-off-by: liuXinGangChina --- .../include/autoware/gnss_poser/gnss_poser_node.hpp | 10 ++++------ sensing/autoware_gnss_poser/package.xml | 2 +- sensing/autoware_gnss_poser/src/gnss_poser_node.cpp | 10 +++++----- 3 files changed, 10 insertions(+), 12 deletions(-) diff --git a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp index 2b34fcf153..deed6748fb 100644 --- a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp +++ b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ #define AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ -#include +#include #include #include @@ -46,9 +46,7 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; - - void callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + void callback_map_projector_info(const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg); void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); @@ -78,7 +76,7 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformListener tf2_listener_; tf2_ros::TransformBroadcaster tf2_broadcaster_; - rclcpp::Subscription::SharedPtr sub_map_projector_info_; + rclcpp::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; rclcpp::Subscription::SharedPtr autoware_orientation_sub_; @@ -87,7 +85,7 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr fixed_pub_; - MapProjectorInfo::Message projector_info_; + autoware_map_msgs::msg::MapProjectorInfo projector_info_; const std::string base_frame_; const std::string gnss_base_frame_; const std::string map_frame_; diff --git a/sensing/autoware_gnss_poser/package.xml b/sensing/autoware_gnss_poser/package.xml index 8dd115b4dd..05703e0a8f 100644 --- a/sensing/autoware_gnss_poser/package.xml +++ b/sensing/autoware_gnss_poser/package.xml @@ -20,7 +20,7 @@ libboost-dev - autoware_component_interface_specs + autoware_map_msgs autoware_geography_utils autoware_sensing_msgs geographic_msgs diff --git a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp index 60775998cc..2787873122 100644 --- a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp +++ b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp @@ -39,8 +39,8 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) gnss_pose_pub_method_(static_cast(declare_parameter("gnss_pose_pub_method"))) { // Subscribe to map_projector_info topic - sub_map_projector_info_ = create_subscription( - MapProjectorInfo::name, rclcpp::QoS{1}, + sub_map_projector_info_ = create_subscription( + "/map/map_projector_info", rclcpp::QoS{1}, std::bind(&GNSSPoser::callback_map_projector_info, this, std::placeholders::_1)); // Set up position buffer @@ -68,7 +68,7 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; } -void GNSSPoser::callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg) +void GNSSPoser::callback_map_projector_info(const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg) { projector_info_ = *msg; received_map_projector_info_ = true; @@ -86,7 +86,7 @@ void GNSSPoser::callback_nav_sat_fix( return; } - if (projector_info_.projector_type == MapProjectorInfo::Message::LOCAL) { + if (projector_info_.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { RCLCPP_ERROR_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "map_projector_info is local projector type. Unable to convert GNSS pose."); @@ -117,7 +117,7 @@ void GNSSPoser::callback_nav_sat_fix( 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, + position.z, gps_point.latitude, gps_point.longitude, autoware_map_msgs::msg::MapProjectorInfo::WGS84, projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{};