class ObstacleDetector(Node):
_node_config_section = "OBSTACLE_DETECTOR"
def __init__(self):
super().__init__(self._node_config_section)
# Node method implementations
def _start_up(self):
self._obstacle_distance_estimator = ObstacleDistanceEstimator()
self._obstacle_detector_sender = Sender(self._node_config_section)
self._sensor_input_camera = ReadCamera()
self._darknet_wrapper = DarknetWrapper()
def _progress(self):
frame_read = self._sensor_input_camera.get_frame()
detected_obstacles: List[
DetectedObject] = self._darknet_wrapper.detect_obstacles(
frame_read)
for detected_obstacle in detected_obstacles:
detected_obstacle.distance = self._obstacle_distance_estimator.estimate(
detected_obstacle.bounding_box.height)
obstacle_detector_result = ObstacleDetectorResult(detected_obstacles)
self._obstacle_detector_sender.send(obstacle_detector_result)
def _shut_down(self):
self._obstacle_detector_sender.close()
class PylonDetector(Node):
_node_config_section = "PYLON_DETECTOR"
def __init__(self):
super().__init__(self._node_config_section)
self._pylon_distance_estimator = PylonDistanceEstimator()
self._sensor_input_camera = ReadCamera()
self._darknet_wrapper = DarknetWrapper()
# Node method implementations
def _start_up(self):
self._pylon_detector_sender = Sender(self._node_config_section)
def _progress(self):
frame_read = self._sensor_input_camera.get_frame()
detected_pylons: List[
DetectedObject] = self._darknet_wrapper.detect_pylons(frame_read)
for detected_pylon in detected_pylons:
detected_pylon.bounding_box = PylonBoundingBoxAdjuster(
frame_read).adjust(detected_pylon.bounding_box)
detected_pylon.distance = self._pylon_distance_estimator.estimate(
detected_pylon.bounding_box.width)
self._pylon_detector_sender.send(PylonDetectorResult(detected_pylons))
def _shut_down(self):
self._pylon_detector_sender.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 FakeObjectDetector(Node):
_node_config_section = "OBJECT_DETECTOR"
def __init__(self):
super().__init__(self._node_config_section)
self.fake_data_reader: FakeDataReader = FakeDataReader("objectresult")
# Node method implementations
def _start_up(self):
self._object_detector_sender = Sender(self._node_config_section)
def _progress(self):
pickle_in = open(self.fake_data_reader.getNextFilePath(), "rb")
object_detector_result: ObjectDetectorResult = pickle.load(pickle_in)
self._object_detector_sender.send(object_detector_result)
def _shut_down(self):
self._object_detector_sender.close()
def _start_up(self):
self._obstacle_distance_estimator = ObstacleDistanceEstimator()
self._obstacle_detector_sender = Sender(self._node_config_section)
self._sensor_input_camera = ReadCamera()
self._darknet_wrapper = DarknetWrapper()
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()
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)
def _start_up(self):
self._object_detector_sender = Sender(self._node_config_section)
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)