JointStateSubscriberMod.cs 4.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113
  1. /*
  2. © Siemens AG, 2017-2019
  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 and Yannic Seidler.
  15. using System.Collections.Generic;
  16. using UnityEngine;
  17. using System;
  18. namespace RosSharp.RosBridgeClient
  19. {
  20. public class JointStateSubscriberMod : UnitySubscriber<MessageTypes.Sensor.JointState>
  21. {
  22. public List<string> JointNames;
  23. public List<JointStateWriterMod> JointStateWriters;
  24. private robotType type;
  25. bool infoLoaded;
  26. // Modified. Used for initializing lists.
  27. public void JointStateSubscriberInit()
  28. {
  29. var loader = GameObject.Find("ObjectLoader").GetComponent<Loader>();
  30. this.Topic = loader.topic;
  31. JointNames = loader.jointNames;
  32. JointStateWriters = loader.jointStateWriters;
  33. if(!infoLoaded)
  34. {
  35. try
  36. {
  37. type = this.GetComponent<RobotInformation>().robotType;
  38. infoLoaded=true;
  39. }
  40. catch(NullReferenceException)
  41. {
  42. Debug.Log("Robot not loaded yet.");
  43. }
  44. }
  45. }
  46. // Added so is subscribing.
  47. protected override void Start()
  48. {
  49. //modified
  50. JointStateSubscriberInit();
  51. base.Start();
  52. Debug.Log("Starting to update Joints");
  53. }
  54. // Modified du to incorrect representation.
  55. // Joints are not updated correctly. Still work to do.
  56. protected override void ReceiveMessage(MessageTypes.Sensor.JointState message)
  57. {
  58. int index;
  59. //Debug.Log("joint Name : " + message.name[0]);
  60. switch (type)
  61. {
  62. case robotType.drz_telemax:
  63. for (int i = 0; i < message.name.Length; i++)
  64. {
  65. index = JointNames.IndexOf(message.name[i]);
  66. if (index != -1)
  67. {
  68. // For some reason backflippers are represented incorrectly rotatet, therefore manually modified until theres a better solution.
  69. // Attention!: Not a generic solution.
  70. // Undo: delete if-else clause, just leave "JointStateWriters[index].Write((float) message.position[i])";.
  71. if ((i==6)||(i==7))
  72. {
  73. JointStateWriters[index].Write(-(float) message.position[i],type);
  74. }
  75. else
  76. {
  77. JointStateWriters[index].Write((float) message.position[i],type);
  78. }
  79. }
  80. }
  81. return;
  82. case robotType.asterix_ugv:
  83. Debug.Log("joint Name : " + message.name[0]);
  84. for (int i = 0; i < message.name.Length; i++)
  85. {
  86. index = JointNames.IndexOf(message.name[i]);
  87. JointStateWriters[index].Write((float) message.position[i],type);
  88. }
  89. return;
  90. }
  91. for (int i = 0; i < message.name.Length; i++)
  92. {
  93. index = JointNames.IndexOf(message.name[i]);
  94. JointStateWriters[index].Write((float) message.position[i],type);
  95. }
  96. }
  97. }
  98. }