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.srv import *
00067 from geometry_msgs.msg import *
00068 import tf.transformations as tft
00069 from std_msgs.msg import Empty as EmptyMsg
00070 
00071 if __name__ == "__main__":
00072         if len(sys.argv) < 2:
00073                 print '[spawn_object.py] Please specify the names of the objects to be loaded'
00074                 sys.exit()
00075         
00076         rospy.init_node("object_spawner")
00077 
00078         # this was missing....
00079         rospy.sleep(5)
00080         rospy.wait_for_service("/gazebo/spawn_urdf_model")
00081 
00082         # check for all objects on parameter server
00083         if not rospy.has_param("/objects"):
00084                 rospy.logerr("No objects uploaded to /objects")
00085                 sys.exit()
00086         all_object_names = rospy.get_param("/objects").keys()
00087 
00088         # if keyword all is in list of object names we'll load all models uploaded to parameter server
00089         if "all" in sys.argv:
00090                 object_names = all_object_names
00091         else:
00092                 object_names = sys.argv
00093                 object_names.pop(0) # remove first element of sys.argv which is file name
00094 
00095         rospy.loginfo("Trying to spawn %s",object_names)
00096         
00097         for name in object_names:
00098                 # check for object on parameter server
00099                 if not rospy.has_param("/objects/%s" % name):
00100                         rospy.logerr("No description for " + name + " found at /objects/" + name)
00101                         continue
00102                 
00103                 # check for model
00104                 if not rospy.has_param("/objects/%s/model" % name):
00105                         rospy.logerr("No model for " + name + " found at /objects/" + name + "/model")
00106                         continue
00107                 model = rospy.get_param("/objects/%s/model" % name)
00108                 
00109                 # check for model_type
00110                 if not rospy.has_param("/objects/%s/model_type" % name):
00111                         rospy.logerr("No model_type for " + name + " found at /objects/" + name + "/model_type")
00112                         continue
00113                 model_type = rospy.get_param("/objects/%s/model_type" % name)
00114                 
00115                 # check for position
00116                 if not rospy.has_param("/objects/%s/position" % name):
00117                         rospy.logerr("No position for " + name + " found at /objects/" + name + "/position")
00118                         continue
00119                 position = rospy.get_param("/objects/%s/position" % name)
00120 
00121                 # check for orientation
00122                 if not rospy.has_param("/objects/%s/orientation" % name):
00123                         rospy.logerr("No orientation for " + name + " found at /objects/" + name + "/orientation")
00124                         continue
00125                 # convert rpy to quaternion for Pose message
00126                 orientation = rospy.get_param("/objects/%s/orientation" % name)
00127                 quaternion = tft.quaternion_from_euler(orientation[0], orientation[1], orientation[2])
00128                 object_pose = Pose()
00129                 object_pose.position.x = float(position[0])
00130                 object_pose.position.y = float(position[1])
00131                 object_pose.position.z = float(position[2])
00132                 object_pose.orientation.x = quaternion[0]
00133                 object_pose.orientation.y = quaternion[1]
00134                 object_pose.orientation.z = quaternion[2]
00135                 object_pose.orientation.w = quaternion[3]
00136 
00137                 try:
00138                         file_localition = roslib.packages.get_pkg_dir('cob_gazebo_objects') + '/objects/' + model + '.' + model_type
00139                 except:
00140                         print "File not found: cob_gazebo_objects" + "/objects/" + model + "." + model_type
00141                         continue
00142 
00143                 # call gazebo service to spawn model (see http://ros.org/wiki/gazebo)
00144                 if model_type == "urdf":
00145                         srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
00146                         file_xml = open(file_localition)
00147                         xml_string=file_xml.read()
00148 
00149                 elif model_type == "urdf.xacro":
00150                         p = os.popen("rosrun xacro xacro.py " + file_localition)
00151                         xml_string = p.read()
00152                         p.close()
00153                         srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
00154 
00155                 elif model_type == "model":
00156                         srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_gazebo_model', SpawnModel)
00157                         file_xml = open(file_localition)
00158                         xml_string=file_xml.read()
00159                 else:
00160                         rospy.logerr('Model type not know. model_type = ' + model_type)
00161                         continue
00162 
00163 
00164                 # check if object is already spawned
00165                 srv_delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
00166                 req = DeleteModelRequest()
00167                 req.model_name = name
00168                 exists = True
00169                 try:
00170                         res = srv_delete_model(name)
00171                 except rospy.ServiceException, e:
00172                         exists = False
00173                         rospy.logdebug("Model %s does not exist in gazebo.", name)
00174 
00175                 if exists:
00176                         rospy.loginfo("Model %s already exists in gazebo. Model will be updated.", name)
00177 
00178                 # spawn new model
00179                 req = SpawnModelRequest()
00180                 req.model_name = name # model name from command line input
00181                 req.model_xml = xml_string
00182                 req.initial_pose = object_pose
00183 
00184                 res = srv_spawn_model(req)
00185         
00186                 # evaluate response
00187                 if res.success == True:
00188                         rospy.loginfo(res.status_message + " " + name)
00189                 else:
00190                         print "Error: model %s not spawn. error message = "% name + res.status_message
00191                 
00192         sim = rospy.get_param('/use_sim_time')
00193                         
00194         if sim is True:
00195     
00196                 rospy.loginfo('Running in simulation, publishing to /sim_spawned topic')
00197       
00198                 pub = rospy.Publisher('/sim_spawned', EmptyMsg,latch=True)
00199                 pub.publish(EmptyMsg())
00200                 pub.publish(EmptyMsg())
00201                 pub.publish(EmptyMsg())
00202       
00203                 rospy.spin()
00204 


srs_user_tests
Author(s): SRS team
autogenerated on Sun Jan 5 2014 12:14:05