def spawnInsertionRoundRod(self):
#Open the original SDF file
path = self.BASE_PATH + 'RoundRod.sdf'
with open(path, 'r') as basefile:
model_xml = basefile.read()
sdf = deepcopy(model_xml)
#Physics constants as calculated with Meshlab
scale = 1e-3
volume = 97677
density = 8000 #Steel
com = (12.5, 12.5, 100)
inertia = (329386560, 0, -5, 329386560, -5, 7592381)
#Modify the SDF to use these physical properties
sdf = self.modifySDF(sdf, scale, density, volume, com, inertia)
#Spawn the insertion base
spawn_model = self.rospy.ServiceProxy('/gazebo/spawn_sdf_model',
SpawnModel)
req = SpawnModelRequest()
req.model_name = "RoundRod"
req.model_xml = sdf
req.initial_pose = Pose()
req.initial_pose.position.x = 0.4
req.initial_pose.position.y = 0
req.initial_pose.position.z = 1.01
resp = spawn_model(req)
time.sleep(1)
def spawnInsertionHexagonalRod(self):
#Open the original SDF file
path = self.BASE_PATH + 'HexRod.sdf'
with open(path, 'r') as basefile:
model_xml = basefile.read()
sdf = deepcopy(model_xml)
#Physics constants as calculated with Meshlab
scale = 1e-3
volume = 108196
density = 8000 #Steel
com = (12.496746, 14.429998, 100)
inertia = (365349728, -3, 0, 365349600, 10, 9387199)
#Modify the SDF to use these physical properties
sdf = self.modifySDF(sdf, scale, density, volume, com, inertia)
#Spawn the insertion base
spawn_model = self.rospy.ServiceProxy('/gazebo/spawn_sdf_model',
SpawnModel)
req = SpawnModelRequest()
req.model_name = "HexRod"
req.model_xml = sdf
req.initial_pose = Pose()
req.initial_pose.position.x = 0.4
req.initial_pose.position.y = 0
req.initial_pose.position.z = 1.01
resp = spawn_model(req)
time.sleep(1)
def spawnInsertionBase(self):
#Open the original SDF file
path = self.BASE_PATH + 'BaseLooseFit.sdf'
with open(path, 'r') as basefile:
model_xml = basefile.read()
base = deepcopy(model_xml)
#Physics constants as calculated with Meshlab
scale = 1e-3
volume = 1402889
density = 8000 #Steel
com = (149.659271, 50, 25)
inertia = (1537954048, 596, 298, 11058070528, -16, 12011485184)
#Modify the SDF to use these physical properties
base = self.modifySDF(base, scale, density, volume, com, inertia)
#Spawn the insertion base
spawn_model = self.rospy.ServiceProxy('/gazebo/spawn_sdf_model',
SpawnModel)
req = SpawnModelRequest()
req.model_name = "InsertionBase"
req.model_xml = base
req.initial_pose = Pose()
req.initial_pose.position.x = 0.2
req.initial_pose.position.y = -0.2
req.initial_pose.position.z = 1.01
resp = spawn_model(req)
time.sleep(1)
def generer_herbe (fichier, nombre_plantes=1):
global dictionary
rospy.init_node("weed_spawner")
rospy.wait_for_service("/gazebo/spawn_urdf_model")
spawnerService = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
request = SpawnModelRequest()
with open(fichier, "r") as stream:
template = stream.read()
for i in range(nombre_plantes):
request.model_name = "Weed_{}".format(i)
urdf = template.format(uniform(0.01, 0.075))
request.model_xml = urdf
x_herbe = uniform(-10,10)
y_herbe = uniform(-10,10)
z_herbe = 0.1/2
dictionary["Weed_{}".format(i)] = [x_herbe, y_herbe]
request.initial_pose.position.x = x_herbe
request.initial_pose.position.y = y_herbe
request.initial_pose.position.z = z_herbe
response = spawnerService(request)
if not response.success:
rospy.logwarn("Unable to spawn weed {}: {}".format(i, response.status_message))
def destroy(data):
global filesdf
global strdebase
global strcolorbase
global HerbeX
global HerbeY
global Herbey
dist = []
DistanceMaxSuppression = 0.5
for i in range(NbHerbe):
dist.append(distance(X[i],Y[i],HerbeX,HerbeY))
dist_min = min(dist)
print("distance minimun ="+str(dist_min))
if dist_min<DistanceMaxSuppression:
ind_min = dist.index(dist_min)
#destruction
rospy.wait_for_service('/gazebo/delete_model')
gazeboDeleteModel = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
requestDel = "Grass"+str(ind_min)
gazeboDeleteModel(requestDel)
#respawn
#requestRespawn
strtochange = "<radius>"+str(Rayon[ind_min])+"</radius>"
filesdf = filesdf.replace(strdebase,strtochange)
strdebase = strtochange
strambientochange = "<ambient> 0 0 0 1</ambient>"
strambiantbefore = "<ambient> 0.3 1 0.3 1</ambient>"
strcolorchange = "<diffuse>0 0 0 1</diffuse>"
filesdf = filesdf.replace(strcolorbase,strcolorchange)
strcolorbase = strcolorchange
filesdf = filesdf.replace(strambiantbefore,strambientochange)
requestRespawn = SpawnModelRequest()
requestRespawn.model_name = "Grass"+str(ind_min)
requestRespawn.model_xml = filesdf
requestRespawn.robot_namespace = "Herbe"+str(ind_min)
requestRespawn.initial_pose.position.x = X[ind_min]
requestRespawn.initial_pose.position.y = Y[ind_min]
requestRespawn.initial_pose.position.z = 0
requestRespawn.initial_pose.orientation.x = 0
requestRespawn.initial_pose.orientation.y = 0
requestRespawn.initial_pose.orientation.z = 0
requestRespawn.initial_pose.orientation.w = 1.0
gazeboSpawnModel(requestRespawn)
X[ind_min] = 1000000
Y[ind_min] = 1000000
else :
print "C'est bien trop loin"
def create_sphere_request(self,modelname, px, py, pz, rr, rp, ry, r):
"""Create a SpawnModelRequest with the parameters of the cube given.
modelname: name of the model for gazebo
px py pz: position of the cube (and it's collision cube)
rr rp ry: rotation (roll, pitch, yaw) of the model
sx sy sz: size of the cube"""
cube = deepcopy(sdf_unit_sphere)
# Replace size of model
cube = cube.replace('RADIUS', str(r))
# Replace modelname
cube = cube.replace('MODELNAME', str(modelname))
req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = cube
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
return req
def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz):
"""Create a SpawnModelRequest with the parameters of the cube given.
modelname: name of the model for gazebo
px py pz: position of the cube (and it's collision cube)
rr rp ry: rotation (roll, pitch, yaw) of the model
sx sy sz: size of the cube"""
cube = deepcopy(sdf_cube)
# Replace size of model
size_str = str(round(sx, 3)) + " " + \
str(round(sy, 3)) + " " + str(round(sz, 3))
cube = cube.replace('SIZEXYZ', size_str)
# Replace modelname
cube = cube.replace('MODELNAME', str(modelname))
req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = cube
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
return req
def spray(self, r):
request = SpawnModelRequest()
request.model_name = 'killbox_%s' % uuid4()
request.model_xml = self.sdf
request.reference_frame = '{}/base_link'.format(self.robot_name)
request.initial_pose.position.z = 0.005
request.initial_pose.position.x = -0.45
request.initial_pose.orientation.w = 1.0
self.spawner(request)
return []
def main():
rospy.init_node("test1_start")
print("Waiting for service /gazebo/spawn_sdf_model")
rospy.wait_for_service("/gazebo/spawn_sdf_model")
print("Service is now available")
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('autonomos_gazebo_simulation')
spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
spawn_req = SpawnModelRequest()
angle = numpy.random.uniform(math.pi-0.3, math.pi+0.3)
pos_x = numpy.random.uniform(1.2,3.8)
spawn_req.model_name = "AutoModelMini"
spawn_req.model_xml = open(pkg_path + '/models/AutoNOMOS_mini/model.sdf','r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = pos_x
spawn_req.initial_pose.position.y = 3.3
spawn_req.initial_pose.position.z = 0.17
spawn_req.initial_pose.orientation.z = math.sin(angle/2)
spawn_req.initial_pose.orientation.w = math.cos(angle/2)
spawn_model(spawn_req)
spawn_req.model_name = "StopSign"
spawn_req.model_xml = open(pkg_path + '/models/stop_sign_small/model.sdf','r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = pos_x - 0.2
spawn_req.initial_pose.position.y = 3.55
spawn_req.initial_pose.position.z = 0.0
spawn_req.initial_pose.orientation.z = math.sin(1.5708/2)
spawn_req.initial_pose.orientation.w = math.cos(1.5708/2)
spawn_model(spawn_req)
def spawn_part(ix_part, part_amount=5, bin_number=1):
# Create service proxy
spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
rospy.wait_for_service('/gazebo/spawn_urdf_model')
# Load URDF
with open(urdf_files[ix_part], "r") as f:
model_xml = f.read()
for item_counter in range(part_amount):
# Object pose
q = tf.transformations.quaternion_from_euler(random.random() * 3.14,
random.random() * 3.14,
random.random() * 3.14)
pose = Pose()
pose.position.x = 0.15 + random.random() * .08
pose.position.y = 0.05 + random.random() * .06 - .112 * (bin_number -
1)
pose.position.z = 1.3 + random.random() * .1 # To avoid collisions
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
# Spawn model
req = SpawnModelRequest()
req.model_name = 'part_{}'.format(ix_part)
req.initial_pose = pose
req.model_xml = model_xml
req.robot_namespace = '/' + str(ix_part)
req.reference_frame = 'world'
req.model_name = 'part_{}_{}'.format(ix_part, item_counter)
spawn_model(req)
rospy.sleep(.2)
def create_model_request(sdf_model, modelname, px, py, pz, rr, rp, ry):
model = deepcopy(sdf_model)
req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = model
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
return req
def spray(self, r):
request = SpawnModelRequest()
request.model_name = 'killbox_%s' % uuid4()
request.model_xml = self.sdf
request.reference_frame = 'thorvald_001/base_link'
request.initial_pose.position.z = 0.005
request.initial_pose.position.x = -0.45
request.initial_pose.orientation.w = 1.0
request.initial_pose.position.y = r.y_diff
res = y_axes_diffResponse()
res.spray_completed = True
# rospy.sleep(0.5)
self.spawner(request)
return res
def SpawnGateRequest(self):
# Model name in Gazebo: Gate***
model_name = 'Gate' + format(self.keyframes['n'] - 1, '03')
# Read in the SDF file
f = open(
os.path.abspath(os.path.join(dirname, '../models/gate/model.sdf')),
'r')
model_xml = f.read()
# Namespace ??
robot_namespace = 'racetrack'
# Reference frame ??
reference_frame = 'world'
# Load the current keyframe coordinates
x = self.keyframes['x'][-1]
y = self.keyframes['y'][-1]
z = self.keyframes['z'][-1]
roll = self.keyframes['roll'][-1]
pitch = self.keyframes['pitch'][-1]
yaw = self.keyframes['yaw'][-1]
# Transfer Euler angles to quaternion
quat = Rotation.from_euler('xyz', [roll, pitch, yaw]).as_quat()
# Message classes Pose, Point, Quaternion of geometry_msgs.msg
current_keyframe = Pose(Point(x, y, z),
Quaternion(quat[0], quat[1], quat[2], quat[3]))
# Service class SpawnModelRequest of gazebo_msgs.srv
spawn_model_request = SpawnModelRequest(model_name, model_xml,
robot_namespace,
current_keyframe,
reference_frame)
self.SpawnModelClient(spawn_model_request)
def SpawnGateRequest(self):
# Model name in Gazebo: Gate***
model_name = 'Gate' + format(self.keyframes['n'] - 1, '03')
# Read in the SDF file # ADDITIONAL MODELS?
dirname = os.path.dirname(__file__)
f = open(
os.path.abspath(os.path.join(dirname, '../models/gate/model.sdf')),
'r')
model_xml = f.read()
# Namespace ??
robot_namespace = 'racetrack'
# Reference frame ??
reference_frame = 'world'
# Load the current keyframe position data
x = self.keyframes['x'][-1]
y = self.keyframes['y'][-1]
z = self.keyframes['z'][-1]
# Load the current keyframe quaternion data
xx, yy, zz, ww = self.CurrentQuaternion()
# Message classes Pose, Point, Quaternion of geometry_msgs.msg
current_keyframe = Pose(Point(x, y, z), Quaternion(xx, yy, zz, ww))
# Service class SpawnModelRequest of gazebo_msgs.srv
spawn_model_request = SpawnModelRequest(model_name, model_xml,
robot_namespace,
current_keyframe,
reference_frame)
self.SpawnModelClient(spawn_model_request)
def spawnTestCube():
path = '../cube.sdf'
with open(path, 'r') as basefile:
model_xml = basefile.read()
base = deepcopy(model_xml)
#Spawn the insertion base
spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
req = SpawnModelRequest()
req.model_name = "Cube"
req.model_xml = base
req.initial_pose = Pose()
req.initial_pose.position.x = 0.5
req.initial_pose.position.y = 0
req.initial_pose.position.z = 1.01
resp = spawn_model(req)
rospy.sleep(1)
def spawn_pallet(pallet_xml, model_name):
spawn_pallet_client = rospy.ServiceProxy("/gazebo/spawn_urdf_model",
SpawnModel)
spawn_model_msg = SpawnModelRequest()
spawn_model_msg.model_xml = pallet_xml
spawn_model_msg.initial_pose.position.x = 0.0
spawn_model_msg.initial_pose.position.y = -0.843470
spawn_model_msg.initial_pose.position.z = 0.80
spawn_model_msg.initial_pose.orientation.x = 0.0
spawn_model_msg.initial_pose.orientation.y = 0.0
spawn_model_msg.initial_pose.orientation.z = 0.0
spawn_model_msg.initial_pose.orientation.w = 1.0
spawn_model_msg.reference_frame = "world"
spawn_model_msg.model_name = model_name # f"pallet{i}"
spawn_pallet_client.call(spawn_model_msg)
def actor_poses_callback(actors):
global agents_list
global xml_string
current_agents = []
for actor in actors.agent_states:
current_agents.append(actor.id)
actor_id = str(actor.id)
if actor.id not in agents_list: # spawn unless spawned before
req = SpawnModelRequest()
req.model_name = 'actor_' + actor_id
req.model_xml = xml_string
req.initial_pose = actor.pose
req.reference_frame = "world"
req.robot_namespace = 'actor_' + actor_id
# rospy.logerr("SPAWNING: {}".format(actor_id))
spawn_model(req)
else: # just update the pose if spawned before
state = ModelState()
state.model_name = 'actor_' + actor_id
state.pose = actor.pose
state.reference_frame = "world"
try:
# rospy.logwarn("{}: set_state service call".format(actor_id))
set_state(state)
except rospy.ServiceException, e:
rospy.logerr("set_state failed: %s" % e)
def spawn_model (model_name, model_xml, robot_ns, init_pose, ref_frame='world',
model_type='sdf'):
print ('%sSpawning %s in Gazebo world%s' % (
ansi_colors.OKCYAN, model_name, ansi_colors.ENDC))
# Options:
# /gazebo/spawn_gazebo_model
# /gazebo/spawn_sdf_model
# /gazebo/spawn_urdf_model
spawn_srv_name = '/gazebo/spawn_%s_model' % model_type
print 'Waiting for ROS service %s...' % spawn_srv_name
rospy.wait_for_service (spawn_srv_name)
spawn_req = SpawnModelRequest ()
# Spawned with name 'robotiq' in ../launch/spawn_robotiq.launch
spawn_req.model_name = model_name
spawn_req.model_xml = model_xml
spawn_req.robot_namespace = robot_ns
# Must spawn at 0 0 0, `.` robot_state_publisher does not have access to
# this pose, so tf of hand will not include this pose!! So it needs to
# be 0, else tf of hand will be wrong.
spawn_req.initial_pose = init_pose
spawn_req.reference_frame = ref_frame
try:
spawn_srv = rospy.ServiceProxy (spawn_srv_name, SpawnModel)
resp = spawn_srv (spawn_req)
except rospy.ServiceException, e:
print ("Service initialization failed: %s" % e)
return False
def create_req(self, model_name, model_xml, initial_pose):
req = SpawnModelRequest()
req.model_name = model_name
req.model_xml = model_xml
req.robot_namespace = ""
req.reference_frame = ""
req.initial_pose = initial_pose
return req
def spawn_model(self, model_name, pose):
""" spawn model into given pose
Args:
model_name (string): 'button_panel_pressed' or 'elevator_door' or 'level2_pressed'
pose (geometry_msgs.msg.Pose): location in simulation
"""
r = SpawnModelRequest()
if model_name == "panel_pressed":
f = open(self.panel_pressed_sdf)
elif model_name == "level2_pressed":
f = open(self.level2_pressed_sdf)
else:
f = open(self.elevator_door_sdf)
r.model_xml = f.read()
f.close()
r.model_name = model_name
r.initial_pose = pose
self.spawn_srv(r)
def spawn_object(self):
'''
Spawn an object at a defined position
https://github.com/ipa320/srs_public/blob/master/srs_user_tests/ros/scripts/spawn_object.py
'''
# print("Spawn object")
# Create position of the object
name = "object_to_push"
# convert rpy to quaternion for Pose message
# orientation = rospy.get_param("/objects/%s/orientation" % name)
orientation = [0.0, 0.0, 0.0]
quaternion = tft.quaternion_from_euler(orientation[0], orientation[1],
orientation[2])
object_pose = Pose()
object_pose.position.x = float(self.object_position[0])
object_pose.position.y = float(self.object_position[1])
object_pose.position.z = float(self.object_position[2])
object_pose.orientation.x = quaternion[0]
object_pose.orientation.y = quaternion[1]
object_pose.orientation.z = quaternion[2]
object_pose.orientation.w = quaternion[3]
# Create object using XACRO
file_localition = roslib.packages.get_pkg_dir(
'reflex_description') + '/urdf/object_to_catch.xacro'
p = os.popen("rosrun xacro xacro.py " + file_localition)
xml_string = p.read()
p.close()
srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel)
# rospy.wait_for_service("/gazebo/spawn_urdf_model")
# srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
# file_xml = open(file_localition)
# xml_string=file_xml.read()
# spawn new model
req = SpawnModelRequest()
req.model_name = name # model name from command line input
req.model_xml = xml_string
req.initial_pose = object_pose
res = srv_spawn_model(req)
def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz):
"""
Create a cube to be spawned in gazebo
@param: px: Position coordinate on x-axis
@param: py: Position coordinate on y-axis
@param: pz: Position coordinate on z-axis
@param: rr: Roll rotation
@param: rp: Pitch rotation
@param: ry: Yaw rotation
@param: sx: Cube size on x-axis
@param: sy: Cube size on y-axis
@param: sz: Cube size on z-axis
"""
# Get Models' Path
model_path = rospkg.RosPack().get_path(
'robotiq_2f_gripper_control') + "/models/"
with open(model_path + 'cube.sdf', 'r') as file:
sdf_cube = file.read().replace('\n', '')
cube = deepcopy(sdf_cube)
# Replace size of model
size_str = str(round(sx, 3)) + " " + \
str(round(sy, 3)) + " " + str(round(sz, 3))
cube = cube.replace('SIZEXYZ', size_str)
# Replace modelname
cube = cube.replace('MODELNAME', str(modelname))
req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = cube
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
return req
def initial_setup(self):
'''
Remove all the models in the sim and spawn a ground plane to run tests with.
TODO: Sometimes all the models don't get deleted correctly - probably a gazeb problem.
'''
models = yield self.model_states.get_next_message()
if 'ground_plane' not in models.name:
s = SpawnModelRequest()
s.model_name = 'ground_plane'
s.model_xml = ground_plane
s.initial_pose = msg_helpers.numpy_quat_pair_to_pose([0, 0, -5], [0, 0, 0, 1])
yield self.spawn_model(s)
for model in models.name:
if model == "channel_marker_1" or model == "sub8" or model == 'ground_plane':
continue
print "MARKER_TEST - Deleting {}".format(model)
self.delete_model(DeleteModelRequest(model_name=model))
self.nh.sleep(1)
def initial_setup(self):
'''
Remove all the models in the sim and spawn a ground plane to run tests with.
TODO: Sometimes all the models don't get deleted correctly - probably a gazeb problem.
'''
models = yield self.model_states.get_next_message()
if 'ground_plane' not in models.name:
s = SpawnModelRequest()
s.model_name = 'ground_plane'
s.model_xml = ground_plane
s.initial_pose = msg_helpers.numpy_quat_pair_to_pose([0, 0, -5], [0, 0, 0, 1])
yield self.spawn_model(s)
for model in models.name:
if model == "channel_marker_1" or model == "sub8" or model == 'ground_plane':
continue
print "MARKER_TEST - Deleting {}".format(model)
self.delete_model(DeleteModelRequest(model_name=model))
self.nh.sleep(1)
def create_box_request(self,modelname, px, py, pz, rr, rp, ry):
"""Create a SpawnModelRequest with the parameters of the cube given.
modelname: name of the model for gazebo
px py pz: position of the cube (and it's collision cube)
rr rp ry: rotation (roll, pitch, yaw) of the model"""
cube = deepcopy(sdf_sand_box)
req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = cube
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
return req
def creat_gazebo_model(self, name):
req = SpawnModelRequest()
rr = rp = ry = 0
shape = self.check_name(name)
# print(shape)
if shape:
self.path = os.path.join(self.dir_path, shape + "/model.sdf")
path = open(self.path, "r")
file = path.read()
# print(file)
if shape == "Pentagon":
px = 0.9
py = 1.0
pz = 1.15
if shape == "Triangle":
px = 0.9
py = 0.4
pz = 1.15
if shape == "Rectangle":
px = 0.9
py = 0.7
pz = 1.15
req.model_name = name
req.model_xml = file
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
result = self.spawn_srv.call(req)
return result
def on_enter(self, userdata):
self._gazebo_model_pose = userdata.gazebo_model_pose
if self._gazebo_model_pose is not None:
if self._model_name == "box_male":
# Get male box Path
model_path = rospkg.RosPack().get_path('birl_baxter_description') + "/urdf/box/"
# Load male box SDF
self._model_xml = ''
with open(model_path + "box_male/robots/box_male.URDF", "r") as box_male_file:
self._model_xml = box_male_file.read().replace('\n', '')
try:
self._srv_result = self._srv.call(self._service_name,
SpawnModelRequest(self._model_name, self._model_xml, "/",
self._gazebo_model_pose, self._model_reference_frame))
rospy.loginfo('loading male box succesfully')
except Exception, e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
self._failed = True
elif self._model_name == "box_female":
# get path
model_path = rospkg.RosPack().get_path('birl_baxter_description') + "/urdf/box/"
# Load female box URDF
box_female_xml = ''
with open(model_path + "box_female/robots/box_female.URDF", "r") as box_female_file:
box_female_xml = box_female_file.read().replace('\n', '')
rospy.wait_for_service('/gazebo/spawn_urdf_model')
try:
self._srv_result = self._srv.call(self._service_name,
SpawnModelRequest(self._model_name, self._model_xml, "/",
self._gazebo_model_pose, self._model_reference_frame))
except Exception, e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
self._failed = True
def spawn_square(x, y, z):
request = SpawnModelRequest()
request.initial_pose.position.x = x
request.initial_pose.position.y = y
request.initial_pose.position.z = z
request.model_name = "square"
with open(
rospkg.RosPack().get_path("dragonfly") +
"/models/square/model.sdf", 'r') as f:
model_xml = f.read()
request.model_xml = model_xml
# print(model_xml)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
service_proxy = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
try:
response = service_proxy(request) # type: SpawnModelResponse
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
def createSpawnMessage(
model_name='',
model_path='',
xyz=[0, 0, 0],
rpy=[0, 0, 0],
reference_frame='world'
):
"""
Create a gazebo_msgs.msg.SpawnModel request message from the parameters.
model_name -- Name of the model in the simulation
model_path -- Path to the sdf file of the model
xyz -- array of length 3 with the xyz coordinates
rpy -- array of length 3 with the rpy rotations. These are converted to a quaternion
reference_frame -- the reference frame of the coordinates
returns -- SpawnModelRequest instance
"""
request = SpawnModelRequest()
request.model_name = model_name
file = open(resource_retriever.get_filename(model_path, use_protocol=False))
request.model_xml = file.read()
file.close()
request.initial_pose.position.x = xyz[0]
request.initial_pose.position.y = xyz[1]
request.initial_pose.position.z = xyz[2]
quaternion = transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
request.initial_pose.orientation.x = quaternion[0]
request.initial_pose.orientation.y = quaternion[1]
request.initial_pose.orientation.z = quaternion[2]
request.initial_pose.orientation.w = quaternion[3]
request.reference_frame = reference_frame
return request
def __init__(self):
rospy.init_node('robot_spawner')
self.ns = rospy.get_namespace()
# Get robot index
try:
index = re.findall('[0-9]+', self.ns)[0]
except IndexError:
index = 0
i = index
# Spawn URDF service client
rospy.wait_for_service(RobotSpawner.SPAWN_URDF_TOPIC)
try:
spawn_urdf_model = rospy.ServiceProxy(
RobotSpawner.SPAWN_URDF_TOPIC, SpawnModel)
# Filling model spawner request
msg = SpawnModelRequest()
# Model name
msg.model_name = "irobot_create2.{}".format(i)
# Robot information from robot_description
robot_description_param = "/create{}/robot_description".format(i)
if rospy.has_param(robot_description_param):
msg.model_xml = rospy.get_param(robot_description_param)
msg.robot_namespace = self.ns
# Using pose from parameter server
msg.initial_pose = Pose()
msg.initial_pose.position.x = rospy.get_param(
"{}/amcl/initial_pose_x".format(self.ns), randint(-10, 10))
msg.initial_pose.position.y = rospy.get_param(
"{}/amcl/initial_pose_y".format(self.ns), randint(-10, 10))
msg.initial_pose.position.z = rospy.get_param(
"{}/amcl/initial_pose_z".format(self.ns), 0.)
yaw = rospy.get_param("{}/amcl/initial_pose_a".format(self.ns), 0.)
q = quaternion_from_euler(0, 0, yaw)
msg.initial_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
msg.reference_frame = "world"
# Spawn model
res = spawn_urdf_model(msg)
print(res.status_message)
except rospy.ServiceException:
print("Could not spawn {}".format(msg.model_name))
exit(1)
print("{} spawned correctly".format(msg.model_name))
def human_task(self, name, px, py, pz):
req = SpawnModelRequest()
rr = rp = ry = 0
shape = self.check_name(name)
# print(shape)
if shape:
self.path = os.path.join(self.dir_path, shape + "/model.sdf")
path = open(self.path, "r")
file = path.read()
# print(file)
req.model_name = name
req.model_xml = file
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
self.spawn_srv.call(req)
</visual>
<velocity_decay>
<linear>0.000000</linear>
<angular>0.000000</angular>
</velocity_decay>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
</sdf>
"""
if __name__ == '__main__':
rospy.init_node('test_spawn')
spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
spawn_srv.wait_for_service()
req = SpawnModelRequest()
req.model_name = 'test_model'
req.model_xml = sdf_model
req.robot_namespace = ""
req.reference_frame = ""
req.initial_pose = Pose()
req.initial_pose.position.z = 1.0
#req.initial_pose.orientation.w = 1.0
rospy.loginfo("Call: " + str(req))
resp = spawn_srv.call(req)
rospy.loginfo("Response: " + str(resp))
