TwistPublisherMod.cs 3.2 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798
  1. /*
  2. © Siemens AG, 2017-2018
  3. Author: Dr. Martin Bischoff (martin.bischoff@siemens.com)
  4. Licensed under the Apache License, Version 2.0 (the "License");
  5. you may not use this file except in compliance with the License.
  6. You may obtain a copy of the License at
  7. <http://www.apache.org/licenses/LICENSE-2.0>.
  8. Unless required by applicable law or agreed to in writing, software
  9. distributed under the License is distributed on an "AS IS" BASIS,
  10. WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  11. See the License for the specific language governing permissions and
  12. limitations under the License.
  13. */
  14. // Modified by Lydia Ebbinghaus
  15. using UnityEngine;
  16. using System;
  17. // Publishes control signals to Ros.
  18. // Attached to Rosbridge object.
  19. namespace RosSharp.RosBridgeClient
  20. {
  21. public class TwistPublisherMod : UnityPublisher<MessageTypes.Geometry.Twist>
  22. {
  23. private MessageTypes.Geometry.Twist message;
  24. private RoboterControl robotControl;
  25. public Vector3 linearVelocity;
  26. public Vector3 angularVelocity;
  27. bool roboterLoaded;
  28. // Start is called before first frame update.
  29. // Modified.
  30. protected override void Start()
  31. {
  32. base.Start();
  33. InitializeMessage();
  34. }
  35. // Changed from fixedUpdate to Update, called once per frame.
  36. void Update()
  37. {
  38. UpdateMessage();
  39. }
  40. // Initialize message.
  41. private void InitializeMessage()
  42. {
  43. message = new MessageTypes.Geometry.Twist();
  44. message.linear = new MessageTypes.Geometry.Vector3();
  45. message.angular = new MessageTypes.Geometry.Vector3();
  46. }
  47. // Get control signal from RobotControl and send to ROS.
  48. // Modified.
  49. private void UpdateMessage()
  50. {
  51. try
  52. {
  53. if(!robotControl)
  54. {
  55. robotControl = GameObject.FindWithTag("robot").GetComponent<RoboterControl>();
  56. }
  57. if(robotControl!=null) roboterLoaded=true;
  58. }
  59. catch(NullReferenceException)
  60. {
  61. Debug.Log("TwistPublisherMod: No Gameobject with tag robot. Robot not loaded yet.");
  62. }
  63. // Use this for old Gamepad input.
  64. //linearVelocity = new Vector3(-robotControl..getControlSignalXBox().lin,0,0);
  65. //angularVelocity = new Vector3(0,robotControl.getControlSignalXBox().ang,0);
  66. if (roboterLoaded) {
  67. linearVelocity = new Vector3(robotControl.getControlSignal().y, 0, 0);
  68. angularVelocity = new Vector3(0, robotControl.getControlSignal().x, 0);
  69. // Other interpretation of telemax cooridinates , therefore modified.
  70. message.linear = GetGeometryVector3(linearVelocity);
  71. message.angular = GetGeometryVector3(-angularVelocity.Unity2Ros());
  72. Publish(message);
  73. }
  74. }
  75. // Modified.
  76. private static MessageTypes.Geometry.Vector3 GetGeometryVector3(Vector3 vector3)
  77. {
  78. var geometryVector3 = new MessageTypes.Geometry.Vector3(vector3.x,vector3.y,vector3.z);
  79. return geometryVector3;
  80. }
  81. }
  82. }