123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869 |
- /*
- © Siemens AG, 2017-2018
- Author: Dr. Martin Bischoff (martin.bischoff@siemens.com)
- 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.
- */
- using UnityEngine;
- namespace RosSharp.RosBridgeClient
- {
- public class TwistPublisher : UnityPublisher<MessageTypes.Geometry.Twist>
- {
- public Transform PublishedTransform;
- private MessageTypes.Geometry.Twist message;
- private float previousRealTime;
- private Vector3 previousPosition = Vector3.zero;
- private Quaternion previousRotation = Quaternion.identity;
- protected override void Start()
- {
- base.Start();
- InitializeMessage();
- }
- private void FixedUpdate()
- {
- UpdateMessage();
- }
- private void InitializeMessage()
- {
- message = new MessageTypes.Geometry.Twist();
- message.linear = new MessageTypes.Geometry.Vector3();
- message.angular = new MessageTypes.Geometry.Vector3();
- }
- private void UpdateMessage()
- {
-
- Vector3 linearVelocity = (PublishedTransform.localPosition - previousPosition) / Time.fixedDeltaTime;
- Vector3 angularVelocity = (PublishedTransform.localRotation.eulerAngles - previousRotation.eulerAngles) / Time.fixedDeltaTime;
- message.linear = GetGeometryVector3(linearVelocity.Unity2Ros());
- message.angular = GetGeometryVector3(-angularVelocity.Unity2Ros());
- previousPosition = PublishedTransform.localPosition;
- previousRotation = PublishedTransform.localRotation;
- Publish(message);
- }
- private static MessageTypes.Geometry.Vector3 GetGeometryVector3(Vector3 vector3)
- {
- MessageTypes.Geometry.Vector3 geometryVector3 = new MessageTypes.Geometry.Vector3();
- geometryVector3.x = vector3.x;
- geometryVector3.y = vector3.y;
- geometryVector3.z = vector3.z;
- return geometryVector3;
- }
- }
- }
|