feat(autoware_gnss_poser): porting from universe to core, autoware_gn…

…ss_poser, add to core repo : v0.0

Signed-off-by: liuXinGangChina <[email protected]>

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
+)

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") .

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

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 <autoware/component_interface_specs/map.hpp>
+#include <autoware/component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <autoware_internal_debug_msgs/msg/bool_stamped.hpp>
+
+#include <boost/circular_buffer.hpp>
+
+#include <tf2/transform_datatypes.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+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<geometry_msgs::msg::Point> & position_buffer);
+  static geometry_msgs::msg::Point get_average_position(
+    const boost::circular_buffer<geometry_msgs::msg::Point> & 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<MapProjectorInfo>::SharedPtr
+    sub_map_projector_info_;
+  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr nav_sat_fix_sub_;
+  rclcpp::Subscription<autoware_sensing_msgs::msg::GnssInsOrientationStamped>::SharedPtr
+    autoware_orientation_sub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_cov_pub_;
+  rclcpp::Publisher<autoware_internal_debug_msgs::msg::BoolStamped>::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<geometry_msgs::msg::Point> 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_

sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="param_file" default="$(find-pkg-share autoware_gnss_poser)/config/gnss_poser.param.yaml"/>
+  <arg name="input_topic_fix" default="/fix"/>
+  <arg name="input_topic_orientation" default="/autoware_orientation"/>
+  <arg name="output_topic_gnss_pose" default="gnss_pose"/>
+  <arg name="output_topic_gnss_pose_cov" default="gnss_pose_cov"/>
+  <arg name="output_topic_gnss_fixed" default="gnss_fixed"/>
+
+  <node pkg="autoware_gnss_poser" exec="gnss_poser" name="gnss_poser" output="both">
+    <remap from="fix" to="$(var input_topic_fix)"/>
+    <remap from="autoware_orientation" to="$(var input_topic_orientation)"/>
+    <remap from="gnss_pose" to="$(var output_topic_gnss_pose)"/>
+    <remap from="gnss_pose_cov" to="$(var output_topic_gnss_pose_cov)"/>
+    <remap from="gnss_fixed" to="$(var output_topic_gnss_fixed)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

sensing/autoware_gnss_poser/package.xml

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_gnss_poser</name>
+  <version>0.0.0</version>
+  <description>The ROS 2 autoware_gnss_poser package</description>
+  <maintainer email="[email protected]">Xingang Liu</maintainer>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">Kento Yabuuchi</maintainer>
+  <maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Ryo Watanabe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <build_depend>libboost-dev</build_depend>
+
+  <depend>autoware_component_interface_specs</depend>
+  <depend>autoware_component_interface_utils</depend>
+  <depend>autoware_geography_utils</depend>
+  <depend>autoware_sensing_msgs</depend>
+  <depend>geographic_msgs</depend>
+  <depend>geographiclib</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>autoware_internal_debug_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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": ["/**"]
+}