diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..1384b6f --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,28 @@ +# See https://pre-commit.com for more information +# See https://pre-commit.com/hooks.html for more hooks +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + # - id: trailing-whitespace + # - id: end-of-file-fixer + # - id: check-yaml + - id: check-added-large-files + +- repo: https://github.com/psf/black + rev: 22.10.0 + hooks: + - id: black + language_version: python3.8 + args: ["-l", '110'] + +- repo: https://github.com/roy-ht/pre-commit-jupyter + rev: v1.2.1 + hooks: + - id: jupyter-notebook-cleanup + args: + - --remove-kernel-metadata + # - --pin-patterns + #- "[pin];[donotremove]" + +exclude: .bumpversion.cfg diff --git a/packages/duckietown/include/duckietown/dtros/dtros.py b/packages/duckietown/include/duckietown/dtros/dtros.py index 193bfc7..772a26c 100644 --- a/packages/duckietown/include/duckietown/dtros/dtros.py +++ b/packages/duckietown/include/duckietown/dtros/dtros.py @@ -1,19 +1,22 @@ import os import rospy +import rospkg from copy import copy +import yaml from std_srvs.srv import SetBool, SetBoolResponse -from duckietown_msgs.srv import \ - NodeGetParamsList, \ - NodeGetParamsListResponse, \ - NodeRequestParamsUpdate, \ - NodeRequestParamsUpdateResponse -from duckietown_msgs.msg import \ - NodeParameter -from duckietown.dtros.constants import \ - NODE_GET_PARAM_SERVICE_NAME, \ - NODE_REQUEST_PARAM_UPDATE_SERVICE_NAME, \ - NODE_SWITCH_SERVICE_NAME +from duckietown_msgs.srv import ( + NodeGetParamsList, + NodeGetParamsListResponse, + NodeRequestParamsUpdate, + NodeRequestParamsUpdateResponse, +) +from duckietown_msgs.msg import NodeParameter +from duckietown.dtros.constants import ( + NODE_GET_PARAM_SERVICE_NAME, + NODE_REQUEST_PARAM_UPDATE_SERVICE_NAME, + NODE_SWITCH_SERVICE_NAME, +) from .dtparam import DTParam from .constants import NodeHealth, NodeType from .diagnostics import DTROSDiagnostics @@ -42,6 +45,8 @@ class DTROS(object): to ``False``. This switch can be used by computationally expensive parts of the node code that are not in callbacks in order to pause their execution. + - We look for a robot specific parameter file and overwrite the default parameters + if it exists Every children node should call the initializer of DTROS. This should be done by having the following line at the top of the children node ``__init__`` method:: @@ -87,15 +92,18 @@ class DTROS(object): """ - def __init__(self, - node_name, - # DT parameters from here - node_type, - help=None, - dt_ghost=False): + def __init__( + self, + node_name, + # DT parameters from here + node_type, + pkg_name=None, + help=None, + dt_ghost=False, + ): # configure singleton if rospy.__instance__ is not None: - raise RuntimeError('You cannot instantiate two objects of type DTROS') + raise RuntimeError("You cannot instantiate two objects of type DTROS") rospy.__instance__ = self if not isinstance(node_type, NodeType): raise ValueError( @@ -104,51 +112,64 @@ def __init__(self, ) # Initialize the node log_level = rospy.INFO - if os.environ.get('DEBUG', 0) in ['1', 'true', 'True', 'enabled', 'Enabled', 'on', 'On']: + if os.environ.get("DEBUG", 0) in ["1", "true", "True", "enabled", "Enabled", "on", "On"]: log_level = rospy.DEBUG rospy.init_node(node_name, log_level=log_level, __dtros__=True) self.node_name = rospy.get_name() self.node_help = help self.node_type = node_type - self.log('Initializing...') + self.log("Initializing...") self.is_shutdown = False self._is_ghost = dt_ghost self._health = NodeHealth.STARTING self._health_reason = None self._ros_handler = get_ros_handler() + if pkg_name is not None: + rospack = rospkg.RosPack() + veh_name = self.node_name.split("/")[1] + pkg_path = rospack.get_path(pkg_name) + param_file_folder = f"{pkg_path}/config/{node_name}" + robot_param_file = param_file_folder + "/" + veh_name + ".yaml" + if os.path.isfile(robot_param_file): + rospy.loginfo("found robot specific parameter file.. loading") + try: + with open(robot_param_file, "r") as stream: + new_params = yaml.load(stream, Loader=yaml.Loader) + except yaml.YAMLError: + msg = f"Error in parsing calibration file {robot_param_file}.. skipping" + rospy.logerr(msg) + rospy.signal_shutdown(msg) + for key in new_params: + rospy.set_param(key, new_params[key]) + # Initialize parameters handling self._parameters = dict() self._rh_paramUpdate = None if self._ros_handler is not None: # decorate the XMLRPC paramUpdate function self._rh_paramUpdate = self._ros_handler.paramUpdate - setattr(self._ros_handler, 'paramUpdate', self._param_update) + setattr(self._ros_handler, "paramUpdate", self._param_update) + # Handle publishers, subscribers, and the state switch self._switch = True self._subscribers = list() self._publishers = list() # create switch service for node - self.srv_switch = rospy.Service( - "~%s" % NODE_SWITCH_SERVICE_NAME, - SetBool, self._srv_switch - ) + self.srv_switch = rospy.Service("~%s" % NODE_SWITCH_SERVICE_NAME, SetBool, self._srv_switch) # create services to manage parameters self._srv_get_params = rospy.Service( - "~%s" % NODE_GET_PARAM_SERVICE_NAME, - NodeGetParamsList, self._srv_get_params_list + "~%s" % NODE_GET_PARAM_SERVICE_NAME, NodeGetParamsList, self._srv_get_params_list ) self._srv_request_params_update = rospy.Service( "~%s" % NODE_REQUEST_PARAM_UPDATE_SERVICE_NAME, - NodeRequestParamsUpdate, self._srv_request_param_update + NodeRequestParamsUpdate, + self._srv_request_param_update, ) # register node against the diagnostics manager if DTROSDiagnostics.enabled(): DTROSDiagnostics.getInstance().register_node( - self.node_name, - self.node_help, - self.node_type, - health=self._health + self.node_name, self.node_help, self.node_type, health=self._health ) # provide a public interface to the context manager to use as `with self.profiler("PHASE")` @@ -188,20 +209,19 @@ def publishers(self): def set_health(self, health, reason=None): if not isinstance(health, NodeHealth): - raise ValueError('Argument \'health\' must be of type duckietown.NodeHealth. ' - 'Got %s instead' % str(type(health))) - self.log('Health status changed [%s] -> [%s]' % (self._health.name, health.name)) + raise ValueError( + "Argument 'health' must be of type duckietown.NodeHealth. " + "Got %s instead" % str(type(health)) + ) + self.log("Health status changed [%s] -> [%s]" % (self._health.name, health.name)) self._health = health self._health_reason = None if reason is None else str(reason) # update node health in the diagnostics manager if DTROSDiagnostics.enabled(): - DTROSDiagnostics.getInstance().update_node( - health=self._health, - health_reason=self._health_reason - ) + DTROSDiagnostics.getInstance().update_node(health=self._health, health_reason=self._health_reason) - def log(self, msg, type='info'): - """ Passes a logging message to the ROS logging methods. + def log(self, msg, type="info"): + """Passes a logging message to the ROS logging methods. Attaches the ros name to the beginning of the message and passes it to a suitable ROS logging method. Use the `type` argument to select the method @@ -217,38 +237,38 @@ def log(self, msg, type='info'): ValueError: if the ``type`` argument is not one of the supported types """ - full_msg = '[%s] %s' % (self.node_name, msg) + full_msg = "[%s] %s" % (self.node_name, msg) # pipe to the right logger - if type == 'debug': + if type == "debug": rospy.logdebug(full_msg) - elif type == 'info': + elif type == "info": rospy.loginfo(full_msg) - elif type == 'warn' or type == 'warning': + elif type == "warn" or type == "warning": self.set_health(NodeHealth.WARNING, full_msg) rospy.logwarn(full_msg) - elif type == 'err' or type == 'error': + elif type == "err" or type == "error": self.set_health(NodeHealth.ERROR, full_msg) rospy.logerr(full_msg) - elif type == 'fatal': + elif type == "fatal": self.set_health(NodeHealth.FATAL, full_msg) rospy.logfatal(full_msg) else: - raise ValueError('Type argument value %s is not supported!' % type) + raise ValueError("Type argument value %s is not supported!" % type) def loginfo(self, msg): - self.log(msg, type='info') + self.log(msg, type="info") def logerr(self, msg): - self.log(msg, type='err') + self.log(msg, type="err") def logfatal(self, msg): - self.log(msg, type='fatal') + self.log(msg, type="fatal") def logwarn(self, msg): - self.log(msg, type='warn') + self.log(msg, type="warn") def logdebug(self, msg): - self.log(msg, type='debug') + self.log(msg, type="debug") def on_switch_on(self): pass @@ -273,21 +293,13 @@ def _srv_switch(self, request): for sub in self.subscribers: sub.active = self._switch # tell the node about the switch - on_switch_fcn = { - False: self.on_switch_off, - True: self.on_switch_on - }[self._switch] + on_switch_fcn = {False: self.on_switch_off, True: self.on_switch_on}[self._switch] on_switch_fcn() # update node switch in the diagnostics manager if DTROSDiagnostics.enabled(): - DTROSDiagnostics.getInstance().update_node( - enabled=self._switch - ) + DTROSDiagnostics.getInstance().update_node(enabled=self._switch) # create a response to the service call - msg = 'Node switched from [%s] to [%s]' % ( - 'on' if old_state else 'off', - 'on' if new_state else 'off' - ) + msg = "Node switched from [%s] to [%s]" % ("on" if old_state else "off", "on" if new_state else "off") # print out the change in state self.log(msg) # reply to the service call @@ -308,12 +320,9 @@ def _srv_get_params_list(self, request): return NodeGetParamsListResponse( parameters=[ NodeParameter( - node=rospy.get_name(), - name=p.name, - help=p.help, - type=p.type.value, - **p.options() - ) for p in self.parameters + node=rospy.get_name(), name=p.name, help=p.help, type=p.type.value, **p.options() + ) + for p in self.parameters ] ) @@ -338,22 +347,23 @@ def _param_update(self, *args, **kwargs): self._rh_paramUpdate(*args, **kwargs) # check data if len(args) < 3: - self.logdebug('Received invalid paramUpdate call from Master') + self.logdebug("Received invalid paramUpdate call from Master") return # get what changed _, param_name, param_value = args[:3] - param_name = param_name.rstrip('/') + param_name = param_name.rstrip("/") self.logdebug('Received paramUpdate("%s", %s)' % (param_name, str(param_value))) # update parameter value if param_name in self._parameters: self._parameters[param_name].set_value(param_value) - self.loginfo('Parameter "%s" has now the value [%s]' % ( - param_name, str(self._parameters[param_name].value) - )) + self.loginfo( + 'Parameter "%s" has now the value [%s]' + % (param_name, str(self._parameters[param_name].value)) + ) def _add_param(self, param): if not isinstance(param, DTParam): - raise ValueError('Expected type duckietown.DTParam, got %s instead' % str(type(param))) + raise ValueError("Expected type duckietown.DTParam, got %s instead" % str(type(param))) self._parameters[param.name] = param def _has_param(self, param): @@ -366,7 +376,7 @@ def _register_subscriber(self, subscriber): self._subscribers.append(subscriber) def _on_shutdown(self): - self.log('Received shutdown request.') + self.log("Received shutdown request.") self.is_shutdown = True # call node on_shutdown self.on_shutdown()