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 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_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 __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 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 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 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 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(): 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_model(self, name, xml_file_name, pose_dict ): rospy.loginfo("Creating model {}".format(name)) xml_model = check_output([ self.path_xacro, self.sdf_file_path ]).strip('\n') request = SpawnModelRequest() request.model_name = name request.model_xml = xml_model request.robot_namespace = '' request.reference_frame = '' request.initial_pose.position.x = pose_dict.x request.initial_pose.position.y = pose_dict.y request.initial_pose.position.z = pose_dict.z request.initial_pose.orientation.x = pose_dict.qx request.initial_pose.orientation.y = pose_dict.qy request.initial_pose.orientation.z = pose_dict.qz request.initial_pose.orientation.w = pose_dict.qw response = self.add_model(request) rospy.loginfo(response)
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 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)
</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 spawn_model( mav_sys_id, vehicle_type, baseport, color, pose, ros_master_uri=None, mavlink_address=None, debug=False, autopilot='ardupilot', gazebo_ip='127.0.0.1', local_ip='127.0.0.1', use_radar=False, ): x, y, yaw = pose if ros_master_uri: original_uri = os.environ[ROS_MASTER_URI] os.environ[ROS_MASTER_URI] = ros_master_uri srv = ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) model_file = "" if vehicle_type == "iris": if autopilot == 'px4': model_directory = "rotors_description" model_file = "urdf/iris_base.xacro" elif autopilot == 'ardupilot': model_directory = 'iris_with_standoffs_demo' model_file = "model.sdf" elif vehicle_type == "delta_wing": if autopilot == 'px4': model_directory = "delta_wing" model_file = "delta_wing.sdf" elif autopilot == 'ardupilot': model_directory = 'zephyr_delta_wing_ardupilot_demo' model_file = "delta_wing.sdf" model_pathname = os.path.join(os.path.dirname(__file__), '..', '..', '..', '..', 'share', 'uctf', 'models', '%s' % model_directory) model_filename = os.path.join(model_pathname, '%s' % model_file) with open(model_filename, 'r') as h: model_xml = h.read() kwargs = { 'mappings': { 'mavlink_udp_port': str(baseport), }, } # some default args for iris (see sitl_gazebo cmake file for list) kwargs['mappings']['enable_mavlink_interface'] = "true" kwargs['mappings']['enable_ground_truth'] = "false" kwargs['mappings']['enable_logging'] = "false" kwargs['mappings']['enable_camera'] = "false" # ros commandline arguments kwargs['mappings']['rotors_description_dir'] = model_pathname kwargs['mappings']['scripts_dir'] = os.path.join(os.path.dirname(__file__), '..', '..', '..', '..', 'share', 'uctf', 'sitl_gazebo_scripts') # additional xacro params for uctf if color: # material_name = 'Gazebo/%s' % color.capitalize() kwargs['mappings']['visual_material'] = color.capitalize() if mavlink_address: kwargs['mappings']['mavlink_addr'] = mavlink_address if autopilot == 'ardupilot': kwargs['mappings']['fdm_addr'] = local_ip kwargs['mappings']['listen_addr'] = gazebo_ip # fdm in is gazebo out kwargs['mappings']['fdm_port_in'] = str(baseport + 5) # fdm out is gazebo in kwargs['mappings']['fdm_port_out'] = str(baseport + 4) kwargs['mappings']['use_radar'] = 'true' if use_radar else 'false' model_xml = xacro(model_xml, **kwargs) if debug: print(model_xml) req = SpawnModelRequest() unique_name = vehicle_type + '_' + str(mav_sys_id) req.model_name = unique_name req.model_xml = model_xml req.robot_namespace = unique_name req.initial_pose.position.x = x req.initial_pose.position.y = y req.initial_pose.position.z = 50.0 req.initial_pose.orientation.x = 0.0 req.initial_pose.orientation.y = 0.0 req.initial_pose.orientation.z = math.sin(yaw / 2.0) req.initial_pose.orientation.w = math.cos(yaw / 2.0) req.reference_frame = '' resp = srv(req) if ros_master_uri: os.environ[ROS_MASTER_URI] = original_uri if resp.success: print(resp.status_message, '(%s)' % unique_name) return 0 else: print(resp.status_message, file=sys.stderr) return 1
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 spawn_model( mav_sys_id, vehicle_type, tcp_port, udp_port, pose, ros_master_uri=None, mavlink_address=None, enable_rangefinder=False, enable_teraranger=False, enable_rangefinder_up=False, enable_ground_truth=False, enable_mobius_camera_down=False, enable_mobius_camera_front=False, enable_mobius_camera_back_right=False, enable_mobius_camera_back_left=False, enable_realsense_front_pitched=False, enable_realsense_down=False, enable_realsense_top=False, enable_realsense_front=False, use_realistic_realsense=False, enable_whycon_box=False, enable_bluefox_camera=False, enable_bluefox_camera_reverse=False, enable_pendulum=False, enable_timepix=False, enable_magnetic_gripper=False, enable_scanse_sweep=False, enable_rplidar=False, enable_teraranger_tower_evo=False, enable_velodyne=False, enable_ouster=False, ouster_model="OS1-16", use_gpu_ray=False, enable_ball_holder=False, enable_thermal_camera=False, enable_water_gun=False, enable_parachute=False, enable_light=False, enable_servo_camera=False, wall_challenge=False, fire_challenge=False, fire_challenge_blanket=False, gps_indoor_jamming = False, enable_uv_leds=False, uvled_fr_l=6, uvled_fr_r=15, enable_uv_leds_beacon=False, uvled_beacon_f=30, enable_uv_camera=False, uvcam_calib_file="~/calib_results.txt", debug=False ): x, y, z, yaw = pose if ros_master_uri: original_uri = os.environ[ROS_MASTER_URI] os.environ[ROS_MASTER_URI] = ros_master_uri model_pathname, model_xml = get_model_xacro_file(vehicle_type) if enable_uv_camera or enable_uv_leds or enable_uv_leds_beacon: check_for_uvdar_package() kwargs = { 'mappings': { 'mavlink_udp_port': str(udp_port), 'mavlink_tcp_port': str(tcp_port), }, } # some default args for simulation (see sitl_gazebo cmake file for list) kwargs['mappings']['namespace'] = "uav" + str(mav_sys_id % 100) kwargs['mappings']['enable_ground_truth'] = "true" if enable_ground_truth else "false" kwargs['mappings']['enable_bluefox_camera'] = "true" if enable_bluefox_camera else "false" kwargs['mappings']['enable_bluefox_camera_reverse'] = "true" if enable_bluefox_camera_reverse else "false" kwargs['mappings']['enable_mobius_camera_down'] = "true" if enable_mobius_camera_down else "false" kwargs['mappings']['enable_mobius_camera_front'] = "true" if enable_mobius_camera_front else "false" kwargs['mappings']['enable_mobius_camera_back_left'] = "true" if enable_mobius_camera_back_left else "false" kwargs['mappings']['enable_mobius_camera_back_right'] = "true" if enable_mobius_camera_back_right else "false" kwargs['mappings']['enable_realsense_front_pitched'] = "true" if enable_realsense_front_pitched else "false" kwargs['mappings']['enable_realsense_down'] = "true" if enable_realsense_down else "false" kwargs['mappings']['enable_realsense_top'] = "true" if enable_realsense_top else "false" kwargs['mappings']['enable_realsense_front'] = "true" if enable_realsense_front else "false" kwargs['mappings']['use_realistic_realsense'] = "true" if use_realistic_realsense else "false" kwargs['mappings']['enable_rangefinder'] = "true" if enable_rangefinder else "false" kwargs['mappings']['enable_teraranger'] = "true" if enable_teraranger else "false" kwargs['mappings']['enable_rangefinder_up'] = "true" if enable_rangefinder_up else "false" kwargs['mappings']['enable_whycon_box'] = "true" if enable_whycon_box else "false" kwargs['mappings']['enable_magnetic_gripper'] = "true" if enable_magnetic_gripper else "false" kwargs['mappings']['enable_scanse_sweep'] = "true" if enable_scanse_sweep else "false" kwargs['mappings']['enable_rplidar'] = "true" if enable_rplidar else "false" kwargs['mappings']['enable_teraranger_tower_evo'] = "true" if enable_teraranger_tower_evo else "false" kwargs['mappings']['enable_pendulum'] = "true" if enable_pendulum else "false" kwargs['mappings']['enable_timepix'] = "true" if enable_timepix else "false" kwargs['mappings']['enable_velodyne'] = "true" if enable_velodyne else "false" kwargs['mappings']['enable_ouster'] = "true" if enable_ouster else "false" kwargs['mappings']['ouster_model'] = ouster_model kwargs['mappings']['use_gpu_ray'] = "true" if use_gpu_ray else "false" kwargs['mappings']['enable_ball_holder'] = "true" if enable_ball_holder else "false" kwargs['mappings']['enable_water_gun'] = "true" if enable_water_gun and not fire_challenge else "false" kwargs['mappings']['enable_parachute'] = "true" if enable_parachute else "false" kwargs['mappings']['enable_light'] = "true" if enable_light else "false" kwargs['mappings']['enable_servo_camera'] = "true" if enable_servo_camera else "false" kwargs['mappings']['enable_thermal_camera'] = "true" if enable_thermal_camera else "false" kwargs['mappings']['wall_challenge'] = "true" if wall_challenge else "false" kwargs['mappings']['fire_challenge'] = "true" if fire_challenge else "false" kwargs['mappings']['fire_challenge_blanket'] = "true" if fire_challenge_blanket else "false" kwargs['mappings']['gps_indoor_jamming'] = "true" if gps_indoor_jamming else "false" kwargs['mappings']['enable_uv_leds'] = "true" if enable_uv_leds else "false" kwargs['mappings']['uvled_fr_l'] = uvled_fr_l kwargs['mappings']['uvled_fr_r'] = uvled_fr_r kwargs['mappings']['enable_uv_leds_beacon'] = "true" if enable_uv_leds_beacon else "false" kwargs['mappings']['uvled_beacon_f'] = uvled_beacon_f kwargs['mappings']['enable_uv_camera'] = "true" if enable_uv_camera else "false" kwargs['mappings']['uvcam_calib_file'] = uvcam_calib_file # ros commandline arguments kwargs['mappings']['mrs_robots_description_dir'] = model_pathname # additional xacro params for uctf if mavlink_address: kwargs['mappings']['mavlink_addr'] = mavlink_address if not detect_unused_arguments_in_xacro(model_xml, vehicle_type, **kwargs): print_error("===================================================================================") print_error(" UAV cannot be spawned with these settings !!!!") print_error(" Please check possible settings for this type of UAV by calling command:") print_error(" spawn --%s --available-sensors" % vehicle_type) print_error("===================================================================================") sys.exit(1) model_xml = parse_xacro(model_xml, **kwargs) if debug: print(model_xml) req = SpawnModelRequest() unique_name = 'uav' + str(mav_sys_id) req.model_name = unique_name req.model_xml = model_xml req.robot_namespace = unique_name req.initial_pose.position.x = x req.initial_pose.position.y = y req.initial_pose.position.z = z req.initial_pose.orientation.x = 0.0 req.initial_pose.orientation.y = 0.0 req.initial_pose.orientation.z = math.sin(yaw / 2.0) req.initial_pose.orientation.w = math.cos(yaw / 2.0) req.reference_frame = '' try: srv = ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) resp = srv(req) except ServiceException, e: print_error("===================================================================================") print_error(" Service call failed: %s" % e) print_error(" Please run gazebo_ros (for example: \"roslaunch mrs_simulation simulation.launch\")") print_error("===================================================================================") sys.exit(1)
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)