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 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)