#!/usr/bin/env python
# 
#  helper script for spawning models in gazebo
# 
# Provides quick access to the services exposed by gazebo

import roslib, time
roslib.load_manifest('gazebo')

import rospy, sys, os
import string

from gazebo import gazebo_interface

from gazebo.msg import *
from gazebo.srv import *
from std_srvs.srv import Empty
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench
import tf.transformations as tft

world_joint_template = """<link name="world"/>
  <joint name="JOINT_NAME" type="floating" >
    <axis xyz="0 0 1" />
    <parent link="world" />
    <child link="LINK_NAME" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>"""
model_template = """<robot name="MODEL_NAME">
  <link name="LINK_NAME">
    <inertial>
      <mass value="MASS" />
      <origin xyz="0 0 0" /> 
      <inertia  ixx="IXX" ixy="0.0"  ixz="0.0"  iyy="IYY"  iyz="0.0"  izz="IZZ" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        VISUAL_GEOMETRY
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0 " />
      <geometry>
        COLLISION_GEOMETRY
      </geometry>
    </collision>
  </link>
  <gazebo reference="LINK_NAME">
    <material>MATERIAL</material>
    <turnGravityOff>GRAVITY</turnGravityOff>
  </gazebo>
</robot>"""

def usage():
    print '''Commands:
    -[urdf|gazebo|trimesh] - specify incoming xml is urdf or gazebo format
    -[file|param] [<file_name>|<param_name>] - source of the model xml or the trimesh file
    -model <model_name> - name of the model to be spawned.
    -reference_frame <entity_name> - optinal: name of the model/body where initial pose is defined.
                                     If left empty or specified as "world", gazebo world frame is used.
    -namespace <ros_namespace> - optional: all subsequent ROS interface plugins will be inside of this namespace.
    -unpause - optional: !!!Experimental!!! unpause physics after spawning model
    -trimesh_mass <mass in kg> - required if -trimesh is used: linear mass
    -trimesh_ixx <moment of inertia in kg*m^2> - required if -trimesh is used: moment of inertia about x-axis
    -trimesh_iyy <moment of inertia in kg*m^2> - required if -trimesh is used: moment of inertia about y-axis
    -trimesh_izz <moment of inertia in kg*m^2> - required if -trimesh is used: moment of inertia about z-axis
    -trimesh_gravity <bool> - required if -trimesh is used: gravity turned on for this trimesh model
    -trimesh_material <material name as a string> - required if -trimesh is used: E.g. Gazebo/Blue
    -trimesh_name <link name as a string> - required if -trimesh is used: name of the link containing the trimesh
    -x <x in meters> - optional: initial pose, use 0 if left out
    -y <y in meters> - optional: initial pose, use 0 if left out
    -z <z in meters> - optional: initial pose, use 0 if left out
    -R <roll in radians> - optional: initial pose, use 0 if left out
    -P <pitch in radians> - optional: initial pose, use 0 if left out
    -Y <yaw in radians> - optional: initial pose, use 0 if left out
    '''
    sys.exit(1)

class SpawnModel():
    def __init__(self):
        self.initial_xyz        = [0,0,0]
        self.initial_rpy        = [0,0,0]
        self.initial_q          = [0,0,0,1]
        self.file_name          = ""
        self.param_name         = ""
        self.model_name         = ""
        self.robot_namespace    = ""
        self.reference_frame    = ""
        self.unpause_physics    = False
        self.urdf_format        = False
        self.gazebo_format      = False

    def parseUserInputs(self):
        # get goal from commandline
        for i in range(0,len(sys.argv)):
          if sys.argv[i] == '-unpause':
            self.unpause_physics = True
          if sys.argv[i] == '-urdf':
            if self.gazebo_format == True:
                print "Error: you cannot specify both urdf and gazebo format xml, must pick one"
                sys.exit(0)
            else:
              self.urdf_format = True;
          if sys.argv[i] == '-gazebo':
            if self.urdf_format == True:
                print "Error: you cannot specify both urdf and gazebo format xml, must pick one"
                sys.exit(0)
            else:
              self.gazebo_format = True;
          if sys.argv[i] == '-param':
            if len(sys.argv) > i+1:
              if self.file_name != "":
                print "Error: you cannot specify file name if parameter name is given, must pick one source of model xml"
                sys.exit(0)
              else:
                self.param_name = sys.argv[i+1]
          if sys.argv[i] == '-file':
            if len(sys.argv) > i+1:
              if self.param_name != "":
                print "Error: you cannot specify parameter if file name is given, must pick one source of model xml"
                sys.exit(0)
              else:
                self.file_name = sys.argv[i+1]
          if sys.argv[i] == '-model':
            if len(sys.argv) > i+1:
              self.model_name = sys.argv[i+1]
          if sys.argv[i] == '-reference_frame':
            if len(sys.argv) > i+1:
              self.reference_frame = sys.argv[i+1]
          if sys.argv[i] == '-namespace':
            if len(sys.argv) > i+1:
              self.robot_namespace = sys.argv[i+1]
          if sys.argv[i] == '-x':
            if len(sys.argv) > i+1:
              self.initial_xyz[0] = float(sys.argv[i+1])
          if sys.argv[i] == '-y':
            if len(sys.argv) > i+1:
              self.initial_xyz[1] = float(sys.argv[i+1])
          if sys.argv[i] == '-z':
            if len(sys.argv) > i+1:
              self.initial_xyz[2] = float(sys.argv[i+1])
          if sys.argv[i] == '-R':
            if len(sys.argv) > i+1:
              self.initial_rpy[0] = float(sys.argv[i+1])
          if sys.argv[i] == '-P':
            if len(sys.argv) > i+1:
              self.initial_rpy[1] = float(sys.argv[i+1])
          if sys.argv[i] == '-Y':
            if len(sys.argv) > i+1:
              self.initial_rpy[2] = float(sys.argv[i+1])

        if not self.gazebo_format and not self.urdf_format:
          print "Error: you must specify incoming format as either urdf or gazebo format xml"
          sys.exit(0)
        if self.model_name == "":
          print "Error: you must specify robot name"
          sys.exit(0)


    def callSpawnService(self):

        if self.file_name != "":
          print "loading model xml from file"
          if os.path.exists(self.file_name):
            if os.path.isdir(self.file_name):
              print "Error: file name is a path?"
              sys.exit(0)
            if not os.path.isfile(self.file_name):
              print "Error: unable to open file"
              sys.exit(0)
          else:
            print "Error: file does not exist"
            sys.exit(0)
          # load file
          f = open(self.file_name,'r')
          model_xml = f.read()
          if model_xml == "":
            print "Error: file is empty"
            sys.exit(0)

        elif self.param_name != "":
          print "loading model xml from ros parameter"
          model_xml = rospy.get_param(self.param_name)
          if model_xml == "":
            print "Error: param does not exist or is empty"
            sys.exit(0)
        else:
          print "Error: user specified param or filename is an empty string"
          sys.exit(0)

        # setting initial pose
        initial_pose = Pose()
        initial_pose.position.x = self.initial_xyz[0]
        initial_pose.position.y = self.initial_xyz[1]
        initial_pose.position.z = self.initial_xyz[2]
        # convert rpy to quaternion for Pose message
        tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2])
        q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
        initial_pose.orientation = q;

        print "attempting to spawn robot in simulation"
        if self.urdf_format:
          success = gazebo_interface.spawn_urdf_model_client(self.model_name,model_xml,self.robot_namespace,initial_pose,self.reference_frame)
        elif self.gazebo_format:
          success = gazebo_interface.spawn_gazebo_model_client(self.model_name,model_xml,self.robot_namespace,initial_pose,self.reference_frame)
        else:
          print "Error: should not be here in spawner helper script, there is a bug"
          sys.exit(0)
        print "spawning success",success

        # unpause physics if user requested
        if self.unpause_physics:
          rospy.wait_for_service('/gazebo/unpause_physics')
          try:
            unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics',Empty)
            unpause_physics()
            return
          except rospy.ServiceException, e:
            print "unpause physics service call failed: %s"%e

if __name__ == "__main__":
    if len(sys.argv) < 2:
        print usage()
    else:
        sm = SpawnModel()
        sm.parseUserInputs()
        sm.callSpawnService()
