class Right_Utility_Frame():
  
    frame = 'base_footprint'
    px = py = pz = 0;
    qx = qy = qz = 0;
    qw = 1;

    def __init__(self):
        rospy.init_node('right_utilitiy_frame_source')
        
        self.updater = rospy.Service('r_utility_frame_update', FrameUpdate, self.update_frame)
        
        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        self.frame = ps.header.frame_id
        self.px = ps.pose.position.x    
        self.py = ps.pose.position.y    
        self.pz = ps.pose.position.z    
        self.qx = ps.pose.orientation.x
        self.qy = ps.pose.orientation.y
        self.qz = ps.pose.orientation.z
        self.qw = ps.pose.orientation.w

        self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "rh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp

class Left_Utility_Frame():
  
    frame = 'base_footprint'
    px = py = pz = 0;
    qx = qy = qz = 0;
    qw = 1;

    def __init__(self):
        rospy.init_node('left_utilitiy_frame_source')
        
        self.updater = rospy.Service('l_utility_frame_update', FrameUpdate, self.update_frame)
        
        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        if not (math.isnan(ps.pose.orientation.x) or 
                math.isnan(ps.pose.orientation.y) or
                math.isnan(ps.pose.orientation.z) or
                math.isnan(ps.pose.orientation.w)):
            self.frame = ps.header.frame_id
            self.px = ps.pose.position.x    
            self.py = ps.pose.position.y    
            self.pz = ps.pose.position.z    
            self.qx = ps.pose.orientation.x
            self.qy = ps.pose.orientation.y
            self.qz = ps.pose.orientation.z
            self.qw = ps.pose.orientation.w
        else:
            rospy.logerr("NAN's sent to l_utility_frame_source")

        self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "lh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp

class OptiTrackClient():
    def __init__(self, addr, port, obj_names, world, dt, rate=120):
        """
        Class tracking optitrack markers through VRPN and publishing their TF frames as well as the transformation
        from the robot's world frame to the optitrack frame
        :param addr: IP of the VRPN server
        :param port: Port of the VRPN server
        :param obj_names: Name of the tracked rigid bodies
        :param world: Name of the robot's world frame (base_link, map, base, ...)
        :param dt: Delta T in seconds whilst a marker is still considered tracked
        :param rate: Rate in Hertz of the publishing loop
        """
        self.rate = rospy.Rate(rate)
        self.obj_names = obj_names
        self.world = world
        self.dt = rospy.Duration(dt)
        self.tfb = TransformBroadcaster()

        self.trackers = []
        for obj in obj_names:
            t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port))
            t.register_change_handler(obj, self.handler, 'position')
            self.trackers.append(t)

        self.tracked_objects = {}

    @property
    def recent_tracked_objects(self):
        """ Only returns the objects that have been tracked less than 20ms ago. """
        f = lambda name: (rospy.Time.now() - self.tracked_objects[name].timestamp)
        return dict([(k, v) for k, v in self.tracked_objects.iteritems() if f(k) < self.dt])

    def handler(self, obj, data):
        self.tracked_objects[obj] = TrackedObject(data['position'],
                                                  data['quaternion'],
                                                  rospy.Time.now())

    def run(self):
        while not rospy.is_shutdown():
            for t in self.trackers:
                t.mainloop()

            # Publish the calibration matrix
            calib_matrix = rospy.get_param("/optitrack/calibration_matrix")
            self.tfb.sendTransform(calib_matrix[0], calib_matrix[1], rospy.Time.now(), "optitrack_frame", self.world)

            # Publish all other markers as frames
            for k, v in self.recent_tracked_objects.iteritems():
                self.tfb.sendTransform(v.position, v.quaternion, v.timestamp, k, "optitrack_frame")

            self.rate.sleep()

class Rebroadcaster(object):
    def __init__(self):
        self.broadcaster = TransformBroadcaster()
        self.ell_param_sub = rospy.Subscriber('ellipsoid_params', EllipsoidParams, self.ell_cb)
        self.transform = None

    def ell_cb(self, ell_msg):
        print "Got transform"
        self.transform = copy.copy(ell_msg.e_frame)

    def send_transform(self):
        print "spinning", self.transform
        if self.transform is not None:
            print "Sending frame"
            self.broadcaster.sendTransform(self.transform)

    def __init__(self):
        rospy.init_node("learn_transform")

        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.possible_base_link_poses = []
        self.baselink_averages = []

        self.rate = rospy.Rate(20)

        self.markers = { "/PATTERN_1": Pose( [-0.56297, 0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_2": Pose( [-0.45097, 0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_3": Pose( [-0.34097, 0.0, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_4": Pose( [-0.34097, -0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_5": Pose( [-0.45097, -0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_6": Pose( [-0.56297, 0.0, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] )
                        }

        self.run()

    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = ROBOT_NAME+"_odom_dummy"
        else:
            self.odom_frame_name = ROBOT_NAME+"_odom"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0))
        self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi))
        self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0))
        self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0))
        self.add_marker_locator(MarkerLocator(4,(0.0,6*12*2.54/100.0),pi))

        self.pose_correction = rospy.get_param('~pose_correction',0.0)
        self.phase_offset = rospy.get_param('~phase_offset',0.0)
        self.is_flipped = rospy.get_param('~is_flipped',False)


        srv = Server(STARPoseConfig, self.config_callback)

        self.marker_sub = rospy.Subscriber("ar_pose_marker",
                                           ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber(ROBOT_NAME+"/odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher(ROBOT_NAME+"/STAR_pose",PoseStamped,queue_size=10)
        self.continuous_pose = rospy.Publisher(ROBOT_NAME+"/STAR_pose_continuous",PoseStamped,queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

    def __init__(self):
        self.listener = TransformListener()
        self.br = TransformBroadcaster()
        self.pub_freq = 100.0
        self.parent_frame_id = "world"
        self.child1_frame_id = "reference1"
        self.child2_frame_id = "reference2"
        self.child3_frame_id = "reference3"
        self.child4_frame_id = "reference4"
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
        rospy.sleep(1.0)

        recorder_0 = RecordingManager("all")
        recorder_1 = RecordingManager("test1")
        recorder_2 = RecordingManager("test2")
        recorder_3 = RecordingManager("test3")

        recorder_0.start()
        recorder_1.start()
        self.pub_line(length=1, time=5)
        recorder_1.stop()
        recorder_2.start()
        self.pub_quadrat(length=2, time=10)
        recorder_2.stop()
        recorder_3.start()
        self.pub_circ(radius=2, time=5)
        recorder_3.stop()
        recorder_0.stop()

    def __init__(self, use_dummy_transform=False):
        rospy.init_node("star_center_positioning_node")

        self.robot_name = rospy.get_param("~robot_name", "")
        self.odom_frame_name = self.robot_name + "_odom"
        self.base_link_frame_name = self.robot_name + "_base_link"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(4, (0.0, 6 * 12 * 2.54 / 100), pi))
        self.add_marker_locator(MarkerLocator(5, (-4 * 12 * 2.54 / 100.0, 14 * 12 * 2.54 / 100.0), pi))

        self.pose_correction = rospy.get_param("~pose_correction", 0.0)
        self.phase_offset = rospy.get_param("~phase_offset", 0.0)
        self.is_flipped = rospy.get_param("~is_flipped", False)

        srv = Server(STARPoseConfig, self.config_callback)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose", PoseStamped, queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous", PoseStamped, queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

    def __init__(self, use_dummy_transform=False):
        print "init"
        rospy.init_node("star_center_positioning_node")
        if use_dummy_transform:
            self.odom_frame_name = ROBOT_NAME + "_odom_dummy"  # identify this odom as ROBOT_NAME's
        else:
            self.odom_frame_name = ROBOT_NAME + "_odom"  # identify this odom as ROBOT_NAME's

        self.marker_locators = {}

        self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))

        self.pose_correction = rospy.get_param("~pose_correction", 0.0)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
        self.odom_sub = rospy.Subscriber(
            ROBOT_NAME + "/odom", Odometry, self.process_odom, queue_size=10
        )  # publish and subscribe to the correct robot's topics
        self.star_pose_pub = rospy.Publisher(ROBOT_NAME + "/STAR_pose", PoseStamped, queue_size=10)
        self.continuous_pose = rospy.Publisher(ROBOT_NAME + "/STAR_pose_continuous", PoseStamped, queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

    def __init__(self):

        if World.tf_listener == None:
            World.tf_listener = TransformListener()
        self._lock = threading.Lock()
        self.surface = None
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer('world_objects')
        bb_service_name = 'find_cluster_bounding_box'
        rospy.wait_for_service(bb_service_name)
        self._bb_service = rospy.ServiceProxy(bb_service_name,
                                            FindClusterBoundingBox)
        rospy.Subscriber('interactive_object_recognition_result',
            GraspableObjectList, self.receive_object_info)
        self._object_action_client = actionlib.SimpleActionClient(
            'object_detection_user_command', UserCommandAction)
        self._object_action_client.wait_for_server()
        rospy.loginfo('Interactive object detection action ' +
                      'server has responded.')
        self.clear_all_objects()
        # The following is to get the table information
        rospy.Subscriber('tabletop_segmentation_markers',
                         Marker, self.receive_table_marker)

        rospy.Subscriber("ar_pose_marker",
                         AlvarMarkers, self.receive_ar_markers)
        self.marker_dims = Vector3(0.04, 0.04, 0.01)

	def __init__(self):
		self.initialized = False		# make sure we don't perform updates before everything is setup
		rospy.init_node('pf')			# tell roscore that we are creating a new node named "pf"

		self.base_frame = "base_link"	# the frame of the robot base
		self.map_frame = "map"			# the name of the map coordinate frame
		self.odom_frame = "odom"		# the name of the odometry coordinate frame
		self.scan_topic = "scan"		# the topic where we will get laser scans from 

		self.n_particles = 300			# the number of particles to use

		self.d_thresh = 0.2				# the amount of linear movement before performing an update
		self.a_thresh = math.pi/6		# the amount of angular movement before performing an update

		self.laser_max_distance = 2.0	# maximumls penalty to assess in the likelihood field model

		# TODO: define additional constants if needed

		#### DELETE BEFORE POSTING
		self.alpha1 = 0.2
		self.alpha2 = 0.2
		self.alpha3 = 0.2
		self.alpha4 = 0.2
		self.z_hit = 0.5
		self.z_rand = 0.5
		self.sigma_hit = 0.1
		##### DELETE BEFORE POSTING

		# Setup pubs and subs

		# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
		self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
		# publish the current particle cloud.  This enables viewing particles in rviz.
		self.particle_pub = rospy.Publisher("particlecloud", PoseArray)
		self.a_particle_pub = rospy.Publisher("particle", PoseStamped)

		# laser_subscriber listens for data from the lidar
		self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

		# enable listening for and broadcasting coordinate transforms
		self.tf_listener = TransformListener()
		self.tf_broadcaster = TransformBroadcaster()

		self.particle_cloud = []

		self.current_odom_xy_theta = []

		# request the map
		# Difficulty level 2

		rospy.wait_for_service("static_map")
		static_map = rospy.ServiceProxy("static_map", GetMap)
		try:
			map = static_map().map
		except:
			print "error receiving map"

		self.occupancy_field = OccupancyField(map)
		self.initialized = True

 def __init__(self):
     self.tfBroadcaster = TransformBroadcaster()
     q = transformations.quaternion_from_euler(0,0,pi)
     self.humanPose = Pose(Point(5, 0, 1.65), Quaternion(*q))
     self.markerPublisher = rospy.Publisher('visualization_marker', Marker)
     rospy.loginfo('Initialized.')
     self.startTime = time.time()
     rospy.init_node('fake_human_node', anonymous=True)

 def __init__(self):
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.active = True
     self.head_pose = PoseStamped()
     self.goal_pub = rospy.Publisher('goal_pose', PoseStamped)
     rospy.Subscriber('/head_center', PoseStamped, self.head_pose_cb)
     rospy.Service('/point_mirror', PointMirror, self.point_mirror_cb)

 def __init__(self, E=1, is_oblate=False):
     self.A = 1
     self.E = E
     #self.B = np.sqrt(1. - E**2)
     self.a = self.A * self.E
     self.is_oblate = is_oblate
     self.center = None
     self.frame_broadcaster = TransformBroadcaster()
     self.center_tf_timer = None

    def __init__(self):
        self.initialized = False        # make sure we don't perform updates before everything is setup
        rospy.init_node('pf')           # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"   # the frame of the robot base
        self.map_frame = "map"          # the name of the map coordinate frame
        self.odom_frame = "odom"        # the name of the odometry coordinate frame
        self.scan_topic = "scan"        # the topic where we will get laser scans from

        self.n_particles = 100          # the number of particles to use

        self.d_thresh = 0.2             # the amount of linear movement before performing an update
        self.a_thresh = math.pi/6       # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0   # maximum penalty to assess in the likelihood field model

        self.sigma = 0.08 

        # TODO: define additional constants if needed

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)

        
            
        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
        self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10)

        # laser_subscriber listens for data from the lidar
        # Dados do Laser: Mapa de verossimilhança/Occupancy field/Likehood map e Traçado de raios.
        # Traçado de raios: Leitura em ângulo que devolve distância, através do sensor. Dado o mapa,
        # extender a direção da distância pra achar um obstáculo. 
        self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

        # enable listening for and broadcasting coordinate transforms
        #atualização de posição(odometria)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.particle_cloud = []

        self.current_odom_xy_theta = []

        #Chamar o mapa a partir de funcao externa
        mapa = chama_mapa.obter_mapa()

        # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
        # TODO: fill in the appropriate service call here.  The resultant map should be assigned be passed
        #       into the init method for OccupancyField

        # for now we have commented out the occupancy field initialization until you can successfully fetch the map
        self.occupancy_field = OccupancyField(mapa)
        self.initialized = True

 def __init__(self):
     rospy.init_node('normal_approach_right')
     rospy.Subscriber('norm_approach_right', PoseStamped, self.update_frame)
     rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose)
     rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move)
     self.goal_out = rospy.Publisher('wt_right_arm_pose_commands', PoseStamped)
     self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped)
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.wt_log_out = rospy.Publisher('wt_log_out', String )

 def __init__(self):
     rospy.init_node('reactive_grasp_right')
     rospy.Subscriber('reactive_grasp_right', PoseStamped, self.update_frame)
     rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose)
     rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move)
     self.goal_out = rospy.Publisher('/reactive_grasp/right/goal', ReactiveGraspActionGoal)
     self.test_out = rospy.Publisher('/right_test_pose', PoseStamped)
     self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped)
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.wt_log_out = rospy.Publisher('wt_log_out', String )

 def __init__(self):
     self.br = TransformBroadcaster()
     self.pub_freq = 20.0
     self.parent_frame_id = "world"
     self.child1_frame_id = "reference1"
     self.child2_frame_id = "reference2"
     self.child3_frame_id = "reference3"
     self.child4_frame_id = "reference4"
     rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
     rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
     rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
     rospy.sleep(1.0)

	def __init__(self):
		self.initialized = False		# make sure we don't perform updates before everything is setup
		rospy.init_node('pf')			# tell roscore that we are creating a new node named "pf"

		self.base_frame = "base_link"	# the frame of the robot base
		self.map_frame = "map"			# the name of the map coordinate frame
		self.odom_frame = "odom"		# the name of the odometry coordinate frame
		self.scan_topic = "scan"		# the topic where we will get laser scans from 

		self.n_particles = 300			# the number of particles to use

		self.d_thresh = 0.2				# the amount of linear movement before performing an update
		self.a_thresh = math.pi/6		# the amount of angular movement before performing an update

		self.laser_max_distance = 2.0	# maximum penalty to assess in the likelihood field model

		# TODO3: define additional constants if needed

		#this seems pretty self explanatory. 

		""" May need to adjust thresh values if robot is to be still.  
		May need to reduce number of particles.
		Dynamic Variables vs. Static Variables, will these be different?
		"""

		# Setup pubs and subs

		# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
		self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
		# publish the current particle cloud.  This enables viewing particles in rviz.
		self.particle_pub = rospy.Publisher("particlecloud", PoseArray)

		# laser_subscriber listens for data from the lidar
		self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

		# enable listening for and broadcasting coordinate transforms
		self.tf_listener = TransformListener()
		self.tf_broadcaster = TransformBroadcaster()

		self.particle_cloud = []

		self.current_odom_xy_theta = []

		# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
		# TODO4: fill in the appropriate service call here.  The resultant map should be assigned be passed
		#		into the init method for OccupancyField
		""" Call the map """
		#rospy call to map "GetMap" imported from nav_msgs
		#set map as called map

		self.occupancy_field = OccupancyField(map)
		self.initialized = True

 def __init__(self):
     if World.tf_listener is None:
         World.tf_listener = TransformListener()
     self._lock = threading.Lock()
     self.surface = None
     self._tf_broadcaster = TransformBroadcaster()
     self._im_server = InteractiveMarkerServer('world_objects')
     self.clear_all_objects()
     self.relative_frame_threshold = 0.4
     # rospy.Subscriber("ar_pose_marker",
     #                  AlvarMarkers, self.receive_ar_markers)
     self.is_looking_for_markers = False
     self.marker_dims = Vector3(0.04, 0.04, 0.01)
     World.world = self