-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathmocap.py
152 lines (111 loc) · 3.96 KB
/
mocap.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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Jul 28 14:56:48 2021
@author: Rayan Bahrami
Safe Autonomous Systems Lab (SAS Lab)
Stevens Institute of Technology
"""
import rospy
import tf
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import numpy as np
import time
class rosClock:
"""
Description ...
This object (rosClock) was adopted from TimeHelper object in
https://github.com/USC-ACTLab/crazyswarm
"""
def __init__(self):
self.rosRate = None
self.rateHz = None
def time(self):
"""Returns the current time in seconds."""
return rospy.Time.now().to_sec()
def sleep(self, duration):
"""Sleeps for the provided duration in seconds."""
rospy.sleep(duration)
def sleepForRate(self, rateHz):
"""Sleeps so that, if called in a loop, executes at specified rate."""
if self.rosRate is None or self.rateHz != rateHz:
self.rosRate = rospy.Rate(rateHz)
self.rateHz = rateHz
self.rosRate.sleep()
def isShutdown(self):
"""Returns true if the script should abort, e.g. from Ctrl-C."""
return rospy.is_shutdown()
class MotionCapture:
"""
TODO:
make the world and child(object) frames configurable.
currently the default structure is consistent only with the
vicon_bridge pkg @ https://github.com/ethz-asl/vicon_bridge
"""
WORLD_FRAME = "/vicon/world"
def __init__(self, OBJECT_FRAME):
rospy.init_node("GroundTruthPose", anonymous=False, disable_signals=True)
self.tflistener = tf.TransformListener()
self.OBJECT_FRAME = (
"/vicon" + str("/") + str(OBJECT_FRAME) + str("/") + str(OBJECT_FRAME)
)
def getPose(self, mode: str = None):
"""
# http://docs.ros.org/en/jade/api/tf/html/python/transformations.html
Parameters
----------
mode : str, optional
DESCRIPTION. The default is None.
mode == "quaternion", "euler", "None"
Returns
-------
TYPE
DESCRIPTION.
euler angles are in radians
"""
self.tflistener.waitForTransform(
MotionCapture.WORLD_FRAME,
self.OBJECT_FRAME,
rospy.Time(0),
rospy.Duration(0.03),
)
position, quaternion = self.tflistener.lookupTransform(
MotionCapture.WORLD_FRAME, self.OBJECT_FRAME, rospy.Time(0)
)
if mode == "quaternion":
rotation = quaternion
else:
(Rx, Ry, Rz) = euler_from_quaternion(quaternion)
rotation = (Rx, Ry, Rz)
# not necessarily:
# (Rx, Ry, Rz) != (roll, pitch, yaw)
# in list format
# TODO:
# specify the units of measurement
# print(position)
# np.array(position)
return np.array(position), np.array(rotation)
if __name__ == "__main__":
# rospy.init_node("GroundTruthPose", anonymous=False)
try:
OBJECT_FRAME = input("Enter the object name defined in VICON Tracker: ")
print("Hello", OBJECT_FRAME + "!")
Drone_GroundTruthPose = MotionCapture(str(OBJECT_FRAME))
rate = rospy.Rate(100) # in Hz
while not rospy.is_shutdown():
pos, rot = Drone_GroundTruthPose.getPose()
print("The Position and Rotation are:")
print("position:", pos)
print("orientation:", rot)
print("Press ctrl+C to stop...")
rate.sleep()
# OBJECT_FRAME = input("Enter the object name defined in VICON Tracker: ")
# print("Hello", OBJECT_FRAME + "!")
# print("The Position and Rotation are:")
# Drone_GroundTruthPose = MotionCapture(str(OBJECT_FRAME))
# pos, rot = Drone_GroundTruthPose.getPose()
# print(pos)
# print(rot)
# except rospy.ROSInterruptException: pass
except KeyboardInterrupt:
pass