def __init__(self):
self._logger = RoboLogger.get_logger()
self._interface = interface_exposure()
self._last_keep_alive = 0
self._is_new_visit = True
self.check_last_keep_alive()
self._interface["init"].function()
# flake8: ignore=E501
from logger import RoboLogger
import threading
import traceback
from armlink import ArmLink
from endeffector import EndEffector
from numpy import radians
import time
import numpy as np
from constant import STOP_ACTION_BRAKE, STOP_ACTION_COAST, STOP_ACTION_HOLD # noqa: F401
from constant import LOGGER_ARM_EE, LOGGER_ARM_MAIN, LOGGER_ARM_MTA, LOGGER_ARM_RESET
log = RoboLogger.getLogger()
class Arm(object):
"""
Implementation of an arm (that contains a collection of links)
"""
__slots__ = [
'__links',
'stopAction',
'lock',
'_urgentStop',
'_killThread',
'__endEffector',
'__motionThreads',
'armZeroPosition',
'armRestPosition',
'armDrivePosition'
def thread_mqtt_listener(self):
"""
Description :
MQTT Thread launching the loop and subscripbing to the right topics
"""
mqtt_default_qos = 2
self.mqtt_topics = [
(topic, mqtt_default_qos)
for topic in self.configuration['mqtt']['subscribedTopics']
]
def on_connect(client, userdata, flags, rc):
log.info(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Connected to MQTT broker. Result code {str(rc)}')
mqtt_connect_result, self.mqtt_connect_mid = client.subscribe(
self.mqtt_topics)
if mqtt_connect_result == mqtt.MQTT_ERR_SUCCESS:
log.warning(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Successfully subscribed to '
f'topics in input config file')
log.debug(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Topics subcribed = {self.mqtt_topics}')
else:
log.error(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
'MQTT Broker subscription problem.')
def on_message(client, userdata, message):
""" callback function used for the mqtt client (called when
a new message is publisehd to one of the queues we subscribe to)
"""
log.info(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Received MID {message.mid} : '
f'"{str(message.payload)}" '
f'on topic {message.topic} with QoS {message.qos}')
self.mqtt_message_queue.put_nowait(message)
def on_disconnect(client, userdata, rc=0):
"""callback for handling disconnects
"""
log.warning(
ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
f'Disconnected MQTT result code = {rc}. '
f'Should automatically re-connect to broker')
def on_subscribe(client, userdata, mid, granted_qos):
if mid == self.mqtt_connect_mid:
log.debug(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Subscribed to topics. Granted '
f'QOS = {granted_qos}')
else:
log.error(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Strange... MID does not match '
f'self.mqtt_connect_mid')
try:
self.mqttClient = mqtt.Client(
client_id=self.configuration["mqtt"]["clientID"],
clean_session=True,
transport=self.configuration["mqtt"]["brokerProto"])
self.mqttClient.enable_logger(logger=RoboLogger.getSpecificLogger(
ct.LOG_TASKMAN_THREAD_MQTT_LISTENER))
self.mqttClient.on_subscribe = on_subscribe
self.mqttClient.on_connect = on_connect
self.mqttClient.on_disconnect = on_disconnect
self.mqttClient.on_message = on_message
self.mqttClient.connect(
host=self.configuration["mqtt"]["brokerIP"],
port=self.configuration["mqtt"]["brokerPort"])
self.mqttClient.loop_forever()
except Exception:
log.error(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
f'Error : {traceback.print_exc()}')
finally:
log.warning(ct.LOG_TASKMAN_THREAD_MQTT_LISTENER,
msg=f'Exiting MQTT connection thread.')
def threadImplMQTT(self):
"""
MQTT Thread launching the loop and subscripbing to the right topics
"""
mqtt_default_qos = 2
self.mqtt_topics = [
(topic, mqtt_default_qos) for topic in
self.robotConfiguration["mqtt"]["subscribedTopics"]]
def on_connect(client, userdata, flags, rc):
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Connected to MQTT broker. '
f'Result code {rc}')
mqtt_connect_result, self.mqtt_connect_mid = \
client.subscribe(self.mqtt_topics)
if mqtt_connect_result == mqtt.MQTT_ERR_SUCCESS:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Successfully subscribed '
f'to {self.mqtt_topics}')
else:
log.error(LOGGER_DDR_THREADIMPLMQTT,
msg=f'MQTT Broker subscription problem.')
def on_message(client, userdata, message):
""" callback function used for the mqtt client (called when
a new message is publisehd to one of the queues we subscribe to)
"""
log.info(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Received MID {message.mid} : '
f'{str(message.payload)} on topic '
f'{message.topic} with QoS {message.qos}')
self.mqttMoveQueue.put_nowait(message)
def on_disconnect(client, userdata, rc=0):
"""callback for handling disconnects
"""
log.info(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Disconnected MQTT result code = {rc}.'
f'Should automatically re-connect to broker')
def on_subscribe(client, userdata, mid, granted_qos):
if mid == self.mqtt_connect_mid:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Subscribed to topics. Granted QOS '
f'= {granted_qos}')
else:
log.error(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Strange... MID doesn\'t match '
f'self.mqtt_connect_mid')
self.mqttClient = mqtt.Client(
client_id="mybot",
clean_session=True,
transport=self.robotConfiguration["mqtt"]["brokerProto"])
self.mqttClient.enable_logger(
logger=RoboLogger.getSpecificLogger(LOGGER_DDR_THREADIMPLMQTT))
self.mqttClient.on_subscribe = on_subscribe
self.mqttClient.on_connect = on_connect
self.mqttClient.on_disconnect = on_disconnect
self.mqttClient.on_message = on_message
self.mqttClient.connect(
host=self.robotConfiguration["mqtt"]["brokerIP"],
port=self.robotConfiguration["mqtt"]["brokerPort"])
self.mqttClient.loop_start()