def insert_all_models_in_scene(self):
for object_name, object_euler_pose in zip(self.object_names, self.object_euler_poses):
object_type = self.find_object_type(object_name)
model_sdf_file = open(self.CONST_DESCRIPTION_PATH + '/models/{}/model.sdf'.format(object_type),
'r').read()
object_pose = self.get_pose(object_euler_pose)
rospy.loginfo("Inserting {} at {}.".format(object_name, object_pose))
spawn_sdf_model_client(object_name, model_sdf_file, "namespace", object_pose, 'world', "gazebo")
def spawn_sphere(self, sphere_name="0", x=0, y=0, z=0):
model_xml = open(os.environ["GDL_PATH"] + "/models/sphere/model.sdf").read()
model_pose = Pose()
model_pose.position.x = x
model_pose.position.y = y
model_pose.position.z = z
robot_namespace = sphere_name
gazebo_interface.spawn_sdf_model_client(model_name=sphere_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
def spawn_model(self, model_name="coke_can"):
model_xml = open(self.models_dir + "/" + model_name + "/model.sdf").read()
model_pose = Pose()
model_pose.position.x = 2
model_pose.position.y = 0.5
model_pose.position.z = 1
robot_namespace = model_name
gazebo_interface.spawn_sdf_model_client(model_name=model_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
def spawn_sphere(self, sphere_name="0", x=0, y=0, z=0):
model_xml = open(os.environ["GDL_PATH"] +
"/models/sphere/model.sdf").read()
model_pose = Pose()
model_pose.position.x = x
model_pose.position.y = y
model_pose.position.z = z
robot_namespace = sphere_name
gazebo_interface.spawn_sdf_model_client(
model_name=sphere_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
def spawn_obstacle(self, model_name, model_type, initial_pose):
# Must be unique in the gazebo world - failure otherwise
# Spawning on top of something else leads to bizarre behavior
# model_filenames = {'box':'box_lus.sdf', 'cylinder':'cylinder.sdf'}
model_filenames = {
'box': 'box_lus.sdf',
'cylinder': 'cylinder.sdf',
'pole': 'pole_005_06.sdf',
'square_post': 'box_02_02_05.sdf'
}
if model_type not in model_filenames:
rospy.logerr(
"Model type [" + str(model_type) +
"] is unknown! Known types are: "
) #TODO: print list of types; maybe model_filenames.iter()?
return False
model_xml = load_model_xml(self.model_path +
model_filenames[model_type])
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
success = gazebo_interface.spawn_sdf_model_client(
model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
#time.sleep(.1)
return success
def spawn_furniture(gazebo_name, name, x, y, z, orientation):
rospy.loginfo('Spawn: {0}'.format(name))
initial_pose = Pose()
initial_pose.position.x = x
initial_pose.position.y = y
initial_pose.position.z = z
roll = orientation[0]
pitch = orientation[1]
yaw = orientation[2]
q = tft.quaternion_from_euler(roll, pitch, yaw)
initial_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
model_xml = model_database_template.replace('MODEL_NAME', name)
gazebo_interface.spawn_sdf_model_client(gazebo_name, model_xml,
rospy.get_namespace(),
initial_pose, "", "/gazebo")
def spawn_model(model_name):
rospy.init_node('spawn_model')
initial_pose = Pose()
model_name = model_name
model_xml = open('../models/ROSCock.sdf').read()
robot_namespace = rospy.get_namespace()
reference_frame = ""
resp = gazebo_interface.spawn_sdf_model_client(model_name,model_xml,robot_namespace,initial_pose, reference_frame, "gazebo/")
def callSpawnService(self):
# wait for model to exist
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):
print "Error: file name is a path?", self.file_name
sys.exit(0)
if not os.path.isfile(self.file_name):
print "Error: unable to open file", self.file_name
sys.exit(0)
else:
print "Error: file does not exist", self.file_name
sys.exit(0)
# load file
f = open(self.file_name,'r')
model_xml = f.read()
if model_xml == "":
print "Error: file is empty", 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 == "":
print "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 == "":
print "Error: an error occured generating the SDF file"
sys.exit(0)
else:
print "Error: user specified param or filename is an empty string"
sys.exit(0)
# spawn model
success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace,
self.initial_pose, self.reference_frame, self.gazebo_namespace)
rospy.loginfo("aqui")
# 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, e:
print "unpause physics service call failed: %s"%e
def spawn_model(self, model_name="coke_can", model_type="coke_can", model_pose=None):
model_xml = open(self.models_dir + "/" + model_type + "/model.sdf").read()
if not model_pose:
model_pose = Pose()
model_pose.position.x = 0
model_pose.position.y = 0
model_pose.position.z = 0
robot_namespace = model_name
gazebo_interface.spawn_sdf_model_client(model_name=model_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
#large models can take a moment to load
while not self.does_world_contain_model(model_name):
sleep(0.5)
def spawn_barrel(self, model_name, initial_pose):
# Must be unique in the gazebo world - failure otherwise
# Spawning on top of something else leads to bizarre behavior
model_path = os.path.expanduser(
"~/.gazebo/models/first_2015_trash_can/model.sdf")
model_xml = load_model_xml(model_path)
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
success = gazebo_interface.spawn_sdf_model_client(
model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
def spawn_object(gazebo_name, name, x, y, z, yaw):
rospy.loginfo('Spawn: {0}'.format(name))
initial_pose = Pose()
initial_pose.position.x = x
initial_pose.position.y = y
initial_pose.position.z = z
roll = 0.0
pitch = 0.0
yaw = math.pi * (random.random() - 0.5)
q = tft.quaternion_from_euler(roll, pitch, yaw)
initial_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
path_xml = _path_xml.replace('MODEL_NAME', name)
with open(path_xml, "r") as f:
model_xml = f.read()
model_xml = model_xml.replace('PATH_TO_MODEL', _path_model)
gazebo_interface.spawn_sdf_model_client(gazebo_name, model_xml,
rospy.get_namespace(),
initial_pose, "", "/gazebo")
def spawn_model(self,
model_name="coke_can",
model_type="coke_can",
model_pose=None):
model_xml = open(self.models_dir + "/" + model_type +
"/model.sdf").read()
if not model_pose:
model_pose = Pose()
model_pose.position.x = 0
model_pose.position.y = 0
model_pose.position.z = 0
robot_namespace = model_name
gazebo_interface.spawn_sdf_model_client(
model_name=model_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
#large models can take a moment to load
while not self.does_world_contain_model(model_name):
sleep(0.5)
def _spawn_sdf_object(self):
"""
Function allowing to spawn an object desribed by a sdf file in gazebo, while allowing automatic collision
generation in moveit and updating the ACM
"""
# Access to the service through the proxy
add_mapping_service = rospy.ServiceProxy('add_gazebo_mapping',
AddGazeboMapping)
# If a path is provided then set the object type accordingly
if self.file_path:
file_to_load = self.file_path
self.object_type = self.file_path
# If a name has not been set, then extract it from the path
if not self.object_name:
self.object_name = file_to_load.split("/")[-2]
else:
file_to_load = self.default_env + '{}/model.sdf'.format(
self.object_type)
# Read the xml file
model_sdf_file = open(file_to_load, 'r').read()
# Add the maping between the name of the object and the type of the object to make the link between
# gazebo and moveit
response = add_mapping_service(self.object_name, self.object_type)
# If something went wrong dispaly an error message and quit
if not response.success:
rospy.logerr(
"An error occured when handling the gazebo object {}".format(
self.object_name))
return
# Otherwise spawn the object in gazebo
spawn_sdf_model_client(self.object_name, model_sdf_file,
rospy.get_namespace(), self.object_pose,
self.reference_frame, "gazebo")
# And update the ACM
response = self.update_acm(0, self.object_name)
def spawn_barrel(self, model_name, initial_pose):
# Must be unique in the gazebo world - failure otherwise
# Spawning on top of something else leads to bizarre behavior
# model_path = os.path.expanduser("~/.gazebo/models/first_2015_trash_can/model.sdf")
# Fot book chapter
# model_path = os.path.expanduser("~/.gazebo/models/drc_practice_blue_cylinder/model.sdf")
path = self.rospack.get_path("nav_configs")
# model_path = os.path.expanduser(path + "/models/box_lus.sdf")
model_path = os.path.expanduser(path + "/models/box_lus.sdf")
model_xml = load_model_xml(model_path)
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
success = gazebo_interface.spawn_sdf_model_client(
model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
def spawn_package_model(self, model_name, package_name, model_path,
initial_pose):
# Must be unique in the gazebo world - failure otherwise
# Spawning on top of something else leads to bizarre behavior
package_path = self.rospack.get_path(package_name)
model_xml = load_model_xml(package_path + model_path)
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
success = gazebo_interface.spawn_sdf_model_client(
model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
return success
def spawn_local_database_model(self, model_name, model_type, initial_pose):
# Must be unique in the gazebo world - failure otherwise
# Spawning on top of something else leads to bizarre behavior
package_path = os.path.expanduser("~/.gazebo/models/")
model_path = model_type + "/model.sdf"
model_xml = load_model_xml(package_path + model_path)
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
success = gazebo_interface.spawn_sdf_model_client(
model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
return success
def spawn_obstacle(self, model_name, initial_pose):
# Must be unique in the gazebo world - failure otherwise
# Spawning on top of something else leads to bizarre behavior
path = self.rospack.get_path("nav_configs")
model_path = path + "/models/"
model_types = ['box_02_02_05.srdf', 'pole_005_06.srdf']
model_type = self.random.choice(model_types)
model_xml = load_model_xml(model_path + model_type)
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
success = gazebo_interface.spawn_sdf_model_client(
model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
def load_sdf(self, filename):
"""Load a SDF file in the simulator."""
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
position = (0., 0., 0.)
orientation = (0., 0., 0., 1.)
model_name = filename.split('/')[-1].split('.')[-2] # assume filename='path/to/file.sdf'
# load file
f = open(filename, 'r')
model_xml = f.read()
if model_xml == "":
rospy.logerr("Error: file is empty %s", filename)
sys.exit(0)
# create initial pose
initial_pose = geomsg.Pose()
initial_pose.position.x = position[0]
initial_pose.position.y = position[1]
initial_pose.position.z = position[2]
q = geomsg.Quaternion()
q.x = orientation[0]
q.y = orientation[1]
q.z = orientation[2]
q.w = orientation[3]
initial_pose.orientation = q
success = gazebo_interface.spawn_sdf_model_client(model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
if not success:
raise ValueError("Could not load the given SDF in Gazebo.")
body_id = len(self.bodies)
self.bodies.append(model_name)
return body_id
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):
print "Error: file name is a path?", self.file_name
sys.exit(0)
if not os.path.isfile(self.file_name):
print "Error: unable to open file", self.file_name
sys.exit(0)
else:
print "Error: file does not exist", self.file_name
sys.exit(0)
# load file
f = open(self.file_name,'r')
model_xml = f.read()
if model_xml == "":
print "Error: file is empty", 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 == "":
print "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 == "":
print "Error: an error occured generating the SDF file"
sys.exit(0)
else:
print "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:
print "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, e:
print "set model configuration service call failed: %s"%e
def run(self):
'''
Run node, spawning model and doing other actions as configured in program arguments.
Returns exit code, 1 for failure, 0 for success
'''
# Wait for model to exist if wait flag is enabled
if self.args.wait:
self.model_exists = False
def models_cb(models):
self.model_exists = self.args.wait in models.name
rospy.Subscriber("%s/model_states" % self.args.gazebo_namespace,
ModelStates, models_cb)
r = rospy.Rate(10)
rospy.loginfo('Waiting for model {} before proceeding.'.format(
self.args.wait))
while not rospy.is_shutdown() and not self.model_exists:
r.sleep()
if rospy.is_shutdown():
return 0
# Load model XML from file
if self.args.file:
rospy.loginfo("Loading model XML from file %s" % self.args.file)
if not os.path.exists(self.args.file):
rospy.logfatal("Error: specified file %s does not exist",
self.args.file)
return 1
if not os.path.isfile(self.args.file):
rospy.logfatal("Error: specified file %s is not a file",
self.args.file)
return 1
# load file
try:
f = open(self.args.file, 'r')
model_xml = f.read()
except IOError as e:
rospy.logerr("Error reading file {}: {}".format(
self.args.file, e))
return 1
if model_xml == "":
rospy.logerr("Error: file %s is empty", self.args.file)
return 1
# Load model XML from ROS param
elif self.args.param:
rospy.loginfo("Loading model XML from ros parameter %s" %
self.args.param)
model_xml = rospy.get_param(self.args.param)
if model_xml == "":
rospy.logerr("Error: param does not exist or is empty")
return 1
# Generate model XML by putting requested model name into request template
elif self.args.database:
rospy.loginfo("Loading model XML from Gazebo Model Database")
model_xml = self.MODEL_DATABASE_TEMPLATE.format(self.args.database)
elif self.args.stdin:
rospy.loginfo("Loading model XML from stdin")
model_xml = sys.stdin.read()
if model_xml == "":
rospy.logerr("Error: stdin buffer was empty")
return 1
# Parse xml to detect invalid xml before sending to gazebo
try:
xml_parsed = xml.etree.ElementTree.fromstring(model_xml)
except xml.etree.ElementTree.ParseError as e:
rospy.logerr('Invalid XML: {}'.format(e))
return 1
# Replace package:// with model:// for mesh tags if flag is set
if self.args.package_to_model:
for element in xml_parsed.iterfind('.//mesh'):
filename_tag = element.get('filename')
if filename_tag is None:
continue
url = urlsplit(filename_tag)
if url.scheme == 'package':
url = SplitResult('model', *url[1:])
element.set('filename', url.geturl())
# Encode xml object back into string for service call
model_xml = xml.etree.ElementTree.tostring(xml_parsed)
# For Python 3
if not isinstance(model_xml, str):
model_xml = model_xml.decode(encoding='ascii')
# Form requested Pose from arguments
initial_pose = Pose()
initial_pose.position.x = rospy.get_param('~x_pos')
initial_pose.position.y = rospy.get_param('~y_pos')
initial_pose.position.z = self.args.z
q = quaternion_from_euler(self.args.R, self.args.P, self.args.Y)
initial_pose.orientation = Quaternion(*q)
# Spawn model using urdf or sdf service based on arguments
success = False
if self.args.urdf:
success = gazebo_interface.spawn_urdf_model_client(
self.args.model, model_xml, self.args.robot_namespace,
initial_pose, self.args.reference_frame,
self.args.gazebo_namespace)
elif self.args.sdf:
success = gazebo_interface.spawn_sdf_model_client(
self.args.model, model_xml, self.args.robot_namespace,
initial_pose, self.args.reference_frame,
self.args.gazebo_namespace)
if not success:
rospy.logerr('Spawn service failed. Exiting.')
return 1
# Apply joint positions if any specified
if len(self.args.joints) != 0:
joint_names = [joint[0] for joint in self.args.joints]
joint_positions = [joint[1] for joint in self.args.joints]
success = gazebo_interface.set_model_configuration_client(
self.args.model, "", joint_names, joint_positions,
self.args.gazebo_namespace)
if not success:
rospy.logerr('SetModelConfiguration service failed. Exiting.')
return 1
# Unpause physics if user requested
if self.args.unpause:
rospy.loginfo('Unpausing physics')
rospy.wait_for_service('%s/unpause_physics' %
self.args.gazebo_namespace)
try:
unpause_physics = rospy.ServiceProxy(
'%s/unpause_physics' % self.args.gazebo_namespace, Empty)
unpause_physics()
except rospy.ServiceException as e:
rospy.logerr(
"Unpause physics service call failed: {}".format(e))
return 1
# If bond enabled, setup shutdown callback and wait for shutdown
if self.args.bond:
rospy.on_shutdown(self._delete_model)
rospy.loginfo('Waiting for shutdown to delete model {}'.format(
self.args.model))
rospy.spin()
return 0
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):
print "Error: file name is a path?", self.file_name
sys.exit(0)
if not os.path.isfile(self.file_name):
print "Error: unable to open file", self.file_name
sys.exit(0)
else:
print "Error: file does not exist", self.file_name
sys.exit(0)
# load file
f = open(self.file_name,'r')
model_xml = f.read()
if model_xml == "":
print "Error: file is empty", 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 == "":
print "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 == "":
print "Error: an error occured generating the SDF file"
sys.exit(0)
else:
print "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:
print "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, e:
print "set model configuration service call failed: %s"%e