-
Notifications
You must be signed in to change notification settings - Fork 4
/
respawnGoal.py
115 lines (99 loc) · 4.52 KB
/
respawnGoal.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
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#!/usr/bin/env python
# Authors: Gilbert #
import rospy
import random
import time
import os
from gazebo_msgs.srv import SpawnModel, DeleteModel
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import Pose
class Respawn():
def __init__(self):
self.modelPath = os.path.dirname(os.path.realpath(__file__))
self.modelPath = self.modelPath.replace('/home/ffd/DRL/SAC','/home/ffd/DRL/SAC/model.sdf')
self.f = open(self.modelPath, 'r')
self.model = self.f.read()
self.stage = 4
#rospy.get_param('/stage_number')
self.goal_position = Pose()
self.init_goal_x = 0.5
self.init_goal_y = 0.5
self.goal_position.position.x = self.init_goal_x
self.goal_position.position.y = self.init_goal_y
self.modelName = 'goal'
self.obstacle_1 = 0.6, 0.6
self.obstacle_2 = 0.6, -0.6
self.obstacle_3 = -0.6, 0.6
self.obstacle_4 = -0.6, -0.6
self.last_goal_x = self.init_goal_x
self.last_goal_y = self.init_goal_y
self.last_index = 0
self.sub_model = rospy.Subscriber('gazebo/model_states', ModelStates, self.checkModel)
self.check_model = False
self.index = 0
def checkModel(self, model):
self.check_model = False
for i in range(len(model.name)):
if model.name[i] == "goal":
self.check_model = True
def respawnModel(self):
while True:
if not self.check_model:
rospy.wait_for_service('gazebo/spawn_sdf_model')
spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
spawn_model_prox(self.modelName, self.model, 'robotos_name_space', self.goal_position, "world")
rospy.loginfo("Goal position : %.1f, %.1f", self.goal_position.position.x,
self.goal_position.position.y)
break
else:
pass
def deleteModel(self):
while True:
if self.check_model:
rospy.wait_for_service('gazebo/delete_model')
del_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
del_model_prox(self.modelName)
break
else:
pass
def getPosition(self, position_check=False, delete=False):
if delete:
self.deleteModel()
if self.stage != 4:
while position_check:
goal_x = random.randrange(-12, 13) / 10.0
goal_y = random.randrange(-12, 13) / 10.0
if abs(goal_x - self.obstacle_1[0]) <= 0.4 and abs(goal_y - self.obstacle_1[1]) <= 0.4:
position_check = True
elif abs(goal_x - self.obstacle_2[0]) <= 0.4 and abs(goal_y - self.obstacle_2[1]) <= 0.4:
position_check = True
elif abs(goal_x - self.obstacle_3[0]) <= 0.4 and abs(goal_y - self.obstacle_3[1]) <= 0.4:
position_check = True
elif abs(goal_x - self.obstacle_4[0]) <= 0.4 and abs(goal_y - self.obstacle_4[1]) <= 0.4:
position_check = True
elif abs(goal_x - 0.0) <= 0.4 and abs(goal_y - 0.0) <= 0.4:
position_check = True
else:
position_check = False
if abs(goal_x - self.last_goal_x) < 1 and abs(goal_y - self.last_goal_y) < 1:
position_check = True
self.goal_position.position.x = goal_x
self.goal_position.position.y = goal_y
else:
while position_check:
goal_x_list = [0.6, 1.9, 0.5, 0.2, -0.8, -1, -1.9, 0.5, 2, 0.5, 0, -0.1, -2]
goal_y_list = [0, -0.5, -1.9, 1.5, -0.9, 1, 1.1, -1.5, 1.5, 1.8, -1, 1.6, -0.8]
self.index = random.randrange(0, 13)
print(self.index, self.last_index)
if self.last_index == self.index:
position_check = True
else:
self.last_index = self.index
position_check = False
self.goal_position.position.x = goal_x_list[self.index]
self.goal_position.position.y = goal_y_list[self.index]
time.sleep(0.5)
self.respawnModel()
self.last_goal_x = self.goal_position.position.x
self.last_goal_y = self.goal_position.position.y
return self.goal_position.position.x, self.goal_position.position.y