def _step(self, action):
u = action
self.counter += 1
derivate_func = self.get_derivate_function()
solver = ode(derivate_func)
solver.set_f_params(u)
t0 = 0
solver.set_initial_value(self.state, t0)
solver.integrate(self.tau)
state = solver.y
self.state = state
x, theta1, theta2 = state[0], state[1], state[2]
cost = normalize_angle(theta1) + normalize_angle(theta2)
reward = -cost
done = bool(self.counter > self.max_iter
or np.abs(x) > self.x_threshold)
return self.state, reward, done, {}
def _step_oc(self, new_state):
self.counter += 1
self.state = new_state
x, theta1, theta2 = new_state[0], new_state[1], new_state[2]
cost = normalize_angle(theta1) + normalize_angle(theta2)
reward = -cost
done = bool(self.counter > self.max_iter
or np.abs(x) > self.x_threshold)
return self.state, reward, done, {}
def predict(self, v_transl, steering_angle, iteration_step=None):
"""
Executes the predict step of the Extended Kalman Filter.
x_k = f(x_k-1, u_k)
P_k = G_k*P_k-1*G_k-1' + Q
:param v_transl: translational velocity of the vehicle [m/s]
:param steering_angle: steering angle of the vehicle [rad/s]
:param iteration_step: discrete time step of the simulation (optional)
"""
u = np.array([
v_transl * self.dt * np.cos(self.estimated_state[2]),
v_transl * self.dt * np.sin(self.estimated_state[2]),
v_transl * self.dt * np.tan(steering_angle) / self.wheelbase
])
G = self._compute_jacobian(v_transl)
self.estimated_state = self.estimated_state + u
self.estimated_state[2] = normalize_angle(self.estimated_state[2])
self.estimation_covariance = G @ self.estimation_covariance @ G.transpose(
) + self.Q
# Custom filter modification: take drift into account
if self.drift_time_seconds is not None and iteration_step is not None:
if self._is_time_to_model_drift(iteration_step):
self.Q = self._get_Q_drift()
else:
self.Q = self.Q_initial
def _append_measurement(self, measurement):
self._timestamps.append(
float(measurement[DEAD_RECKONING_TIMESTAMP_COL]))
self.positions_x.append(float(measurement[DEAD_RECKONING_X_COL]))
self.positions_y.append(float(measurement[DEAD_RECKONING_Y_COL]))
angle = normalize_angle(float(measurement[DEAD_RECKONING_THETA_COL]))
self.positions_theta.append(angle)
def track_room_marker(drone_info):
yaw = 0
if not drone_info.search_marker:
if not drone_info.room_marker_tracked == -1:
yaw = - K_ANG_Z*drone_info.marker_displacement
#search the closest tag wrt the current position
else:
distances = []
#append left
d_left = math.sqrt((RoomTagCoor.LeftX - drone_info.x)**2 + (RoomTagCoor.LeftY - drone_info.y)**2)
distances.append(d_left)
d_front = math.sqrt((RoomTagCoor.FrontX - drone_info.x)**2 + (RoomTagCoor.FrontY - drone_info.y)**2)
distances.append(d_front)
#...others#
min_dist = min(distances)
min_index = distances.index(min_dist)
#this is verbose but preserves the usage of arbitrary tag ids
if min_index == 0:
new_ref_tag = TagIds.RoomLeftTag
y = RoomTagCoor.LeftY
x = RoomTagCoor.LeftX
elif min_index == 1:
new_ref_tag = TagIds.RoomFrontTag
y = RoomTagCoor.FrontY
x = RoomTagCoor.FrontX
#...others...#
if drone_info.room_marker_tracked == TagIds.RoomLeftTag:
cur_index = 0
elif drone_info.room_marker_tracked == TagIds.RoomFrontTag:
cur_index = 1
#...others...#
#if it is the ref tag, it's ok the current displacement'
if drone_info.room_marker_tracked == new_ref_tag: #the last condition should not be necessary
yaw = - K_ANG_Z*drone_info.marker_displacement
#otherwise, rotate to track the closest room marker
else:
target_yaw = utils.rad_to_deg(math.atan((y - drone_info.y)/(x - drone_info.x)))
if ((y > drone_info.y and x < drone_info.x) or (y < drone_info.y and x < drone_info.x)):
target_yaw += 180
target_yaw = utils.normalize_angle(target_yaw)
rospy.loginfo("target yaw: %f" %target_yaw)
yaw, dr = rotate(target_yaw, drone_info.alpha, K_ANG_Z_EXPL)
return yaw
def avoid_drones(command, drone_info, drone_2_info, navdata):
roll, pitch, yaw, gaz = [command.linear.y, command.linear.x, command.angular.z, command.linear.z]
#the b & f flight gives the precedence
if not drone_info.task == DroneTask.FlyH:
drones = [drone_2_info]
forbid_pitch_front = False
forbid_pitch_back = False
forbid_roll_left = False
forbid_roll_right = False
for drone in drones:
distance = ((drone.x - drone_info.x)**2 + (drone.y - drone_info.y)**2)
if distance < TOL_DRONE:
angle_between_drones = utils.rad_to_deg(math.atan((drone.y - drone_info.y)/(drone.x - drone_info.x)))
if ((drone.y > drone_info.y and drone.x < drone_info.x) or
(drone.y < drone_info.y and drone.x < drone_info.x)):
angle_between_drones += 180
while angle_between_drones > 180: angle_between_drones -= 360
while angle_between_drones < -180: angle_between_drones += 360
control_angle = drone_info.alpha - angle_between_drones
control_angle = utils.normalize_angle(control_angle)
if control_angle > -45 and control_angle < 45: forbid_pitch_front = True
elif control_angle > 45 and control_angle < 135: forbid_roll_right = True
elif control_angle > -135 and control_angle < -45: forbid_roll_left = True
else: forbid_pitch_back = True
if forbid_pitch_front:
if navdata.vx > 0.2 and pitch > 0: pitch = -1 #if speed is too high, hovering is not sufficient to stop the drone!
else: pitch = 0
elif forbid_pitch_back:
if navdata.vx < -0.2 and pitch < 0: pitch = 1
else: pitch = 0
if forbid_roll_left:
if navdata.vy > 0.2 and roll > 0: roll = -1
else: pitch = 0
elif forbid_roll_right:
if navdata.vy < -0.2 and roll < 0: roll = 1
else: pitch = 0
rospy.loginfo("forbid pitch front: %d" %forbid_pitch_front)
rospy.loginfo("forbid pitch back: %d" %forbid_pitch_back)
rospy.loginfo("forbid pitch left: %d" %forbid_roll_left)
rospy.loginfo("forbid pitch right: %d" %forbid_roll_right)
return [roll, pitch, yaw, gaz]
def update_state(self):
self.d_time()
omegaRight = self.vr / self.radius # rad за сек
omegaLeft = self.vl / self.radius #
# фактическая линейная скорость центра робота
V = (self.radius / 2) * (omegaRight + omegaLeft
) #/1000#;//mm/s * 1000 -> M/S
# фактическая угловая скорость поворота робота
omega = (self.radius / self.length) * (omegaRight - omegaLeft)
self.yaw += self.delta_time * omega
self.yaw = normalize_angle(self.yaw)
self.x += self.delta_time * V * math.cos(self.yaw)
self.y += self.delta_time * V * math.sin(self.yaw)
def update(self, u_d, psi_d, r_d):
self.x[2] = normalize_angle(self.x[2], psi_d)
Rz = np.array([[ np.cos(self.x[2]),-np.sin(self.x[2]), 0],
[ np.sin(self.x[2]), np.cos(self.x[2]), 0],
[ 0 , 0 , 1]])
self.x[0:3] += self.h * np.dot(Rz, self.x[3:6])
self.x[3:6] += self.h * np.dot(np.diag([1/self.m, 1/self.m, 1/self.Iz]),
self.Tau(u_d, psi_d, r_d) - self.Cvv() - self.Dvv())
while self.x[2] >= np.pi:
self.x[2] -= 2*np.pi
while self.x[2] < -np.pi:
self.x[2] += 2*np.pi
return self.x
def update_state(self):
self.current_time = time.time()
self.dt = self.current_time - self.prev_time
self.prev_time = self.current_time
self.omegaRight = self.vr / self.WHEELS_RAD
self.omegaLeft = self.vl / self.WHEELS_RAD
# фактическая линейная скорость центра робота
self.linear_velocity = (self.WHEELS_RAD / 2) * (self.omegaRight +
self.omegaLeft)
#//m/s
# фактическая угловая скорость поворота робота
self.angular_velocity = (self.WHEELS_RAD / self.WHEELS_DIST) * (
self.omegaRight - self.omegaLeft)
self.yaw += (self.angular_velocity * self.dt
) #; # // направление в рад
self.yaw = normalize_angle(self.yaw)
self.x += self.linear_velocity * math.cos(
self.yaw) * self.dt # // в метрах
self.y += self.linear_velocity * math.sin(self.yaw) * self.dt #
def update(self, u_d, psi_d, r_d):
self.x[2] = normalize_angle(self.x[2], psi_d)
Rz = np.array([[ np.cos(self.x[2]),-np.sin(self.x[2]), 0],
[ np.sin(self.x[2]), np.cos(self.x[2]), 0],
[ 0 , 0 , 1]])
self.x[0:3] += self.h * np.dot(Rz, self.x[3:6])
self.x[3:6] += self.h * np.dot(np.diag([1/self.m, 1/self.m, 1/self.Iz]),
self.Tau(u_d, psi_d, r_d) - self.Cvv() - self.Dvv())
while self.x[2] >= np.pi:
self.x[2] -= 2*np.pi
while self.x[2] < -np.pi:
self.x[2] += 2*np.pi
return self.x
def task_controller(self, event):
if not (self.low.navdata.status == DroneStatus.Landed or self.low.navdata.status == DroneStatus.Emergency):
if self.low.drone_info.task == DroneTask.DemoGesture:
if self.low.drone_info.change_demo:
print "change demo was true\n."
self.low.drone_info.change_demo = False
print "Sending rot exploration\n."
self.low.send_rot_exploration()
self.low.drone_info.last_yaw = self.low.drone_info.alpha
'''if self.low.drone_info.gesture == Gestures.Left:
self.low.go_at_point(self.low.drone_info.x + math.cos(utils.deg_to_rad(self.low.drone_info.alpha - 120)),
self.low.drone_info.y + math.sin(utils.deg_to_rad(self.low.drone_info.alpha - 120)),
self.low.drone_info.z, self.low.drone_info.alpha - 120)
elif self.low.drone_info.gesture == Gestures.Right:
self.low.go_at_point(self.low.drone_info.x + math.cos(utils.deg_to_rad(self.low.drone_info.alpha + 120)),
self.low.drone_info.y + math.sin(utils.deg_to_rad(self.low.drone_info.alpha + 120)),
self.low.drone_info.z, self.low.drone_info.alpha + 120)'''
elif ((self.low.drone_info.command == DroneCommands.Rotate or self.low.drone_info.command == DroneCommands.GoAtPoint) and
self.low.drone_info.tag_to_follow is not None):
print "Sending follow tag"
print self.low.drone_info.tag_to_follow
self.low.send_follow_tag()
elif (self.low.drone_info.command == DroneCommands.Rotate and self.low.drone_info.last_yaw is not None and
abs(utils.normalize_angle(self.low.drone_info.alpha - self.low.drone_info.last_yaw)) > 135):
print "Exceeded rotation allowed. Going back."
self.pubNobodyFound.publish(Empty())
self.low.drone_info.last_yaw = None
utils.led_animation(5)
self.low.drone_info.tag_to_follow = self.low.drone_info.last_tag_to_follow
self.low.drone_info.last_tag_to_follow = None
self.low.pubSwitchMarker.publish(Int16(self.low.drone_info.tag_to_follow))
if self.low.drone_info.search_yaw > 0:
self.low.drone_info.last_seen = -1
else: self.low.drone_info.last_seen = 1
self.low.send_follow_tag()
def show(self):
rover = self._rover
tx, ty = rover.pos
yaw = rover.yaw
yaw = normalize_angle(np.deg2rad(yaw))
map_nav = rover.worldmap[:, :, 2]
map_obs = rover.worldmap[:, :, 0]
#cimg = (np.logical_and(
# np.greater(map_nav, 20),
# np.greater(map_obs, 2)) * 255).astype(np.uint8)
cimg = (np.greater(map_obs, OBS_THRESH) * 255).astype(np.uint8)
cimg = cv2.cvtColor(cimg, cv2.COLOR_GRAY2BGR)
if self._frontier is not None:
cimg[..., 1] = self._frontier # show in green
if rover.goal is not None:
cv2.circle(cimg, tuple(np.int_(rover.pos)), 2, [0.0, 255, 0.0])
cv2.circle(cimg, tuple(np.int_(rover.goal)), 2, [0.0, 0.0, 255])
if rover.path is not None:
for (p0, p1) in zip(rover.path[:-1], rover.path[1:]):
x0, y0 = p0
x1, y1 = p1
cv2.line(cimg, (x0, y0), (x1, y1), (255, 0, 0), 1)
#if rover.p0 is not None:
# for (p0, p1) in zip(rover.p0[:-1], rover.p0[1:]):
# x0,y0 = p0
# x1,y1 = p1
# #cv2.line( (y0,x0), (y1,x1), (128)
# cv2.line(cimg, (x0,y0), (x1,y1), (255,255,0), 1)
# cimg will show mission status; position, goal, boundary, path.
cv2.imshow('cimg', np.flipud(cimg))
cv2.imwrite('cimg.png', np.flipud(cimg[::-1]))
cv2.waitKey(10)
def follow_belt(self):
roll, pitch, yaw, gaz = [0 for i in range(4)]
x_med, y_med, dist_med, pitch_med, count = get_average_tag_info(self.tags_buffer_belt, TagIds.BeltFakeTag)
if count is not 0:
if not self.controller_pos_x.setpoint == 2.0:
self.controller_pos_x.setpoint = 2.0
if not self.controller_pos_z.setpoint == 0.1:
self.controller_pos_z.setpoint = 0.1
if not self.drone_info.belt_located:
self.drone_info.belt_located = True
displacement_x = utils.rad_to_deg(math.atan2(x_med, dist_med))
yaw = -K_ANG_Z*displacement_x
other_drones = filter(lambda drone: drone.command == DroneCommands.FollowBelt and drone.belt_in_sight and
drone.status is DroneStatus.Flying, [self.drone_2_info, self.drone_3_info, self.drone_4_info])
roll = get_roll_follow_belt(self.drone_info, other_drones,
self.controller_pos_y, self.controller_speed_y, self.navdata)
#keep the distance
pitch = - self.controller_pos_x.update(dist_med, -self.navdata.vx)
if pitch < 0: pitch = pitch*2
gaz = self.controller_pos_z.update(y_med, self.navdata.vz)
#for later in case
if displacement_x < 0:
#for knowing from where to start rotating
self.drone_info.last_yaw = POWER_YAW
else:
self.drone_info.last_yaw = -POWER_YAW
self.drone_info.last_z = self.drone_info.z
self.drone_info.alpha_start_expl = self.drone_info.alpha
else:
if self.drone_info.z < ALT:
#should not happen unless it's the beginning
self.setup_pid(None, None, ALT + 0.1, None, 0.0)
roll = self.controller_speed_y.update(self.navdata.vy,0.0)
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
elif self.drone_info.belt_located:
self.setup_pid(None, None, self.drone_info.last_z, 0.0, 0.0)
roll = self.controller_speed_y.update(self.navdata.vy,0.0)
pitch = self.controller_speed_x.update(self.navdata.vx, 0.0)
diff = self.drone_info.alpha - self.drone_info.alpha_start_expl
diff = utils.normalize_angle(diff)
if diff < -20:
yaw = POWER_YAW
self.drone_info.last_yaw = yaw
elif diff > 20:
yaw = -POWER_YAW
self.drone_info.last_yaw = yaw
else: yaw = self.drone_info.last_yaw
gaz = self.controller_pos_z.update(self.drone_info.z, self.navdata.vz)
utils.led_animation(5)
return utils.adjust_command(roll, pitch, yaw, gaz)
def orbit(self, box_tag):
roll, pitch, yaw, gaz = [0 for i in range(4)]
x_med, y_med, dist_med, pitch_med, count = get_average_tag_info(self.tags_buffer_box, box_tag)
if count is not 0:
#if values were changed after lost of visual tracking
if not self.controller_pos_x.setpoint == 1.6:
self.controller_pos_x.setpoint = 1.6
if not self.controller_pos_z.setpoint == 0.1:
self.controller_pos_z.setpoint = 0.1
#the alpha angle computed with side markers is surely related to the correct bundle!
displacement_x = utils.rad_to_deg(math.atan2(x_med, dist_med))
if not self.drone_info.box_located:
self.drone_info.box_located = True
#update box position (if only odometry is used, it will move!)
self.drone_info.x_box = self.drone_info.x + math.cos(utils.deg_to_rad(self.drone_info.alpha - displacement_x))*dist_med
self.drone_info.y_box = self.drone_info.y + math.sin(utils.deg_to_rad(self.drone_info.alpha - displacement_x))*dist_med
self.pubBoxPosition.publish(Point(self.drone_info.x_box, self.drone_info.y_box, 0.0))
yaw = -K_ANG_Z*displacement_x
if self.drone_info.command is DroneCommands.OrbitSync:
distances = []
if (self.drone_2_info.box_in_sight and self.drone_2_info.command is DroneCommands.OrbitSync and
self.drone_2_info.status is DroneStatus.Flying):
dist = self.drone_2_info.alpha_box - self.drone_info.alpha_box
dist = utils.normalize_angle(dist)
distances.append(dist)
if (self.drone_3_info.box_in_sight and self.drone_3_info.command is DroneCommands.OrbitSync and
self.drone_3_info.status is DroneStatus.Flying):
dist = self.drone_3_info.alpha_box - self.drone_info.alpha_box
dist = utils.normalize_angle(dist)
distances.append(dist)
#...fill with other distances#
if not distances == []:
distances_pos = filter(lambda x: x >= 0, distances)
distances_neg = filter(lambda x: x < 0, distances)
if not distances_pos == []:
dd = min(distances_pos)
else: dd = 360 + min(distances_neg)
if not distances_neg == []:
ds = -max(distances_neg)
else: ds = 360 - max(distances_pos)
self.controller_speed_y.setpoint = -(VEL_TAN + (dd - ds)/float(1000))
else: self.controller_speed_y.setpoint = -VEL_TAN
rospy.loginfo("setpoint 1: %f" %self.controller_speed_y.setpoint)
#keep the distance
pitch = - self.controller_pos_x.update(dist_med, -self.navdata.vx)/float(2) #receive the dist_med
roll = self.controller_speed_y.update(self.navdata.vy, 0.0)
gaz = self.controller_pos_z.update(y_med, self.navdata.vz)
self.drone_info.last_z = self.drone_info.z
else:
#if visual contact is lost, try to restore it using odometry informations
if self.drone_info.box_located:
self.target_x = self.drone_info.x_box
self.target_y = self.drone_info.y_box
self.target_z = self.drone_info.last_z
self.target_yaw = utils.rad_to_deg(math.atan((self.target_y - self.drone_info.y)/(self.target_x - self.drone_info.x)))
if ((self.target_y > self.drone_info.y and self.target_x < self.drone_info.x) or
(self.target_y < self.drone_info.y and self.target_x < self.drone_info.x)):
self.target_yaw += 180
self.target_yaw = utils.normalize_angle(self.target_yaw)
self.setup_pid(0.0, None, None, None, 0.0)
roll, pitch, yaw, gaz = self.position_control(1.5)
pitch = pitch / float(2)
else:
#should not happen unless at the beginning
self.setup_pid(None, None, ALT, None, 0.0)
roll, pitch, yaw, gaz = self.rot_exploration()
utils.led_animation(5)
return utils.adjust_command(roll, pitch, yaw, gaz)
def __call__(self, rover):
""" Assume RGB Image Input """
if self._first:
# initialize.
# TODO : fix more properly
self._first = False
rover.goal = None
rover.p0 = None
rover.path = None
rover.rock = None
# Unpack Data
img = rover.img
tx, ty = rover.pos
yaw, pitch, roll = [
np.deg2rad(e) for e in rover.yaw, rover.pitch, rover.roll
]
yaw, pitch, roll = [normalize_angle(e)
for e in [yaw, pitch, roll]] #to +-np.pi
map, map_gt = rover.worldmap, rover.ground_truth
# decide whether or not data is good in general
update_map = abs(pitch) < np.deg2rad(self._th_deg) and \
abs(roll) < np.deg2rad(self._th_deg)
h, w = img.shape[:2]
mh, mw = map.shape[:2]
#cv2.imshow('mapped', np.flipud(mapped))
# Warp
warped = cv2.warpPerspective(img, self._M,
(w, h)) # keep same size as input image
if self._hsv:
warped = cv2.cvtColor(warped, cv2.COLOR_RGB2HSV)
# Threshold
nav, (wx, wy), (r, a) = self.convert(warped,
self._th_nav,
yaw,
tx,
ty,
mw,
mh,
polar=True)
if update_map:
map[wy, wx, 2] = np.clip(map[wy, wx, 2] + 10, 0, 255)
map[wy, wx, 0] = np.clip(map[wy, wx, 0] - 10, 0, 255)
obs, (wx, wy) = self.convert(warped,
self._th_obs,
yaw,
tx,
ty,
mw,
mh,
polar=False)
if update_map:
map[wy, wx, 0] = np.clip(map[wy, wx, 0] + 5, 0, 255)
map[wy, wx, 2] = np.clip(map[wy, wx, 2] - 5, 0, 255)
roc, (wx, wy) = self.convert(warped,
self._th_roc,
yaw,
tx,
ty,
mw,
mh,
polar=False)
if update_map:
map[wy, wx, 1] = np.clip(map[wy, wx, 1] + 1, 0, 255)
if len(wx) > 0:
rover.rock = (np.mean(wx), np.mean(wy))
else:
rover.rock = None
stat = np.zeros_like(warped)
stat[nav, 2] = 255
stat[:, 1] = 0
stat[obs, 0] = 255
for rad in range(10):
cv2.circle(stat, (w / 2, h), np.int32(rad * self._scale),
(255, 255, 255), 1)
# mark filtered region
fil = np.zeros((h, w), dtype=np.uint8)
center = int(w / 2), int(h)
axlen = self._th_rng * self._scale
axes = int(axlen), int(axlen)
ang = np.rad2deg(self._th_ang)
cv2.ellipse(
fil,
center,
axes,
0,
-90 - ang,
-90 + ang, # start+finish
255,
-1)
sel = np.logical_not(fil)
print np.shape(sel)
stat[sel, :] = np.uint8(stat[sel, :] * 0.5)
# = stat*0.5 + (255 - fil[...,np.newaxis])*0.5
thresh = np.zeros_like(img)
thresh[:, :, 2] = 255 * self._threshold(
cv2.cvtColor(img, cv2.COLOR_RGB2HSV), self._th_nav)
thresh[:, :, 0] = 255 * self._threshold(
cv2.cvtColor(img, cv2.COLOR_RGB2HSV), self._th_obs)
# Create Visualizations
overlay = cv2.addWeighted(map, 1, map_gt, 0.5, 0)
maxh = max(h, mh)
maxw = max(w, mw)
viz = np.zeros((maxh * 2, maxw * 2, 3))
viz[0:h, 0:w] = img
#viz[0:h, w:w+w] = warped
viz[0:h, w:w + w] = stat
viz[h:h + mh, 0:mw] = np.flipud(overlay)
#viz[h:h+mh, mw:mw+mw] = np.flipud(map)
viz[h:h + h, mw:mw + w] = thresh
return viz, (r, a)
def _estimate_state(particles, particles_count, conf_xy, conf_theta):
""" Optimised function, see estimate_state for more info """
# limits for considering participating to the state estimation
theta_lim = np.radians(5)
xy_lim = 1.5
# particles = self.particles
max_index = particles_count - 1
iterations_count = round(particles_count/100) # 500
tests_count = round(particles_count/100) # 500
# assert iterations_count <= max_index and tests_count <= max_index
# no replacement
tmp = []
for i in range(max_index):
tmp.append(i)
lin = np.array(tmp)
# np.array(np.linspace(0, max_index, max_index + 1), dtype=np.int)
iteration_indices = np.random.choice(lin, iterations_count, replace=False)
test_indices = np.random.choice(lin, tests_count, replace=False)
best_index = -1
support, best_support = 0, 0
# tries a certain number of times
for i in range(iterations_count):
index = iteration_indices[i]
x = particles[index, 0]
y = particles[index, 1]
theta = particles[index, 2]
support = 0
for j in range(tests_count):
o_index = test_indices[j]
o_x = particles[o_index, 0]
o_y = particles[o_index, 1]
o_theta = particles[o_index, 2]
# compute distance
dist_xy = np.sqrt((x - o_x) * (x - o_x) + (y - o_y) * (y - o_y))
dist_theta = ut.normalize_angle(theta - o_theta)
if dist_xy < xy_lim and dist_theta < theta_lim:
support += 1
# if it beats best, replace best
if support > best_support:
best_index = index
best_support = support
# then do the averaging for best index
x = particles[best_index, 0]
y = particles[best_index, 1]
theta = particles[best_index, 2]
count, conf_count, xs, ys, ths = 0, 0, 0, 0, 0
sins, coss = [], []
for j in range(tests_count):
o_index = test_indices[j]
o_x = particles[o_index, 0]
o_y = particles[o_index, 1]
o_theta = particles[o_index, 2]
dist_xy = np.sqrt((x - o_x) * (x - o_x) + (y - o_y) * (y - o_y))
dist_theta = ut.normalize_angle(theta - o_theta)
if dist_xy < xy_lim and dist_theta < theta_lim:
sins.append(np.sin(o_theta))
coss.append(np.cos(o_theta))
# ths += ut.normalize_angle(theta)
xs += o_x
ys += o_y
count += 1
if dist_xy < conf_xy and dist_theta < conf_theta:
conf_count += 1
# assert count > 0, count
x_m = xs / count
y_m = ys / count
theta_m = np.arctan2(np.sum(np.asarray(sins)), np.sum(np.asarray(coss))) # ths / count
return np.array([x_m, y_m, theta_m]), float(conf_count) / float(tests_count)
def get_roll_follow_belt(drone_info, other_drones, controller_speed_y, controller_pos_y, navdata):
distances = []
dist_front = []
dist_front_nat = []
drone_2_info = None
drone_3_info = None
drone_4_info = None
for drone in other_drones:
if drone_2_info is None: drone_2_info = drone
elif drone_3_info is None: drone_3_info = drone
elif drone_4_info is None: drone_4_info = drone
if drone_2_info is not None:
dist = drone_2_info.alpha_belt - drone_info.alpha_belt
dist = utils.normalize_angle(dist)
distances.append(dist)
dist_front.append(abs(drone_2_info.alpha_belt))
dist_front_nat.append(drone_2_info.alpha_belt)
if drone_3_info is not None:
dist = drone_3_info.alpha_belt - drone_info.alpha_belt
dist = utils.normalize_angle(dist)
distances.append(dist)
dist_front.append(abs(drone_3_info.alpha_belt))
dist_front_nat.append(drone_3_info.alpha_belt)
if drone_4_info is not None:
dist = drone_4_info.alpha_belt - drone_info.alpha_belt
dist = utils.normalize_angle(dist)
distances.append(dist)
dist_front.append(abs(drone_4_info.alpha_belt))
dist_front_nat.append(drone_4_info.alpha_belt)
if distances == [] or abs(drone_info.alpha_belt) < min(dist_front): # I am alone or the "master"
drone_info.role = 0
if abs(drone_info.alpha_belt) < 35:
roll = controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(drone_info.alpha_belt)), -navdata.vy)
else:
if drone_info.alpha_belt > 0: controller_speed_y.setpoint = Y_SP
else: controller_speed_y.setpoint = -Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
else:
drone_info.role = 1
if len(distances) == 1:
if drone_info.alpha_belt < -145 or drone_info.alpha_belt > 145:
if drone_info.alpha_belt > 0:
angle_ref = drone_info.alpha_belt - 180
else: angle_ref = drone_info.alpha_belt + 180
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt > 0: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
elif len(distances) > 1:
front = dist_front.index(min(dist_front)) + 2 #this is the drone I don't have to consider
dist_front_nat.pop(front - 2)
go_90 = False
go_minus_90 = False
#choose pal angle
if len(distances) == 2:
if front == 2:
pal_angle = drone_3_info.alpha_belt
else: #should be 3
pal_angle = drone_2_info.alpha_belt
if drone_info.alpha_belt > 0 and pal_angle > 0:
if drone_info.alpha_belt > pal_angle: #I have to go to the other side
go_minus_90 = True
else:
go_90 = True
elif drone_info.alpha_belt < 0 and pal_angle < 0:
if drone_info.alpha_belt < pal_angle:
go_90 = True
else:
go_minus_90 = True
else:
if drone_info.alpha_belt > 0:
go_90 = True
else: go_minus_90 = True
elif len(distances) == 3:
all_same_side = False
go_behind = False
dist_pos = filter(lambda x: x > 0, dist_front_nat)
dist_neg = filter(lambda x: x < 0, dist_front_nat)
if ((dist_neg == [] and drone_info.alpha_belt > 0) or
(dist_pos == [] and drone_info.alpha_belt < 0)):
all_same_side = True
if ((all_same_side and drone_info.alpha_belt > 0 and drone_info.alpha_belt > min(dist_pos)
and drone_info.alpha_belt < max(dist_pos)) or
(all_same_side and drone_info.alpha_belt < 0 and
drone_info.alpha_belt < max(dist_neg) and drone_info.alpha_belt > min(dist_neg))):
go_behind = True
else: #not all on the same side or on the same side but I don't have to go behind
if all_same_side:
if drone_info.alpha_belt > 0:
if drone_info.alpha_belt < min(dist_pos):
go_90 = True
elif drone_info.alpha_belt > max(dist_pos):
go_minus_90 = True
else:
if drone_info.alpha_belt > max(dist_neg):
go_minus_90 = True
elif drone_info.alpha_belt < min(dist_neg):
go_90 = True
else:
if drone_info.alpha_belt > 0:
if not dist_pos == []:
pal_pos = dist_pos[0]
if drone_info.alpha_belt < pal_pos:
go_90 = True
else: go_behind = True
else: go_90 = True
else:
if not dist_neg == []:
pal_neg = dist_neg[0]
if drone_info.alpha_belt > pal_neg:
go_minus_90 = True
else: go_behind = True
else: go_minus_90 = True
if go_behind:
if drone_info.alpha_belt < -145 or drone_info.alpha_belt > 145:
if drone_info.alpha_belt > 0:
angle_ref = drone_info.alpha_belt - 180
else: angle_ref = drone_info.alpha_belt + 180
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt > 0: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
if go_90:
if drone_info.alpha_belt > 0:
if drone_info.alpha_belt > 55 or drone_info.alpha_belt < 125:
angle_ref = drone_info.alpha_belt - 90
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt < 90: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
else:
controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
elif go_minus_90:
if drone_info.alpha_belt < 0:
if drone_info.alpha_belt < -55 or drone_info.alpha_belt > -125:
angle_ref = drone_info.alpha_belt + 90
roll = -controller_pos_y.update(dist_med*math.tan(utils.deg_to_rad(angle_ref)), -navdata.vy)
else:
if drone_info.alpha_belt < -90: controller_speed_y.setpoint = -Y_SP
else: controller_speed_y.setpoint = +Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
else:
controller_speed_y.setpoint = -Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)
'''else:
distances_pos = filter(lambda x: x >= 0, distances)
distances_neg = filter(lambda x: x < 0, distances)
if not distances_pos == []:
dd = min(distances_pos)
else: dd = 360 + min(distances_neg)
if not distances_neg == []:
ds = -max(distances_neg)
else: ds = 360 - max(distances_pos)
controller_speed_y.setpoint = -(dd - ds)/float(150)
if controller_speed_y.setpoint > Y_SP: controller_speed_y.setpoint = Y_SP
elif controller_speed_y.setpoint < -Y_SP: controller_speed_y.setpoint = -Y_SP
roll = controller_speed_y.update(navdata.vy, 0.0)'''
return roll
def plan(self):
# stop and think ...
#self.stop()
res = self.swerve()
# prioritize getting unstuck ...
if res[0] == 'unstuck':
return 'unstuck', {'prv': 'plan', 'pargs': {}}
#return res
#otherwise keep on planning ...
# unpack data
rover = self._rover
tx, ty = rover.pos
yaw = rover.yaw
yaw = normalize_angle(np.deg2rad(yaw))
map_obs = rover.worldmap[:, :, 0]
map_roc = rover.worldmap[:, :, 1]
map_nav = rover.worldmap[:, :, 2]
# first check rocks ...
#goal = self
# define mapped region ...
ker = cv2.getStructuringElement(cv2.MORPH_DILATE, (3, 3))
mapped = np.logical_or(
np.greater(map_nav, 20),
np.greater(map_obs, OBS_THRESH),
)
mapped = 255 * mapped.astype(np.uint8)
mapped = cv2.erode(mapped,
cv2.getStructuringElement(cv2.MORPH_ERODE, (3, 3)),
iterations=1)
cnt = cv2.findContours(mapped.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[1]
# define frontiers (where nav doesn't meet obs)
mapped.fill(0)
goal = None
if len(cnt) > 0:
map_nav = cv2.dilate(map_nav, ker, iterations=1)
cv2.drawContours(mapped, cnt, -1, 255)
frontier = np.logical_and(map_nav, mapped)
frontier = 255 * frontier.astype(np.uint8)
self._frontier = frontier
#cv2.imshow('frontier', np.flipud(frontier))
fy, fx = frontier.nonzero() #(2,N)
# basic filter : no obstacles!
good_goal = (map_obs[fy, fx] <= 0)
fy = fy[good_goal]
fx = fx[good_goal]
if np.size(fy) > 0:
# order frontiers, good ones at the beginning
fx, fy = score_frontier(tx, ty, yaw, fx, fy)
for goal in zip(fx, fy):
# try goals sequentially
res = self.moveto(goal)
if res[0] != 'abort':
# good plan, go forth!
return res
# keep planning
#return 'swerve', {}
return 'plan', {}
def moveto_local(self, path=None, phase='turn', rtol=2.0):
"""
Follow local waypoints.
Currently, moveto_local() doesn't handle scenarios
such as impossible plans, getting stuck, or hitting obstacles.
"""
rover = self._rover
# TODO :
# if (realized_cannot_get_to_point) then
# ask_for_new_global_plan()
# logic for re-routing local path
# to avoid obstacles - like rocks, for instance.
if len(path) == 0:
# completed goal! start planning.
rover.goal = None
return 'plan', {}
#return 'swerve', {}
target = path[0]
tx, ty = target
dx, dy = np.subtract(target, rover.pos)
da = np.arctan2(dy, dx) - np.deg2rad(rover.yaw)
da = normalize_angle(da)
dr = np.sqrt(dx**2 + dy**2)
if dr <= rtol:
# accept current position + move on
return 'moveto_local', {
'path': path[1:],
'phase': 'turn',
'rtol': rtol
}
if np.abs(da) >= np.deg2rad(90):
# correct for large angular error
return 'moveto_local', {
'path': path,
'phase': 'turn',
'rtol': rtol
}
if phase == 'turn':
dadr = np.abs(da) / dr #15
if np.abs(da) <= np.deg2rad(10): #~+-10 deg.
# accept current angle + go forwards
return 'moveto_local', {
'path': path,
'phase': 'forward',
'rtol': rtol
}
steer = np.clip(np.rad2deg(da), -15.0, 15.0)
self.turn(steer)
elif phase == 'forward':
# turn less responsively
steer = np.clip(np.rad2deg(da), -15.0, 15.0)
if len(rover.nav_angles) > rover.stop_forward:
if rover.rock is None:
# then precise control isn't as important
steer_o = np.clip(np.mean(rover.nav_angles * 180 / np.pi),
-15, 15)
steer = 0.5 * steer + 0.5 * steer_o
self.move(target_vel=np.clip(0.5 * dr, 0, rover.max_vel),
steer=steer)
else:
self.move(target_vel=np.clip(0.25 * dr, 0, rover.max_vel),
steer=steer)
#if phase == 'forward' and self.check_stuck():
# # probably quicker to ask for a new plan.
# return 'moveto_local', {'path' : [], 'phase' : 'abort', 'rtol':rtol}
if self._info['nomove_cnt'] > 120:
if np.abs(da) <= np.deg2rad(10):
# aligned with goal, probably bad plan
# abort-like
return 'plan', {}
else:
self._info['try_unstuck_cnt'] += 1
if self._info['try_unstuck_cnt'] > 1: # try 1 time
self._info['try_unstuck_cnt-'] = 0
# really stuck. ask for a new plan!
return 'moveto_local', {
'path': [],
'phase': 'abort',
'rtol': rtol
}
else:
# try to get self unstuck.
return 'unstuck', {
'dir': -np.sign(da),
'prv': 'moveto_local',
'pargs': {
'path': path,
'phase': phase,
'rtol': rtol
}
}
return 'moveto_local', {'path': path, 'phase': phase, 'rtol': rtol}