-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathspawn.py
executable file
·48 lines (38 loc) · 1.64 KB
/
spawn.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#!/usr/bin/env python
import rospy, tf, random
import sys
import os
from gazebo_msgs.srv import DeleteModel, SpawnModel # for Gazebo
from geometry_msgs.msg import Pose # for object orientation
#initialising node in which the service resides
rospy.init_node('spawn_brick',log_level=rospy.INFO)
quaternion = tf.transformations.quaternion_from_euler(0,0,1.570796)
#defining brick orientation, which will be translated into quarternion
# defining pose of object to be spawned
initial_pose = Pose()
initial_pose.position.x = 0.5
initial_pose.position.y = 0.5
initial_pose.position.z = 0.2
initial_pose.orientation.x = quaternion[0]
initial_pose.orientation.y = quaternion[1]
initial_pose.orientation.z = quaternion[2]
initial_pose.orientation.w = quaternion[3]
#Hopefully should work on everyones computer if not directly change this
file = os.path.expanduser('~/.gazebo/models/Brick/model-1_4.sdf')
# file = os.path.expanduser('~/.gazebo/models/new_brick/model.sdf')
#file = "ENTER ABS PATH TO THE model-1_4 file on your computer"
f = open(file, "r")
sdff = f.read()
rospy.wait_for_service('gazebo/spawn_sdf_model')
spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
# function that defines the service gen_brick
def gen_brick_handler(req):
i = random.randint(1,5000)
spawn_model_prox("brick_"+str(i), sdff, "brick_"+str(i), initial_pose, "world")
resp = TriggerResponse()
return resp
#CODE FOR MAKING this node into a service
from std_srvs.srv import Trigger, TriggerResponse
gen_brick_s = rospy.Service('gen_brick', Trigger, gen_brick_handler)
19 # spin() simply keeps python from exiting until this node is stopped
rospy.spin()