#!/usr/bin/python



#   shelf_collision_planning_scene
#
#   Created on: April 15, 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 shape_msgs.msg
import moveit_msgs.msg
from tf import TransformListener
import tf
from tf_conversions import posemath
from geometry_msgs.msg import Pose, PoseStamped
import rospkg
import sys
import numpy as np
import PyKDL as kdl
import copy
import random

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

    rospy.loginfo('[shelf_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-2)
            rotation_tolerance = rospy.get_param('rotation_tolerance', 5*np.pi/180.0)
            break
        except:
            rospy.sleep(random.uniform(0,1))
            continue

    collision_object_pub = rospy.Publisher('/collision_object', moveit_msgs.msg.CollisionObject)

    F_shelf_real_cad = kdl.Frame(kdl.Rotation.RPY(np.pi*0.5, 0.0, np.pi*0.5), kdl.Vector(0.4421, 0.0, -0.303))


    tf_listener = TransformListener()

    object_added = False

    # get filepath of shelf stl file
    rp = rospkg.RosPack()
    try:
        shelf_mesh_fp = rp.get_path('pr2_amazon_challenge_sim')
    except:
        rospy.logerr('[shelf_collision_planning_scene]: shelf mesh file not found!')
        sys.exit(1)

    shelf_mesh_fp = shelf_mesh_fp + '/gazebo_models/kiva_pod/meshes/pod_lowres.stl'

    r = rospy.Rate(1.0)

    F_shelf_previous = kdl.Frame()

    shelf_collision_object_msg = moveit_msgs.msg.CollisionObject()

    print_err_count = 0

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

            # transform to cad model reference frame
            F_shelf_real = posemath.fromTf(T_shelf)
            F_shelf_cad = F_shelf_real*F_shelf_real_cad
            T_shelf = posemath.toTf(F_shelf_cad)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            if print_err_count > 3:
                print_err_count = 0
                rospy.logerr('[shelf_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:

            shelf_pose_stamped = PoseStamped()
            shelf_pose_stamped.pose = posemath.toMsg(F_shelf)
            shelf_pose_stamped.header.stamp = rospy.Time.now()
            shelf_pose_stamped.header.frame_id = base_frame_id

            planning_scene.add_mesh('shelf', shelf_pose_stamped, shelf_mesh_fp)
            object_added = True

            F_shelf_previous = posemath.fromMsg(shelf_pose_stamped.pose)


        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):
                shelf_pose = posemath.toMsg(F_shelf)
                shelf_collision_object_msg.operation = shelf_collision_object_msg.MOVE
                shelf_collision_object_msg.id = 'shelf'
                shelf_collision_object_msg.mesh_poses = [shelf_pose]
                shelf_collision_object_msg.header.stamp = rospy.Time.now()
                shelf_collision_object_msg.header.frame_id = base_frame_id

                collision_object_pub.publish(shelf_collision_object_msg)

                F_shelf_previous = copy.deepcopy(F_shelf)

        r.sleep()

