diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 993cf2d9da9e1..bd9886d376cad 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 3487e9515c48a..b271f75eeeb6c 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | +| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | @@ -33,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates | `gnss_frame` | string | "gnss" | frame id | | `gnss_base_frame` | string | "gnss_base_link" | frame id | | `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System | | `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml deleted file mode 100644 index e931e5adceb68..0000000000000 --- a/sensing/gnss_poser/config/set_local_origin.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - - # Latitude of local origin - latitude: 40.81187906 - - # Longitude of local origin - longitude: 29.35810110 - - # Altitude of local origin - altitude: 47.62 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 4dda5d1276c54..5cd0767f28130 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -42,7 +43,7 @@ enum class MGRSPrecision { GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, - int height_system) + const std::string & target_vertical_datum) { GNSSStat utm; @@ -50,16 +51,9 @@ GNSSStat NavSatFix2UTM( GeographicLib::UTMUPS::Forward( nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, utm.y); - - std::string target_height_system; - if (height_system == 0) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } utm.z = geography_utils::convert_height( nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", - target_height_system); + target_vertical_datum); utm.latitude = nav_sat_fix_msg.latitude; utm.longitude = nav_sat_fix_msg.longitude; utm.altitude = nav_sat_fix_msg.altitude; @@ -71,24 +65,20 @@ GNSSStat NavSatFix2UTM( GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, - sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) + 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( - nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, + geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); - std::string target_height_system; - if (height_system == 0) { - target_height_system = "EGM2008"; - } else { - target_height_system = "WGS84"; - } + utm_origin.z = geography_utils::convert_height( - nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, - "WGS84", target_height_system); + 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; @@ -104,7 +94,7 @@ GNSSStat NavSatFix2LocalCartesianUTM( 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_height_system) - + "WGS84", target_vertical_datum) - utm_origin.z; } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( @@ -143,9 +133,9 @@ GNSSStat UTM2MGRS( GNSSStat NavSatFix2MGRS( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, - const rclcpp::Logger & logger, int height_system) + const rclcpp::Logger & logger, const std::string & vertical_datum) { - const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system); + const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, vertical_datum); const auto mgrs = UTM2MGRS(utm, precision, logger); return mgrs; } 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 4baaec2dfe80e..05acfa967eb96 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -17,6 +17,8 @@ #include "gnss_poser/convert.hpp" #include "gnss_poser/gnss_stat.hpp" +#include +#include #include #include @@ -48,6 +50,9 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callbackGnssInsOrientationStamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); @@ -55,8 +60,8 @@ 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, CoordinateSystem coordinate_system, - int height_system); + 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); @@ -81,6 +86,7 @@ class GNSSPoser : public rclcpp::Node tf2_ros::TransformListener tf2_listener_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; rclcpp::Subscription::SharedPtr autoware_orientation_sub_; @@ -89,20 +95,18 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr fixed_pub_; - CoordinateSystem coordinate_system_; + MapProjectorInfo::Message projector_info_; std::string base_frame_; std::string gnss_frame_; std::string gnss_base_frame_; std::string map_frame_; - - sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; + bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; boost::circular_buffer position_buffer_; autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int height_system_; int gnss_pose_pub_method; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index d33bd022c3714..acd21a83d96e4 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -18,8 +18,6 @@ namespace gnss_poser { -enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 }; - struct GNSSStat { GNSSStat() diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 8a0ffb8cab8c3..60e952c6845f4 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -2,7 +2,6 @@ - @@ -13,10 +12,8 @@ - - @@ -31,11 +28,8 @@ - - - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 77380bf5492a1..7d80bc903a550 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -14,6 +14,9 @@ libboost-dev autoware_sensing_msgs + component_interface_specs + component_interface_utils + geographic_msgs geographiclib geography_utils geometry_msgs diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 4f0723944913b..9cd5a3ad34098 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -34,16 +34,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), msg_gnss_ins_orientation_stamped_( std::make_shared()), - height_system_(declare_parameter("height_system", 1)), gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) { - int coordinate_system = - declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); - coordinate_system_ = static_cast(coordinate_system); - - nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0); - nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0); - nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); + // Subscribe to map_projector_info topic + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); int buff_epoch = declare_parameter("buff_epoch", 1); position_buffer_.set_capacity(buff_epoch); @@ -61,9 +58,24 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); } +void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + projector_info_ = *msg; + received_map_projector_info_ = true; +} + void GNSSPoser::callbackNavSatFix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { + // Return immediately if map_projector_info has not been received yet. + if (!received_map_projector_info_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "map_projector_info has not been received yet. Check if the map_projection_loader is " + "successfully launched."); + return; + } + // check fixed topic const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); @@ -80,8 +92,8 @@ void GNSSPoser::callbackNavSatFix( return; } - // get position in coordinate_system - const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_); + // get position + const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_); const auto position = getPosition(gnss_stat); geometry_msgs::msg::Pose gnss_antenna_pose{}; @@ -186,20 +198,22 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix } GNSSStat GNSSPoser::convert( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system, - int height_system) + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + const MapProjectorInfo::Message & map_projector_info) { GNSSStat gnss_stat; - if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { + if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) { gnss_stat = NavSatFix2LocalCartesianUTM( - nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::MGRS) { + 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(), height_system); + 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 Coordinate System"); + "Unknown Projector type"); } return gnss_stat; }