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