class FileConnector(Node):
_node_config_name = "FILE_CONNECTOR"
_logger = logging.getLogger("FileConnector")
def __init__(self):
super().__init__(self._node_config_name)
def _start_up(self):
self._steering_command_generator_receiver = Receiver(
"STEERING_COMMAND_GENERATOR")
self._file = open('steeringcommands.txt', 'w')
self._file.write('Steering angle(RAD);Velocity(m/s)\n')
self._file.flush()
def _progress(self):
steering_command_generator_result: SteeringCommandGeneratorResult = self._steering_command_generator_receiver.receive(
)
if not self._file.closed:
angle = steering_command_generator_result.steering_angel
speed = steering_command_generator_result.velocity_meters_per_second
self._file.write(f"{angle};{speed}\n")
self._file.flush()
self._logger.debug(f"wrote {angle};{speed} to file")
def _shut_down(self):
self._steering_command_generator_receiver.close()
self._file.close()
class UartConnector(Node):
_node_config_name = "UART_CONNECTOR"
_logger = logging.getLogger("UartConnector")
_BAUDRATE = 57600
_TX_INTERVAL = 1 # in seconds
def __init__(self):
super().__init__(self._node_config_name)
def _start_up(self):
self._steering_command_generator_receiver = Receiver(
"STEERING_COMMAND_GENERATOR")
self.ser = serial.Serial(
port='/dev/ttyTHS1',
baudrate=self._BAUDRATE,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
)
def _progress(self):
self._steering_command_generator_result = self._steering_command_generator_receiver.receive(
)
while self.ser.is_open:
self.ser.write(self._steering_command_generator_result.encode())
self._logger.debug('Sent ' +
self._steering_command_generator_result +
' to serial connection')
time.sleep(self._TX_INTERVAL)
def _shut_down(self):
self._steering_command_generator_receiver.close()
self.ser.close()
class SteeringCommandGenerator(Node):
_node_config_name = "STEERING_COMMAND_GENERATOR"
_logger = logging.getLogger("SteeringCommandGenerator")
_imu_data_reader = ReadIMU()
def __init__(self):
super().__init__(self._node_config_name)
self._nomad = Nomad()
# https://github.com/pytransitions/transitions#automatic-transitions-for-all-states
self._state_machine = Machine(
model=self._nomad,
states=StatesNomad().states,
transitions=TransitionsNomad().transitions,
initial="Start",
auto_transitions=False,
model_attribute='_state',
queued=True)
# Node method implementations
def _start_up(self):
self._object_detector_receiver = Receiver("OBJECT_DETECTOR")
self._uart_output_sender = Sender(self._node_config_name)
self._nomad.set_sender(self._uart_output_sender)
def _progress(self):
self._nomad.data = self._object_detector_receiver.receive()
self._nomad.imu_data = self._imu_data_reader.get_Data()
# test data
# self._nomad.data = self._create_fake_data()
current_state_name = self._nomad.state
trigger_for_next_state = self._state_machine.get_triggers(
current_state_name)
internal_trigger_for_current_state = [
trigger for trigger in trigger_for_next_state
if "internal_" in trigger
]
self._nomad.trigger(internal_trigger_for_current_state[0])
def _shut_down(self):
self._object_detector_receiver.close()
self._uart_output_sender.close()
class ObjectDetectorRecorder(Node):
_node_config_section = "RECORDER"
def __init__(self):
super().__init__(self._node_config_section)
self._sensor_input_camera = ReadCamera()
def _start_up(self):
self.fake_data_reader: FakeDataReader = FakeDataReader("jpg")
self._object_detector_receiver = Receiver("OBJECT_DETECTOR")
self._fake_sonar = ReadSonar()
def _progress(self):
object_detector_result = self._object_detector_receiver.receive()
print("SAVED NEW")
filenamePath = "/home/manuel/PREN/workspaces/informatik/object_detection/darknet/fakedata"
filename = filenamePath + '/' + self.fake_data_reader.getNextTimestamp(
) + ".objectresult"
pickle_out = open(filename, "wb")
pickle.dump(object_detector_result, pickle_out)
pickle_out.close()
def _shut_down(self):
self._object_detector_receiver.close()
class ObjectDetector(Node):
_node_config_section = "OBJECT_DETECTOR"
def __init__(self):
super().__init__(self._node_config_section)
self._camera_center_range = ObjectDetector.load_camera_center_range()
# Node method implementations
def _start_up(self):
self._pylon_detector_receiver = Receiver("PYLON_DETECTOR")
self._obstacle_detector_receiver = Receiver("OBSTACLE_DETECTOR")
self._object_detector_sender = Sender(self._node_config_section)
self._fake_sonar = ReadSonar()
def _progress(self):
pylon_detector_result = self._pylon_detector_receiver.receive()
obstacle_detector_result = self._obstacle_detector_receiver.receive()
self._set_measured_distance(obstacle_detector_result, pylon_detector_result)
self._object_detector_sender.send(ObjectDetectorResult(pylon_detector_result.pylons +
obstacle_detector_result.obstacles))
def _set_measured_distance(self, obstacle_detector_result: ObstacleDetectorResult,
pylon_detector_result: PylonDetectorResult):
sonar_data: SonarData = self._fake_sonar.get_Data()
if not sonar_data.contact_top and sonar_data.contact_bottom: # square timber
centred_obstacle = obstacle_detector_result.get_nearest_obstacle_which_intersects(self._camera_center_range)
if centred_obstacle is not None:
centred_obstacle.distance = Distance(sonar_data.distance_bottom, True)
else:
obstacle_detector_result.obstacles.append(self.generateAdditionalObstacle(sonar_data.distance_bottom))
elif sonar_data.contact_top and sonar_data.contact_bottom: # pylon or pylon and square timber
if sonar_data.distance_top + 5 >= sonar_data.distance_bottom >= sonar_data.distance_top - 15: # pylon
centred_pylon = pylon_detector_result.get_nearest_pylon_which_intersects(self._camera_center_range)
if centred_pylon is not None:
centred_pylon.distance = Distance(sonar_data.distance_top, True)
else: # pylon behind square_timber
centred_pylon = pylon_detector_result.get_nearest_pylon_which_intersects(self._camera_center_range)
if centred_pylon is not None:
centred_pylon.distance = Distance(sonar_data.distance_top, True)
centred_obstacle = obstacle_detector_result.get_nearest_obstacle_which_intersects(
self._camera_center_range)
if centred_obstacle is not None:
centred_obstacle.distance = Distance(sonar_data.distance_bottom, True)
else:
obstacle_detector_result.obstacles.append(self.generateAdditionalObstacle(sonar_data.distance_bottom))
def generateAdditionalObstacle(self, distance_buttom: float) -> DetectedObject:
min_y = int(832 - 1.22 * distance_buttom)
max_y = int(832 - 1.22 * distance_buttom+ 40)
bounding_box: BoundingBox = BoundingBox.of_rectangle(int(0), min_y, int(832), max_y)
distance: Distance = Distance(distance_buttom, True)
return DetectedObject(DetectedObjectType.SquareTimber, bounding_box, distance, 100, [])
def _shut_down(self):
self._pylon_detector_receiver.close()
self._obstacle_detector_receiver.close()
self._object_detector_sender.close()
@staticmethod
def load_camera_center_range():
config = configparser.ConfigParser()
config.read('config.ini')
min_x = config.getint('CAMERA_CENTER_RANGE', 'MIN_X')
min_y = config.getint('CAMERA_CENTER_RANGE', 'MIN_Y')
max_x = config.getint('CAMERA_CENTER_RANGE', 'MAX_X')
max_y = config.getint('CAMERA_CENTER_RANGE', 'MAX_Y')
return BoundingBox.of_rectangle(min_x, min_y, max_x, max_y)
class NomadVisualizer(Node):
_node_config_section = "VISUALIZER"
_LOWER_RED_MIN = np.array([0, 100, 100], np.uint8)
_LOWER_RED_MAX = np.array([10, 255, 255], np.uint8)
_UPPER_RED_MIN = np.array([160, 100, 100], np.uint8)
_UPPER_RED_MAX = np.array([179, 255, 255], np.uint8)
def __init__(self):
super().__init__(self._node_config_section)
self._sensor_input_camera = ReadCamera()
def _start_up(self):
self.fake_objectresult_reader: FakeDataReader = FakeDataReader("objectresult")
self._steering_detector_receiver = Receiver("STEERING_COMMAND_GENERATOR")
self._fake_sonar = ReadSonar()
self._fake_imu = ReadIMU()
def _progress(self):
frame_read = self._sensor_input_camera.get_frame()
pickle_in = open(self.fake_objectresult_reader.getNextFilePath(), "rb")
object_detector_result: ObjectDetectorResult = pickle.load(pickle_in)
steering_command_generator_result: SteeringCommandGeneratorResult = self._steering_detector_receiver.receive()
sonar_data: SonarData = self._fake_sonar.get_Data()
imu_data: IMUData = self._fake_imu.get_Data()
draw_detected_objects(frame_read, object_detector_result.get_detected_objects)
# draw camera center range on image
camera_center_range = ObjectDetector.load_camera_center_range()
min_point = (int(camera_center_range.min_x), int(camera_center_range.min_y))
max_point = (int(camera_center_range.max_x), int(camera_center_range.max_y))
# cv2.rectangle(frame_read, min_point, max_point, (255, 0, 0), 1)
# draw ultrasonic information
cv2.putText(frame_read, "Ultrasonic Top: " + str(sonar_data.distance_top),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 20),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Ultrasonic Bottom: " + str(sonar_data.distance_bottom),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 5),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
# draw imu information
cv2.putText(frame_read, "Accel x: " + str(imu_data.acc_x),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 120),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Accel y: " + str(imu_data.acc_y),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 105),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Accel z: " + str(imu_data.acc_z),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 90),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Gyro x: " + str(imu_data.gyro_x),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 75),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Gyro y: " + str(imu_data.gyro_y),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 60),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Gyro z: " + str(imu_data.gyro_z),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 45),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "state: " + str(steering_command_generator_result.state),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 210),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "velocity m/s: " + str(steering_command_generator_result.velocity_meters_per_second),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 195),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "steering angel: " + str(steering_command_generator_result.steering_angel),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 180),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "driving direction: " + str(steering_command_generator_result.driving_direction),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 165),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "curve radius centimeters: " + str(steering_command_generator_result.curve_radius_centimeters),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 150),
cv2.FONT_HERSHEY_SIMPLEX,
0.5, [255, 0, 0], 2)
cv2.imshow('NOMAD_Visualizer', frame_read)
cv2.waitKey(3)
def _shut_down(self):
self._steering_detector_receiver.close()
class ObjectDetectorVisualizer(Node):
_node_config_section = "VISUALIZER"
_LOWER_RED_MIN = np.array([0, 100, 100], np.uint8)
_LOWER_RED_MAX = np.array([10, 255, 255], np.uint8)
_UPPER_RED_MIN = np.array([160, 100, 100], np.uint8)
_UPPER_RED_MAX = np.array([179, 255, 255], np.uint8)
def __init__(self):
super().__init__(self._node_config_section)
self._sensor_input_camera = ReadCamera()
def _start_up(self):
self._object_detector_receiver = Receiver("OBJECT_DETECTOR")
self._fake_sonar = ReadSonar()
self._fake_imu = ReadIMU()
def _progress(self):
frame_read = self._sensor_input_camera.get_frame()
object_detector_result = self._object_detector_receiver.receive()
sonar_data: SonarData = self._fake_sonar.get_Data()
imu_data: IMUData = self._fake_imu.get_Data()
draw_detected_objects(frame_read,
object_detector_result.get_detected_objects)
# draw camera center range on image
camera_center_range = ObjectDetector.load_camera_center_range()
min_point = (int(camera_center_range.min_x),
int(camera_center_range.min_y))
max_point = (int(camera_center_range.max_x),
int(camera_center_range.max_y))
# cv2.rectangle(frame_read, min_point, max_point, (255, 0, 0), 1)
# draw ultrasonic information
cv2.putText(frame_read,
"Ultrasonic Top: " + str(sonar_data.distance_top),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.putText(frame_read,
"Ultrasonic Bottom: " + str(sonar_data.distance_bottom),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
# draw imu information
cv2.putText(frame_read, "Accel x: " + str(imu_data.acc_x),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 120),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Accel y: " + str(imu_data.acc_y),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 105),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Accel z: " + str(imu_data.acc_z),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 90),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Gyro x: " + str(imu_data.gyro_x),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 75),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Gyro y: " + str(imu_data.gyro_y),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.putText(frame_read, "Gyro z: " + str(imu_data.gyro_z),
(int(camera_center_range.max_x) - 200,
int(camera_center_range.max_y) - 45),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, [255, 0, 0], 2)
cv2.imshow('Pylon_Detector_Visualizer', frame_read)
cv2.waitKey(3)
def _shut_down(self):
self._object_detector_receiver.close()