class Robot: # TODO move initializations out of __init__() def __init__(self): self.state = ObjState.ON self.move_sub = MQTTSubscriber(ROBOT_MOVE) self.move_sub.subscribe_topic(self.move) self.pose = Pose() def move(self, client, userdata, message): """ Callback for subscription to robot/move_command """ msg = json.loads(message.payload.decode("utf-8")) if self.state == ObjState.ON: move_pose = Pose(msg["move"]["x"], msg["move"]["y"], msg["move"]["z"]) y = self._simulate_y_err(move_pose.y) self.pose = Pose(move_pose.x, y, move_pose.z) def disable(self): """ Disable pubsub and turn off """ self.state = ObjState.OFF self.move_sub.disconnect() def _simulate_y_err(self, y) -> float: """ Return random value max +/-5% of input y value 20% of the time (negative 20% of time) Else, returns y """ rand_max = 0.05 * y rand_mult = 1 if random.random() < 0.8 else -1 return y if (random.random() >= 0.2 ) else y + rand_mult * (rand_max * random.random())
def __init__(self, db=INFLUXDB_DBNAME): self.state = ObjState.ON self.connect_influxdb(db) self.sensor_sub = MQTTSubscriber(SENSOR_DETECTIONS) self.sensor_sub.subscribe_topic(self.handle_sensor_data) self.last_known_desired_pose = Pose() self.controller_sub = MQTTSubscriber(ROBOT_MOVE) self.controller_sub.subscribe_topic(self.handle_controller_data)
def __init__(self, robot, placement_pose, ID): super().__init__(robot, placement_pose, ID, SensorType.BASIC) self.state = ObjState.ON self.detection_topic = SENSOR_PREFIX + str(self.id) + DETECTION_SUFFIX self.detection_pub = MQTTPublisher(self.detection_topic) self.state_topic = SENSOR_PREFIX + str(self.id) + STATE_SUFFIX self.state_sub = MQTTSubscriber(self.state_topic) self.state_sub.subscribe_topic(self.control_state) self.disabled = False
def test_sendJSON(): """ Tests subscriber and publisher wrapper with JSON messages """ def assert_rx_message(client, userdata, message): global JSON_MSG_COUNT JSON_MSG_COUNT += 1 jsonifyed = json.loads(message.payload.decode("utf-8")) assert(jsonifyed == DATA) assert(ObjState(jsonifyed["state"]) == ObjState.ON) global JSON_MSG_COUNT TOPIC = 'test/json' DATA = {"sensor": "basic sensor", "id": 4, "state": ObjState.ON.value} pub = MQTTPublisher(TOPIC) pub.start_publish() sub = MQTTSubscriber(TOPIC) sub.subscribe_topic(assert_rx_message) time.sleep(0.025) rc = pub.publish_topic(json.dumps(DATA)) assert(rc == MQTTPahoRC.SUCCESS) time.sleep(0.025) pub.disconnect() sub.disconnect() assert(JSON_MSG_COUNT == 1)
def __init__(self, robot, print_design): self.state = ObjState.ON self.sensor_sub = MQTTSubscriber(SENSOR_DETECTIONS) self.sensor_sub.subscribe_topic(self.handle_sensor_data) self.move_pub = MQTTPublisher(ROBOT_MOVE) self.state_sub = MQTTSubscriber(CONTROLLER_PREFIX + STATE_SUFFIX) self.state_sub.subscribe_topic(self.control_state) self.sensor_msg_count = 0 self.sensor_msg_count_prev = 0 self.closed_loop = True self.disabled = False self.print_design = print_design self.pattern_seq = 0 self.print_completed = False self.robot_actual_pose = Pose() self.robot_previous_pose = Pose() self.robot_desired_pose = Pose() self.relative_move = Pose()
def test_simpleSubscriber(): """ Tests subscriber wrapper with mqtt publishers """ def assert_rx_message(client, userdata, message): global SUB_MSG_COUNT SUB_MSG_COUNT += 1 assert(message.payload.decode("utf-8") == DATA) global SUB_MSG_COUNT TOPIC = 'test/data' DATA = 'my message data here' simple_pub = mqtt.Client() simple_pub.connect(MOSQUITTO_SERVER, MOSQUITTO_PORT, MOSQUITTO_TIMEOUT) sub = MQTTSubscriber(TOPIC) assert(sub._connected == False) assert(sub._subscribed == False) sub.subscribe_topic(assert_rx_message) assert(sub._connected == True) assert(sub._subscribed == True) time.sleep(0.05) simple_pub.publish(TOPIC, DATA) sub.unsubscribe() assert(sub._connected == True) assert(sub._subscribed == False) simple_pub.publish(TOPIC, DATA) sub.resubscribe() assert(sub._connected == True) assert(sub._subscribed == True) sub.disconnect() time.sleep(0.01) assert(sub._connected == False) assert(sub._subscribed == False) simple_pub.disconnect() assert(SUB_MSG_COUNT == 1)
class DBWriter(): def __init__(self, db=INFLUXDB_DBNAME): self.state = ObjState.ON self.connect_influxdb(db) self.sensor_sub = MQTTSubscriber(SENSOR_DETECTIONS) self.sensor_sub.subscribe_topic(self.handle_sensor_data) self.last_known_desired_pose = Pose() self.controller_sub = MQTTSubscriber(ROBOT_MOVE) self.controller_sub.subscribe_topic(self.handle_controller_data) def connect_influxdb(self, db_name): """ Sets up connection to InfluxDB Creates database if doesn't exist """ self.influxdb_client = InfluxDBClient(INFLUXDB_HOST, INFLUXDB_PORT, INFLUXDB_USER, INFLUXDB_PW, db_name) databases = self.influxdb_client.get_list_database() if len(list(filter(lambda db: db['name'] == db_name, databases))) == 0: self.influxdb_client.create_database(db_name) self.influxdb_client.switch_database(db_name) def disable(self): self.state = ObjState.OFF self.disabled = True self.sensor_sub.disconnect() self.controller_sub.disconnect() def handle_sensor_data(self, client, userdata, message): """ Callback for sensor data """ msg = json.loads(message.payload.decode("utf-8")) self.influxdb_client.write_points( self._convert_to_influxdb_measurement(msg)) def handle_controller_data(self, client, userdata, message): """ Callback for controller data, update last known desired pose """ msg = json.loads(message.payload.decode("utf-8")) self.last_known_desired_pose = Pose(msg["pose"]["x"], msg["pose"]["y"], msg["pose"]["z"]) def _convert_to_influxdb_measurement(self, message): """ Creates InfluxDB entry """ return [{ 'measurement': "detection", 'tags': { 'source': "basic_sensor" }, 'time': message["time"], 'fields': { "x": float(message["detection"]["x"]), "y": float(message["detection"]["y"]), "z": float(message["detection"]["z"]) } }, { 'measurement': "desired_pose", 'tags': { 'source': "controller" }, 'time': message["time"], 'fields': { "x": float(self.last_known_desired_pose.x), "y": float(self.last_known_desired_pose.y), "z": float(self.last_known_desired_pose.z) } }]
class BasicSensor(Sensor): def __init__(self, robot, placement_pose, ID): super().__init__(robot, placement_pose, ID, SensorType.BASIC) self.state = ObjState.ON self.detection_topic = SENSOR_PREFIX + str(self.id) + DETECTION_SUFFIX self.detection_pub = MQTTPublisher(self.detection_topic) self.state_topic = SENSOR_PREFIX + str(self.id) + STATE_SUFFIX self.state_sub = MQTTSubscriber(self.state_topic) self.state_sub.subscribe_topic(self.control_state) self.disabled = False def disable(self): """ Disable pubsub and turn off """ self.disabled = True self.state = ObjState.OFF self.pub_thread.join() self.state_sub.disconnect() self.detection_pub.stop_publish() self.detection_pub.disconnect() def control_state(self, client, userdata, message): """ Callback for subscription to sensor/ID/state """ msg = json.loads(message.payload.decode("utf-8")) if ObjState(msg["control_state"]) == ObjState.ON: # Turn on and enable publisher self.state = ObjState.ON self.detection_pub.start_publish() elif ObjState(msg["control_state"]) == ObjState.OFF: # Turn off and disable publisher self.state = ObjState.OFF self.detection_pub.stop_publish() def operate(self): """ Periodic publish operation """ self.disabled = False self.state = ObjState.ON self.pub_thread = threading.Thread(target=self._publish_thread) self.pub_thread.start() def _publish_thread(self): self.detection_pub.start_publish() period = 1.0 / BASIC_SENSOR_DETECTION_FREQ while not self.disabled: begin_time = time.time() self.detection_pub.publish_topic(self._detect_msg()) # Sleep for remainder of loop time.sleep(period - (begin_time - time.time())) def _read(self) -> Pose: """ Dummy function!!!! Returns current robot pose in global frame Should be simulated somehow as true sensor """ return self.robot.pose def _detect_msg(self) -> str: """ Creates message of detected sensor location (global frame currently) """ sensor_pose = self._read() return json.dumps({ "time": gen_influxdb_time(), "detection": { "x": sensor_pose.x, "y": sensor_pose.y, "z": sensor_pose.z } })
class Controller: def __init__(self, robot, print_design): self.state = ObjState.ON self.sensor_sub = MQTTSubscriber(SENSOR_DETECTIONS) self.sensor_sub.subscribe_topic(self.handle_sensor_data) self.move_pub = MQTTPublisher(ROBOT_MOVE) self.state_sub = MQTTSubscriber(CONTROLLER_PREFIX + STATE_SUFFIX) self.state_sub.subscribe_topic(self.control_state) self.sensor_msg_count = 0 self.sensor_msg_count_prev = 0 self.closed_loop = True self.disabled = False self.print_design = print_design self.pattern_seq = 0 self.print_completed = False self.robot_actual_pose = Pose() self.robot_previous_pose = Pose() self.robot_desired_pose = Pose() self.relative_move = Pose() def disable(self): """ Disable pubsub and turn off """ self.disabled = True self.state = ObjState.OFF self.pub_thread.join() self.move_pub.stop_publish() self.move_pub.disconnect() self.state_sub.disconnect() self.sensor_sub.disconnect() def control_state(self, client, userdata, message): """ Callback for subscription to controller/state Turns closed loop correction of robot pose off Still allows periodic move commands to robot """ msg = json.loads(message.payload.decode("utf-8")) if ObjState(msg["control_state"]) == ObjState.ON: self.closed_loop = True elif ObjState(msg["control_state"]) == ObjState.OFF: self.closed_loop = False def handle_sensor_data(self, client, userdata, message): """ Callback for sensor data, given in global frame for basic_sensor currently """ msg = json.loads(message.payload.decode("utf-8")) self.sensor_msg_count += 1 self.robot_actual_pose = Pose(msg["detection"]["x"], msg["detection"]["y"], msg["detection"]["z"]) def operate(self): """ Operation of read cycle and publish cycle """ self.disabled = False self.state = ObjState.ON self.pub_thread = threading.Thread(target=self._publish_thread) self.pub_thread.start() def _publish_thread(self): self.move_pub.start_publish() period = 1.0 / MOVE_COMMAND_FREQ while not (self.disabled or self.print_completed): begin_time = time.time() # Compute next move from updated sensor info (or not), and publish self._compute_move_command() # Save how many sensor messages sent up to this time publishing self.sensor_msg_count_prev = self.sensor_msg_count self.move_pub.publish_topic(self._pose_msg()) # Sleep for remainder of loop time.sleep(period - (begin_time - time.time())) # Finished print if self.print_completed: self.disable() def _compute_move_command(self): """ Computes the desired global coord and relative move command according to pattern """ self.robot_desired_pose = self._get_next_pattern_location() # If open loop or no new sensor messages published since previous publish or no sensor messages if not self.closed_loop or self.sensor_msg_count == self.sensor_msg_count_prev: self.relative_move = self.robot_desired_pose - self.robot_previous_pose # If closed loop else: self.relative_move = self.robot_desired_pose - self.robot_actual_pose # Previous pose is now current target pose (should be where robot is moving ) self.robot_previous_pose = self.robot_desired_pose def _get_next_pattern_location(self): """ Returns next coord pair in pattern """ next_move = self.print_design[self.pattern_seq] self.pattern_seq += 1 # End of desired controller pattern, ending if self.pattern_seq == len(self.print_design): self.print_completed = True return Pose(0.0, 0.0, 0.0) return next_move def _pose_msg(self) -> str: """ Creates message of robot move command (relative) and desired end pose (global frame) """ return json.dumps({ "time": gen_influxdb_time(), "move": { "x": self.relative_move.x, "y": self.relative_move.y, "z": self.relative_move.z }, "pose": { "x": self.robot_desired_pose.x, "y": self.robot_desired_pose.y, "z": self.robot_desired_pose.z } })
def __init__(self): self.state = ObjState.ON self.move_sub = MQTTSubscriber(ROBOT_MOVE) self.move_sub.subscribe_topic(self.move) self.pose = Pose()