1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768 |
- using UnityEngine;
- namespace RosSharp.RosBridgeClient
- {
- public class PoseStampedSubscriber : UnitySubscriber<MessageTypes.Geometry.PoseStamped>
- {
-
- public Vector3 position;
- public Quaternion rotation;
- public bool isMessageReceived;
- protected override void Start()
- {
- base.Start();
- }
-
- private void Update()
- {
- if (isMessageReceived)
- ProcessMessage();
- }
- protected override void ReceiveMessage(MessageTypes.Geometry.PoseStamped message)
- {
- position = GetPosition(message).Ros2Unity();
- rotation = GetRotation(message).Ros2Unity();
- isMessageReceived = true;
- }
- private void ProcessMessage()
- {
-
-
- }
- private Vector3 GetPosition(MessageTypes.Geometry.PoseStamped message)
- {
- return new Vector3(
- (float)message.pose.position.x,
- (float)message.pose.position.y,
- (float)message.pose.position.z);
- }
- private Quaternion GetRotation(MessageTypes.Geometry.PoseStamped message)
- {
- return new Quaternion(
- (float)message.pose.orientation.x,
- (float)message.pose.orientation.y,
- (float)message.pose.orientation.z,
- (float)message.pose.orientation.w);
- }
- }
- }
|