def update(self):
xy_dist = G.euclid_distance(self.pos[:2], self._current_line[1][:2])
z_dist = np.abs(self.pos[2] - self._current_line[1][2])
if xy_dist <= config.LINE_END_XY_THRESHOLD and z_dist <= config.LINE_END_Z_THRESHOLD:
config.pprint('LINE CHANGED')
# reached the target, request to follow the next line
self._wps = self._wps[1:]
try:
self._current_line = [self._wps[0], self._wps[1]]
except IndexError:
config.pprint('Out of waypoints!')
sys.exit()
# create a Path message with whatever frame we received the localisation in
path = Path()
path.header.frame_id = self._frame_id
# Path.poses is a PoseStamped list
# so we have to create these objects
ps1 = PoseStamped()
ps1.header.frame_id = self._frame_id
ps1.pose.position.x = self._current_line[0][0]
ps1.pose.position.y = self._current_line[0][1]
ps1.pose.position.z = self._current_line[0][2]
path.poses.append(ps1)
ps2 = PoseStamped()
ps2.header.frame_id = self._frame_id
ps2.pose.position.x = self._current_line[1][0]
ps2.pose.position.y = self._current_line[1][1]
ps2.pose.position.z = self._current_line[1][2]
path.poses.append(ps2)
self.publisher.publish(path)
def _observe(self):
pos = self.auv.get_position()
dist = G.euclid_distance(pos, self.target_point)
heading = self.auv.get_heading()
angle = G.directed_angle([1, 0], pos)
prox = self.auv.get_proximity()
angvel = self.auv.get_angular_velocity()
return dist, heading, angle, angvel, prox
def update_curve(self, data):
# data should contain Path, with multiple, that represent a discretized curve
# we will only use the x,y component for now
points = []
for p in data.poses:
points.append(p.pose.position)
if len(points) < 1:
print('No curve received')
return
line = None
# the segment intersects a circle of radius r if
# the first point is closer than r and the second is further
# we also want the 'last' one that intersects, not the first
# that particular segment is 'forward'.
# p1 inside, p2 outside should not happen for more than 1 point
selfpos = self.pos[:2]
for i in range(len(points) - 1, 0, -1):
p1 = (points[i - 1].x, points[i - 1].y, points[i - 1].z)
p2 = (points[i].x, points[i].y, points[i].z)
p1d = G.euclid_distance(selfpos, p1[:2])
p2d = G.euclid_distance(selfpos, p2[:2])
#print(p1d, p2d)
if p1d > config.LOOK_AHEAD_R:
print('p1d:', p1d, 'p2d:', p2d)
# the first point is inside, check the second one
if p2d < config.LOOK_AHEAD_R:
# we are intersecting!
line = (p1, p2)
if line is None:
print('No segment in range, using first segment')
p1 = (points[0].x, points[0].y, points[0].z)
p2 = (points[1].x, points[1].y, points[1].z)
line = (p1, p2)
p1, p2 = line
p1 = list(p1)
#p1[0] += 3
#p1[1] += 3
line = [p1, p2]
# set these to be used later
self._current_line = line
# self._frame_id = data.header.frame_id
# elongate the line for visualization purposes
x1, y1, z1 = line[0]
x2, y2, z2 = line[1]
slope = (y2 - y1) / (x2 - x1)
d = -5
x2 += d
y2 += d * slope
x1 -= d
y1 -= d * slope
# publish the current line
pose1 = Pose()
pose1.position.x = x1
pose1.position.y = y1
pose1.position.z = z1
stamped1 = PoseStamped()
stamped1.pose = pose1
pose2 = Pose()
pose2.position.x = x2
pose2.position.y = y2
pose2.position.z = z2
stamped2 = PoseStamped()
stamped2.pose = pose2
path = Path()
path.poses = [stamped1, stamped2]
path.header.frame_id = self._frame_id
self.debug_line_pub.publish(path)