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 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 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 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_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 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 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)
</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))
</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))
def main():
rospy.init_node("test2_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(-3.8, -1.2)
spawn_req.model_name = "AutoModel_Obstacle1"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_static/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)
angle = numpy.random.uniform(-math.pi / 2.0 - 0.2, -math.pi / 2.0 + 0.2)
pos_y = numpy.random.uniform(-1, 1)
spawn_req.model_name = "AutoModel_Obstacle2"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_static/model.sdf', 'r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = -5.4
spawn_req.initial_pose.position.y = pos_y
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)
angle = numpy.random.uniform(0, 3.0 * math.pi / 2.0)
pos_x = -2.2 + 1.55 * math.cos(angle)
pos_y = 0.5 + 1.55 * math.sin(angle)
spawn_req.model_name = "AutoModel_Obstacle3"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_static/model.sdf', 'r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = pos_x
spawn_req.initial_pose.position.y = pos_y
spawn_req.initial_pose.position.z = 0.17
spawn_req.initial_pose.orientation.z = math.sin((angle + math.pi / 2) / 2)
spawn_req.initial_pose.orientation.w = math.cos((angle + math.pi / 2) / 2)
spawn_model(spawn_req)
angle = numpy.random.uniform(-math.pi / 2.0 - 0.2, -math.pi / 2.0 + 0.2)
pos_y = numpy.random.uniform(-1.25, -1.5)
spawn_req.model_name = "AutoModel_Obstacle4"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_static/model.sdf', 'r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = 0.65
spawn_req.initial_pose.position.y = pos_y
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)
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 main():
rospy.init_node("test2_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)
get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
spawn_req = SpawnModelRequest()
get_link_req = GetLinkStateRequest()
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)
spawn_req.model_name = "AutoModel_Obstacle1"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_obstacle/model.sdf', 'r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = pos_x - 2
spawn_req.initial_pose.position.y = 3.3
spawn_req.initial_pose.position.z = 0.17
spawn_req.initial_pose.orientation.z = 1.0
spawn_req.initial_pose.orientation.w = 0.0
spawn_model(spawn_req)
angle = numpy.random.uniform(0, 3.0 * math.pi / 2.0)
pos_x = -2.2 + 1.55 * math.cos(angle)
pos_y = 0.5 + 1.55 * math.sin(angle)
spawn_req.model_name = "AutoModel_Obstacle2"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_static/model.sdf', 'r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = pos_x
spawn_req.initial_pose.position.y = pos_y
spawn_req.initial_pose.position.z = 0.17
spawn_req.initial_pose.orientation.z = math.sin((angle + math.pi / 2) / 2)
spawn_req.initial_pose.orientation.w = math.cos((angle + math.pi / 2) / 2)
spawn_model(spawn_req)
angle = numpy.random.uniform(-math.pi / 2.0 - 0.2, -math.pi / 2.0 + 0.2)
pos_y = numpy.random.uniform(-1.25, -1.5)
spawn_req.model_name = "AutoModel_Obstacle3"
spawn_req.model_xml = open(
pkg_path + '/models/AutoNOMOS_mini_static/model.sdf', 'r').read()
spawn_req.robot_namespace = ''
spawn_req.initial_pose.position.x = 0.65
spawn_req.initial_pose.position.y = pos_y
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)
get_link_req.link_name = 'AutoModel_Obstacle1::base_link'
initial_speed = -numpy.random.randint(80, 120)
pub_speed = rospy.Publisher("/AutoModel_Obstacle1/manual_control/speed",
Int16,
queue_size=1)
pub_steer = rospy.Publisher("/AutoModel_Obstacle1/manual_control/steering",
Int16,
queue_size=1)
loop = rospy.Rate(10)
while not rospy.is_shutdown():
link_state = get_link_state(get_link_req)
obstacle_x = link_state.link_state.pose.position.x
obstacle_y = link_state.link_state.pose.position.y
steering = 135 if obstacle_x < -4.18 else 90 - int(100 *
(3.3 - obstacle_y))
speed = initial_speed if obstacle_y > 1.8 else 0
pub_speed.publish(speed)
pub_steer.publish(steering)
loop.sleep()
y = 10 * random.random()- 5
rayon = 0.06*random.random() + 0.01
X.append(x)
Y.append(y)
Rayon.append(rayon)
Name.append("Grass"+str(i))
#Creation aleatoire d'herbe.{value for value in variable}
strtochange = "<radius>"+str(rayon)+"</radius>"
filesdf = filesdf.replace(strdebase,strtochange)
strdebase = strtochange
request = SpawnModelRequest()
request.model_name = "Grass"+str(i)
request.model_xml = filesdf
request.robot_namespace = "Herbe"+str(i)
request.initial_pose.position.x = x
request.initial_pose.position.y = y
request.initial_pose.position.z = 0.05
request.initial_pose.orientation.x = 0
request.initial_pose.orientation.y = 0
request.initial_pose.orientation.z = 0
request.initial_pose.orientation.w = 1.0
response = gazeboSpawnModel(request)
# Herbe = "Grass"+str(i)+" "+str(x) +" "+str(y)+" "+str(rayon)+" "
#HerbesStr+=Herbe
#rossrv info SpawnModel
#geometry_msgs/Point
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_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)
