-
Notifications
You must be signed in to change notification settings - Fork 10
/
sensors.py
79 lines (69 loc) · 3.47 KB
/
sensors.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
import GameLogic
from datetime import datetime
def main():
if (GameLogic.messageRate > 0 and GameLogic.messageRateCounter > 60 / GameLogic.messageRate):
base_joint = GameLogic.getCurrentScene().objects["base_joint"]
main_arm = GameLogic.getCurrentScene().objects["main_arm"]
second_arm = GameLogic.getCurrentScene().objects["second_arm"]
head = GameLogic.getCurrentScene().objects["head"]
head_mount = GameLogic.getCurrentScene().objects["head_mount"]
hand = GameLogic.getCurrentScene().objects["BarrettHand"]
gripper = GameLogic.getCurrentScene().objects["Finger1.1"]
base_position = base_joint.localOrientation.to_euler().z
main_arm_position = main_arm.localOrientation.to_euler().y
second_arm_position = second_arm.localOrientation.to_euler().y
head_mount_position = head_mount.localOrientation.to_euler().x
head_position = head.localOrientation.to_euler().y
hand_position = hand.worldPosition
gripper_position = gripper.localOrientation.to_euler().y
if (GameLogic.handContact % 2 == 1):
hand_contact = True
else:
hand_contact = False
message_roboticarm = """
{
"entity": "RoboticArm",
"basePosition": %s,
"mainArmPosition": %s,
"secondArmPosition": %s,
"headPosition": %s,
"headMountPosition": %s,
"gripperPosition": %s,
"handPosition": {
"x": %s,
"y": %s,
"z": %s
},
"gripperHasContact": "%s"
}
""" % (
str(-base_position),
str(main_arm_position),
str(second_arm_position),
str(head_position),
str(head_mount_position),
str(gripper_position),
str(hand_position.x),
str(hand_position.y),
str(hand_position.z),
str(hand_contact)
)
GameLogic.server.client.publish("Sensor-Simulation", payload=message_roboticarm, qos=0, retain=False)
GameLogic.messageRateCounter = 0
if (GameLogic.secondaryMessageRate > 0 and GameLogic.secondaryMessageRateCounter > 60 / GameLogic.secondaryMessageRate):
slider = GameLogic.getCurrentScene().objects["SliderPanel"]
adjuster = GameLogic.getCurrentScene().objects["Adjuster1"]
platform = GameLogic.getCurrentScene().objects["Platform"]
slider_position = slider.worldPosition.x
adjuster_position = adjuster.worldPosition.x
platform_position = platform.localOrientation.to_euler().y
heatplate_temperature = GameLogic.heatplateTemperature
message_slider = "{ \"entity\": \"Slider\", \"sliderPosition\": %s }" % (str(slider_position))
message_adjuster = "{ \"entity\": \"Conveyor\", \"adjusterPosition\": %s }" % (str(adjuster_position))
message_platform = "{ \"entity\": \"TestingRig\", \"platformPosition\": %s, \"heatplateTemperature\": %s}" % (str(platform_position), str(heatplate_temperature))
GameLogic.server.client.publish("Sensor-Simulation", payload=message_slider, qos=0, retain=False)
GameLogic.server.client.publish("Sensor-Simulation", payload=message_adjuster, qos=0, retain=False)
GameLogic.server.client.publish("Sensor-Simulation", payload=message_platform, qos=0, retain=False)
GameLogic.secondaryMessageRateCounter = 0
GameLogic.messageRateCounter = GameLogic.messageRateCounter + 1
GameLogic.secondaryMessageRateCounter = GameLogic.secondaryMessageRateCounter + 1