318 lines
14 KiB
Python
Executable File
318 lines
14 KiB
Python
Executable File
#!/usr/bin/env python
|
|
#
|
|
# Copyright 2013 Open Source Robotics Foundation
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
#
|
|
# Desc: helper script for spawning models in gazebo
|
|
# Author: John Hsu, Dave Coleman
|
|
#
|
|
|
|
import rospy, sys, os, time
|
|
import string
|
|
import warnings
|
|
import re
|
|
|
|
from gazebo_ros import gazebo_interface
|
|
|
|
from gazebo_msgs.msg import *
|
|
from gazebo_msgs.srv import *
|
|
from std_srvs.srv import Empty
|
|
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench
|
|
import tf.transformations as tft
|
|
|
|
model_database_template = """<sdf version="1.4">
|
|
<world name="default">
|
|
<include>
|
|
<uri>model://MODEL_NAME</uri>
|
|
</include>
|
|
</world>
|
|
</sdf>"""
|
|
|
|
def usage():
|
|
print('''Commands:
|
|
-[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro
|
|
-[file|param|database] [<file_name>|<param_name>|<model_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.
|
|
-gazebo_namespace <gazebo ros_namespace> - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g. /gazebo/spawn_model).
|
|
-robot_namespace <robot ros_namespace> - optional: change ROS namespace of gazebo-plugins.
|
|
-unpause - optional: !!!Experimental!!! unpause physics after spawning model
|
|
-wait - optional: !!!Experimental!!! wait for model to exist
|
|
-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
|
|
-J <joint_name joint_position> - optional: initialize the specified joint at the specified value
|
|
-package_to_model - optional: convert urdf <mesh filename="package://..." to <mesh filename="model://..."
|
|
-b - optional: bond to gazebo and delete the model when this program is interrupted
|
|
''')
|
|
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.database_name = ""
|
|
self.model_name = ""
|
|
self.robot_namespace = rospy.get_namespace()
|
|
self.gazebo_namespace = "/yyy"
|
|
self.reference_frame = ""
|
|
self.unpause_physics = False
|
|
self.wait_for_model = ""
|
|
self.wait_for_model_exists = False
|
|
self.urdf_format = False
|
|
self.sdf_format = False
|
|
self.joint_names = []
|
|
self.joint_positions = []
|
|
self.package_to_model = False
|
|
self.bond = False
|
|
|
|
def parseUserInputs(self):
|
|
# get goal from commandline
|
|
for i in range(0,len(sys.argv)):
|
|
if sys.argv[i] == '-h' or sys.argv[i] == '--help' or sys.argv[i] == '-help':
|
|
usage()
|
|
sys.exit(1)
|
|
if sys.argv[i] == '-unpause':
|
|
self.unpause_physics = True
|
|
if sys.argv[i] == '-urdf':
|
|
if self.sdf_format == True:
|
|
rospy.logerr("Error: you cannot specify both urdf and sdf format xml, must pick one")
|
|
sys.exit(0)
|
|
else:
|
|
self.urdf_format = True;
|
|
if sys.argv[i] == '-sdf' or sys.argv[i] == '-gazebo':
|
|
if self.urdf_format == True:
|
|
rospy.logerr("Error: you cannot specify both urdf and sdf format xml, must pick one")
|
|
sys.exit(0)
|
|
else:
|
|
if sys.argv[i] == '-gazebo':
|
|
rospy.logwarn("Deprecated: the -gazebo tag is now -sdf")
|
|
warnings.warn("Deprecated: the -gazebo tag is now -sdf", DeprecationWarning)
|
|
self.sdf_format = True;
|
|
if sys.argv[i] == '-J':
|
|
if len(sys.argv) > i+2:
|
|
self.joint_names.append(sys.argv[i+1])
|
|
self.joint_positions.append(float(sys.argv[i+2]))
|
|
else:
|
|
rospy.logerr("Error: must specify a joint name and joint value pair")
|
|
sys.exit(0)
|
|
if sys.argv[i] == '-param':
|
|
if len(sys.argv) > i+1:
|
|
if self.file_name != "" or self.database_name != "":
|
|
rospy.logerr("Error: you cannot specify file name if parameter or database 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 != "" or self.database_name != "":
|
|
rospy.logerr("Error: you cannot specify parameter if file or database 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] == '-database':
|
|
if len(sys.argv) > i+1:
|
|
if self.param_name != "" or self.file_name != "":
|
|
rospy.logerr("Error: you cannot specify parameter if file or parameter name is given, must pick one source of model xml")
|
|
sys.exit(0)
|
|
else:
|
|
self.database_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] == '-wait':
|
|
if len(sys.argv) > i+1:
|
|
self.wait_for_model = 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] == '-robot_namespace':
|
|
if len(sys.argv) > i+1:
|
|
self.robot_namespace = 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] == '-gazebo_namespace':
|
|
if len(sys.argv) > i+1:
|
|
self.gazebo_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 sys.argv[i] == '-package_to_model':
|
|
self.package_to_model = True;
|
|
if sys.argv[i] == '-b':
|
|
self.bond = True
|
|
|
|
if not self.sdf_format and not self.urdf_format:
|
|
rospy.logerr("Error: you must specify incoming format as either urdf or sdf format xml")
|
|
sys.exit(0)
|
|
if self.model_name == "":
|
|
rospy.logerr("Error: you must specify model name")
|
|
sys.exit(0)
|
|
|
|
def checkForModel(self,model):
|
|
for n in model.name:
|
|
if n == self.wait_for_model:
|
|
self.wait_for_model_exists = True
|
|
|
|
|
|
# Generate a blank SDF file with an include for the model from the model database
|
|
def createDatabaseCode(self, database_name):
|
|
return model_database_template.replace("MODEL_NAME", database_name);
|
|
|
|
def callSpawnService(self):
|
|
|
|
# wait for model to exist
|
|
rospy.init_node('spawn_model')
|
|
|
|
if not self.wait_for_model == "":
|
|
rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel)
|
|
r= rospy.Rate(10)
|
|
while not rospy.is_shutdown() and not self.wait_for_model_exists:
|
|
r.sleep()
|
|
|
|
if rospy.is_shutdown():
|
|
sys.exit(0)
|
|
|
|
if self.file_name != "":
|
|
rospy.loginfo("Loading model XML from file")
|
|
if os.path.exists(self.file_name):
|
|
if os.path.isdir(self.file_name):
|
|
rospy.logerr("Error: file name is a path? %s", self.file_name)
|
|
sys.exit(0)
|
|
if not os.path.isfile(self.file_name):
|
|
rospy.logerr("Error: unable to open file %s", self.file_name)
|
|
sys.exit(0)
|
|
else:
|
|
rospy.logerr("Error: file does not exist %s", self.file_name)
|
|
sys.exit(0)
|
|
# load file
|
|
f = open(self.file_name,'r')
|
|
model_xml = f.read()
|
|
if model_xml == "":
|
|
rospy.logerr("Error: file is empty %s", self.file_name)
|
|
sys.exit(0)
|
|
|
|
# ROS Parameter
|
|
elif self.param_name != "":
|
|
rospy.loginfo( "Loading model XML from ros parameter")
|
|
model_xml = rospy.get_param(self.param_name)
|
|
if model_xml == "":
|
|
rospy.logerr("Error: param does not exist or is empty")
|
|
sys.exit(0)
|
|
|
|
# Gazebo Model Database
|
|
elif self.database_name != "":
|
|
rospy.loginfo( "Loading model XML from Gazebo Model Database")
|
|
model_xml = self.createDatabaseCode(self.database_name)
|
|
if model_xml == "":
|
|
rospy.logerr("Error: an error occured generating the SDF file")
|
|
sys.exit(0)
|
|
else:
|
|
rospy.logerr("Error: user specified param or filename is an empty string")
|
|
sys.exit(0)
|
|
|
|
if self.package_to_model:
|
|
model_xml = re.sub("<\s*mesh\s+filename\s*=\s*([\"|'])package://","<mesh filename=\g<1>model://", model_xml)
|
|
|
|
# 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;
|
|
|
|
# spawn model
|
|
if self.urdf_format:
|
|
success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace,
|
|
initial_pose, self.reference_frame, self.gazebo_namespace)
|
|
elif self.sdf_format:
|
|
success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace,
|
|
initial_pose, self.reference_frame, self.gazebo_namespace)
|
|
else:
|
|
rospy.logerr("Error: should not be here in spawner helper script, there is a bug")
|
|
sys.exit(0)
|
|
|
|
# set model configuration before unpause if user requested
|
|
if len(self.joint_names) != 0:
|
|
try:
|
|
success = gazebo_interface.set_model_configuration_client(self.model_name, self.param_name,
|
|
self.joint_names, self.joint_positions, self.gazebo_namespace)
|
|
except rospy.ServiceException as e:
|
|
rospy.logerr("Set model configuration service call failed: %s", e)
|
|
|
|
# unpause physics if user requested
|
|
if self.unpause_physics:
|
|
rospy.wait_for_service('%s/unpause_physics'%(self.gazebo_namespace))
|
|
try:
|
|
unpause_physics = rospy.ServiceProxy('%s/unpause_physics'%(self.gazebo_namespace), Empty)
|
|
unpause_physics()
|
|
except rospy.ServiceException as e:
|
|
rospy.logerr("Unpause physics service call failed: %s", e)
|
|
|
|
return
|
|
|
|
|
|
def callDeleteService(self):
|
|
try:
|
|
delete_model = rospy.ServiceProxy('%s/delete_model'%(self.gazebo_namespace), DeleteModel)
|
|
delete_model(model_name=self.model_name)
|
|
except rospy.ServiceException as e:
|
|
rospy.logerr("Delete model service call failed: %s", e)
|
|
|
|
if __name__ == "__main__":
|
|
if len(sys.argv) < 2:
|
|
print(usage())
|
|
else:
|
|
print("SpawnModel script started") # make this a print incase roscore has not been started
|
|
sm = SpawnModel()
|
|
sm.parseUserInputs()
|
|
sm.callSpawnService()
|
|
|
|
if sm.bond:
|
|
rospy.on_shutdown(sm.callDeleteService)
|
|
rospy.spin()
|