feat(geo_pose_projector): use geo_pose_projector in eagleye (autoware…

…foundation#5513)

* feat(tier4_geo_pose_projector): use tier4_geo_pose_projector in eagleye

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

* style(pre-commit): autofix

* fix(eagleye): split fix2pose

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

* style(pre-commit): autofix

* fix name: fuser -> fusion

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

* style(pre-commit): autofix

* update

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

* style(pre-commit): autofix

* update readme

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

* style(pre-commit): autofix

* add #include <string>

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

* add rclcpp in dependency

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

* style(pre-commit): autofix

* add limitation in readme

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

* style(pre-commit): autofix

* update tier4_localization_launch

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

* update tier4_localization_launch

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

* rename package

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

* style(pre-commit): autofix

---------

Signed-off-by: kminoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml

@@ -145,9 +145,13 @@
   </group>
 
   <group if="$(var use_eagleye_pose)">
-    <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml"/>
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml">
-      <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
+    <arg name="geo_pose_topic_name" default="eagleye/geo_pose_with_covariance"/>
+    <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml">
+      <arg name="output_geo_pose" value="$(var geo_pose_topic_name)"/>
+    </include>
+    <include file="$(find-pkg-share geo_pose_projector)/launch/geo_pose_projector.launch.xml">
+      <arg name="input_geo_pose" value="$(var geo_pose_topic_name)"/>
+      <arg name="output_pose" value="$(var output_pose_with_cov_name)"/>
     </include>
   </group>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -16,11 +16,11 @@
 
   <exec_depend>ar_tag_based_localizer</exec_depend>
   <exec_depend>automatic_pose_initializer</exec_depend>
-  <exec_depend>eagleye_geo_pose_converter</exec_depend>
   <exec_depend>eagleye_geo_pose_fusion</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
+  <exec_depend>geo_pose_projector</exec_depend>
   <exec_depend>gyro_odometer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>

localization/geo_pose_projector/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.14)
+project(geo_pose_projector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(geo_pose_projector
+  src/geo_pose_projector_node.cpp
+  src/geo_pose_projector.cpp
+)
+ament_target_dependencies(geo_pose_projector)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

localization/geo_pose_projector/README.md

@@ -0,0 +1,28 @@
+# geo_pose_projector
+
+## Overview
+
+This node is a simple node that subscribes to the geo-referenced pose topic and publishes the pose in the map frame.
+
+## Subscribed Topics
+
+| Name                      | Type                                                 | Description         |
+| ------------------------- | ---------------------------------------------------- | ------------------- |
+| `input_geo_pose`          | `geographic_msgs::msg::GeoPoseWithCovarianceStamped` | geo-referenced pose |
+| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectedObjectInfo`        | map projector info  |
+
+## Published Topics
+
+| Name          | Type                                            | Description                           |
+| ------------- | ----------------------------------------------- | ------------------------------------- |
+| `output_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | pose in map frame                     |
+| `/tf`         | `tf2_msgs::msg::TFMessage`                      | tf from parent link to the child link |
+
+## Parameters
+
+{{ json_to_markdown("localization/geo_pose_projector/schema/geo_pose_projector.schema.json") }}
+
+## Limitations
+
+The covariance conversion may be incorrect depending on the projection type you are using. The covariance of input topic is expressed in (Latitude, Longitude, Altitude) as a diagonal matrix.
+Currently, we assume that the x axis is the east direction and the y axis is the north direction. Thus, the conversion may be incorrect when this assumption breaks, especially when the covariance of latitude and longitude is different.

localization/geo_pose_projector/config/geo_pose_projector.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    publish_tf: true
+    parent_frame: "map"
+    child_frame: "pose_estimator_base_link"

localization/geo_pose_projector/launch/geo_pose_projector.launch.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_geo_pose" default="/geo_pose_with_covariance"/>
+  <arg name="output_pose" default="/pose_with_covariance"/>
+  <arg name="param_path" default="$(find-pkg-share geo_pose_projector)/config/geo_pose_projector.param.yaml"/>
+
+  <node pkg="geo_pose_projector" exec="geo_pose_projector" name="geo_pose_projector" output="screen">
+    <remap from="input_geo_pose" to="$(var input_geo_pose)"/>
+    <remap from="output_pose" to="$(var output_pose)"/>
+
+    <param from="$(var param_path)"/>
+  </node>
+</launch>

localization/geo_pose_projector/package.xml

@@ -0,0 +1,30 @@
+<?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>geo_pose_projector</name>
+  <version>0.1.0</version>
+  <description>The geo_pose_projector package</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Koji Minoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>component_interface_specs</depend>
+  <depend>component_interface_utils</depend>
+  <depend>geographic_msgs</depend>
+  <depend>geography_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization/geo_pose_projector/schema/geo_pose_projector.schema.json

@@ -0,0 +1,43 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for geo_pose_projector",
+  "type": "object",
+  "definitions": {
+    "geo_pose_projector": {
+      "type": "object",
+      "properties": {
+        "publish_tf": {
+          "type": "boolean",
+          "description": "whether to publish tf",
+          "default": true
+        },
+        "parent_frame": {
+          "type": "string",
+          "description": "parent frame for published tf",
+          "default": "map"
+        },
+        "child_frame": {
+          "type": "string",
+          "description": "child frame for published tf",
+          "default": "pose_estimator_base_link"
+        }
+      },
+      "required": ["publish_tf", "parent_frame", "child_frame"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/geo_pose_projector"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

localization/geo_pose_projector/src/geo_pose_projector.cpp

@@ -0,0 +1,103 @@
+// Copyright 2023 Autoware Foundation
+//
+// 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 "geo_pose_projector.hpp"
+
+#include <geography_utils/height.hpp>
+#include <geography_utils/projection.hpp>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <string>
+
+GeoPoseProjector::GeoPoseProjector()
+: Node("geo_pose_projector"), publish_tf_(declare_parameter<bool>("publish_tf"))
+{
+  // 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) { projector_info_ = *msg; });
+
+  // Subscribe to geo_pose topic
+  geo_pose_sub_ = create_subscription<GeoPoseWithCovariance>(
+    "input_geo_pose", 10, [this](const GeoPoseWithCovariance::SharedPtr msg) { on_geo_pose(msg); });
+
+  // Publish pose topic
+  pose_pub_ = create_publisher<PoseWithCovariance>("output_pose", 10);
+
+  // Publish tf
+  if (publish_tf_) {
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+    parent_frame_ = declare_parameter<std::string>("parent_frame");
+    child_frame_ = declare_parameter<std::string>("child_frame");
+  }
+}
+
+void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg)
+{
+  if (!projector_info_) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 1000 /* ms */, "map_projector_info is not received yet.");
+    return;
+  }
+
+  // get position
+  geographic_msgs::msg::GeoPoint gps_point;
+  gps_point.latitude = msg->pose.pose.position.latitude;
+  gps_point.longitude = msg->pose.pose.position.longitude;
+  gps_point.altitude = msg->pose.pose.position.altitude;
+  geometry_msgs::msg::Point position =
+    geography_utils::project_forward(gps_point, projector_info_.value());
+  position.z = geography_utils::convert_height(
+    position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84,
+    projector_info_.value().vertical_datum);
+
+  // Convert geo_pose to pose
+  PoseWithCovariance projected_pose;
+  projected_pose.header = msg->header;
+  projected_pose.pose.pose.position = position;
+  projected_pose.pose.pose.orientation = msg->pose.pose.orientation;
+  projected_pose.pose.covariance = msg->pose.covariance;
+
+  // Covariance in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate.
+  // TODO(TIER IV): This swap may be invalid when using other projector type.
+  projected_pose.pose.covariance[0] = msg->pose.covariance[7];
+  projected_pose.pose.covariance[7] = msg->pose.covariance[0];
+
+  pose_pub_->publish(projected_pose);
+
+  // Publish tf
+  if (publish_tf_) {
+    tf2::Transform transform;
+    transform.setOrigin(tf2::Vector3(
+      projected_pose.pose.pose.position.x, projected_pose.pose.pose.position.y,
+      projected_pose.pose.pose.position.z));
+    const auto localization_quat = tf2::Quaternion(
+      projected_pose.pose.pose.orientation.x, projected_pose.pose.pose.orientation.y,
+      projected_pose.pose.pose.orientation.z, projected_pose.pose.pose.orientation.w);
+    transform.setRotation(localization_quat);
+
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped.header = msg->header;
+    transform_stamped.header.frame_id = parent_frame_;
+    transform_stamped.child_frame_id = child_frame_;
+    transform_stamped.transform = tf2::toMsg(transform);
+    tf_broadcaster_->sendTransform(transform_stamped);
+  }
+}

localization/geo_pose_projector/src/geo_pose_projector.hpp

@@ -0,0 +1,58 @@
+// Copyright 2023 Autoware Foundation
+//
+// 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 GEO_POSE_PROJECTOR_HPP_
+#define GEO_POSE_PROJECTOR_HPP_
+
+#include <component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geographic_msgs/msg/geo_pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <memory>
+#include <optional>
+#include <string>
+
+class GeoPoseProjector : public rclcpp::Node
+{
+private:
+  using GeoPoseWithCovariance = geographic_msgs::msg::GeoPoseWithCovarianceStamped;
+  using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using MapProjectorInfo = map_interface::MapProjectorInfo;
+
+public:
+  GeoPoseProjector();
+
+private:
+  void on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg);
+
+  component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
+  rclcpp::Subscription<GeoPoseWithCovariance>::SharedPtr geo_pose_sub_;
+  rclcpp::Publisher<PoseWithCovariance>::SharedPtr pose_pub_;
+
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  std::optional<MapProjectorInfo::Message> projector_info_ = std::nullopt;
+
+  const bool publish_tf_;
+
+  std::string parent_frame_;
+  std::string child_frame_;
+};
+
+#endif  // GEO_POSE_PROJECTOR_HPP_

localization/geo_pose_projector/src/geo_pose_projector_node.cpp

@@ -0,0 +1,29 @@
+// Copyright 2023 Autoware Foundation
+//
+// 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 "geo_pose_projector.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<GeoPoseProjector>());
+  rclcpp::shutdown();
+
+  return 0;
+}