diff --git a/sensing/autoware_gnss_poser/CHANGELOG.rst b/sensing/autoware_gnss_poser/CHANGELOG.rst deleted file mode 100644 index d97f28dbe618b..0000000000000 --- a/sensing/autoware_gnss_poser/CHANGELOG.rst +++ /dev/null @@ -1,74 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_gnss_poser -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.41.0 (2025-01-29) -------------------- -* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base -* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) -* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(component_interface_specs): prefix package and namespace with autoware (`#9094 `_) -* chore(autoware_gnss_poser): make source codes follow the coding rules (`#8703 `_) - * Follow the coding rules in autoware_gnss_poser - * Edit _core to _node inside files - * Fixed AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP\_ part - --------- -* chore(gnss_poser): add autoware prefix to gnss_poser (`#8323 `_) - * add "autoware" prefix to gnss_poser - * Fixed typos and left overs - * Fixed directory mistake - * style(pre-commit): autofix - * Removed gnss_poser line from CODEOWNERS - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com> -* Contributors: Esteve Fernandez, TaikiYamada4, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/sensing/autoware_gnss_poser/CMakeLists.txt b/sensing/autoware_gnss_poser/CMakeLists.txt deleted file mode 100644 index 94317dd21a238..0000000000000 --- a/sensing/autoware_gnss_poser/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_gnss_poser) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -## Find non-ROS library -find_package(PkgConfig) -find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h - PATH_SUFFIXES GeographicLib -) - -set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) -find_library(GeographicLib_LIBRARIES - NAMES Geographic -) - -set(GNSS_POSER_HEADERS - include/autoware/gnss_poser/gnss_poser_node.hpp -) - -ament_auto_add_library(gnss_poser_node SHARED - src/gnss_poser_node.cpp - ${GNSS_POSER_HEADERS} -) - -target_link_libraries(gnss_poser_node - Geographic -) - -rclcpp_components_register_node(gnss_poser_node - PLUGIN "autoware::gnss_poser::GNSSPoser" - EXECUTABLE gnss_poser -) - -ament_auto_package(INSTALL_TO_SHARE - config - launch -) diff --git a/sensing/autoware_gnss_poser/README.md b/sensing/autoware_gnss_poser/README.md deleted file mode 100644 index cdd67bde027d2..0000000000000 --- a/sensing/autoware_gnss_poser/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# gnss_poser - -## Purpose - -The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. - -This node subscribes to NavSatFix to publish the pose of **base_link**. The data in NavSatFix represents the antenna's position. Therefore, it performs a coordinate transformation using the tf from `base_link` to the antenna's position. The frame_id of the antenna's position refers to NavSatFix's `header.frame_id`. -(**Note that `header.frame_id` in NavSatFix indicates the antenna's frame_id, not the Earth or reference ellipsoid.** [See also NavSatFix definition.](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html)) - -If the transformation from `base_link` to the antenna cannot be obtained, it outputs the pose of the antenna position without performing coordinate transformation. - -## Inner-workings / Algorithms - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | -| `/map/map_projector_info` | `autoware_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) | - -### Output - -| Name | Type | Description | -| ------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | -| `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | -| `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | -| `~/output/gnss_fixed` | `tier4_debug_msgs::msg::BoolStamped` | gnss fix status | - -## Parameters - -### Core Parameters - -{{ json_to_markdown("sensing/autoware_gnss_poser/schema/gnss_poser.schema.json") }} - -## Assumptions / Known limits - -## (Optional) Error detection and handling - -## (Optional) Performance characterization - -## (Optional) References/External links - -## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml b/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml deleted file mode 100644 index 1c00b43c2066a..0000000000000 --- a/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - base_frame: base_link - gnss_base_frame: gnss_base_link - map_frame: map - buff_epoch: 1 - use_gnss_ins_orientation: true - gnss_pose_pub_method: 0 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 deleted file mode 100644 index d4ba9b994cbcb..0000000000000 --- a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp +++ /dev/null @@ -1,107 +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 AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ -#define AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include - -#include - -namespace autoware::gnss_poser -{ -class GNSSPoser : public rclcpp::Node -{ -public: - explicit GNSSPoser(const rclcpp::NodeOptions & node_options); - -private: - using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo; - - void callback_map_projector_info(const MapProjectorInfo::Message::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); - - static bool is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); - static bool can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - static geometry_msgs::msg::Point get_median_position( - const boost::circular_buffer & position_buffer); - static geometry_msgs::msg::Point get_average_position( - const boost::circular_buffer & position_buffer); - static geometry_msgs::msg::Quaternion get_quaternion_by_heading(const int heading); - static geometry_msgs::msg::Quaternion get_quaternion_by_position_difference( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); - - bool get_transform( - const std::string & target_frame, const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); - bool get_static_transform( - const std::string & target_frame, const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, - const builtin_interfaces::msg::Time & stamp); - void publish_tf( - const std::string & frame_id, const std::string & child_frame_id, - const geometry_msgs::msg::PoseStamped & pose_msg); - - tf2::BufferCore tf2_buffer_; - tf2_ros::TransformListener tf2_listener_; - tf2_ros::TransformBroadcaster tf2_broadcaster_; - - autoware::component_interface_utils::Subscription::SharedPtr - sub_map_projector_info_; - rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; - rclcpp::Subscription::SharedPtr - autoware_orientation_sub_; - - rclcpp::Publisher::SharedPtr pose_pub_; - rclcpp::Publisher::SharedPtr pose_cov_pub_; - rclcpp::Publisher::SharedPtr fixed_pub_; - - MapProjectorInfo::Message projector_info_; - const std::string base_frame_; - const std::string gnss_base_frame_; - const std::string map_frame_; - 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 gnss_pose_pub_method_; -}; -} // namespace autoware::gnss_poser - -#endif // AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ diff --git a/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml b/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml deleted file mode 100755 index ef2e2661a16c0..0000000000000 --- a/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/sensing/autoware_gnss_poser/package.xml b/sensing/autoware_gnss_poser/package.xml deleted file mode 100644 index 50f9a32156f85..0000000000000 --- a/sensing/autoware_gnss_poser/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - autoware_gnss_poser - 0.41.0 - The ROS 2 autoware_gnss_poser package - Yamato Ando - Masahiro Sakamoto - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - Ryu Yamamoto - Apache License 2.0 - Ryo Watanabe - - ament_cmake_auto - autoware_cmake - - libboost-dev - - autoware_component_interface_specs_universe - autoware_component_interface_utils - autoware_geography_utils - autoware_sensing_msgs - geographic_msgs - geographiclib - geometry_msgs - rclcpp - rclcpp_components - sensor_msgs - tf2 - tf2_geometry_msgs - tf2_ros - tier4_debug_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json b/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json deleted file mode 100644 index d6e104b7432f2..0000000000000 --- a/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json +++ /dev/null @@ -1,65 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Gnss Poser Node", - "type": "object", - "definitions": { - "gnss_poser": { - "type": "object", - "properties": { - "base_frame": { - "type": "string", - "default": "base_link", - "description": "frame id for base_frame" - }, - "gnss_base_frame": { - "type": "string", - "default": "gnss_base_link", - "description": "frame id for gnss_base_frame" - }, - "map_frame": { - "type": "string", - "default": "map", - "description": "frame id for map_frame" - }, - "use_gnss_ins_orientation": { - "type": "boolean", - "default": "true", - "description": "use Gnss-Ins orientation" - }, - "gnss_pose_pub_method": { - "type": "integer", - "default": "0", - "minimum": 0, - "maximum": 2, - "description": "0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect." - }, - "buff_epoch": { - "type": "integer", - "default": "1", - "minimum": 0, - "description": "Buffer epoch" - } - }, - "required": [ - "base_frame", - "gnss_base_frame", - "map_frame", - "use_gnss_ins_orientation", - "gnss_pose_pub_method", - "buff_epoch" - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/gnss_poser" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp deleted file mode 100644 index e6d59b7edfc16..0000000000000 --- a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp +++ /dev/null @@ -1,417 +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. - -#include "autoware/gnss_poser/gnss_poser_node.hpp" - -#include -#include - -#include - -#include -#include -#include -#include - -namespace autoware::gnss_poser -{ -GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("gnss_poser", node_options), - tf2_listener_(tf2_buffer_), - tf2_broadcaster_(*this), - base_frame_(declare_parameter("base_frame")), - gnss_base_frame_(declare_parameter("gnss_base_frame")), - map_frame_(declare_parameter("map_frame")), - use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), - msg_gnss_ins_orientation_stamped_( - std::make_shared()), - gnss_pose_pub_method_(static_cast(declare_parameter("gnss_pose_pub_method"))) -{ - // Subscribe to map_projector_info topic - const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); - adaptor.init_sub( - sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { - callback_map_projector_info(msg); - }); - - // Set up position buffer - int buff_epoch = static_cast(declare_parameter("buff_epoch")); - position_buffer_.set_capacity(buff_epoch); - - // Set subscribers and publishers - nav_sat_fix_sub_ = create_subscription( - "fix", rclcpp::QoS{1}, - std::bind(&GNSSPoser::callback_nav_sat_fix, this, std::placeholders::_1)); - autoware_orientation_sub_ = - create_subscription( - "autoware_orientation", rclcpp::QoS{1}, - std::bind(&GNSSPoser::callback_gnss_ins_orientation_stamped, this, std::placeholders::_1)); - - pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); - pose_cov_pub_ = create_publisher( - "gnss_pose_cov", rclcpp::QoS{1}); - fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); - - // Set msg_gnss_ins_orientation_stamped_ with temporary values (not to publish zero value - // covariances) - msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_x = 1.0; - msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_y = 1.0; - msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; -} - -void GNSSPoser::callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg) -{ - projector_info_ = *msg; - received_map_projector_info_ = true; -} - -void GNSSPoser::callback_nav_sat_fix( - 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; - } - - if (projector_info_.projector_type == MapProjectorInfo::Message::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."); - return; - } - - // check fixed topic - const bool is_status_fixed = is_fixed(nav_sat_fix_msg_ptr->status); - - // publish is_fixed topic - tier4_debug_msgs::msg::BoolStamped is_fixed_msg; - is_fixed_msg.stamp = this->now(); - is_fixed_msg.data = is_status_fixed; - fixed_pub_->publish(is_fixed_msg); - - if (!is_status_fixed) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Not Fixed Topic. Skipping Calculate."); - return; - } - - // get position - 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 = - 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); - - geometry_msgs::msg::Pose gnss_antenna_pose{}; - - // publish pose immediately - if (!gnss_pose_pub_method_) { - gnss_antenna_pose.position = position; - } else { - // fill position buffer - position_buffer_.push_front(position); - if (!position_buffer_.full()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Buffering Position. Output Skipped."); - return; - } - // publish average pose or median pose of position buffer - gnss_antenna_pose.position = (gnss_pose_pub_method_ == 1) - ? get_average_position(position_buffer_) - : get_median_position(position_buffer_); - } - - // calc gnss antenna orientation - geometry_msgs::msg::Quaternion orientation; - if (use_gnss_ins_orientation_) { - orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; - } else { - static auto prev_position = gnss_antenna_pose.position; - orientation = get_quaternion_by_position_difference(gnss_antenna_pose.position, prev_position); - prev_position = gnss_antenna_pose.position; - } - - gnss_antenna_pose.orientation = orientation; - - tf2::Transform tf_map2gnss_antenna{}; - tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); - - // get TF from gnss_antenna to base_link - auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); - - const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; - get_static_transform( - gnss_frame, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); - tf2::Transform tf_gnss_antenna2base_link{}; - tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); - - // transform pose from gnss_antenna(in map frame) to base_link(in map frame) - tf2::Transform tf_map2base_link{}; - tf_map2base_link = tf_map2gnss_antenna * tf_gnss_antenna2base_link; - - geometry_msgs::msg::PoseStamped gnss_base_pose_msg{}; - gnss_base_pose_msg.header.stamp = nav_sat_fix_msg_ptr->header.stamp; - gnss_base_pose_msg.header.frame_id = map_frame_; - tf2::toMsg(tf_map2base_link, gnss_base_pose_msg.pose); - - // publish gnss_base_link pose in map frame - pose_pub_->publish(gnss_base_pose_msg); - - // publish gnss_base_link pose_cov in map frame - geometry_msgs::msg::PoseWithCovarianceStamped gnss_base_pose_cov_msg; - gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; - gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; - gnss_base_pose_cov_msg.pose.covariance[7 * 0] = - can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; - gnss_base_pose_cov_msg.pose.covariance[7 * 1] = - can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; - gnss_base_pose_cov_msg.pose.covariance[7 * 2] = - can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; - - if (use_gnss_ins_orientation_) { - gnss_base_pose_cov_msg.pose.covariance[7 * 3] = - std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_x, 2); - gnss_base_pose_cov_msg.pose.covariance[7 * 4] = - std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_y, 2); - gnss_base_pose_cov_msg.pose.covariance[7 * 5] = - std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z, 2); - } else { - gnss_base_pose_cov_msg.pose.covariance[7 * 3] = 0.1; - gnss_base_pose_cov_msg.pose.covariance[7 * 4] = 0.1; - gnss_base_pose_cov_msg.pose.covariance[7 * 5] = 1.0; - } - - pose_cov_pub_->publish(gnss_base_pose_cov_msg); - - // broadcast map to gnss_base_link - publish_tf(map_frame_, gnss_base_frame_, gnss_base_pose_msg); -} - -void GNSSPoser::callback_gnss_ins_orientation_stamped( - const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg) -{ - *msg_gnss_ins_orientation_stamped_ = *msg; -} - -bool GNSSPoser::is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) -{ - return nav_sat_status_msg.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; -} - -bool GNSSPoser::can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) -{ - return nav_sat_fix_msg.position_covariance_type > - sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; -} - -geometry_msgs::msg::Point GNSSPoser::get_median_position( - const boost::circular_buffer & position_buffer) -{ - auto get_median = [](std::vector array) { - std::sort(std::begin(array), std::end(array)); - const size_t median_index = array.size() / 2; - double median = (array.size() % 2) - ? (array.at(median_index)) - : ((array.at(median_index) + array.at(median_index - 1)) / 2); - return median; - }; - - std::vector array_x; - std::vector array_y; - std::vector array_z; - for (const auto & position : position_buffer) { - array_x.push_back(position.x); - array_y.push_back(position.y); - array_z.push_back(position.z); - } - - geometry_msgs::msg::Point median_point; - median_point.x = get_median(array_x); - median_point.y = get_median(array_y); - median_point.z = get_median(array_z); - return median_point; -} - -geometry_msgs::msg::Point GNSSPoser::get_average_position( - const boost::circular_buffer & position_buffer) -{ - std::vector array_x; - std::vector array_y; - std::vector array_z; - for (const auto & position : position_buffer) { - array_x.push_back(position.x); - array_y.push_back(position.y); - array_z.push_back(position.z); - } - - geometry_msgs::msg::Point average_point; - average_point.x = - std::reduce(array_x.begin(), array_x.end()) / static_cast(array_x.size()); - average_point.y = - std::reduce(array_y.begin(), array_y.end()) / static_cast(array_y.size()); - average_point.z = - std::reduce(array_z.begin(), array_z.end()) / static_cast(array_z.size()); - return average_point; -} - -geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_heading(const int heading) -{ - int heading_conv = 0; - // convert heading[0(North)~360] to yaw[-M_PI(West)~M_PI] - if (heading >= 0 && heading <= 27000000) { - heading_conv = 9000000 - heading; - } else { - heading_conv = 45000000 - heading; - } - const double yaw = (heading_conv * 1e-5) * M_PI / 180.0; - - tf2::Quaternion quaternion; - quaternion.setRPY(0, 0, yaw); - - return tf2::toMsg(quaternion); -} - -geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_position_difference( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point) -{ - const double yaw = std::atan2(point.y - prev_point.y, point.x - prev_point.x); - tf2::Quaternion quaternion; - quaternion.setRPY(0, 0, yaw); - return tf2::toMsg(quaternion); -} - -bool GNSSPoser::get_transform( - const std::string & target_frame, const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) -{ - if (target_frame == source_frame) { - transform_stamped_ptr->header.stamp = this->now(); - transform_stamped_ptr->header.frame_id = target_frame; - transform_stamped_ptr->child_frame_id = source_frame; - transform_stamped_ptr->transform.translation.x = 0.0; - transform_stamped_ptr->transform.translation.y = 0.0; - transform_stamped_ptr->transform.translation.z = 0.0; - transform_stamped_ptr->transform.rotation.x = 0.0; - transform_stamped_ptr->transform.rotation.y = 0.0; - transform_stamped_ptr->transform.rotation.z = 0.0; - transform_stamped_ptr->transform.rotation.w = 1.0; - return true; - } - - try { - *transform_stamped_ptr = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), ex.what()); - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str()); - - transform_stamped_ptr->header.stamp = this->now(); - transform_stamped_ptr->header.frame_id = target_frame; - transform_stamped_ptr->child_frame_id = source_frame; - transform_stamped_ptr->transform.translation.x = 0.0; - transform_stamped_ptr->transform.translation.y = 0.0; - transform_stamped_ptr->transform.translation.z = 0.0; - transform_stamped_ptr->transform.rotation.x = 0.0; - transform_stamped_ptr->transform.rotation.y = 0.0; - transform_stamped_ptr->transform.rotation.z = 0.0; - transform_stamped_ptr->transform.rotation.w = 1.0; - return false; - } - return true; -} - -bool GNSSPoser::get_static_transform( - const std::string & target_frame, const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, - const builtin_interfaces::msg::Time & stamp) -{ - if (target_frame == source_frame) { - transform_stamped_ptr->header.stamp = stamp; - transform_stamped_ptr->header.frame_id = target_frame; - transform_stamped_ptr->child_frame_id = source_frame; - transform_stamped_ptr->transform.translation.x = 0.0; - transform_stamped_ptr->transform.translation.y = 0.0; - transform_stamped_ptr->transform.translation.z = 0.0; - transform_stamped_ptr->transform.rotation.x = 0.0; - transform_stamped_ptr->transform.rotation.y = 0.0; - transform_stamped_ptr->transform.rotation.z = 0.0; - transform_stamped_ptr->transform.rotation.w = 1.0; - return true; - } - - try { - *transform_stamped_ptr = tf2_buffer_.lookupTransform( - target_frame, source_frame, - tf2::TimePoint(std::chrono::seconds(stamp.sec) + std::chrono::nanoseconds(stamp.nanosec))); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), ex.what()); - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str()); - - transform_stamped_ptr->header.stamp = stamp; - transform_stamped_ptr->header.frame_id = target_frame; - transform_stamped_ptr->child_frame_id = source_frame; - transform_stamped_ptr->transform.translation.x = 0.0; - transform_stamped_ptr->transform.translation.y = 0.0; - transform_stamped_ptr->transform.translation.z = 0.0; - transform_stamped_ptr->transform.rotation.x = 0.0; - transform_stamped_ptr->transform.rotation.y = 0.0; - transform_stamped_ptr->transform.rotation.z = 0.0; - transform_stamped_ptr->transform.rotation.w = 1.0; - return false; - } - return true; -} - -void GNSSPoser::publish_tf( - const std::string & frame_id, const std::string & child_frame_id, - const geometry_msgs::msg::PoseStamped & pose_msg) -{ - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.header.frame_id = frame_id; - transform_stamped.child_frame_id = child_frame_id; - transform_stamped.header.stamp = pose_msg.header.stamp; - - transform_stamped.transform.translation.x = pose_msg.pose.position.x; - transform_stamped.transform.translation.y = pose_msg.pose.position.y; - transform_stamped.transform.translation.z = pose_msg.pose.position.z; - - tf2::Quaternion tf_quaternion; - tf2::fromMsg(pose_msg.pose.orientation, tf_quaternion); - transform_stamped.transform.rotation.x = tf_quaternion.x(); - transform_stamped.transform.rotation.y = tf_quaternion.y(); - transform_stamped.transform.rotation.z = tf_quaternion.z(); - transform_stamped.transform.rotation.w = tf_quaternion.w(); - - tf2_broadcaster_.sendTransform(transform_stamped); -} -} // namespace autoware::gnss_poser - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gnss_poser::GNSSPoser)