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()