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