class AvoidanceController():
def __init__(self, my_id, bodies):
self.gain = 2. #x,y direction
self.gain_z = 0. #z direction
self.tg = TrajectoryGenerator()
self.my_id = my_id
self.bodies = bodies
self.states = []
for i in range(0, len(self.bodies)):
self.states.append(QuadPositionDerived())
rospy.Subscriber("/body_data/id_" + str(self.bodies[i]),
QuadPositionDerived, self.__set_states)
def get_potential_output(self):
distances = self.__get_distances()
directions = self.__get_directions()
u = [0., 0., 0.]
for i in range(0, len(distances)):
if (distances[i] < 0.1):
for j in range(0, 2):
u[j] += self.gain / 0.1 * directions[i][j]
u[2] += self.gain_z / 0.1 * directions[i][2]
else:
for j in range(0, 2):
u[j] += self.gain / distances[i] * directions[i][j]
u[2] += self.gain_z / distances[i] * directions[i][2]
return u
def get_blending_constant(self):
alpha_max = 0.8 # <= 1.
alpha_min = 0. # >=0.
r_min = 1.2
r_max = 2.
k = (alpha_min - alpha_max) / (r_max - r_min)
m = alpha_max - k * r_min
distances = self.__get_distances()
if self.gain == 0:
return 0
elif min(distances) <= r_min:
return alpha_max
elif min(distances) > r_max:
return alpha_min
else:
return (k * min(distances) + m)
def __set_states(self, data):
topic_name = data._connection_header["topic"]
for i in range(0, len(self.bodies)):
name = "/body_data/id_" + str(self.bodies[i])
if topic_name == name:
self.states[i] = data
def __get_my_state(self):
for i in range(0, len(self.bodies)):
if self.bodies[i] == self.my_id:
return self.states[i]
def __get_distances(self):
distances = []
for i in range(0, len(self.states)):
if self.bodies[i] != self.my_id:
distance = self.tg.get_distance(
self.__get_pos_from_state(self.__get_my_state()),
self.__get_pos_from_state(self.states[i]))
distances.append(distance)
return distances
def __get_directions(self):
directions = []
for i in range(0, len(self.states)):
if self.bodies[i] != self.my_id:
direction = self.tg.get_direction2(
self.__get_pos_from_state(self.__get_my_state()),
self.__get_pos_from_state(self.states[i]))
directions.append(direction)
return directions
def __get_pos_from_state(self, state):
return [state.x, state.y, state.z]
