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 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 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 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 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 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 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 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 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_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 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 spawn_bricks(self, name, size, transform):
rospy.loginfo('SPAWN BRICK [' + name + ']')
req = SpawnModelRequest()
req.model_name = name
req.model_xml = create_brick(name, size)
req.initial_pose = frame_to_pose(transform)
# should this be unique so ros_control can use each individually?
req.robot_namespace = "/bricks"
self.spawn_model(req)
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 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 spawn_treasure_pose_model(self, idd):
tf_stamped = transform_matrix_to_TransformStamped(self.spots_world_frames[idd], "0", str(idd))
req = SpawnModelRequest()
req.model_name = "pose_"+str(idd)
req.model_xml = self.treasure_model
req.robot_namespace = "pose_"+str(idd)
req.reference_frame = "world"
req.initial_pose.position.x = tf_stamped.transform.translation.x
req.initial_pose.position.y = tf_stamped.transform.translation.y
req.initial_pose.position.z = tf_stamped.transform.translation.z
req.initial_pose.orientation = tf_stamped.transform.rotation
resp = self.spawn_sdf_model(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 main():
rospy.init_node("products_spawner")
spawn_client = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
body_wrench_client = rospy.ServiceProxy("/gazebo/apply_body_wrench",
ApplyBodyWrench)
rate = rospy.Rate(0.1) # 10 sec
# srv variables
body_wrench_msg = ApplyBodyWrenchRequest()
spawn_model_msg = SpawnModelRequest()
# wait for services
spawn_client.wait_for_service()
rospy.loginfo("spawn_urdf_model service is ready")
body_wrench_client.wait_for_service()
rospy.loginfo("apply_body_wrench service is ready")
# get path for urdf file
product_path = rospy.get_param("product_path")
product_xml = open(product_path).read()
# initial conveyor belt movement
body_wrench_msg.wrench.force.x = -0.00012
body_wrench_msg.wrench.force.y = 0.0
body_wrench_msg.wrench.force.z = 0.0
body_wrench_msg.reference_frame = "world"
body_wrench_msg.start_time = rospy.Time(0, 0)
body_wrench_msg.duration = rospy.Duration(1000, 0)
# initial spawn model
i = 0
spawn_model_msg.model_xml = product_xml
spawn_model_msg.initial_pose.position.x = 4.0
spawn_model_msg.initial_pose.position.y = 0.843470
spawn_model_msg.initial_pose.position.z = 0.71
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 = 0.0
spawn_model_msg.reference_frame = "world"
spawn_model_msg.model_name = f"product_{i}"
while not rospy.is_shutdown():
spawn_model_msg.model_name = f"product_{i}"
spawn_client.call(spawn_model_msg)
body_wrench_msg.body_name = spawn_model_msg.model_name + "::base_link"
body_wrench_client.call(body_wrench_msg)
i = i + 1
rate.sleep()
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 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_unknown_obstacle(obstacle_name, obstacle_model_xml, obstacle_pose):
# Service proxy to spawn urdf object in Gazebo.
spawn_obstacle_service = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel)
# Create service request.
spawn_obstacle_request = SpawnModelRequest()
spawn_obstacle_request.model_name = obstacle_name
spawn_obstacle_request.model_xml = obstacle_model_xml
spawn_obstacle_request.robot_namespace = ''
spawn_obstacle_request.initial_pose = obstacle_pose
spawn_obstacle_request.reference_frame = 'map'
# Call spawn model service.
spawn_obstacle_response = spawn_obstacle_service(spawn_obstacle_request)
if (not spawn_obstacle_response.success):
rospy.logerr('Could not spawn unknown obstacle')
rospy.logerr(spawn_obstacle_response.status_message)
else:
rospy.loginfo(spawn_obstacle_response.status_message)
def spawn_ur5e_2f85(robot_side, sim):
x, y, z, R, P, Y, __ = load_ur5e_2f85_params(robot_side, sim)
robot_description = rospy.get_param('/' + robot_side +
'/robot_description')
q = quaternion_from_euler(R, P, Y)
req = SpawnModelRequest()
rospy.loginfo("Waiting for gazebo/spawn_urdf_model...")
rospy.wait_for_service('gazebo/spawn_urdf_model')
rospy.loginfo("gazebo/spawn_urdf_model is active")
try:
spawn_model_proxy = rospy.ServiceProxy('gazebo/spawn_urdf_model',
SpawnModel)
except rospy.ServiceException as e:
rospy.logerr("Service call to SpawnModel failed!")
robot_position = Point(x=x, y=y, z=z)
robot_orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
print(robot_position)
print(robot_orientation)
robot_pose = Pose(position=robot_position, orientation=robot_orientation)
try:
robot_name = robot_side + '_ur5e_2f85'
frame_name = ''
rospy.logdebug("Spawning robot with name: {}".format(robot_name))
rospy.logdebug("Frame name: {}".format(frame_name))
req.model_name = robot_name
req.model_xml = robot_description
req.initial_pose = robot_pose
req.robot_namespace = robot_side
req.reference_frame = ''
spawn_model_proxy(req)
except Exception as e:
print(e)
rospy.logerr("{} couldn't be spawned.".format(robot_name))
return None
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 spawn():
req = SpawnModelRequest()
req.model_name = "model"
req.model_xml = "/home/rbe3002/catkin_ws/src/turtlebot3/turtlebot3_description/rviz/model.rviz"
req.robot_namespace = "model"
req.initial_pose.position.x = 0.0
req.initial_pose.position.y = 0.0
req.initial_pose.position.z = 0.0
req.initial_pose.orientation.x = 0.0
req.initial_pose.orientation.y = 0.0
req.initial_pose.orientation.z = 0.0
req.initial_pose.orientation.w = 1.0
req.reference_frame = ''
try:
srv = ServiceProxy('/gazebo/set_model_state', SpawnModel)
resp = srv(req)
except ServiceException, e:
print(" Service call failed: %s" % e)
return
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 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 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
</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))
