diff --git a/sensing/autoware_gnss_poser/CMakeLists.txt b/sensing/autoware_gnss_poser/CMakeLists.txt new file mode 100644 index 0000000000..94317dd21a --- /dev/null +++ b/sensing/autoware_gnss_poser/CMakeLists.txt @@ -0,0 +1,39 @@ +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 new file mode 100644 index 0000000000..dc339eefcd --- /dev/null +++ b/sensing/autoware_gnss_poser/README.md @@ -0,0 +1,45 @@ +# gnss_poser + +## Overview + +The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. + +## Design + +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. + +## 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` | `autoware_internal_debug_msgs::msg::BoolStamped` | gnss fix status | + +## Parameters + +Parameters in below table + +| Name | Type | Default | Description | +| ------------------------ | -----------------------| ------------------------ | -------------------------------------------------------------- | +| `base_frame` | `string` | `base_link` | frame id for base_frame | +| `gnss_base_frame` | `string` | `gnss_base_link` | frame id for gnss_base_frame | +| `map_frame` | `string` | `map` | frame id for map_frame | +| `use_gnss_ins_orientation` | `boolean` | `true` | use Gnss-Ins orientation | +| `gnss_pose_pub_method` | `integer` | `0` | 0: Instant Value 1: Average Value 2: Median Value. If `buffer_epoch` is set to 0, `gnss_pose_pub_method` loses affect. Range: 0~2. | +| `buff_epoch` | `integer` | `1` | Buffer epoch. Range: 0~inf. | + +All above parameters can be changed in config file [gnss_poser.param.yaml](./config/gnss_poser.param.yaml "Click here to open config file") . diff --git a/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml b/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml new file mode 100644 index 0000000000..1c00b43c20 --- /dev/null +++ b/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml @@ -0,0 +1,8 @@ +/**: + 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 new file mode 100644 index 0000000000..f817a11292 --- /dev/null +++ b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp @@ -0,0 +1,107 @@ +// 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::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 new file mode 100644 index 0000000000..ef2e2661a1 --- /dev/null +++ b/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_gnss_poser/package.xml b/sensing/autoware_gnss_poser/package.xml new file mode 100644 index 0000000000..c8a8ac5a41 --- /dev/null +++ b/sensing/autoware_gnss_poser/package.xml @@ -0,0 +1,44 @@ + + + + autoware_gnss_poser + 0.0.0 + The ROS 2 autoware_gnss_poser package + Xingang Liu + 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 + 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 + autoware_internal_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 new file mode 100644 index 0000000000..d6e104b743 --- /dev/null +++ b/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json @@ -0,0 +1,65 @@ +{ + "$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 new file mode 100644 index 0000000000..52e1764135 --- /dev/null +++ b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp @@ -0,0 +1,417 @@ +// 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 + autoware_internal_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)