def set_configuration(self, controllers, joint_names, joint_positions):
# Need to stop controllers in order to change configuration, otherwise the controllers
# Avoid changing the configuration
self.set_controllers_state(controllers, False)
# set model configuration before unpause if user requested
if len(joint_names) != 0:
try:
success = gazebo_interface.set_model_configuration_client(
self.model_name, self.param_name, joint_names, joint_positions, self.gazebo_namespace
)
print "Model configuration changed"
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 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
lastGenBest = 10000
while True:
generation += 1
arfitness *= 0
arfitness += 10000
for k in range(la):
pause_physics()
res_world()
res_sim()
gazebo_namespace = '/gazebo'
success = gazebo_interface.set_model_configuration_client(model_name, param_name, \
joint_names, joint_positions, gazebo_namespace)
success = unpause(gazebo_namespace)
unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty)
arz[:, k] = stats.truncnorm.rvs((-1) / 0.5, (1) / 0.5,
loc=0,
scale=0.5,
size=(nVar, ))
arx[:,
k] = (xmean + sigma * (np.matmul(B, np.matmul(D, arz[:, k]))))
arx[:, k] = np.clip(arx[:, k], -1, 1)
Cweights = arx[:, k].reshape((numCtrl, numOmega))
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