spawn_object.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 # Copyright (c) 2010 \n
00007 # Fraunhofer Institute for Manufacturing Engineering
00008 # and Automation (IPA) \n\n
00009 #
00010 #################################################################
00011 #
00012 # \note
00013 # Project name: Care-O-bot Research
00014 # \note
00015 # ROS stack name: cob_environments
00016 # \note
00017 # ROS package name: cob_gazebo_objects
00018 #
00019 # \author
00020 # Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00021 # \author
00022 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00023 #
00024 # \date Date of creation: Feb 2012
00025 #
00026 # \brief
00027 # Implements script server functionalities.
00028 #
00029 #################################################################
00030 #
00031 # Redistribution and use in source and binary forms, with or without
00032 # modification, are permitted provided that the following conditions are met:
00033 #
00034 # - Redistributions of source code must retain the above copyright
00035 # notice, this list of conditions and the following disclaimer. \n
00036 # - Redistributions in binary form must reproduce the above copyright
00037 # notice, this list of conditions and the following disclaimer in the
00038 # documentation and/or other materials provided with the distribution. \n
00039 # - Neither the name of the Fraunhofer Institute for Manufacturing
00040 # Engineering and Automation (IPA) nor the names of its
00041 # contributors may be used to endorse or promote products derived from
00042 # this software without specific prior written permission. \n
00043 #
00044 # This program is free software: you can redistribute it and/or modify
00045 # it under the terms of the GNU Lesser General Public License LGPL as
00046 # published by the Free Software Foundation, either version 3 of the
00047 # License, or (at your option) any later version.
00048 #
00049 # This program is distributed in the hope that it will be useful,
00050 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00051 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00052 # GNU Lesser General Public License LGPL for more details.
00053 #
00054 # You should have received a copy of the GNU Lesser General Public
00055 # License LGPL along with this program.
00056 # If not, see < http://www.gnu.org/licenses/>.
00057 #
00058 #################################################################
00059 import sys
00060 import roslib
00061 roslib.load_manifest('cob_bringup_sim')
00062 
00063 import rospy
00064 import os
00065 
00066 from gazebo_msgs.srv import *
00067 from geometry_msgs.msg import *
00068 import tf.transformations as tft
00069 
00070 if __name__ == "__main__":
00071         if len(sys.argv) < 2:
00072                 print '[spawn_object.py] Please specify the names of the objects to be loaded'
00073                 sys.exit()
00074         
00075         rospy.init_node("object_spawner")
00076 
00077         # check for all objects on parameter server
00078         if not rospy.has_param("/objects"):
00079                 rospy.logerr("No objects uploaded to /objects")
00080                 sys.exit()
00081         all_object_names = rospy.get_param("/objects").keys()
00082 
00083         # if keyword all is in list of object names we'll load all models uploaded to parameter server
00084         if "all" in sys.argv:
00085                 object_names = all_object_names
00086         else:
00087                 object_names = sys.argv
00088                 object_names.pop(0) # remove first element of sys.argv which is file name
00089 
00090         rospy.loginfo("Trying to spawn %s",object_names)
00091         
00092         for name in object_names:
00093                 # check for object on parameter server
00094                 if not rospy.has_param("/objects/%s" % name):
00095                         rospy.logerr("No description for " + name + " found at /objects/" + name)
00096                         continue
00097                 
00098                 # check for model
00099                 if not rospy.has_param("/objects/%s/model" % name):
00100                         rospy.logerr("No model for " + name + " found at /objects/" + name + "/model")
00101                         continue
00102                 model = rospy.get_param("/objects/%s/model" % name)
00103                 
00104                 # check for model_type
00105                 if not rospy.has_param("/objects/%s/model_type" % name):
00106                         rospy.logerr("No model_type for " + name + " found at /objects/" + name + "/model_type")
00107                         continue
00108                 model_type = rospy.get_param("/objects/%s/model_type" % name)
00109                 
00110                 # check for position
00111                 if not rospy.has_param("/objects/%s/position" % name):
00112                         rospy.logerr("No position for " + name + " found at /objects/" + name + "/position")
00113                         continue
00114                 position = rospy.get_param("/objects/%s/position" % name)
00115 
00116                 # check for orientation
00117                 if not rospy.has_param("/objects/%s/orientation" % name):
00118                         rospy.logerr("No orientation for " + name + " found at /objects/" + name + "/orientation")
00119                         continue
00120                 # convert rpy to quaternion for Pose message
00121                 orientation = rospy.get_param("/objects/%s/orientation" % name)
00122                 quaternion = tft.quaternion_from_euler(orientation[0], orientation[1], orientation[2])
00123                 object_pose = Pose()
00124                 object_pose.position.x = float(position[0])
00125                 object_pose.position.y = float(position[1])
00126                 object_pose.position.z = float(position[2])
00127                 object_pose.orientation.x = quaternion[0]
00128                 object_pose.orientation.y = quaternion[1]
00129                 object_pose.orientation.z = quaternion[2]
00130                 object_pose.orientation.w = quaternion[3]
00131 
00132                 try:
00133                         file_localition = roslib.packages.get_pkg_dir('cob_gazebo_objects') + '/objects/' + model + '.' + model_type
00134                 except:
00135                         print "File not found: cob_gazebo_objects" + "/objects/" + model + "." + model_type
00136                         continue
00137 
00138                 # call gazebo service to spawn model (see http://ros.org/wiki/gazebo)
00139                 if model_type == "urdf":
00140                         srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
00141                         file_xml = open(file_localition)
00142                         xml_string=file_xml.read()
00143 
00144                 elif model_type == "urdf.xacro":
00145                         p = os.popen("rosrun xacro xacro.py " + file_localition)
00146                         xml_string = p.read()
00147                         p.close()
00148                         srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
00149 
00150                 elif model_type == "model":
00151                         srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_gazebo_model', SpawnModel)
00152                         file_xml = open(file_localition)
00153                         xml_string=file_xml.read()
00154                 else:
00155                         rospy.logerr('Model type not know. model_type = ' + model_type)
00156                         continue
00157 
00158 
00159                 # check if object is already spawned
00160                 srv_delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
00161                 req = DeleteModelRequest()
00162                 req.model_name = name
00163                 exists = True
00164                 try:
00165                         rospy.wait_for_service('/gazebo/delete_model')
00166                         res = srv_delete_model(name)
00167                 except rospy.ServiceException, e:
00168                         exists = False
00169                         rospy.logdebug("Model %s does not exist in gazebo.", name)
00170 
00171                 if exists:
00172                         rospy.loginfo("Model %s already exists in gazebo. Model will be updated.", name)
00173 
00174                 # spawn new model
00175                 req = SpawnModelRequest()
00176                 req.model_name = name # model name from command line input
00177                 req.model_xml = xml_string
00178                 req.initial_pose = object_pose
00179 
00180                 res = srv_spawn_model(req)
00181         
00182                 # evaluate response
00183                 if res.success == True:
00184                         rospy.loginfo(res.status_message + " " + name)
00185                 else:
00186                         print "Error: model %s not spawn. error message = "% name + res.status_message
00187 


cob_bringup_sim
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 12:48:24