-
Notifications
You must be signed in to change notification settings - Fork 0
/
mover.py
51 lines (48 loc) · 1.58 KB
/
mover.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
49
50
51
import time
import rospy
import rosservice
from gazebo_msgs.srv import SetModelState, SetModelStateRequest
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
def rosMover():
fp=open('piano_states.txt','r');
print "Finished opening state file..."
rospy.wait_for_service('/gazebo/set_model_state')
print "Finished waiting for setter service..."
set_model=rospy.ServiceProxy('/gazebo/set_model_state',SetModelState)
print "Moving piano..."
for line in fp:
line.strip()
states=line.split("|")
for s in states:
elements=s.split(",")
config=[float(e) for e in elements]
tx,ty,tz,rx,ry,rz,rw=config
req=SetModelStateRequest()
req.model_state.model_name='piano2'
req.model_state.pose.position.x=tx
req.model_state.pose.position.y=ty
req.model_state.pose.position.z=tz
req.model_state.pose.orientation.x=rx
req.model_state.pose.orientation.y=ry
req.model_state.pose.orientation.z=rz
req.model_state.pose.orientation.w=rw
set_model(req)
time.sleep(2)
print "Finsihed moving piano"
return
def rosViewer():
service_list=rosservice.get_service_list()
print service_list
rospy.wait_for_service('/gazebo/get_model_state');
print "finished waiting for service..."
get_model_srv=rospy.ServiceProxy('/gazebo/get_model_state',GetModelState)
model=GetModelStateRequest()
model.model_name='piano2'
while not rospy.is_shutdown():
result=get_model_srv(model)
print '[',result.pose.position.x,',',result.pose.position.y,',',result.pose.position.z,']'
time.sleep(1)
if __name__=="__main__":
print "Starting Python..."
rosMover()
print "Exiting Python"