pyCBD/examples/AGV/env.py

"""
Pure code-based environment building blocks for AGV simulation.
"""
from CBD.CBD import BaseBlock

class PathBlock(BaseBlock):
	def __init__(self, block_name, path_file, path_width=0.027, grace=0.001, black=55, white=5):
		BaseBlock.__init__(self, block_name, ["x", "y"], ["color", "offset"])

		self.path_width = path_width
		self.grace = grace
		self.black = black
		self.white = white

		self.path = []
		with open(path_file, 'r') as file:
			for line in file:
				try:
					pos = [float(l.strip()) for l in line.split(',')]
				except Exception as e:
					continue
				self.path.append(pos[:2])

	def compute(self, curIteration):
		position = self.getInputSignal(curIteration, "x").value, self.getInputSignal(curIteration, "y").value
		color = self.white
		offset = float('inf')

		for i, p1 in enumerate(self.path):
			p2 = self.path[i-1]

			dist = self.distance_to_line(p2, p1, position)
			if dist < offset:
				offset = dist

			if dist < (self.path_width / 2.0):
				color = self.black
			elif color == self.white and dist <= (self.path_width / 2.0 + self.grace):
				# Gradually go from black to white over a distance of grace
				d = dist - self.path_width / 2.0
				color = self.black + (d / self.grace * (self.white - self.black))

		self.appendToSignal(color, "color")
		self.appendToSignal(offset, "offset")

	def distance_to_line(self, p1, p2, p3):
		"""
		Computes the distance from a point p3 to the line segment given by (p1, p2).
		Implementation based upon:
			https://stackoverflow.com/a/6853926
		"""
		px = p2[0] - p1[0]
		py = p2[1] - p1[1]

		norm = px * px + py * py

		if norm == 0.0:
			ud = 0.0
		else:
			ud = float((p3[0] - p1[0]) * px + (p3[1] - p1[1]) * py) / norm
			ud = min(max(ud, 0.0), 1.0)  # limits line segment to endpoints

		xd = p1[0] + ud * px
		yd = p1[1] + ud * py

		dx = xd - p3[0]
		dy = yd - p3[1]

		return (dx * dx + dy * dy) ** 0.5