-
Notifications
You must be signed in to change notification settings - Fork 104
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #178 from tier4/feature/add_odometry_sensor
feat(odometry_sensor): add ground truth odometry to Unity scene
- Loading branch information
Showing
9 changed files
with
397 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
118 changes: 118 additions & 0 deletions
118
Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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); | ||
| } | ||
| } | ||
| } |
11 changes: 11 additions & 0 deletions
11
Assets/AWSIM/Scripts/Sensors/Odometry/OdometryRos2Publisher.cs.meta
Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.
Oops, something went wrong.
Oops, something went wrong.