def test_controller_closed_loop(fake_robot):
"""
Tests controller closed loop functionality:
- controller publishes robot move commands at 1 Hz
- controller is able to receive sensor input and correct the move commands to robot
"""
def assert_rx_message(client, userdata, message):
msg = json.loads(message.payload.decode("utf-8"))
assert (msg["move"]["x"] % 1 == 0.5)
assert (msg["move"]["y"] % 1 == 0.5)
assert (msg["move"]["z"] % 1 == 0.5)
SENSOR_TOPIC = 'sensor/4/detection'
FAKE_ROBOT_LOC = json.dumps({
"time": gen_influxdb_time(),
"detection": {
"x": 0.5,
"y": 0.5,
"z": 0.5
}
})
MOVE_PLAN = [
Pose(1.0, 1.0, 1.0),
Pose(2.0, 2.0, 2.0),
Pose(3.0, 3.0, 3.0),
Pose(4.0, 4.0, 4.0),
Pose(5.0, 5.0, 5.0)
]
# Create subscriber
client = mqtt.Client()
client.connect(MOSQUITTO_SERVER, MOSQUITTO_PORT, MOSQUITTO_TIMEOUT)
client.on_message = assert_rx_message
client.subscribe(ROBOT_MOVE)
client.loop_start()
# Create controller
controller = Controller(fake_robot, MOVE_PLAN)
time.sleep(0.05)
assert (controller.state == ObjState.ON)
assert (controller.state_sub._connected == True)
assert (controller.state_sub._subscribed == True)
# Run for 2s
controller.operate()
client.publish(SENSOR_TOPIC, FAKE_ROBOT_LOC)
client.publish(SENSOR_TOPIC, FAKE_ROBOT_LOC)
client.publish(SENSOR_TOPIC, FAKE_ROBOT_LOC)
time.sleep(TEST_DURATION)
assert (controller.move_pub._connected == True)
assert (controller.move_pub._publishing == True)
# Disconnect all
controller.disable()
client.loop_stop()
client.disconnect()
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 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 main():
# Create robot, sensors, controller
robot = Robot()
basic_sensor = BasicSensor(robot, Pose(), 1)
controller = Controller(robot, FlatCircle)
db_writer = DBWriter()
# Run sensors/controllers
basic_sensor.operate()
controller.operate()
# Add Ctrl+C handler
handler = SIGINT_handler()
signal.signal(signal.SIGINT, handler.signal_handler)
# Run until interrupt or print completed
while True:
if handler.SIGINT or controller.print_completed:
break
# Disable objects
robot.disable()
basic_sensor.disable()
controller.disable()
db_writer.disable()
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 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 test_controller_print_completes(fake_robot):
"""
Tests controller finish print functionality
"""
def assert_rx_message(client, userdata, message):
global MSG_COUNT
MSG_COUNT += 1
global MSG_COUNT
MOVE_PLAN = [
Pose(1.0, 1.0, 1.0),
Pose(2.0, 2.0, 2.0),
Pose(3.0, 3.0, 3.0),
Pose(4.0, 4.0, 4.0),
Pose(5.0, 5.0, 5.0)
]
# Create subscriber
client = mqtt.Client()
client.connect(MOSQUITTO_SERVER, MOSQUITTO_PORT, MOSQUITTO_TIMEOUT)
client.on_message = assert_rx_message
client.subscribe(ROBOT_MOVE)
client.loop_start()
# Create controller
controller = Controller(fake_robot, MOVE_PLAN)
time.sleep(0.05)
assert (controller.state == ObjState.ON)
assert (controller.state_sub._connected == True)
assert (controller.state_sub._subscribed == True)
# Run
controller.operate()
time.sleep(len(MOVE_PLAN) * 1.5)
assert (controller.disabled == True)
assert (controller.move_pub._connected == True)
assert (controller.move_pub._publishing == True)
assert (MSG_COUNT == len(MOVE_PLAN))
# Disconnect all
controller.disable()
client.loop_stop()
client.disconnect()
def _draw_circle(radius, z_range):
pts = []
z = 1.0
for z in z_range:
for t in range(0, 360):
t = math.radians(t)
x = radius * math.cos(t) + 1.25 * radius
y = radius * math.sin(t) + 1.25 * radius
pts.append(Pose(x, y, z))
return pts
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 _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 _draw_spiral(radius):
pts = []
dist = 0.0
step = radius / 360
res = 3 * radius
t = 0.0
z = 0.0
while dist * math.hypot(math.cos(t), math.sin(t)) < radius:
x = dist * math.cos(t) + 1.25 * radius
y = dist * math.sin(t) + 1.25 * radius
if (t % 360 == 0.0): z += 1
pts.append(Pose(x, y, z))
dist += step
t += res
return pts
def test_robot_verifyMoves():
"""
Tests basic robot functionality:
- robot updates pose when receives move command
"""
pub = mqtt.Client()
pub.connect(MOSQUITTO_SERVER, MOSQUITTO_PORT, MOSQUITTO_TIMEOUT)
MOVE_DATA = json.dumps({
"time": gen_influxdb_time(),
"move": {
"x": 2.0,
"y": 4.0,
"z": -1.5
},
"pose": {
"x": 2.0,
"y": 4.0,
"z": -1.5
}
})
robot = Robot()
assert (robot.state == ObjState.ON)
assert (robot.move_sub._connected)
assert (robot.move_sub._subscribed)
assert (robot.pose == Pose(0.0, 0.0, 0.0))
pub.publish(ROBOT_MOVE, MOVE_DATA)
time.sleep(0.05)
assert (robot.pose.x == 2.0)
assert (robot.pose.z == -1.5)
y_lower_bounds = 0.95 * 4.0
y_upper_bounds = 1.05 * 4.0
assert (robot.pose.y >= y_lower_bounds and robot.pose.y <= y_upper_bounds)
robot.disable()
time.sleep(0.05)
assert (robot.state == ObjState.OFF)
assert (robot.move_sub._connected == False)
assert (robot.move_sub._subscribed == False)
pub.disconnect()
def __init__(self):
self.state = ObjState.ON
self.move_sub = MQTTSubscriber(ROBOT_MOVE)
self.move_sub.subscribe_topic(self.move)
self.pose = Pose()
def test_basicSensor(fake_robot):
"""
Tests basic sensor functionality:
- sensor is able to publish robot information
- sensor is able to recieve off commands and stop publishing
- sensor is able to be turned back on and reassume publishing
"""
def assert_rx_message(client, userdata, message):
global PUB_MSG_COUNT
PUB_MSG_COUNT += 1
assert (message.payload.decode("utf-8") == DETECTION)
global PUB_MSG_COUNT
DETECTION_TOPIC = 'sensor/4/detection'
DETECTION = json.dumps({
"time": gen_influxdb_time(),
"detection": {
"x": 5.0,
"y": 6.0,
"z": -3.2
}
})
STATE_TOPIC = 'sensor/4/state'
STATE_ON = json.dumps({
"time": gen_influxdb_time(),
"control_state": ObjState.ON.value
})
STATE_OFF = json.dumps({
"time": gen_influxdb_time(),
"control_state": ObjState.OFF.value
})
# Create subscriber
client = mqtt.Client()
client.connect(MOSQUITTO_SERVER, MOSQUITTO_PORT, MOSQUITTO_TIMEOUT)
client.on_message = assert_rx_message
client.subscribe(DETECTION_TOPIC)
client.loop_start()
# Create sensor
basic_sensor = BasicSensor(fake_robot, Pose(), SENSOR_ID)
time.sleep(0.05)
assert (basic_sensor.state == ObjState.ON)
assert (basic_sensor.state_sub._connected == True)
assert (basic_sensor.state_sub._subscribed == True)
# Run for 2s
basic_sensor.operate()
time.sleep(TEST_DURATION)
assert (basic_sensor.detection_pub._connected == True)
assert (basic_sensor.detection_pub._publishing == True)
# Turn off for 2s
client.publish(STATE_TOPIC, STATE_OFF)
time.sleep(TEST_DURATION)
# Turn on for 2s
client.publish(STATE_TOPIC, STATE_ON)
time.sleep(TEST_DURATION)
# Disconnect all
basic_sensor.disable()
client.loop_stop()
client.disconnect()
assert (PUB_MSG_COUNT == BASIC_SENSOR_DETECTION_FREQ * TEST_DURATION * 2)
def fake_robot() -> FakeRobot:
return FakeRobot(Pose(5.0, 6.0, -3.2))