def spawn_kinect(self):
model_xml = rospy.get_param("robot_description")
#f = PyKDL.Frame(PyKDL.Rotation.RPY(0, math.pi, math.pi), PyKDL.Vector(0, 0, 2))
f = PyKDL.Frame(PyKDL.Rotation.RPY(0, math.pi+math.pi/4.0, math.pi), PyKDL.Vector(0, 0, 2))
f = PyKDL.Frame(PyKDL.Rotation.RPY(0, math.pi/4.0, 0), PyKDL.Vector(0, 0, 2))
model_pose = tf_conversions.posemath.toMsg(f)
robot_namespace = self.camera_name
gazebo_interface.spawn_urdf_model_client(model_name=self.camera_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
def spawn_kinect(self):
model_xml = rospy.get_param("robot_description")
#f = PyKDL.Frame(PyKDL.Rotation.RPY(0, math.pi, math.pi), PyKDL.Vector(0, 0, 2))
f = PyKDL.Frame(
PyKDL.Rotation.RPY(0, math.pi + math.pi / 4.0, math.pi),
PyKDL.Vector(0, 0, 2))
f = PyKDL.Frame(PyKDL.Rotation.RPY(0, math.pi / 4.0, 0),
PyKDL.Vector(0, 0, 2))
model_pose = tf_conversions.posemath.toMsg(f)
#model_pose = Pose()
robot_namespace = self.camera_name
gazebo_interface.spawn_urdf_model_client(
model_name=self.camera_name,
model_xml=model_xml,
robot_namespace=robot_namespace,
initial_pose=model_pose,
reference_frame="world",
gazebo_namespace=self.gazebo_namespace)
def spawnURDF(self, modelName, modelXml, pose):
# setting initial pose
initial_pose = Pose()
initial_pose.position.x = pose[0]
initial_pose.position.y = pose[1]
initial_pose.position.z = pose[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])
q = Quaternion(0, 0, 0, 1)
initial_pose.orientation = q
# spawn model
success = gazebo_interface.spawn_urdf_model_client(
modelName, modelXml, rospy.get_namespace(), initial_pose, "",
self.gazeboNamespace)
print 'Spawned robot ', modelName
def load_urdf(self, filename, position, orientation):
"""Load a URDF file in the simulator."""
robot_namespace = rospy.get_namespace()
gazebo_namespace = "/gazebo"
reference_frame = ""
model_name = filename.split('/')[-1].split('.')[0] # assume filename='path/to/file(.xacro).urdf'
# if xacro file, use xacro.py with the list of arguments
# 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_urdf_model_client(model_name, model_xml, robot_namespace, initial_pose,
reference_frame, gazebo_namespace)
if not success:
raise ValueError("Could not load the given URDF 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 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
