def find_segments_of(path):
segments = []
- last_segment = PathSegment(0, path[0])
- for i in range(len(path)):
- if (path[i][4] != last_segment.type()
- or sgn(path[i][3]) != last_segment.direction()):
- last_segment.set_goal(i, path[i])
- segments.append(last_segment)
- last_segment = PathSegment(i, path[i])
- last_segment.set_goal(len(path) - 1, path[-1])
- segments.append(last_segment)
- # Fix speed when the segment direction is 0.
- for i in range(1, len(segments) - 1):
- ls = segments[i-1]
- cs = segments[i]
- ns = segments[i+1]
- if cs.direction() == 0 and ls.direction() == ns.direction():
- cs.init_pose[3] = ls.init_pose[3]
- elif cs.direction() == 0:
- cs.init_pose[3] = ns.init_pose[3]
+ segment_start_index = 0
+ for i in range(1, len(path)):
+ if sgn(path[i][3]) != sgn(path[segment_start_index + 1][3]):
+ segments.append(PathSegment(segment_start_index, i, path))
return segments
:param goal_index: Goal index
:param goal_pose: Goal pose
"""
- def __init__(self, ii, ip):
+ def __init__(self, ii, ip, path):
"""Create new PathSegment object.
:param ii: Initial index
self.init_pose = list(ip)
self.goal_index = None
self.goal_pose = None
+ self.path = []
+ self.len = self.precalc_path(path)
def __repr__(self):
- st = self.type()
- if st == 1:
- st = "left"
- elif st == -1:
- st = "right"
- else:
- st = "straight"
di = self.direction()
if di > 0:
di = "forward"
else:
di = "stopped"
sl = self.len()
- return f"[{self.init_index}, {self.goal_index}] {st} {di} {sl}"
-
- def set_goal(self, gi, gp):
- self.goal_index = gi
- self.goal_pose = gp
-
- def type(self):
- """Return segment type.
-
- Segment type can be:
-
- - 1 ~ turning left
- - -1 ~ turning right
- - 0 ~ straight
- """
- return self.init_pose[4]
+ return f"[{self.init_index}, {self.goal_index}] {di} {sl}"
def direction(self):
"""Return segment direction.
- -1 ~ backward
- 0 ~ stopped
"""
- return sgn(self.init_pose[3])
-
- def len(self):
- if self.type() == 0:
- return ed(self.goal_pose, self.init_pose)
- else:
- return arc_len(self.init_pose, self.goal_pose)
-
- def pose_at(self, distance):
+ return sgn(self.path[self.init_index+1])
+
+ def preccalc_path(self, path):
+ l = 0.0
+ for i in range(self.init_index, self.goal_index):
+ o = object()
+ o.x = path[i][0]
+ o.y = path[i][1]
+ o.h = path[i][2] # heading
+ o.v = path[i][3] # speed
+ o.t = path[i][4] # type
+ o.dist = l
+ self.path.append(o)
+ if i < self.goal_index:
+ l += ed(self.path[i], self.path[i+1])
+ return l
+
+ def turn_type_at(self, distance):
"""Return pose at specific distance.
Well, this does not work well.
"""
- if self.type() == 0:
- h = atan2(
- self.goal_pose[1] - self.init_pose[1],
- self.goal_pose[0] - self.init_pose[0])
- return [
- self.init_pose[0] + distance * cos(h),
- self.init_pose[1] + distance * sin(h),
- h, 0, None, None]
- else:
- x = self.init_pose[0]
- y = self.init_pose[1]
- h = self.init_pose[2]
- h += (
- self.direction() * distance / BicycleCar().wb
- * tan(deg_to_rad(36) * self.type()))
- x += self.direction() * distance * cos(h)
- y += self.direction() * distance * sin(h)
- return [x, y, h, h, None, None]
+ i = 0
+ while distance >= self.path[i].dist && distance < self.path[i + 1].dist:
+ i += 1
+ # todo interpolate and return avg. x, y
+ return self.path[i+1].t
class BicycleCar:
# Comment the following three lines to have the single
# BicycleCar model over all the segments without reseting the
# model at the beginning of each segment.
- old_sp = c.sp
- c = BicycleCar(*s.init_pose[:3])
- c.sp = old_sp
+# old_sp = c.sp
+# c = BicycleCar(*s.init_pose[:3])
+# c.sp = old_sp
# ---
c.acc = set_max(s.direction(), c.max_acc)
c.dwa = set_max(s.type(), c.max_wa)
d = 0
- while d < s.len():
- if c.braking and sgn(c.sp) == sgn(c.acc):
- break
- if (s_is_cusp
- and (max(1, s.len()/2) > abs(s.len() - d))
- and not c.braking):
- c.brake_within(abs(s.len() - d))
- op = c.pose()
+ while d < s.len:
+ if breaking_distance(c.sp) > s.len - d:
+ add = -max
+ else:
+ c.sp = max (c.sp + DT + max_acc, mac_speed)
+ c.wa = s.turn_type_at(d)
+ c.next()
+ d += c.sp * DT
+ c.sp = 0
+ c.acc = 0
+ for _ in range(count_to(3)):
c.next()
traj.append(c.pose())
- if s.type() == 0:
- d += c.ed(op)
- else:
- d += arc_len(op, c.pose())
- if c.braking and s_is_cusp:
- c.sp = 0
- c.acc = 0
- for _ in range(count_to(3)):
- c.next()
- traj.append(c.pose())
- c.acc = set_max(path_segments[i+1].direction(), c.max_acc)
- c.dwa = set_max(path_segments[i+1].type(), c.max_wa)
- for _ in range(count_to(c.ttwa(c.dwa))):
- c.next()
- traj.append(c.pose())
- c.braking = False
+ c.acc = set_max(path_segments[i+1].direction(), c.max_acc)
+ c.dwa = set_max(path_segments[i+1].type(), c.max_wa)
+ for _ in range(count_to(c.ttwa(c.dwa))):
+ c.next()
+ traj.append(c.pose())
+ c.braking = False
# ---
# Perform in slot maneuver.
assert sgn(c.sp) == sgn(c.acc)
projects / hubacji1 / path-to-traj.git / commitdiff