def __init__(self, proj, shared_data, host, port, x_VICON_name,
y_VICON_name, theta_VICON_name):
"""
Pose handler for VICON system
host (string): The ip address of VICON system (default="10.0.0.102")
port (int): The port of VICON system (default=800)
x_VICON_name (string): The name of the stream for x pose of the robot in VICON system (default="SubjectName:SegmentName <t-X>")
y_VICON_name (string): The name of the stream for y pose of the robot in VICON system (default="SubjectName:SegmentName <t-Y>")
theta_VICON_name (string): The name of the stream for orintation of the robot in VICON system (default="SubjectName:SegmentName <a-Z>")
"""
self.host = host
self.port = port
self.x = x_VICON_name
self.y = y_VICON_name
self.theta = theta_VICON_name
self.s = _pyvicon.ViconStreamer()
self.s.connect(self.host, self.port)
self.s.selectStreams(["Time", self.x, self.y, self.theta])
self.s.startStreams()
# Wait for first data to come in
while self.s.getData() is None:
pass
def __init__(self,
host="10.0.0.102",
port=800,
x_VICON_name="KUKAyouBot2:main body <t-X>",
y_VICON_name="KUKAyouBot2:main body <t-Y>",
theta_VICON_name="KUKAyouBot2:main body <a-Z>",
z_VICON_name="KUKAyoubot2:main body <t-Z>"):
# def __init__(self, host="10.0.0.102", port=800, x_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-X>", y_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-Y>", theta_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <a-Z>"):
"""
Pose handler for VICON system
host (string): The ip address of VICON system (default="10.0.0.102")
port (int): The port of VICON system (default=800)
x_VICON_name (string): The name of the stream for x pose of the robot in VICON system (default="SubjectName:SegmentName <t-X>")
y_VICON_name (string): The name of the stream for y pose of the robot in VICON system (default="SubjectName:SegmentName <t-Y>")
theta_VICON_name (string): The name of the stream for orintation of the robot in VICON system (default="SubjectName:SegmentName <a-Z>")
"""
br = tf.TransformBroadcaster()
self.host = host
self.port = port
self.x = x_VICON_name
self.y = y_VICON_name
self.z = z_VICON_name
self.theta = theta_VICON_name
self.s = _pyvicon.ViconStreamer()
self.s.connect(self.host, self.port)
self.s.selectStreams(["Time", self.x, self.y, self.z, self.theta])
self.s.startStreams()
# Wait for first data to come in
while self.s.getData() is None:
pass
while not rospy.is_shutdown():
print self.s.getData()
pose = self.getPose()
br.sendTransform(
(pose[1], pose[2], pose[3]),
tf.transformations.quaternion_from_euler(0, 0, pose[4]),
rospy.Time.now(), "youbot", "vicon")
def __init__(
self,
host="10.0.0.102",
port=800,
x_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-X>",
y_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-Y>",
theta_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <a-Z>"
):
"""
Pose handler for VICON system
host (string): The ip address of VICON system (default="10.0.0.102")
port (int): The port of VICON system (default=800)
x_VICON_name (string): The name of the stream for x pose of the robot in VICON system (default="SubjectName:SegmentName <t-X>")
y_VICON_name (string): The name of the stream for y pose of the robot in VICON system (default="SubjectName:SegmentName <t-Y>")
theta_VICON_name (string): The name of the stream for orintation of the robot in VICON system (default="SubjectName:SegmentName <a-Z>")
"""
br = tf.TransformBroadcaster()
self.host = host
self.port = port
self.x = x_VICON_name
self.y = y_VICON_name
self.theta = theta_VICON_name
self.s = _pyvicon.ViconStreamer()
self.s.connect(self.host, self.port)
self.s.selectStreams(["Time", self.x, self.y, self.theta])
self.s.startStreams()
# Wait for first data to come in
# while self.s.getData() is None: pass
rate = rospy.rate(10.0)
while not rospy.is_shutdown():
print self.s.getData()
#br.sendTransform((msg.x, msg.y, 0),
# tf.transformations.quaternion_from_euler(0, 0, msg.theta),
# rospy.Time.now(),
# turtlename,
# "world")
rate.sleep()
def checkIfViconObjectNearLocation(self,
vicon_object_name,
poseX,
poseY,
range=0.5,
initial=False):
"""
check if vicon object is near location specified.
vicon_object_name (string): name of the vicon object (default='folder:mainBody')
poseX (float): x-coorindate of the location (default=0.0)
poseY (float): y-coorindate of the location (default=0.0)
"""
if initial:
print "Connecting to Vicon server..."
self.viconServer[vicon_object_name] = _pyvicon.ViconStreamer()
self.viconServer[vicon_object_name].connect("10.0.0.102", 800)
#model_name = "GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01"
#model_name = "folder:mainBody"
self.viconServer[vicon_object_name].selectStreams(["Time"] + [
"{} <{}>".format(vicon_object_name, s) for s in ("t-X", "t-Y")
])
self.viconServer[vicon_object_name].startStreams()
# Wait for first data to come in
while self.viconServer[vicon_object_name].getData() is None:
pass
else:
(t, x, y) = self.viconServer[vicon_object_name].getData()
(t, x, y) = [t / 100, x / 1000, y / 1000]
# Find our current configuration
pose = [poseX / 1000, poseY / 1000]
#if math.sqrt((pose[0]-x)**2+(pose[1]-y)**2) < range:
#print >>sys.__stdout__, "See" + vicon_object_name + ": location: " + str(pose) + vicon_object_name +": " + str(x) + str(y) + "range: " + str(math.sqrt((pose[0]-x)**2+(pose[1]-y)**2))
return math.sqrt((pose[0] - x)**2 + (pose[1] - y)**2) < range
def __init__(self, host="128.84.189.209", port=800, \
x_VICON_name="KinectPB:KinectPB <t-X>", y_VICON_name="KinectPB:KinectPB <t-Y>", z_VICON_name="KinectPB:KinectPB <t-Z>", \
phi_VICON_name="KinectPB:KinectPB <a-X>", theta_VICON_name="KinectPB:KinectPB <a-Y>", psi_VICON_name="KinectPB:KinectPB <a-Z>"):
# def __init__(self, host="10.0.0.102", port=800, x_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-X>", y_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-Y>", theta_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <a-Z>"):
"""
Pose handler for VICON system
host (string): The ip address of VICON system (default="10.0.0.102")
port (int): The port of VICON system (default=800)
x_VICON_name (string): The name of the stream for x pose of the robot in VICON system (default="SubjectName:SegmentName <t-X>")
y_VICON_name (string): The name of the stream for y pose of the robot in VICON system (default="SubjectName:SegmentName <t-Y>")
theta_VICON_name (string): The name of the stream for orintation of the robot in VICON system (default="SubjectName:SegmentName <a-Z>")
"""
br = tf.TransformBroadcaster()
self.host = host
self.port = port
self.x = x_VICON_name
self.y = y_VICON_name
self.z = z_VICON_name
self.phi = phi_VICON_name
self.theta = theta_VICON_name
self.psi = psi_VICON_name
self.s = _pyvicon.ViconStreamer()
self.s.connect(self.host, self.port)
self.s.selectStreams(
["Time", self.x, self.y, self.z, self.phi, self.theta, self.psi])
self.s.startStreams()
rate = rospy.Rate(10.0)
# Wait for first data to come in
while self.s.getData() is None:
print "Waiting for data..."
pass
print "Stream acquired"
while not rospy.is_shutdown():
# print self.s.getData()
pose = self.getPose()
print "pose: "
print pose
A = math.sqrt(
math.pow(pose[4], 2) + math.pow(pose[5], 2) +
math.pow(pose[6], 2))
W = math.cos(A / 2)
if A < 1e-15:
xq = 0
yq = 0
zq = 0
else:
xq = pose[4] / A * math.sin(A / 2)
yq = pose[5] / A * math.sin(A / 2)
zq = pose[6] / A * math.sin(A / 2)
q = [xq, yq, zq, W]
br.sendTransform(
(pose[1], pose[2], pose[3]),
q,
rospy.Time.now(), # should we be use vicon time instead?
"kinectPB",
"vicon")
# br.sendTransform((-0.1, -0.3, 0), transformations.quaternion_from_euler(0, 0, 0, 'rxyz'), rospy.Time.now(), "base", "kinectPB")
# br.sendTransform((-0.1, -0.3, 0), transformations.quaternion_from_euler(0, 0, 0, 'rxyz'), rospy.Time.now(), "baseGoal", "sensorGoal")
rate.sleep()
def __init__(self,
proj,
shared_data,
compareAgainstVicon=False,
track=.119,
wheelBase=.178,
driveWheelTeeth=1,
driveMotorTeeth=1,
wheelDiameter=.0415,
startX=0.0,
startY=0.0,
startTheta=1.5708):
"""
Pose handler for dead reckoning
compareAgainstVicon (bool): Whether Vicon data is to be compared during this simulation (default=False)
track (float): The distance in meters between the left and right wheels on the robot (default=.119)
wheelBase (float): The distance in meters between the front and back wheels on the robot (default=.178)
driveWheelTeeth (int): The teeth on the gear that is directly connected to the drive wheels [1 implies none] (default=1)
driveMotorTeeth (int): The teeth on the gear attached to the drive motor [1 implies none] (default=1)
wheelDiameter (float): The diameter of the drive wheels on the robot (default=.0415)
startX (float): The starting x position of the robot (default=0.0)
startY (float): The starting y position of the robot (default=0.0)
startTheta (float): The starting facing angle of the robot (default=1.5708)
"""
# definition of globals
self.proj = proj
self.x = startX
self.y = startY
self.theta = startTheta #start facing in the positive y direction
self.track = track
self.wheelBase = wheelBase
self.wheelDiameter = wheelDiameter
self.gearRatio = float(driveMotorTeeth / driveWheelTeeth)
self.base = array([self.x, self.y,
self.theta]) #a starting pose for the robot
self.steerAngle = 0 #start with wheels straight on car type bot
print 'Current Pose: ' + str(self.x) + ',' + str(self.y) + ',' + str(
self.theta)
#initialize vicon if you want to compare your data
self.compareAgainstVicon = compareAgainstVicon
if (compareAgainstVicon):
self.s = _pyvicon.ViconStreamer()
self.s.connect("10.0.0.102", 800)
self.s.selectStreams(
['Time', "NXT:NXT <t-X>", "NXT:NXT <t-Y>", "NXT:NXT <a-Z>"])
self.s.startStreams()
while self.s.getData() is None:
pass
self.vPose = self.s.getData()
#know if actuation is causing movement
self.act = proj.h_instance['actuator']
#to clarify when actuating movement is happening
self.actuating = False
#clear log files for location writing
f = open('log.txt', 'w')
f.close()
g = open('raw.txt', 'w')
g.close()
