#!/usr/bin/python

#   bin_collision_planning_scene
#
#   Created on: May 7, 2015
#   Authors:   Francisco Vina
#             fevb <at> kth.se
#

#  Copyright (c) 2015, Francisco Vina, CVAP, KTH
#    All rights reserved.

#    Redistribution and use in source and binary forms, with or without
#    modification, are permitted provided that the following conditions are met:
#       * Redistributions of source code must retain the above copyright
#         notice, this list of conditions and the following disclaimer.
#       * Redistributions in binary form must reproduce the above copyright
#         notice, this list of conditions and the following disclaimer in the
#         documentation and/or other materials provided with the distribution.
#       * Neither the name of KTH nor the
#         names of its contributors may be used to endorse or promote products
#         derived from this software without specific prior written permission.

#    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
#    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
#    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
#    DISCLAIMED. IN NO EVENT SHALL KTH BE LIABLE FOR ANY
#    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
#    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
#    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
#    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
#    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


import rospy
from moveit_commander import PlanningSceneInterface
import moveit_msgs.msg
from tf import TransformListener, TransformBroadcaster
import tf
from tf_conversions import posemath
from geometry_msgs.msg import PoseStamped
import numpy as np
import PyKDL as kdl
import copy
import random


if __name__=='__main__':
    rospy.init_node('order_bin_collision_planning_scene')

    rospy.loginfo('[order_bin_collision_planning_scene]: Waiting for Moveit to load')
    rospy.wait_for_service('/move_group/ompl/set_parameters')

    planning_scene = PlanningSceneInterface()
    rospy.sleep(5.0)

    while not rospy.is_shutdown():
        try:
            base_frame_id = rospy.get_param('base_frame_id', '/base_laser_link')
            shelf_frame_id = rospy.get_param('shelf_frame_id', '/shelf_frame')
            translation_tolerance = rospy.get_param('translation_tolerance', 1e-3)
            rotation_tolerance = rospy.get_param('rotation_tolerance', 0.1*np.pi/180.0)
            # get ros parameters for order bin
            order_bin_params = rospy.get_param('/order_bin')
            break
        except:
            rospy.sleep(random.uniform(0,1))
            continue


    collision_object_pub = rospy.Publisher('/collision_object', moveit_msgs.msg.CollisionObject)
    order_bin_pose = order_bin_params['pose']
    order_bin_size = order_bin_params['size']

    x = order_bin_pose[0]
    y = order_bin_pose[1]
    z = order_bin_pose[2]

    R = order_bin_pose[3]
    P = order_bin_pose[4]
    Y = order_bin_pose[5]

    F_shelf_order_bin = kdl.Frame(kdl.Rotation.RPY(R, P, Y), kdl.Vector(x, y, z))

    F_shelf_order_bin.p[2] = F_shelf_order_bin.p[2] + order_bin_size[2]*0.5

    # collision object stuff
    pose_shelf_order_bin = PoseStamped()
    pose_shelf_order_bin.pose = posemath.toMsg(F_shelf_order_bin)
    pose_shelf_order_bin.header.frame_id = shelf_frame_id

    collision_object_msg = moveit_msgs.msg.CollisionObject()

    F_shelf_previous = kdl.Frame()

    # tf stuff
    F_shelf_order_bin.p[2] = F_shelf_order_bin.p[2] - order_bin_size[2]*0.5
    translation_shelf_order_bin, rotation_shelf_order_bin = posemath.toTf(F_shelf_order_bin)
    tf_listener = TransformListener()
    tf_broadcaster = TransformBroadcaster()

    # define each of the layers of the bin

    # front layer
    front_layer_pose = copy.deepcopy(pose_shelf_order_bin)
    front_layer_pose.pose.position.x = front_layer_pose.pose.position.x + order_bin_size[0]*0.5
    front_layer_size = copy.deepcopy(order_bin_size)
    front_layer_size[0] = 0.03

    # back layer
    back_layer_pose = copy.deepcopy(pose_shelf_order_bin)
    back_layer_pose.pose.position.x = back_layer_pose.pose.position.x - order_bin_size[0]*0.5
    back_layer_size = copy.deepcopy(order_bin_size)
    back_layer_size[0] = 0.03

    # left layer
    left_layer_pose = copy.deepcopy(pose_shelf_order_bin)
    left_layer_pose.pose.position.y = left_layer_pose.pose.position.y + order_bin_size[1]*0.5
    left_layer_size = copy.deepcopy(order_bin_size)
    left_layer_size[1] = 0.03
    left_layer_size[0] = left_layer_size[0] - 0.031

    # right layer
    right_layer_pose = copy.deepcopy(pose_shelf_order_bin)
    right_layer_pose.pose.position.y = right_layer_pose.pose.position.y - order_bin_size[1]*0.5
    right_layer_size = copy.deepcopy(order_bin_size)
    right_layer_size[1] = 0.03
    right_layer_size[0] = right_layer_size[0] - 0.031

    # bottom layer
    bottom_layer_pose = copy.deepcopy(pose_shelf_order_bin)

    bottom_layer_pose.pose.position.z = bottom_layer_pose.pose.position.z - order_bin_size[2]*0.5
    bottom_layer_size = copy.deepcopy(order_bin_size)
    bottom_layer_size[2] = 0.03
    bottom_layer_size[0] = bottom_layer_size[0] - 0.031
    bottom_layer_size[1] = bottom_layer_size[1] - 0.031

    object_added = False


    r = rospy.Rate(1.0)

    print_err_count = 0

    while not rospy.is_shutdown():
        try:
            T_shelf = tf_listener.lookupTransform(base_frame_id, shelf_frame_id, rospy.Time(0))

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            if print_err_count > 3:
                print_err_count = 0
                rospy.logerr('[order_bin_collision_planning_scene]: transform unavailable between ' + base_frame_id + ' and ' + shelf_frame_id)

            print_err_count = print_err_count + 1
            r.sleep()
            continue

        F_shelf = posemath.fromTf(T_shelf)

        if not object_added:

            front_layer_pose.header.stamp = rospy.Time.now()
            back_layer_pose.header.stamp = rospy.Time.now()
            left_layer_pose.header.stamp = rospy.Time.now()
            right_layer_pose.header.stamp = rospy.Time.now()
            bottom_layer_pose.header.stamp = rospy.Time.now()

            names = ['front', 'back', 'left', 'right', 'bottom']
            poses = [front_layer_pose, back_layer_pose, left_layer_pose, right_layer_pose, bottom_layer_pose]
            sizes = [front_layer_size, back_layer_size, left_layer_size, right_layer_size, bottom_layer_size]

            for i in xrange(5):
                planning_scene.add_box('order_bin_' + names[i], poses[i], sizes[i])
                rospy.sleep(0.2)

            object_added = True
            F_shelf_previous = copy.deepcopy(F_shelf)

            # publish tf
            tf_broadcaster.sendTransform(translation_shelf_order_bin, rotation_shelf_order_bin, rospy.Time.now(), 'order_bin', shelf_frame_id)

        else:


            # update only if there is significant change of shelf pose
            (angle_rotation, axis_rotation) = (F_shelf.M.Inverse()*F_shelf_previous.M).GetRotAngle()
            angle_rotation = np.abs(angle_rotation)


            translation = (F_shelf.p-F_shelf_previous.p).Norm()

            if (translation>translation_tolerance) or (angle_rotation>rotation_tolerance):

                for i in xrange(5):
                    collision_object_msg.operation = collision_object_msg.MOVE
                    collision_object_msg.id = 'order_bin_' + names[i]
                    collision_object_msg.mesh_poses = [poses[i].pose]
                    collision_object_msg.header.stamp = rospy.Time.now()
                    collision_object_msg.header.frame_id = shelf_frame_id
                    collision_object_pub.publish(collision_object_msg)
                    rospy.sleep(0.2)

                F_shelf_previous = copy.deepcopy(F_shelf)

            # publish tf
            tf_broadcaster.sendTransform(translation_shelf_order_bin, rotation_shelf_order_bin, rospy.Time.now(), 'order_bin', shelf_frame_id)

        r.sleep()