diff --git a/Assets/AWSIM/Prefabs/Sensors/OdometrySensor.prefab b/Assets/AWSIM/Prefabs/Sensors/OdometrySensor.prefab
new file mode 100644
index 000000000..9cc5e4e16
--- /dev/null
+++ b/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
diff --git a/Assets/AWSIM/Prefabs/Sensors/OdometrySensor.prefab.meta b/Assets/AWSIM/Prefabs/Sensors/OdometrySensor.prefab.meta
new file mode 100644
index 000000000..c517b7c7f
--- /dev/null
+++ b/Assets/AWSIM/Prefabs/Sensors/OdometrySensor.prefab.meta
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: efd0c11a0617722e6ab99129b604739b
+PrefabImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab b/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab
index 8d0a8deb8..ee81bd6c5 100644
--- a/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab
+++ b/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
diff --git a/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity b/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity
index da7a92675..8953a199c 100644
--- a/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity
+++ b/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
diff --git a/Assets/AWSIM/Scripts/Sensors/Odometry.meta b/Assets/AWSIM/Scripts/Sensors/Odometry.meta
new file mode 100644
index 000000000..f16aeebcb
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Sensors/Odometry.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 0ad2cfeda8fc726b7928556004573e27
+folderAsset: yes
+DefaultImporter:
+ externalObjects: {}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs
new file mode 100644
index 000000000..c21b76f68
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs
@@ -0,0 +1,118 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using ROS2;
+
+namespace AWSIM
+{
+ ///
+ /// Convert the data output from Odometry Sensor to ROS2 msg and Publish.
+ ///
+ [RequireComponent(typeof(OdometryRos2Publisher))]
+ public class OdometryRos2Publisher : MonoBehaviour
+ {
+ ///
+ /// Topic name in pose msg.
+ ///
+ public string Topic = "/awsim/ground_truth/localization/kinematic_state";
+
+ ///
+ /// Pose sensor frame id.
+ ///
+ public string FrameID = "base_link";
+
+ ///
+ /// QoS settings.
+ ///
+ 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 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();
+
+ // 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(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(odometryPublisher);
+ }
+ }
+}
\ No newline at end of file
diff --git a/Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs.meta b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs.meta
new file mode 100644
index 000000000..0e407eb82
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7d38bf8600adc068b85469ec916e9330
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/AWSIM/Scripts/Sensors/Odometry/OdometrySensor.cs b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometrySensor.cs
new file mode 100644
index 000000000..c1e874706
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometrySensor.cs
@@ -0,0 +1,106 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace AWSIM
+{
+ ///
+ /// Outputs the true value of odometry.
+ /// TODO: Currently the Mgrs coordinate system is output, but refactor the Environment to Unity coordinate system + offset.
+ ///
+ public class OdometrySensor : MonoBehaviour
+ {
+ ///
+ /// This data is output from OdometrySensor at the OutputHz cycle.
+ ///
+ public class OutputData
+ {
+ public Vector3 Position;
+ public Quaternion Rotation;
+ public Vector3 linearVelocity;
+ public Vector3 angularVelocity;
+
+
+ public OutputData()
+ {
+ Position = new Vector3();
+ Rotation = new Quaternion();
+ linearVelocity = new Vector3();
+ angularVelocity = new Vector3();
+ }
+ }
+
+ ///
+ /// Data output hz.
+ /// Sensor processing and callbacks are called in this hz.
+ ///
+ [Range(0, 100)]
+ public int OutputHz = 60; // Set default the same as fixed update hz.
+ Vector3 lastPosition; // Previous frame position used for acceleration calculation.
+ Vector3 lastLocalVelocity; // Previous frame velocity used for acceleration calculation.
+ QuaternionD lastRotation; // Previous frame rotation used for angular velocity calculation.
+
+ ///
+ /// Delegate used in callbacks.
+ ///
+ public delegate void OnOutputDataDelegate(OutputData outputData);
+
+ ///
+ /// Called each time data is output.
+ ///
+ public OnOutputDataDelegate OnOutputData;
+
+ float timer = 0;
+ OutputData outputData = new OutputData();
+ Transform m_transform;
+
+ void Start()
+ {
+ m_transform = transform;
+ lastPosition = transform.position;
+ lastRotation = new QuaternionD(transform.rotation);
+ }
+
+ void FixedUpdate()
+ {
+ // Matching output to hz.
+ timer += Time.deltaTime;
+ var interval = 1.0f / OutputHz;
+ interval -= 0.00001f; // Allow for accuracy errors.
+ if (timer < interval)
+ return;
+ timer = 0;
+
+ // update ground truth position and rotation.
+ var rosPosition = ROS2Utility.UnityToRosPosition(m_transform.position);
+ outputData.Position = rosPosition + Environment.Instance.MgrsOffsetPosition; // TODO: Handled in Unity coordinate system
+ // ros gnss sensor's pos + mgrs offset pos.
+
+ var rosRotation = ROS2Utility.UnityToRosRotation(m_transform.rotation);
+ outputData.Rotation = rosRotation;
+
+ // Compute angular velocity.
+ var currentRotation = new QuaternionD(transform.rotation);
+ var deltaRotation = currentRotation * QuaternionD.Inverse(lastRotation);
+ deltaRotation.ToAngleAxis(out var angle, out var axis);
+ var angularVelocity = (1.0f / Time.deltaTime) * (float)angle * axis;
+ var localAngularVelocity = transform.InverseTransformDirection(angularVelocity);
+ lastRotation = currentRotation;
+
+ // Compute local velocity.
+ var localVelocity = (transform.InverseTransformDirection(transform.position - lastPosition)) / Time.deltaTime;
+ lastPosition = transform.position;
+
+ // TODO: Temporarily avoid NaN values. Needs investigation.
+ if (float.IsNaN(localAngularVelocity.x) || float.IsNaN(localAngularVelocity.y) || float.IsNaN(localAngularVelocity.z))
+ localAngularVelocity = Vector3.zero;
+
+ // Update output data.
+ outputData.linearVelocity = localVelocity;
+ outputData.angularVelocity = localAngularVelocity;
+
+ // Calls registered callbacks
+ OnOutputData.Invoke(outputData);
+ }
+ }
+}
\ No newline at end of file
diff --git a/Assets/AWSIM/Scripts/Sensors/Odometry/OdometrySensor.cs.meta b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometrySensor.cs.meta
new file mode 100644
index 000000000..bd0817ae5
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Sensors/Odometry/OdometrySensor.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: c30610523d9cbf018803304ee709d387
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant: