#!/usr/bin/env python
"""
Build a map of observed markers relative to the camera.
Pipeline
1. Subscribe detected markers pose
2. Subscribe ego pose in time
3. Transform markers local pose to world pose
4. Data association: Identify unique gates by Weighted nearest neighbour (euclidean distance threshold?)
4.5. Correct flipping Z axis with a median moving window?
5. Update pose estimation per observed gate. Lineal KF
6. Publish results as markers for visualization and topic for Planning
7. RMSE Error calculation with ground truth

TODO:
 - Include new message array with confidence per gate by numbers of observations per period
 - Use real ego pose covariances to compute KF
 - Weighted median average for low confidence gates

"""


from __future__ import print_function

import rospy

from visualization_msgs.msg import Marker, MarkerArray
from markertracker_node.msg import GateMarker, GateMarkersArray
from tf2_geometry_msgs import do_transform_pose
from geometry_msgs.msg import Point, PoseWithCovarianceStamped, PoseArray, Pose  #, Point32

import tf2_ros as tf2
import math
#import tf.transformations


class Gate:

    def __init__(self, _id):

        # TODO: Move to params
        self.observations_size = 75  # Only the most relevant observations are kept

        # Put better ones at top, worse ones at the button
        self.observations = list()

        self.total_observations = 0
        self.confidence = 0.0

        self.id = _id
        self.name = 'Gate'+str(_id)

        print('*** New gate:' + self.name)

    def add_marker_observation(self, marker):

        self.total_observations += 1

        observation = {}
        observation['pose'] = marker.pose_cov_stamped.pose.pose.pose
        observation['covariance'] = marker.pose_cov_stamped.pose.covariance
        _area = self._area_of_quadrilateral(marker.corners)
        observation['weight'] = _area

        if len(self.observations) >= self.observations_size:  # start popping
            if _area < self.observations[-1]['weight']:
                return
            else:
                self.observations.pop(-1)

        self.observations.append(observation)

        self.observations = sorted(self.observations, key=lambda o: o['weight'], reverse=True)

        self.confidence = self._calc_confidence()

        #print(self.observations[0])

    def _calc_confidence(self):
        # [0,1] Confidence based on observations to be sure about a gate is real
        #confidence = self.total_observations / self.observations_size

        confidence = len(self.observations) / float(self.observations_size)
        if confidence > 1.0: confidence = 1.0

        return confidence

    def get_best_pose_estimation(self):

        pose = self.observations[0]['pose']

        return pose

    def _area_of_quadrilateral(self, corners):
        a = math.sqrt((corners[0] - corners[2]) ** 2 + (corners[1] - corners[3]) ** 2)
        b = math.sqrt((corners[2] - corners[4]) ** 2 + (corners[3] - corners[5]) ** 2)
        c = math.sqrt((corners[4] - corners[6]) ** 2 + (corners[5] - corners[7]) ** 2)
        d = math.sqrt((corners[6] - corners[0]) ** 2 + (corners[7] - corners[1]) ** 2)
        return (a + b + c + d) / 2

    def euclidean_distance_observation(self, marker):
        point1 = self.observations[0]['pose'].position
        point2 = marker.pose_cov_stamped.pose.pose.pose.position
        return math.sqrt((point2.x - point1.x) ** 2 + (point2.y - point1.y) ** 2 + (point2.z - point1.z) ** 2)


class GateMap:

    def __init__(self):
        self.gates = list()
        # TODO: move to params
        self.d_threshold = 3  # cluster observation when closer than threshold by euclidean distance in meters

    def process_marker_observation(self, marker):

        # First observation

        if len(self.gates) == 0:
            gate = Gate(1)
            gate.add_marker_observation(marker)
            self.gates.append(gate)
            return

        for g in self.gates:

            print(g.name, g.total_observations, g.confidence)

            if g.euclidean_distance_observation(marker) < self.d_threshold:
                g.add_marker_observation(marker)
                return

        # Create a new gate in the map
        gate = Gate(len(self.gates)+1)
        gate.add_marker_observation(marker)
        self.gates.append(gate)




class SubscriberPublisherObservations:

    def __init__(self, node_name):

        self.node_name = node_name

        # Params
        _input_markers_topic = rospy.get_param("~input_markers_topic", default='/markertracker_node/gate_markers')
        self.camera_frame_id = rospy.get_param("~parent_frame_id", default='firefly/vi_sensor/base_link')
        self.fixed_frame_id = rospy.get_param("~fixed_frame_id", default='world')

        # Subscribers
        self.markers_sub = rospy.Subscriber(_input_markers_topic, GateMarkersArray, self._markers_callback, queue_size=100)
        self.tf_buffer = tf2.Buffer()
        self.tf_listener = tf2.TransformListener(self.tf_buffer)

        # To store the gate map
        self.gates_map = GateMap()


        # Publishers
        self.poses_pub = rospy.Publisher(node_name + '/poses', PoseArray, queue_size=100)
        self.viz_markers_pub = rospy.Publisher(node_name + '/visualization_markers', MarkerArray, queue_size=100)

        ## Rospy loop
        r = rospy.Rate(5)
        while not rospy.is_shutdown():
            #self.marker_viz_pub.publish(self.ground_truth_markers)
            #self.gate_pose_pub.publish(self.ground_truth_poses)

            # Publish best map estimations
            self._build_gate_messages_and_publish()

            r.sleep()



    def _build_gate_messages_and_publish(self):

        #TODO: move to params
        _confidence_threshold = 0.05  # Ignore gates with lower confidence than threshold

        if len(self.gates_map.gates) == 0: return False

        marker_viz_array = MarkerArray()
        pose_array = PoseArray()
        pose_array.header.frame_id = self.fixed_frame_id

        for g in self.gates_map.gates:

            if g.confidence < _confidence_threshold: continue

            pose = g.get_best_pose_estimation()
            gate_id = g.id

            pose_array.poses.append(pose)

            marker_msg = self._create_gate_rviz_marker(pose, gate_id, self.fixed_frame_id)
            _str_label = "{} {:.0f}%".format(g.name, g.confidence*100)
            label_msg = self._create_gate_rviz_label(pose, gate_id, _str_label, self.fixed_frame_id)
            marker_viz_array.markers.append(label_msg)
            marker_viz_array.markers.append(marker_msg)


        self.poses_pub.publish(pose_array)
        self.viz_markers_pub.publish(marker_viz_array)


    def _create_gate_rviz_marker(self, pose, gate_id, frame_id):

        marker = Marker()
        marker.header.stamp = rospy.Time.now()

        marker.header.frame_id = frame_id
        marker.ns = self.node_name + '/gate_marker'
        marker.id = gate_id

        marker.type = marker.CUBE
        marker.action = marker.ADD

        # TODO: dims to params
        marker.scale.x = 0.025
        marker.scale.y = 1
        marker.scale.z = 1

        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 1.0
        marker.color.a = 0.5

        marker.lifetime = rospy.Duration()  # forever

        marker.pose = pose

        return marker

    def _create_gate_rviz_label(self, pose, gate_id, text, frame_id):

        marker = Marker()
        marker.header.stamp = rospy.Time.now()

        marker.header.frame_id = frame_id
        marker.ns = self.node_name + '/gate_label'
        marker.id = gate_id

        marker.text = text
        marker.type = marker.TEXT_VIEW_FACING
        marker.action = marker.ADD

        # TODO: dims to params
        #marker.scale.x = 0.025
        #marker.scale.y = 1
        marker.scale.z = 0.5

        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 1.0
        marker.color.a = 0.75

        marker.lifetime = rospy.Duration()  # forever

        marker.pose = pose

        return marker


    def _markers_callback(self, data):

        if len(data.marker) <= 0:
            return None

        stamp = data.camera_frame_stamp  # For synchronizing TF

        try:
            camera_tf = self.tf_buffer.lookup_transform(self.fixed_frame_id, self.camera_frame_id, stamp, rospy.Duration(0.1))
            #print('camera_tf', camera_tf)
        except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException):
            return None

        # world_pose_array = MarkersArray()

        for m in data.marker:
            pose = m.pose_cov_stamped.pose
            #print(pose)
            pose_stamped_world = do_transform_pose(pose, camera_tf)
            m.pose_cov_stamped.pose.pose = pose_stamped_world
            self.gates_map.process_marker_observation(m)

            #print(pose_stamped_world)





if __name__ == '__main__':

    _node_name = 'gates_map_node'

    print('* {} starting... '.format(_node_name), end="")

    rospy.init_node(_node_name, anonymous=True)

    SubscriberPublisherObservations(_node_name)

    print('Ready.')

    rospy.spin()

#!/usr/bin/env python
"""
Build a map of observed markers relative to the camera.
Pipeline
1. Subscribe detected markers pose
2. Subscribe ego pose in time
3. Transform markers local pose to world pose
4. Data association: Identify unique gates by Weighted nearest neighbour (euclidean distance threshold?)
4.5. Correct flipping Z axis with a median moving window?
5. Update pose estimation per observed gate. Lineal KF
6. Publish results as markers for visualization and topic for Planning
7. RMSE Error calculation with ground truth

TODO:
 - Include new message array with confidence per gate by numbers of observations per period
 - Use real ego pose covariances to compute KF
 - Weighted median average for low confidence gates

"""


from __future__ import print_function

import rospy

from visualization_msgs.msg import Marker, MarkerArray
from markertracker_node.msg import GateMarker, GateMarkersArray
from tf2_geometry_msgs import do_transform_pose
from geometry_msgs.msg import Point, PoseWithCovarianceStamped, PoseArray, Pose  #, Point32

import tf2_ros as tf2
import math
#import tf.transformations


class Gate:

    def __init__(self, _id):

        # TODO: Move to params
        self.observations_size = 75  # Only the most relevant observations are kept

        # Put better ones at top, worse ones at the button
        self.observations = list()

        self.total_observations = 0
        self.confidence = 0.0

        self.id = _id
        self.name = 'Gate'+str(_id)

        print('*** New gate:' + self.name)

    def add_marker_observation(self, marker):

        self.total_observations += 1

        observation = {}
        observation['pose'] = marker.pose_cov_stamped.pose.pose.pose
        observation['covariance'] = marker.pose_cov_stamped.pose.covariance
        _area = self._area_of_quadrilateral(marker.corners)
        observation['weight'] = _area

        if len(self.observations) >= self.observations_size:  # start popping
            if _area < self.observations[-1]['weight']:
                return
            else:
                self.observations.pop(-1)

        self.observations.append(observation)

        self.observations = sorted(self.observations, key=lambda o: o['weight'], reverse=True)

        self.confidence = self._calc_confidence()

        #print(self.observations[0])

    def _calc_confidence(self):
        # [0,1] Confidence based on observations to be sure about a gate is real
        #confidence = self.total_observations / self.observations_size

        confidence = len(self.observations) / float(self.observations_size)
        if confidence > 1.0: confidence = 1.0

        return confidence

    def get_best_pose_estimation(self):

        pose = self.observations[0]['pose']

        return pose

    def _area_of_quadrilateral(self, corners):
        a = math.sqrt((corners[0] - corners[2]) ** 2 + (corners[1] - corners[3]) ** 2)
        b = math.sqrt((corners[2] - corners[4]) ** 2 + (corners[3] - corners[5]) ** 2)
        c = math.sqrt((corners[4] - corners[6]) ** 2 + (corners[5] - corners[7]) ** 2)
        d = math.sqrt((corners[6] - corners[0]) ** 2 + (corners[7] - corners[1]) ** 2)
        return (a + b + c + d) / 2

    def euclidean_distance_observation(self, marker):
        point1 = self.observations[0]['pose'].position
        point2 = marker.pose_cov_stamped.pose.pose.pose.position
        return math.sqrt((point2.x - point1.x) ** 2 + (point2.y - point1.y) ** 2 + (point2.z - point1.z) ** 2)


class GateMap:

    def __init__(self):
        self.gates = list()
        # TODO: move to params
        self.d_threshold = 3  # cluster observation when closer than threshold by euclidean distance in meters

    def process_marker_observation(self, marker):

        # First observation

        if len(self.gates) == 0:
            gate = Gate(1)
            gate.add_marker_observation(marker)
            self.gates.append(gate)
            return

        for g in self.gates:

            print(g.name, g.total_observations, g.confidence)

            if g.euclidean_distance_observation(marker) < self.d_threshold:
                g.add_marker_observation(marker)
                return

        # Create a new gate in the map
        gate = Gate(len(self.gates)+1)
        gate.add_marker_observation(marker)
        self.gates.append(gate)




class SubscriberPublisherObservations:

    def __init__(self, node_name):

        self.node_name = node_name

        # Params
        _input_markers_topic = rospy.get_param("~input_markers_topic", default='/markertracker_node/gate_markers')
        self.camera_frame_id = rospy.get_param("~parent_frame_id", default='firefly/vi_sensor/base_link')
        self.fixed_frame_id = rospy.get_param("~fixed_frame_id", default='world')

        # Subscribers
        self.markers_sub = rospy.Subscriber(_input_markers_topic, GateMarkersArray, self._markers_callback, queue_size=100)
        self.tf_buffer = tf2.Buffer()
        self.tf_listener = tf2.TransformListener(self.tf_buffer)

        # To store the gate map
        self.gates_map = GateMap()


        # Publishers
        self.poses_pub = rospy.Publisher(node_name + '/poses', PoseArray, queue_size=100)
        self.viz_markers_pub = rospy.Publisher(node_name + '/visualization_markers', MarkerArray, queue_size=100)

        ## Rospy loop
        r = rospy.Rate(5)
        while not rospy.is_shutdown():
            #self.marker_viz_pub.publish(self.ground_truth_markers)
            #self.gate_pose_pub.publish(self.ground_truth_poses)

            # Publish best map estimations
            self._build_gate_messages_and_publish()

            r.sleep()



    def _build_gate_messages_and_publish(self):

        #TODO: move to params
        _confidence_threshold = 0.05  # Ignore gates with lower confidence than threshold

        if len(self.gates_map.gates) == 0: return False

        marker_viz_array = MarkerArray()
        pose_array = PoseArray()
        pose_array.header.frame_id = self.fixed_frame_id

        for g in self.gates_map.gates:

            if g.confidence < _confidence_threshold: continue

            pose = g.get_best_pose_estimation()
            gate_id = g.id

            pose_array.poses.append(pose)

            marker_msg = self._create_gate_rviz_marker(pose, gate_id, self.fixed_frame_id)
            _str_label = "{} {:.0f}%".format(g.name, g.confidence*100)
            label_msg = self._create_gate_rviz_label(pose, gate_id, _str_label, self.fixed_frame_id)
            marker_viz_array.markers.append(label_msg)
            marker_viz_array.markers.append(marker_msg)


        self.poses_pub.publish(pose_array)
        self.viz_markers_pub.publish(marker_viz_array)


    def _create_gate_rviz_marker(self, pose, gate_id, frame_id):

        marker = Marker()
        marker.header.stamp = rospy.Time.now()

        marker.header.frame_id = frame_id
        marker.ns = self.node_name + '/gate_marker'
        marker.id = gate_id

        marker.type = marker.CUBE
        marker.action = marker.ADD

        # TODO: dims to params
        marker.scale.x = 0.025
        marker.scale.y = 1
        marker.scale.z = 1

        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 1.0
        marker.color.a = 0.5

        marker.lifetime = rospy.Duration()  # forever

        marker.pose = pose

        return marker

    def _create_gate_rviz_label(self, pose, gate_id, text, frame_id):

        marker = Marker()
        marker.header.stamp = rospy.Time.now()

        marker.header.frame_id = frame_id
        marker.ns = self.node_name + '/gate_label'
        marker.id = gate_id

        marker.text = text
        marker.type = marker.TEXT_VIEW_FACING
        marker.action = marker.ADD

        # TODO: dims to params
        #marker.scale.x = 0.025
        #marker.scale.y = 1
        marker.scale.z = 0.5

        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 1.0
        marker.color.a = 0.75

        marker.lifetime = rospy.Duration()  # forever

        marker.pose = pose

        return marker


    def _markers_callback(self, data):

        if len(data.marker) <= 0:
            return None

        stamp = data.camera_frame_stamp  # For synchronizing TF

        try:
            camera_tf = self.tf_buffer.lookup_transform(self.fixed_frame_id, self.camera_frame_id, stamp, rospy.Duration(0.1))
            #print('camera_tf', camera_tf)
        except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException):
            return None

        # world_pose_array = MarkersArray()

        for m in data.marker:
            pose = m.pose_cov_stamped.pose
            #print(pose)
            pose_stamped_world = do_transform_pose(pose, camera_tf)
            m.pose_cov_stamped.pose.pose = pose_stamped_world
            self.gates_map.process_marker_observation(m)

            #print(pose_stamped_world)





if __name__ == '__main__':

    _node_name = 'gates_map_node'

    print('* {} starting... '.format(_node_name), end="")

    rospy.init_node(_node_name, anonymous=True)

    SubscriberPublisherObservations(_node_name)

    print('Ready.')

    rospy.spin()

#!/usr/bin/env python Build a map of observed markers relative to the camera. Pipeline 1. Subscribe detected markers pose 2. Subscribe ego pose in time 3. Transform markers local pose to world pose 4. Data association: Identify unique gates by Weighted nearest neighbour (euclidean distance threshold?) 4.5. Correct flipping Z axis with a median moving window? 5. Update pose estimation per observed gate. Lineal KF 6. Publish results as markers for visualization and topic for Planning 7. RMSE Error calculation with ground truth  TODO:  - Include new message array with confidence per gate by numbers of observations per period  - Use real ego pose covariances to compute KF  - Weighted median average for low confidence gates #, Point32 #import tf.transformations # TODO: Move to params # Only the most relevant observations are kept # Put better ones at top, worse ones at the button # start popping #print(self.observations[0]) # [0,1] Confidence based on observations to be sure about a gate is real #confidence = self.total_observations / self.observations_size # TODO: move to params # cluster observation when closer than threshold by euclidean distance in meters # First observation # Create a new gate in the map # Params # Subscribers # To store the gate map # Publishers ## Rospy loop #self.marker_viz_pub.publish(self.ground_truth_markers) #self.gate_pose_pub.publish(self.ground_truth_poses) # Publish best map estimations #TODO: move to params # Ignore gates with lower confidence than threshold # TODO: dims to params # forever # TODO: dims to params #marker.scale.x = 0.025 #marker.scale.y = 1 # forever # For synchronizing TF #print('camera_tf', camera_tf) # world_pose_array = MarkersArray() #print(pose) #print(pose_stamped_world)