NoVRRoboterControl.cs 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141
  1. /* NoVRRoboterControl.cs
  2. * authors: Lydia ebbinghaus, Ayumi Bischoff, Yannic Seidler
  3. */
  4. using System.Collections;
  5. using UnityEngine;
  6. using UnityEngine.InputSystem;
  7. using RosSharp.RosBridgeClient;
  8. using Valve.VR.InteractionSystem;
  9. // Class to Control the Robot in Unity and getting control signals over the Controller.
  10. // Attach to robot prefab in 2D mode (not recommended)
  11. public class NoVRRoboterControl : MonoBehaviour
  12. {
  13. // Just for using XBoxController &Keayboard for control.
  14. XBoxController controler;
  15. float moveforward;
  16. float movebackward;
  17. float rotateL;
  18. float rotateR;
  19. // Subscriber of position and inputreader of VR-controller.
  20. private PoseStampedSubscriber robot_pose_sub;
  21. Vector2 controlSignal;
  22. // Calibration variables.
  23. private int changed;
  24. public int calibrationtime;
  25. public float speed;
  26. private void Awake()
  27. {
  28. controler = new XBoxController();
  29. MovementSetUp();
  30. }
  31. // Setup of control with Input of old gamepad or keyboard..
  32. private void MovementSetUp()
  33. {
  34. controler.Movement.Moveforward.performed += (InputAction.CallbackContext context) => moveforward = context.ReadValue<float>() * speed;
  35. controler.Movement.Moveforward.canceled += (InputAction.CallbackContext context) => moveforward = 0;
  36. controler.Movement.Movebackward.performed += (InputAction.CallbackContext context) => movebackward = context.ReadValue<float>() * speed;
  37. controler.Movement.Movebackward.canceled += (InputAction.CallbackContext context) => movebackward = 0;
  38. controler.Movement.RotationL.performed += (InputAction.CallbackContext context) => rotateL = context.ReadValue<float>() * speed;
  39. controler.Movement.RotationL.canceled += (InputAction.CallbackContext context) => rotateL = 0;
  40. controler.Movement.RotationR.performed += (InputAction.CallbackContext context) => rotateR = context.ReadValue<float>() * speed;
  41. controler.Movement.RotationR.canceled += (InputAction.CallbackContext context) => rotateR = 0;
  42. }
  43. private void OnEnable()
  44. {
  45. Debug.Log("Controller enabled");
  46. controler.Movement.Enable();
  47. }
  48. private void OnDisable()
  49. {
  50. Debug.Log("Controller disabled");
  51. controler.Movement.Disable();
  52. }
  53. // Start is called before the first frame update.
  54. // Starting coroutine and declerating variables.
  55. void Start()
  56. {
  57. changed=0;
  58. robot_pose_sub = GameObject.Find("RosBridge").GetComponent<PoseStampedSubscriber>();
  59. Debug.Log("Starting to update position");
  60. StartCoroutine("timer");
  61. }
  62. // Update is called once per frame.
  63. // Calibrates robot in the first "calibrationtime" cycles.
  64. void Update()
  65. {
  66. if(changed<calibrationtime)
  67. {
  68. calibrate();
  69. changed++;
  70. }
  71. // Experimental: Enable to control robot in unity and in simulation parallel.
  72. //moveRobot();
  73. }
  74. // Moving robot in unity.
  75. private void moveRobot()
  76. {
  77. Vector3 b = new Vector3(-movebackward,0,0)*Time.deltaTime;
  78. transform.Translate(b, Space.Self);
  79. Vector3 m = new Vector3(moveforward,0,0)*Time.deltaTime;
  80. transform.Translate(m, Space.Self);
  81. Vector2 r = new Vector2(rotateR, -rotateL) *speed*10* Time.deltaTime;
  82. transform.Rotate(0, r.x, 0);
  83. transform.Rotate(0, r.y, 0);
  84. }
  85. // Set position of robot to position of real robot.
  86. private void calibrate()
  87. {
  88. this.transform.position = robot_pose_sub.position;
  89. this.transform.rotation = robot_pose_sub.rotation;
  90. }
  91. // Timer in a coroutine to calibrate every 0.1f seconds.
  92. IEnumerator timer()
  93. {
  94. while(true)
  95. {
  96. calibrate();
  97. yield return new WaitForSeconds(0.05f);
  98. }
  99. }
  100. // Returns control signal for linear and angular velocity for old gamepad input
  101. public (float lin, float ang) getControlSignalXBox()
  102. {
  103. float x,y;
  104. if(moveforward==0)
  105. {
  106. x=movebackward;
  107. }else
  108. {
  109. x=-moveforward;
  110. }
  111. if(rotateL==0){
  112. y=rotateR;
  113. }else
  114. {
  115. y=-rotateL;
  116. }
  117. return (x*Time.deltaTime*10, y*speed*10* Time.deltaTime);
  118. }
  119. }