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 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 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 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 spawn_and_delete_test_serverclient():
rospy.init_node("object_spawner", log_level=rospy.DEBUG)
spawndelete_obj = SpawnDeleteClass()
rospy.logwarn("Waiting for /spawndelete_models_server...")
rospy.wait_for_service('/spawndelete_models_server')
rospy.logwarn("Waiting for /spawndelete_models_server...READY")
spawndelete_client = rospy.ServiceProxy('/spawndelete_models_server',
SpawnModel)
object_pose = Pose()
object_pose.position.z = 0.7
model_name = "standard_apple"
request = SpawnModelRequest()
request.model_name = model_name
request.model_xml = "spawn_robot_tools_pkg"
request.robot_namespace = "SPAWN"
request.initial_pose = object_pose
request.reference_frame = "models"
response = spawndelete_client(request)
object_pose.position.x = 0.1
model_name2 = "standard_cube"
request.model_name = model_name2
request.initial_pose = object_pose
response = spawndelete_client(request)
time.sleep(5)
request.model_name = model_name
request.robot_namespace = "DELETE"
response = spawndelete_client(request)
request.model_name = model_name2
request.robot_namespace = "DELETE"
response = spawndelete_client(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_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_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 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 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 spawn_new_model(self, model_name, model_type, models_package_name,
object_pose):
path_to_package = self._rospack.get_path(models_package_name)
base_dir_localition = os.path.join(path_to_package, model_type)
result_ok = True
result_msg = ""
if model_type == "models":
model_dir_localition = os.path.join(base_dir_localition,
model_name)
file_localition = os.path.join(model_dir_localition, 'model.sdf')
else:
file_localition = os.path.join(base_dir_localition,
model_name + '.' + model_type)
try:
os.path.isfile(file_localition)
except:
result_ok = False
result_msg = "File not found: " + str(file_localition)
rospy.logerr(result_msg)
return result_ok, result_msg
# call gazebo service to spawn model (see http://ros.org/wiki/gazebo)
if model_type == "urdf":
srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel)
file_xml = open(file_localition)
xml_string = file_xml.read()
elif model_type == "xacro":
p = os.popen("rosrun xacro xacro " + file_localition)
xml_string = p.read()
p.close()
srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel)
elif model_type == "models":
srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model',
SpawnModel)
file_xml = open(file_localition)
xml_string = file_xml.read()
else:
result_ok = False
result_msg = 'Model type not know. model_type = ' + model_type
rospy.logerr(result_msg)
return result_ok, result_msg
# spawn new model
req = SpawnModelRequest()
req.model_name = model_name
req.model_xml = xml_string
req.initial_pose = object_pose
res = srv_spawn_model(req)
# evaluate response
if res.success:
result_msg = res.status_message + " " + model_name
rospy.loginfo(result_msg)
else:
result_msg = "Error: model %s not spawn. error message = " % model_name + res.status_message
rospy.logerr(result_msg)
result_ok = res.success
result_msg = res.status_message
return result_ok, result_msg
def spawn_cylinder(pos, size, id, scale):
cylinder_sdf_model = """<?xml version='1.0'?>
<sdf version='1.6'>
<model name='unit_cylinder'>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<visual name='visual'>
<geometry>
<cylinder>
<radius>""" + repr(size[0] * scale) + """</radius>
<length>""" + repr(size[1] * scale) + """</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
<shader type='pixel'/>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<pose frame=''>0 0 0 0 -0 0</pose>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<radius>""" + repr(size[0] * scale) + """</radius>
<length>""" + repr(size[1] * scale) + """</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<static>1</static>
<allow_auto_disable>1</allow_auto_disable>
</model>
</sdf>
"""
req = SpawnModelRequest()
req.model_name = 'cylinder_' + repr(id)
req.model_xml = cylinder_sdf_model
req.robot_namespace = ""
req.reference_frame = ""
req.initial_pose = Pose()
req.initial_pose.position.x = pos[0] * scale
req.initial_pose.position.y = pos[1] * scale
req.initial_pose.position.z = pos[2] * scale + 0.5 * size[1] * scale
req.initial_pose.orientation.x = pos[3] * scale
req.initial_pose.orientation.y = pos[4] * scale
req.initial_pose.orientation.z = pos[5] * scale
req.initial_pose.orientation.w = pos[6] * scale
return req
def laser_cone(self):
# flag pour boucle -> trouver tf2
transf = 0
orient = Quaternion(0, 0, 0, 1)
print("While.")
while transf == 0:
try: #listen to tf
transforme = self.buf.lookup_transform('odom',
'cameraBras_link',
rospy.Time(0))
xc = transforme.transform.translation.x
yc = transforme.transform.translation.y
zc = 0.08
transf = 1
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
#print("err.")
continue
#getting laser position et "printing" laser on gazebo
laser = "laser"
self.delete_model(laser)
laser_pose = Pose(Point(x=xc, y=yc, z=zc), orient)
request = SpawnModelRequest()
request.model_name = laser
request.model_xml = self.laser_urdf
print(request.model_xml)
request.initial_pose = laser_pose
response = self.spawn_model(request)
if not response.success:
rospy.logwarn("Unable to spawn sdf: {}".format(
response.status_message))
#deleting plants
l_dists = []
for i in self.plants:
plant_name = "plant{}".format(i)
p = self.get_model_state("floor", plant_name)
x = -p.pose.position.x
y = -p.pose.position.y
l_dists.append(np.sqrt((x - xc)**2 + (y - yc)**2))
print(i, round(x, 3), round(y, 3))
print("\n")
print(l_dists)
if len(l_dists) > 0:
ind = np.argmin(l_dists)
ind2 = self.plants[ind]
self.plants.remove(ind2)
print(ind)
print(self.plants, "plant{}".format(ind2), "\n\n")
self.delete_model("plant{}".format(ind2))
self.delete_model(laser)
return ind
srv_delete_model = rospy.ServiceProxy(
'gazebo/delete_model', DeleteModel
) # TODO this service causes gazebo (current groovy version) to crash
try:
res = srv_delete_model(key)
except rospy.ServiceException:
rospy.logdebug(
"Error while trying to call Service /gazebo/get_world_properties."
)
rospy.loginfo(
"Model %s already exists in gazebo. Model will be deleted and added again.",
key)
# spawn new model
req = SpawnModelRequest()
req.model_name = key # model name from command line input
req.model_xml = xml_string
req.initial_pose = object_pose
try:
res = srv_spawn_model(req)
except rospy.service.ServiceException:
break
# evaluate response
if res.success == True:
rospy.loginfo(res.status_message + " " + key)
else:
rospy.logerr("Error: model %s not spawn. error message = " % key +
res.status_message)
</geometry>
</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))
</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))
def build_gz_cube_req(name, frame_id, x, y, z, roll, pitch, yaw, width, height,
depth, mass, color):
"""
create the gazebo SpawnModel service request for a cube
"""
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
q = transform.transformations.quaternion_from_euler(roll, pitch, yaw)
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
model = SpawnModelRequest()
model.model_name = name
sdf = objectify.Element('sdf', version='1.4')
sdf.model = objectify.Element('model', name=name)
sdf.model.static = 'false'
sdf.model.link = objectify.Element('link', name='link')
sdf.model.link.inertial = objectify.Element('inertial')
sdf.model.link.inertial.mass = mass
xx = mass / 12.0 * (height * height + depth * depth)
yy = mass / 12.0 * (width * width + depth * depth)
zz = mass / 12.0 * (width * width + height * height)
sdf.model.link.inertial.inertia = objectify.Element('inertia')
sdf.model.link.inertial.inertia.ixx = xx
sdf.model.link.inertial.inertia.iyy = yy
sdf.model.link.inertial.inertia.izz = zz
sdf.model.link.inertial.inertia.ixy = 0.0
sdf.model.link.inertial.inertia.iyz = 0.0
sdf.model.link.inertial.inertia.ixz = 0.0
sdf.model.link.collision = objectify.Element('collision', name='collision')
sdf.model.link.collision.geometry = objectify.Element('geometry')
sdf.model.link.collision.geometry.box = objectify.Element('box')
sdf.model.link.collision.geometry.box.size = '{} {} {}'.format(
width, height, depth)
sdf.model.link.collision.surface = objectify.Element('surface')
sdf.model.link.collision.surface.friction = objectify.Element('friction')
sdf.model.link.collision.surface.friction.ode = objectify.Element('ode')
sdf.model.link.collision.surface.friction.ode.mu = 1000000
sdf.model.link.collision.surface.friction.ode.mu2 = 1000000
sdf.model.link.collision.surface.friction.ode.fdir1 = '1.0 1.0 1.0'
sdf.model.link.collision.surface.friction.ode.slip1 = 0.0
sdf.model.link.collision.surface.friction.ode.slip2 = 0.0
sdf.model.link.collision.surface.bounce = objectify.Element('bounce')
sdf.model.link.collision.surface.bounce.restitution_coefficient = 0.0
sdf.model.link.collision.surface.bounce.threshold = 100000.0
sdf.model.link.collision.surface.contact = objectify.Element('contact')
sdf.model.link.collision.surface.contact.ode = objectify.Element('ode')
sdf.model.link.collision.surface.contact.ode.soft_cfm = 0.0
sdf.model.link.collision.surface.contact.ode.soft_erp = 0.2
sdf.model.link.collision.surface.contact.ode.kp = 10000000
sdf.model.link.collision.surface.contact.ode.kd = 1
sdf.model.link.collision.surface.contact.ode.max_vel = 0.0
sdf.model.link.collision.surface.contact.ode.min_depth = 0.001
sdf.model.link.visual = objectify.Element('visual', name='visual')
sdf.model.link.visual.geometry = objectify.Element('geometry')
sdf.model.link.visual.geometry.box = objectify.Element('box')
sdf.model.link.visual.geometry.box.size = '{} {} {}'.format(
width, height, depth)
sdf.model.link.visual.material = objectify.Element('material')
sdf.model.link.visual.material.script = objectify.Element('script')
sdf.model.link.visual.material.script = objectify.Element('script')
sdf.model.link.visual.material.script.uri = 'file://media/materials/scripts/gazebo.material'
sdf.model.link.visual.material.script.name = 'Gazebo/{}'.format(color)
objectify.deannotate(sdf)
etree.cleanup_namespaces(sdf)
model.model_xml = (etree.tostring(sdf,
encoding='utf-8',
xml_declaration=True)).decode("utf-8")
model.robot_namespace = "cube_spawner"
model.initial_pose = p
model.reference_frame = frame_id
# return etree.tostring(sdf, encoding='utf-8', xml_declaration=True)
return model
spawner = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
urdf_file = ''
with open(
"/home/user/car_ws/src/car_description/urdf/simple_car_description_gazebo.urdf",
"r") as f:
urdf_file = f.read()
orientation = tf.transformations.quaternion_from_euler(0, 0, 0)
pose = Pose()
pose.position.x = 0.0
pose.position.y = 0.0
pose.position.z = 0.0
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
request = SpawnModelRequest()
request.model_name = "car"
request.model_xml = urdf_file
request.robot_namespace = ""
request.initial_pose = pose
request.reference_frame = "world"
response = spawner(request)
print(response.success)
print(response.status_message)
