feat(odometry_sensor): add ground truth odometry to Unity scene

Assets/AWSIM/Prefabs/Sensors/OdometrySensor.prefab

@@ -0,0 +1,66 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!1 &3648756099762559989
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 3648756099762557962}
+  - component: {fileID: 3648756099762557960}
+  - component: {fileID: 3648756099762557963}
+  m_Layer: 6
+  m_Name: OdometrySensor
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &3648756099762557962
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3648756099762559989}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: 0, y: 0, z: 0}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 0
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &3648756099762557960
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3648756099762559989}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: c30610523d9cbf018803304ee709d387, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  OutputHz: 60
+--- !u!114 &3648756099762557963
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 3648756099762559989}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 7d38bf8600adc068b85469ec916e9330, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  Topic: /awsim/ground_truth/localization/kinematic_state
+  FrameID: base_link
+  QosSettings:
+    ReliabilityPolicy: 2
+    DurabilityPolicy: 2
+    HistoryPolicy: 1
+    Depth: 1

Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab

@@ -1741,6 +1741,7 @@ Transform:
   m_Children:
   - {fileID: 4843604171253800091}
   - {fileID: 2465439771431800668}
+  - {fileID: 1195138626504501104}
   m_Father: {fileID: 635807138616992815}
   m_RootOrder: 1
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
@@ -3649,6 +3650,72 @@ MeshRenderer:
   m_SortingLayer: 0
   m_SortingOrder: 0
   m_AdditionalVertexStreams: {fileID: 0}
+--- !u!1001 &2465439770172797818
+PrefabInstance:
+  m_ObjectHideFlags: 0
+  serializedVersion: 2
+  m_Modification:
+    m_TransformParent: {fileID: 5512666411487739948}
+    m_Modifications:
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_RootOrder
+      value: 2
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalPosition.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalPosition.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalPosition.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalRotation.w
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalRotation.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalRotation.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalRotation.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762557963, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: Topic
+      value: /awsim/ground_truth/localization/kinematic_state
+      objectReference: {fileID: 0}
+    - target: {fileID: 3648756099762559989, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+      propertyPath: m_Name
+      value: OdometrySensor
+      objectReference: {fileID: 0}
+    m_RemovedComponents: []
+  m_SourcePrefab: {fileID: 100100000, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+--- !u!4 &1195138626504501104 stripped
+Transform:
+  m_CorrespondingSourceObject: {fileID: 3648756099762557962, guid: efd0c11a0617722e6ab99129b604739b, type: 3}
+  m_PrefabInstance: {fileID: 2465439770172797818}
+  m_PrefabAsset: {fileID: 0}
 --- !u!1001 &2465439770178020022
 PrefabInstance:
   m_ObjectHideFlags: 0

Assets/AWSIM/Scenes/Main/AutowareSimulation.unity

@@ -4242,6 +4242,9 @@ PrefabInstance:
     - target: {fileID: 2019290181275176336, guid: 164645b2d105b6b4ab4e3d2e7c65dab2, type: 3}
       propertyPath: EnableGravity
       value: 1
+    - target: {fileID: 1195138626504501105, guid: 164645b2d105b6b4ab4e3d2e7c65dab2, type: 3}
+      propertyPath: Topic
+      value: /awsim/ground_truth/localization/kinematic_state
       objectReference: {fileID: 0}
     - target: {fileID: 2963106999894261215, guid: 164645b2d105b6b4ab4e3d2e7c65dab2, type: 3}
       propertyPath: m_Radius

Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs

@@ -0,0 +1,118 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using ROS2;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// Convert the data output from Odometry Sensor to ROS2 msg and Publish.
+    /// </summary>
+    [RequireComponent(typeof(OdometryRos2Publisher))]
+    public class OdometryRos2Publisher : MonoBehaviour
+    {
+        /// <summary>
+        /// Topic name in pose msg.
+        /// </summary>
+        public string Topic = "/awsim/ground_truth/localization/kinematic_state";
+
+        /// <summary>
+        /// Pose sensor frame id.
+        /// </summary>
+        public string FrameID = "base_link";
+
+        /// <summary>
+        /// QoS settings.
+        /// </summary>
+        public QoSSettings QosSettings = new QoSSettings()
+        {
+            ReliabilityPolicy = ReliabilityPolicy.QOS_POLICY_RELIABILITY_BEST_EFFORT,
+            DurabilityPolicy = DurabilityPolicy.QOS_POLICY_DURABILITY_VOLATILE,
+            HistoryPolicy = HistoryPolicy.QOS_POLICY_HISTORY_KEEP_LAST,
+            Depth = 1,
+        };
+
+        IPublisher<nav_msgs.msg.Odometry> odometryPublisher;
+        nav_msgs.msg.Odometry msg;
+        geometry_msgs.msg.PoseWithCovariance pose;
+        geometry_msgs.msg.TwistWithCovariance twist;
+        std_msgs.msg.String frame_id;
+        OdometrySensor sensor;
+
+        // Start is called before the first frame update
+        void Start()
+        {
+            // Get OdometrySensor component.
+            sensor = GetComponent<OdometrySensor>();
+
+            // Set callback.
+            sensor.OnOutputData += Publish;
+
+            // Create msg.
+            msg = new nav_msgs.msg.Odometry()
+            {
+                Header = new std_msgs.msg.Header()
+                {
+                    Frame_id = FrameID,
+                },
+                Child_frame_id = "",
+                Pose = new geometry_msgs.msg.PoseWithCovariance(),
+                Twist = new geometry_msgs.msg.TwistWithCovariance(),
+            };
+
+            // Create publisher.
+            var qos = QosSettings.GetQoSProfile();
+            odometryPublisher = SimulatorROS2Node.CreatePublisher<nav_msgs.msg.Odometry>(Topic, qos);
+        }
+
+        void Publish(OdometrySensor.OutputData outputData)
+        {
+            // Converts data output from Pose to ROS2 msg
+            var rosPosition = outputData.Position;
+            var rosRotation = outputData.Rotation;
+
+            // TODO: Add double[36] covariance
+            msg.Pose.Pose.Position.X = rosPosition.x;
+            msg.Pose.Pose.Position.Y = rosPosition.y;
+            msg.Pose.Pose.Position.Z = rosPosition.z;
+
+            msg.Pose.Pose.Orientation.X = rosRotation.x;
+            msg.Pose.Pose.Orientation.Y = rosRotation.y;
+            msg.Pose.Pose.Orientation.Z = rosRotation.z;
+            msg.Pose.Pose.Orientation.W = rosRotation.w;
+
+             // Converts data output from Twist to ROS2 msg
+            var rosLinearVelocity = ROS2Utility.UnityToRosPosition(outputData.linearVelocity);
+            var rosAngularVelocity = ROS2Utility.UnityToRosAngularVelocity(outputData.angularVelocity);
+            msg.Twist.Twist.Linear.X = rosLinearVelocity.x;
+            msg.Twist.Twist.Linear.Y = rosLinearVelocity.y;
+            msg.Twist.Twist.Linear.Z = rosLinearVelocity.z;
+
+            msg.Twist.Twist.Angular.X = rosAngularVelocity.x;
+            msg.Twist.Twist.Angular.Y = rosAngularVelocity.y;
+            msg.Twist.Twist.Angular.Z = rosAngularVelocity.z;
+
+            // Add covariance 6x6
+            const int size = 6;
+            for (int i = 0; i < size; i++)
+            {
+                msg.Pose.Covariance[i * size + i] = 1;
+                msg.Twist.Covariance[i * size + i] = 1;
+            }
+
+            // Update msg header.
+            var header = msg as MessageWithHeader;
+            SimulatorROS2Node.UpdateROSTimestamp(ref header);
+
+            msg.Child_frame_id = "base_link";
+
+            // Publish to ROS2.
+            odometryPublisher.Publish(msg);
+        }
+
+        void OnDestroy()
+        {
+            SimulatorROS2Node.RemovePublisher<nav_msgs.msg.Odometry>(odometryPublisher);
+        }
+    }
+}