Loader.cs 8.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187
  1. /* Loader.cs
  2. * author: Yannic Seidler & Lydia Ebbinghaus
  3. */
  4. using System.Collections.Generic;
  5. using UnityEngine;
  6. using Valve.VR.InteractionSystem;
  7. using RosSharp.RosBridgeClient;
  8. // Class that prepares the scene for the simulation.
  9. // Attached to GameObject ObjectLoader (Scene: Simulation).
  10. public class Loader : MonoBehaviour
  11. {
  12. public List<string> jointNames { get; set; }
  13. public List<JointStateWriterMod> jointStateWriters { get; set; }
  14. public string topic { get; set; }
  15. SceneManagement manager;
  16. private PoseStampedSubscriber robot_pose_sub;
  17. private GameObject rosBridge;
  18. Vector3 offsetFromRobot;
  19. float angle;
  20. // Start is called before the first frame update.
  21. // Sets robot and player into simulation scene and activates all scripts that are attached to the rosbridge.
  22. void Start()
  23. {
  24. rosBridge = GameObject.Find("RosBridge");
  25. foreach(MonoBehaviour script in rosBridge.GetComponents<MonoBehaviour>())
  26. {
  27. script.enabled = true;
  28. }
  29. robot_pose_sub = rosBridge.GetComponent<PoseStampedSubscriber>();
  30. manager = GameObject.Find("SceneManagement").GetComponent<SceneManagement>();
  31. GameObject robotToLoad = manager.simRobot!= null ? manager.simRobot : null;
  32. // Sets robot into scene.
  33. if (robotToLoad != null)
  34. {
  35. robotToLoad = Instantiate(robotToLoad, robot_pose_sub.position, robot_pose_sub.rotation);
  36. }
  37. SetupSimulation(robotToLoad, robotToLoad.GetComponent<RobotInformation>().robotType);
  38. // Sets player into scene.
  39. Vector3 offset = -(Player.instance.feetPositionGuess - Player.instance.trackingOriginTransform.position);
  40. Player.instance.trackingOriginTransform.position = robotToLoad.transform.position + offset + offsetFromRobot;
  41. Player.instance.transform.GetChild(0).RotateAround(Player.instance.hmdTransform.position, Vector3.up, angle);
  42. // Sets Objects used for teleporting.
  43. GameObject.Find("Teleporting").transform.position = Player.instance.trackingOriginTransform.position;
  44. GameObject.Find("TeleportingGround").transform.position = Player.instance.trackingOriginTransform.position + offset;
  45. // Used for updating joints (works for telemax only until now).
  46. rosBridge.AddComponent<JointStateSubscriberMod>();
  47. Debug.Log("Jointsub added");
  48. }
  49. // Sets variables for positioning the player. Sets joints of a robot for the JointStateSubscriberMod.
  50. private void SetupSimulation(GameObject simRobot, robotType robot)
  51. {
  52. topic = "";
  53. jointNames = new List<string>();
  54. jointStateWriters = new List<JointStateWriterMod>();
  55. switch (robot)
  56. {
  57. case robotType.drz_telemax:
  58. offsetFromRobot = -3f * simRobot.transform.forward;
  59. angle = simRobot.transform.rotation.eulerAngles.y - Player.instance.hmdTransform.rotation.eulerAngles.y;
  60. // Configure joints
  61. topic = "/telemax_control/joint_states";
  62. try
  63. {
  64. jointNames.Add("arm_joint_0");
  65. jointNames.Add("arm_joint_1");
  66. jointNames.Add("arm_joint_2");
  67. jointNames.Add("arm_joint_3");
  68. jointNames.Add("arm_joint_4");
  69. jointNames.Add("arm_joint_5");
  70. jointNames.Add("flipper_back_left_joint");
  71. jointNames.Add("flipper_back_right_joint");
  72. jointNames.Add("flipper_front_left_joint");
  73. jointNames.Add("flipper_front_right_joint");
  74. jointNames.Add("gripper_joint");
  75. List<JointStateWriterMod> temp = new List<JointStateWriterMod>(simRobot.GetComponentsInChildren<JointStateWriterMod>());
  76. assignMatchingJoint(temp, jointStateWriters, "arm_link_0");
  77. assignMatchingJoint(temp, jointStateWriters, "arm_link_1");
  78. assignMatchingJoint(temp, jointStateWriters, "arm_link_2");
  79. assignMatchingJoint(temp, jointStateWriters, "arm_link_3");
  80. assignMatchingJoint(temp, jointStateWriters, "arm_link_4");
  81. assignMatchingJoint(temp, jointStateWriters, "arm_link_5");
  82. assignMatchingJoint(temp, jointStateWriters, "flipper_back_left_link");
  83. assignMatchingJoint(temp, jointStateWriters, "flipper_back_right_link");
  84. assignMatchingJoint(temp, jointStateWriters, "flipper_front_left_link");
  85. assignMatchingJoint(temp, jointStateWriters, "flipper_front_right_link");
  86. assignMatchingJoint(temp, jointStateWriters, "gripper_servo_link");
  87. }
  88. catch (System.Exception e)
  89. {
  90. Debug.Log(e.Message);
  91. }
  92. break;
  93. // To make this work attach "hinge joints" and "JointSTateWriterMod" to arm links and flipper.
  94. // Position and angle of joints doesn't fit to the one of real robot yet.
  95. case robotType.asterix_ugv:
  96. offsetFromRobot = 2f * simRobot.transform.forward;
  97. angle = 180f + simRobot.transform.rotation.eulerAngles.y - Player.instance.hmdTransform.rotation.eulerAngles.y;
  98. //Configure joints
  99. /*topic = "/arm_manipulator_control/joint_states";
  100. try
  101. {
  102. jointNames.Add("arm_link_0");
  103. jointNames.Add("arm_link_1");
  104. jointNames.Add("arm_link_2");
  105. jointNames.Add("arm_link_3");
  106. jointNames.Add("arm_link_4");
  107. jointNames.Add("arm_link_5");
  108. jointNames.Add("flipper_back_left_joint");
  109. jointNames.Add("flipper_back_right_joint");
  110. jointNames.Add("flipper_front_left_joint");
  111. jointNames.Add("flipper_front_right_joint");
  112. jointNames.Add("gripper_joint");
  113. List<JointStateWriterMod> temp = new List<JointStateWriterMod>(simRobot.GetComponentsInChildren<JointStateWriterMod>());
  114. assignMatchingJoint(temp, jointStateWriters, "arm_joint_0");
  115. assignMatchingJoint(temp, jointStateWriters, "arm_joint_1");
  116. assignMatchingJoint(temp, jointStateWriters, "arm_joint_3");
  117. assignMatchingJoint(temp, jointStateWriters, "arm_joint_3");
  118. assignMatchingJoint(temp, jointStateWriters, "arm_joint_4");
  119. assignMatchingJoint(temp, jointStateWriters, "arm_joint_5");
  120. assignMatchingJoint(temp, jointStateWriters, "flipper_back_left_link");
  121. assignMatchingJoint(temp, jointStateWriters, "flipper_back_right_link");
  122. assignMatchingJoint(temp, jointStateWriters, "flipper_front_left_link");
  123. assignMatchingJoint(temp, jointStateWriters, "flipper_front_right_link");
  124. assignMatchingJoint(temp, jointStateWriters, "gripper_servo_link");
  125. }
  126. catch (System.Exception e)
  127. {
  128. Debug.Log(e.Message);
  129. }*/
  130. break;
  131. }
  132. }
  133. // Helps assigning joints to a list that gets passed to the JointStateSubscriberMod.
  134. void assignMatchingJoint(List<JointStateWriterMod> inList, List<JointStateWriterMod> outList, string match)
  135. {
  136. foreach (JointStateWriterMod item in inList)
  137. {
  138. if (item.gameObject.name.Contains(match))
  139. {
  140. outList.Add(item);
  141. inList.Remove(item);
  142. break;
  143. }
  144. }
  145. }
  146. }