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()