diff --git a/motor_controller/odriver/CMakeLists.txt b/motor_controller/odriver/CMakeLists.txt
index b331be84..335cba05 100644
--- a/motor_controller/odriver/CMakeLists.txt
+++ b/motor_controller/odriver/CMakeLists.txt
@@ -6,6 +6,19 @@ find_package(ament_cmake REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(std_msgs REQUIRED)
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_copyright REQUIRED)
+  find_package(ament_cmake_flake8 REQUIRED)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_xmllint REQUIRED)
+
+  set(MAX_LINE_LENGTH 200)
+  ament_copyright()
+  ament_flake8(MAX_LINE_LENGTH ${MAX_LINE_LENGTH})
+  ament_lint_cmake()
+  ament_xmllint()
+endif()
+
 install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
 
 install(PROGRAMS
diff --git a/motor_controller/odriver/package.xml b/motor_controller/odriver/package.xml
index 21f789c7..b4ea9ed7 100644
--- a/motor_controller/odriver/package.xml
+++ b/motor_controller/odriver/package.xml
@@ -22,7 +22,7 @@
  SOFTWARE.
 -->
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="2">
+<package format="3">
   <name>odriver</name>
   <version>2.0.0</version>
   <description>The odriver package</description>
diff --git a/motor_controller/odriver/script/odriver_node.py b/motor_controller/odriver/script/odriver_node.py
index 93f72bf2..5d80f7fc 100755
--- a/motor_controller/odriver/script/odriver_node.py
+++ b/motor_controller/odriver/script/odriver_node.py
@@ -1,92 +1,93 @@
 #!/usr/bin/env python3
 
- ###############################################################################
- # Copyright (c) 2019  Carnegie Mellon University
- #
- # Permission is hereby granted, free of charge, to any person obtaining a copy
- # of this software and associated documentation files (the "Software"), to deal
- # in the Software without restriction, including without limitation the rights
- # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- # copies of the Software, and to permit persons to whom the Software is
- # furnished to do so, subject to the following conditions:
- #
- # The above copyright notice and this permission notice shall be included in
- # all copies or substantial portions of the Software.
- #
- # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- # THE SOFTWARE.
- ###############################################################################
+###############################################################################
+# Copyright (c) 2019  Carnegie Mellon University
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+###############################################################################
 
 # Initial Committer
 # EagleZ, dapengz@andrew.cmu.edu
 
-'''ROS Imports'''
+# ROS Imports
 from odrive.pyfibre import fibre
 import signal
 import sys
 import rclpy
 from rclpy.duration import Duration
-from rclpy.exceptions import ROSInterruptException, InvalidServiceNameException
+from rclpy.exceptions import ROSInterruptException
 import time
 import logging
 import traceback
 from odriver_msgs.msg import MotorStatus
 from odriver_msgs.msg import MotorTarget
-from std_msgs.msg import Header
-
+from diagnostic_updater import Updater, DiagnosticTask
+from diagnostic_msgs.msg import DiagnosticStatus
+from std_srvs.srv import SetBool
 
-'''Functional Imports'''
-#import serial
+# Functional Imports
 import odrive
-from odrive.utils import dump_errors
-
-from odrive.enums import *
+from odrive.enums import AXIS_STATE_IDLE
+from odrive.enums import AXIS_STATE_CLOSED_LOOP_CONTROL
+from odrive.enums import CONTROL_MODE_VELOCITY_CONTROL
+from odrive.enums import AXIS_ERROR_WATCHDOG_TIMER_EXPIRED
+from odrive.enums import AXIS_ERROR_NONE
+from odrive.enums import ODRIVE_ERROR_NONE
+from odrive.enums import MOTOR_ERROR_NONE
+from odrive.enums import ENCODER_ERROR_NONE
+from odrive.enums import CONTROLLER_ERROR_NONE
+from odrive.enums import SENSORLESS_ESTIMATOR_ERROR_NONE
+from odrive.enums import MOTOR_ERROR_CONTROL_DEADLINE_MISSED
 import odrive.enums
-odrive_error_codes_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ODRIVE_ERROR_" in name]
-axis_error_codes_tup =  [(name, value) for name, value in odrive.enums.__dict__.items() if "AXIS_ERROR_" in name]
-motor_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "MOTOR_ERROR_" in name]
-controller_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "CONTROLLER_ERROR_" in name]
-encoder_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ENCODER_ERROR_" in name]
-sensorless_estimator_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "SENSORLESS_ESTIMATOR_ERROR_" in name]
-
-import time
 import numpy as np
 import threading
 from packaging import version
 
-from diagnostic_updater import Updater, DiagnosticTask
-from diagnostic_msgs.msg import DiagnosticStatus
 
-from std_srvs.srv import SetBool
+odrive_error_codes_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ODRIVE_ERROR_" in name]
+axis_error_codes_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "AXIS_ERROR_" in name]
+motor_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "MOTOR_ERROR_" in name]
+controller_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "CONTROLLER_ERROR_" in name]
+encoder_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ENCODER_ERROR_" in name]
+sensorless_estimator_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "SENSORLESS_ESTIMATOR_ERROR_" in name]
 
-PRINTDEBUG=False
+PRINTDEBUG = False
 
-ODRIVE_VERSIONS=[[0,5,1],[0,5,4]]
+ODRIVE_VERSIONS = [[0, 5, 1], [0, 5, 4]]
 
 '''Parameter'''
-freq = 40 #Hz
-pause_between_commands = 0.001 #sec
-serialReading_timeout=0.01 #sec
-serialWriting_timeout=0.01 #sec
-lock=threading.Lock()
-use_checksum=False
+freq = 40  # Hz
+pause_between_commands = 0.001  # sec
+serialReading_timeout = 0.01  # sec
+serialWriting_timeout = 0.01  # sec
+lock = threading.Lock()
+use_checksum = False
 
 '''Configuarable parameter'''
 meter_per_count = None
-leftIs1 = False # left is axis0, right is axis1
-isClockwise = True # set true if sign = 1 corresponds to the clockwise direction. set false if sign = 1 corresponds to the counter-clockwise direction.
+leftIs1 = False  # left is axis0, right is axis1
+isClockwise = True  # set true if sign = 1 corresponds to the clockwise direction. set false if sign = 1 corresponds to the counter-clockwise direction.
 signLeft = -1.0
 signRight = 1.0
 gainLeft = 1.0
 gainRight = 1.0
 
-
-
 '''Global Varaible'''
 spd0_c, spd1_c = 0, 0
 loopCtrl_on = 0
@@ -102,20 +103,21 @@
 
 
 def is_firmware_equal(odrv, od_version):
-    return  (odrv.fw_version_major == od_version[0] and \
-            odrv.fw_version_minor == od_version[1] and \
+    return (odrv.fw_version_major == od_version[0] and
+            odrv.fw_version_minor == od_version[1] and
             odrv.fw_version_revision == od_version[2])
 
 
 def is_firmware_supported(odrv):
-    return any((is_firmware_equal(odrv,x) for x in ODRIVE_VERSIONS))
+    return any((is_firmware_equal(odrv, x) for x in ODRIVE_VERSIONS))
+
 
 def clear_errors(odrv):
     global fw_version
-    
+
     if version.parse("0.5.2") <= fw_version:
         odrv.clear_errors()
-    else: # fw_version <= 0.5.1
+    else:  # fw_version <= 0.5.1
         # The following try block throws an error when an odrv object returns a wrong version number due to a bug related to firmware.
         try:
             odrv.axis0.clear_errors()
@@ -124,7 +126,6 @@ def clear_errors(odrv):
             odrv.clear_errors()
 
 
-
 def find_controller(port, clear=False, reset_watchdog_error=False):
     port = None if not port else port
     '''Hardware Initialization'''
@@ -141,9 +142,9 @@ def find_controller(port, clear=False, reset_watchdog_error=False):
             logger.info("Finding Odrive controller... : " + str(port))
             logging.basicConfig(level=logging.DEBUG)
             odrv0 = odrive.find_any(timeout=5, channel_termination_token=channel_termination_token) if port is None else odrive.find_any(path=port, timeout=5)
-        except:
+        except:  # noqa: 722
             logger.error(traceback.format_exc())
-            logger.error("Check Odrive connection: " + str(port) +  " doesn't exist! ")
+            logger.error("Check Odrive connection: " + str(port) + " doesn't exist! ")
             time.sleep(1)
             continue
         else:
@@ -163,11 +164,11 @@ def find_controller(port, clear=False, reset_watchdog_error=False):
         index_not_found = True
     if use_index and index_not_found:
         return
-        
+
     if fw_version_str != "":
         fw_version = version.parse(fw_version_str)
     else:
-        fw_version = version.parse(".".join(map(str,[odrv0.fw_version_major, odrv0.fw_version_minor, odrv0.fw_version_revision])))
+        fw_version = version.parse(".".join(map(str, [odrv0.fw_version_major, odrv0.fw_version_minor, odrv0.fw_version_revision])))
 
     clear_errors(odrv0)
 
@@ -180,22 +181,25 @@ def find_controller(port, clear=False, reset_watchdog_error=False):
 
 
 def reset_error_watchdog_timer_expired():
-    if odrv0.axis0.error & AXIS_ERROR_WATCHDOG_TIMER_EXPIRED != 0:#odrv0.axis0.error == AXIS_ERROR_WATCHDOG_TIMER_EXPIRED:
+    if odrv0.axis0.error & AXIS_ERROR_WATCHDOG_TIMER_EXPIRED != 0:  # odrv0.axis0.error == AXIS_ERROR_WATCHDOG_TIMER_EXPIRED:
         odrv0.axis0.error = odrv0.axis0.error & ~AXIS_ERROR_WATCHDOG_TIMER_EXPIRED
         logger.info("Reset axis0.error from AXIS_ERROR_WATCHDOG_TIMER_EXPIRED to AXIS_ERROR_NONE.")
-    if odrv0.axis1.error & AXIS_ERROR_WATCHDOG_TIMER_EXPIRED != 0:#odrv0.axis1.error == AXIS_ERROR_WATCHDOG_TIMER_EXPIRED:
+    if odrv0.axis1.error & AXIS_ERROR_WATCHDOG_TIMER_EXPIRED != 0:  # odrv0.axis1.error == AXIS_ERROR_WATCHDOG_TIMER_EXPIRED:
         odrv0.axis1.error = odrv0.axis1.error & ~AXIS_ERROR_WATCHDOG_TIMER_EXPIRED
         logger.info("Reset axis1.error from AXIS_ERROR_WATCHDOG_TIMER_EXPIRED to AXIS_ERROR_NONE.")
 
+
 def _system_has_error(odrv):
     return (odrv.error != ODRIVE_ERROR_NONE)
 
+
 def _axis_has_error(axis):
     return (axis.error != AXIS_ERROR_NONE
-     or axis.motor.error != MOTOR_ERROR_NONE
-     or axis.encoder.error != ENCODER_ERROR_NONE
-     or axis.controller.error != CONTROLLER_ERROR_NONE
-     or axis.sensorless_estimator.error != SENSORLESS_ESTIMATOR_ERROR_NONE)
+            or axis.motor.error != MOTOR_ERROR_NONE
+            or axis.encoder.error != ENCODER_ERROR_NONE
+            or axis.controller.error != CONTROLLER_ERROR_NONE
+            or axis.sensorless_estimator.error != SENSORLESS_ESTIMATOR_ERROR_NONE)
+
 
 def _odrv_has_error(odrv):
     if version.parse("0.5.2") <= fw_version:
@@ -203,8 +207,9 @@ def _odrv_has_error(odrv):
     else:
         return _axis_has_error(odrv.axis0) or _axis_has_error(odrv.axis1)
 
-'''Subscriber Routine'''
+
 def MotorTargetRoutine(data):
+    '''Subscriber Routine'''
     global spd0_c
     global spd1_c
     global loopCtrl_on
@@ -218,11 +223,12 @@ def MotorTargetRoutine(data):
         count_motorTarget = 1
     else:
         count_motorTarget += 1
-    #print(spd0_c, spd1_c)
+    # print(spd0_c, spd1_c)
     if leftIs1:
         spd0_c, spd1_c = spd1_c, spd0_c
     lock.release()
 
+
 class OdriveDeviceTask(DiagnosticTask):
     def __init__(self, name):
         super().__init__(name)
@@ -241,7 +247,7 @@ def run(self, stat):
 
             if not is_firmware_supported(odrv0):
                 stat.summary(DiagnosticStatus.ERROR,
-                             "version %d.%d.%d is not matched with required version"%(
+                             "version %d.%d.%d is not matched with required version" % (
                                  odrv0.fw_version_major, odrv0.fw_version_minor, odrv0.fw_version_revision
                                  ))
                 lock.release()
@@ -260,26 +266,27 @@ def run(self, stat):
                 lock.release()
                 return stat
 
-
-            if (odrv0.axis0.encoder.config.use_index and odrv0.axis0.encoder.index_found == False) or \
-               (odrv0.axis1.encoder.config.use_index and odrv0.axis1.encoder.index_found == False):
+            if (odrv0.axis0.encoder.config.use_index and not odrv0.axis0.encoder.index_found) or \
+               (odrv0.axis1.encoder.config.use_index and not odrv0.axis1.encoder.index_found):
                 stat.summary(DiagnosticStatus.ERROR, "Encoder did not found z-index. Please turn the wheels a few times.")
                 lock.release()
                 return stat
 
             stat.summary(DiagnosticStatus.OK,
-                         "version: %d.%d.%d"%(odrv0.fw_version_major, odrv0.fw_version_minor, odrv0.fw_version_revision))
+                         "version: %d.%d.%d" % (odrv0.fw_version_major, odrv0.fw_version_minor, odrv0.fw_version_revision))
             lock.release()
-        except:
+        except:  # noqa: 722
             lock.release()
             pass
         return stat
 
+
 def dumps_errors(stat):
-    anyError=False
-    
+    anyError = False
+
     # Check system wide error
     if version.parse("0.5.2") <= fw_version:
+        errors = []
         if (getattr(odrv0, 'error') != 0):
             anyError = True
             foundError = False
@@ -293,7 +300,7 @@ def dumps_errors(stat):
             stat.add("odrv", ",".join(errors))
         else:
             stat.add("odrv", "no error")
-        
+
     # Check axis error
     axes = [(name, getattr(odrv0, name)) for name in dir(odrv0) if name.startswith('axis')]
     axes.sort()
@@ -325,7 +332,7 @@ def dumps_errors(stat):
             if (remote_obj.error != 0):
                 anyError = True
                 foundError = False
-                errors=[]
+                errors = []
                 errorcodes_tup = [(name, val) for name, val in errorcodes.items() if 'ERROR_' in name]
                 for codename, codeval in errorcodes_tup:
                     if remote_obj.error & codeval != 0:
@@ -338,8 +345,9 @@ def dumps_errors(stat):
                 stat.add(axis_name+"_"+name, "no error")
     return "OK" if not anyError else "Error"
 
+
 class TopicCheckTask(DiagnosticTask):
-    def __init__(self, name, topic, topic_type, callback=lambda x:x):
+    def __init__(self, name, topic, topic_type, callback=lambda x: x):
         DiagnosticTask.__init__(self, name)
         self.sub = node.create_subscription(topic_type, topic, self.topic_callback, 10)
         self.callback = callback
@@ -350,7 +358,7 @@ def topic_callback(self, msg):
         self.topic_count += 1
 
     def run(self, stat):
-        now = node.get_clock().now()
+        # now = node.get_clock().now()
 
         if self.topic_count == 0:
             stat.summary(DiagnosticStatus.ERROR, "not working")
@@ -377,10 +385,12 @@ def _relaunch_odrive():
 def _need_relaunch_error_motor(axis):
     return (axis.motor.error & MOTOR_ERROR_CONTROL_DEADLINE_MISSED) != 0
 
+
 def _need_relaunch_error(odrv):
     return _need_relaunch_error_motor(odrv.axis0) or _need_relaunch_error_motor(odrv.axis1)
 
-def _error_recovery(relaunch = True):
+
+def _error_recovery(relaunch=True):
     if _need_relaunch_error(odrv0):
         _relaunch_odrive()
     else:
@@ -393,8 +403,8 @@ def _error_recovery(relaunch = True):
 logger = None
 
 
-'''Main()'''
 def main():
+    '''Main()'''
 
     pub = node.create_publisher(MotorStatus, 'motorStatus', 10)
 
@@ -423,18 +433,18 @@ def main():
     encoder_bandwidth = node.declare_parameter("encoder_bandwidth", 200).value
     motor_bandwidth = node.declare_parameter("motor_bandwidth", 200).value
 
-    wtimer =node.declare_parameter("wd_timeout", 1.0).value
+    wtimer = node.declare_parameter("wd_timeout", 1.0).value
     wait_first_command = node.declare_parameter("wait_first_command", True).value  # does not set watchdog timer before receiving first motorTarget input.
     reset_watchdog_error = node.declare_parameter("reset_watchdog", True).value  # reset watchdog timeout error at start-up.
     connection_timeout = Duration(seconds=node.declare_parameter("connection_timeout", 5.0).value)
     fw_version_str = node.declare_parameter("fw_version", "").value
 
-    ## Diagnostic Updater
+    # Diagnostic Updater
     updater = Updater(node)
     updater.add(TopicCheckTask("Motor Target", "motorTarget", MotorTarget, MotorTargetRoutine))
     updater.add(OdriveDeviceTask("ODrive"))
 
-    path = node.declare_parameter("path", "").value  #specify path(e.g. usb:0001:0008) from .launch file, but not yet tested _aksg
+    path = node.declare_parameter("path", "").value  # specify path(e.g. usb:0001:0008) from .launch file, but not yet tested _aksg
     find_controller(path, reset_watchdog_error=reset_watchdog_error)
 
     # fuction to convert errorcode to a list of error name
@@ -444,10 +454,10 @@ def errorcode_to_list(error_code, tup):
             if error_code & codeval != 0:
                 error_list.append(codename)
         return error_list
-        
+
     def errorcode_to_list_odrive(error_code):
         return errorcode_to_list(error_code, odrive_error_codes_tup)
-    
+
     def errorcode_to_list_axis(error_code):
         return errorcode_to_list(error_code, axis_error_codes_tup)
 
@@ -474,21 +484,21 @@ def set_config():
             odrv0.axis1.encoder.config.bandwidth = encoder_bandwidth
             odrv0.axis0.motor.config.current_control_bandwidth = motor_bandwidth
             odrv0.axis1.motor.config.current_control_bandwidth = motor_bandwidth
-        except:
+        except:  # noqa: 722
             pass
     set_config()
 
-    last_feed = 0
+#    last_feed = 0
 
     rate = node.create_rate(freq)
     ms = MotorStatus()
 
-    mode_written=None
-    spd0_c_written,spd1_c_written=None,None
+    mode_written = None
+    spd0_c_written, spd1_c_written = None, None
 
     def stop_control():
-        od_writeSpd(0,0)
-        od_writeSpd(1,0)
+        od_writeSpd(0, 0)
+        od_writeSpd(1, 0)
         od_setWatchdogTimer(0)
         od_writeMode(0)
 
@@ -516,7 +526,7 @@ def stop_control():
         try:
             odrv0.axis0
             odrv0.axis1
-        except:
+        except:  # noqa: 722
             # if changes from True to False
             if odrv0_is_active:
                 time_disconnect = node.get_clock().now()
@@ -525,7 +535,7 @@ def stop_control():
 
             # reset written values
             mode_written = None
-            spd0_c_written , spd1_c_written = None , None
+            spd0_c_written, spd1_c_written = None, None
 
             import traceback
             logger.error("Failed to access odrv0 axes.", throttle_duration_sec=5.0)
@@ -539,7 +549,7 @@ def stop_control():
                     stop_control()
                     if reset_watchdog_error:
                         reset_error_watchdog_timer_expired()
-                except:
+                except:  # noqa: 722
                     import traceback
                     logger.error("Failed to reset odrv0 control.")
                     logger.error(traceback.format_exc())
@@ -548,68 +558,68 @@ def stop_control():
                 else:
                     odrv0_is_active = True
 
-        ## read error
-        #getResponse("r axis0.motor.error", "error    0")
-        #getResponse("r axis1.motor.error", "error    1")
+        # read error
+        # getResponse("r axis0.motor.error", "error    0")
+        # getResponse("r axis1.motor.error", "error    1")
 
         # error check
         try:
             if _odrv_has_error(odrv0):
                 if version.parse("0.5.2") <= fw_version:
                     logger.error("odrv0.error=" +
-                        str(errorcode_to_list_odrive(odrv0.error)) +
-                        ", odrv0.axis0.error=" +
-                        str(errorcode_to_list_axis(odrv0.axis0.error)) +
-                        ", odrv0.axis0.motor.error=" + 
-                        str(errorcode_to_list_motor(odrv0.axis0.motor.error)) +
-                        ", odrv0.axis0.controller.error=" + 
-                        str(errorcode_to_list_controller(odrv0.axis0.controller.error)) +
-                        ", odrv0.axis0.encoder.error=" + 
-                        str(errorcode_to_list_encoder(odrv0.axis0.encoder.error)) +
-                        ", odrv0.axis0.sensorless_estimator.error=" + 
-                        str(errorcode_to_list_sensorless_estimator(odrv0.axis0.sensorless_estimator.error)) +
-                        ", odrv0.axis1.error=" + 
-                        str(errorcode_to_list_axis(odrv0.axis1.error)) + 
-                        ", odrv0.axis1.motor.error=" + 
-                        str(errorcode_to_list_motor(odrv0.axis1.motor.error)) +
-                        ", odrv0.axis1.controller.error=" + 
-                        str(errorcode_to_list_controller(odrv0.axis1.controller.error)) +
-                        ", odrv0.axis1.encoder.error=" + 
-                        str(errorcode_to_list_encoder(odrv0.axis1.encoder.error)) +
-                        ", odrv0.axis1.sensorless_estimator.error=" + 
-                        str(errorcode_to_list_sensorless_estimator(odrv0.axis1.sensorless_estimator.error)),
-                        throttle_duration_sec=5.0)
+                                 str(errorcode_to_list_odrive(odrv0.error)) +
+                                 ", odrv0.axis0.error=" +
+                                 str(errorcode_to_list_axis(odrv0.axis0.error)) +
+                                 ", odrv0.axis0.motor.error=" +
+                                 str(errorcode_to_list_motor(odrv0.axis0.motor.error)) +
+                                 ", odrv0.axis0.controller.error=" +
+                                 str(errorcode_to_list_controller(odrv0.axis0.controller.error)) +
+                                 ", odrv0.axis0.encoder.error=" +
+                                 str(errorcode_to_list_encoder(odrv0.axis0.encoder.error)) +
+                                 ", odrv0.axis0.sensorless_estimator.error=" +
+                                 str(errorcode_to_list_sensorless_estimator(odrv0.axis0.sensorless_estimator.error)) +
+                                 ", odrv0.axis1.error=" +
+                                 str(errorcode_to_list_axis(odrv0.axis1.error)) +
+                                 ", odrv0.axis1.motor.error=" +
+                                 str(errorcode_to_list_motor(odrv0.axis1.motor.error)) +
+                                 ", odrv0.axis1.controller.error=" +
+                                 str(errorcode_to_list_controller(odrv0.axis1.controller.error)) +
+                                 ", odrv0.axis1.encoder.error=" +
+                                 str(errorcode_to_list_encoder(odrv0.axis1.encoder.error)) +
+                                 ", odrv0.axis1.sensorless_estimator.error=" +
+                                 str(errorcode_to_list_sensorless_estimator(odrv0.axis1.sensorless_estimator.error)),
+                                 throttle_duration_sec=5.0)
                 else:
                     logger.error("odrv0.axis0.error=" +
-                        str(errorcode_to_list_axis(odrv0.axis0.error)) +
-                        ", odrv0.axis0.motor.error=" + 
-                        str(errorcode_to_list_motor(odrv0.axis0.motor.error)) +
-                        ", odrv0.axis0.controller.error=" + 
-                        str(errorcode_to_list_controller(odrv0.axis0.controller.error)) +
-                        ", odrv0.axis0.encoder.error=" + 
-                        str(errorcode_to_list_encoder(odrv0.axis0.encoder.error)) +
-                        ", odrv0.axis0.sensorless_estimator.error=" + 
-                        str(errorcode_to_list_sensorless_estimator(odrv0.axis0.sensorless_estimator.error)) +
-                        ", odrv0.axis1.error=" + 
-                        str(errorcode_to_list_axis(odrv0.axis1.error)) + 
-                        ", odrv0.axis1.motor.error=" + 
-                        str(errorcode_to_list_motor(odrv0.axis1.motor.error)) +
-                        ", odrv0.axis1.controller.error=" + 
-                        str(errorcode_to_list_controller(odrv0.axis1.controller.error)) +
-                        ", odrv0.axis1.encoder.error=" + 
-                        str(errorcode_to_list_encoder(odrv0.axis1.encoder.error)) +
-                        ", odrv0.axis1.sensorless_estimator.error=" + 
-                        str(errorcode_to_list_sensorless_estimator(odrv0.axis1.sensorless_estimator.error)),
-                        throttle_duration_sec=5.0)
+                                 str(errorcode_to_list_axis(odrv0.axis0.error)) +
+                                 ", odrv0.axis0.motor.error=" +
+                                 str(errorcode_to_list_motor(odrv0.axis0.motor.error)) +
+                                 ", odrv0.axis0.controller.error=" +
+                                 str(errorcode_to_list_controller(odrv0.axis0.controller.error)) +
+                                 ", odrv0.axis0.encoder.error=" +
+                                 str(errorcode_to_list_encoder(odrv0.axis0.encoder.error)) +
+                                 ", odrv0.axis0.sensorless_estimator.error=" +
+                                 str(errorcode_to_list_sensorless_estimator(odrv0.axis0.sensorless_estimator.error)) +
+                                 ", odrv0.axis1.error=" +
+                                 str(errorcode_to_list_axis(odrv0.axis1.error)) +
+                                 ", odrv0.axis1.motor.error=" +
+                                 str(errorcode_to_list_motor(odrv0.axis1.motor.error)) +
+                                 ", odrv0.axis1.controller.error=" +
+                                 str(errorcode_to_list_controller(odrv0.axis1.controller.error)) +
+                                 ", odrv0.axis1.encoder.error=" +
+                                 str(errorcode_to_list_encoder(odrv0.axis1.encoder.error)) +
+                                 ", odrv0.axis1.sensorless_estimator.error=" +
+                                 str(errorcode_to_list_sensorless_estimator(odrv0.axis1.sensorless_estimator.error)),
+                                 throttle_duration_sec=5.0)
                 logger.warn("Odrive error. trying recovery" + ("(relaunch)..." if odrv0_is_active else "..."))
-                _error_recovery(relaunch = odrv0_is_active)
+                _error_recovery(relaunch=odrv0_is_active)
                 time_disconnect = node.get_clock().now()
                 odrv0_is_active = False
                 mode_written = None
-                spd0_c_written , spd1_c_written = None , None
+                spd0_c_written, spd1_c_written = None, None
                 rate.sleep()
                 continue
-        except:
+        except:  # noqa: 722
             import traceback
             exception_string = traceback.format_exc()
             logger.error(exception_string)
@@ -620,22 +630,25 @@ def stop_control():
         global lock
         lock.acquire()
         try:
-            if(mode_written!=loopCtrl_on):
-                if PRINTDEBUG: print('w m ', loopCtrl_on)
+            if (mode_written != loopCtrl_on):
+                if PRINTDEBUG:
+                    print('w m ', loopCtrl_on)
                 if od_writeMode(loopCtrl_on):
-                    mode_written=loopCtrl_on
-
-            if(spd0_c_written!=spd0_c):
-                if PRINTDEBUG: print('w 0 {:0.2f}'.format(spd0_c))
-                if od_writeSpd(0,spd0_c):
-                    spd0_c_written=spd0_c
-
-            if(spd1_c_written!=spd1_c):
-                if PRINTDEBUG: print('w 1 {:0.2f}'.format(spd1_c))
-                if od_writeSpd(1,spd1_c):
-                    spd1_c_written=spd1_c
+                    mode_written = loopCtrl_on
+
+            if (spd0_c_written != spd0_c):
+                if PRINTDEBUG:
+                    print('w 0 {:0.2f}'.format(spd0_c))
+                if od_writeSpd(0, spd0_c):
+                    spd0_c_written = spd0_c
+
+            if (spd1_c_written != spd1_c):
+                if PRINTDEBUG:
+                    print('w 1 {:0.2f}'.format(spd1_c))
+                if od_writeSpd(1, spd1_c):
+                    spd1_c_written = spd1_c
             lock.release()
-        except:
+        except:  # noqa: 722
             lock.release()
             import traceback
             exception_string = traceback.format_exc()
@@ -659,7 +672,7 @@ def stop_control():
                     # call watchdog_feed only when motorTarget is being updated. odrive motors stop when motorTarget update stops.
                     od_feedWatchdogTimer()
                     previous_count_motorTarget = count_motorTarget
-        except:
+        except:  # noqa: 722
             import traceback
             exception_string = traceback.format_exc()
             logger.error("Failed to set watchdog timer")
@@ -678,19 +691,18 @@ def stop_control():
 #                last_feed = nw
 #                od_feedWatchdogTimer()
 
-
         enc0 = enc1 = spd0 = spd1 = None
         current_setpoint_0 = current_setpoint_1 = None
         current_measured_0 = current_measured_1 = None
         # update encoder counts and speed
         try:
-            enc0, spd0 = odrv0.axis0.encoder.pos_estimate, odrv0.axis0.encoder.vel_estimate#getFloats(getResponse("f 0"))
-            enc1, spd1 = odrv0.axis1.encoder.pos_estimate, odrv0.axis1.encoder.vel_estimate#getFloats(getResponse("f 1"))
+            enc0, spd0 = odrv0.axis0.encoder.pos_estimate, odrv0.axis0.encoder.vel_estimate  # getFloats(getResponse("f 0"))
+            enc1, spd1 = odrv0.axis1.encoder.pos_estimate, odrv0.axis1.encoder.vel_estimate  # getFloats(getResponse("f 1"))
             current_setpoint_0 = odrv0.axis0.motor.current_control.Iq_setpoint
             current_setpoint_1 = odrv0.axis1.motor.current_control.Iq_setpoint
             current_measured_0 = odrv0.axis0.motor.current_control.Iq_measured
             current_measured_1 = odrv0.axis1.motor.current_control.Iq_measured
-        except:
+        except:  # noqa: 722
             print("Reading TRY failed!")
             rate.sleep()
             import traceback
@@ -703,77 +715,81 @@ def stop_control():
             ms.header.stamp = node.get_clock().now().to_msg()
 
             if leftIs1:
-                ms.dist_left_c  = enc1
+                ms.dist_left_c = enc1
                 ms.dist_right_c = enc0
-                ms.spd_left_c   = spd1
-                ms.spd_right_c  = spd0
+                ms.spd_left_c = spd1
+                ms.spd_right_c = spd0
 
                 ms.current_setpoint_left = current_setpoint_1 * signLeft
                 ms.current_setpoint_right = current_setpoint_0 * signRight
                 ms.current_measured_left = current_measured_1 * signLeft
                 ms.current_measured_right = current_measured_0 * signRight
             else:
-                ms.dist_left_c  = enc0
+                ms.dist_left_c = enc0
                 ms.dist_right_c = enc1
-                ms.spd_left_c   = spd0
-                ms.spd_right_c  = spd1
+                ms.spd_left_c = spd0
+                ms.spd_right_c = spd1
 
                 ms.current_setpoint_left = current_setpoint_0 * signLeft
                 ms.current_setpoint_right = current_setpoint_1 * signRight
                 ms.current_measured_left = current_measured_0 * signLeft
                 ms.current_measured_right = current_measured_1 * signRight
 
-            ms.dist_left_c  *= signLeft / gainLeft
+            ms.dist_left_c *= signLeft / gainLeft
             ms.dist_right_c *= signRight / gainRight
-            ms.spd_left_c  *= signLeft / gainLeft
+            ms.spd_left_c *= signLeft / gainLeft
             ms.spd_right_c *= signRight / gainRight
 
-            ms.dist_left  = ms.dist_left_c  * meter_per_round
+            ms.dist_left = ms.dist_left_c * meter_per_round
             ms.dist_right = ms.dist_right_c * meter_per_round
-            ms.spd_left  = ms.spd_left_c  * meter_per_round
+            ms.spd_left = ms.spd_left_c * meter_per_round
             ms.spd_right = ms.spd_right_c * meter_per_round
             pub.publish(ms)
 
         rate.sleep()
 
     # stop motor before exit
-    od_writeSpd(0,0)
-    od_writeSpd(1,0)
+    od_writeSpd(0, 0)
+    od_writeSpd(1, 0)
     od_setWatchdogTimer(0)
     od_writeMode(0)
-    #print "shutdown and reboot"
-    #od_reboot()
+    # print "shutdown and reboot"
+    # od_reboot()
+
 
 def getFloats(text):
     if text is None:
         return None, None
     items = text.split(" ")
     if len(items) < 2:
-        print("Illegal input:'%s'"%(text))
+        print("Illegal input:'%s'" % (text))
         return None, None
     try:
         return float(items[0]), float(items[1])
-    except:
-        print("Illegal input:'%s'"%(text))
+    except:  # noqa: 722
+        print("Illegal input:'%s'" % (text))
         return None, None
 
+
 def od_reboot():
-    #od.write('sb\n')##assuming "sr\n" instead here.. https://docs.odriverobotics.com/ascii-protocol.html
+    # od.write('sb\n')##assuming "sr\n" instead here.. https://docs.odriverobotics.com/ascii-protocol.html
     odrv0.reboot()
 
+
 def od_writeSpd(ch, spd):
     try:
         ctrl = None
-        if(ch==0):
+        if (ch == 0):
             ctrl = odrv0.axis0.controller
         else:
             ctrl = odrv0.axis1.controller
         ctrl.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
         ctrl.input_vel = spd
         return 1
-    except:
+    except:  # noqa: 722
         raise
 
+
 def od_setWatchdogTimer(sec):
     if 0 < sec:
         # store previous watchdog_timeout values to use them later
@@ -799,22 +815,19 @@ def od_setWatchdogTimer(sec):
         odrv0.axis0.config.watchdog_timeout = sec
         odrv0.axis1.config.watchdog_timeout = sec
 
+
 def od_feedWatchdogTimer():
     odrv0.axis0.watchdog_feed()
     odrv0.axis1.watchdog_feed()
 
-def od_writeSpd_obsolete(ch,spd):
-    spd_s=str(spd)[:7]
-    ch=int(ch!=0)
-    cmd='v ' +str(ch)+' '+spd_s
-    result = od_write(cmd)
-    if PRINTDEBUG: print( cmd, result)
-    return result
+
+selectMode = 1
+
 
 def od_writeMode(loopCtrl_on):
     global selectMode
     try:
-        if (loopCtrl_on==1):
+        if (loopCtrl_on == 1):
             odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
             odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
             selectMode = loopCtrl_on
@@ -824,17 +837,9 @@ def od_writeMode(loopCtrl_on):
             odrv0.axis1.requested_state = AXIS_STATE_IDLE
             selectMode = loopCtrl_on
             return 1
-    except:
+    except:  # noqa: 722
         raise
 
-# For more information about the number in command:
-# https://github.com/madcowswe/ODrive/blob/master/tools/odrive/enums.py OR contact EagleZ
-def od_writeMode_obsolete(loopCtrl_on):
-    if (loopCtrl_on==1):
-        result = (od_write('w axis0.requested_state 8') and od_write('w axis1.requested_state 8'))
-    else:
-        result = (od_write('w axis0.requested_state 1') and od_write('w axis1.requested_state 1'))
-    return result
 
 def checksum(st):
     cs = 0
@@ -842,56 +847,15 @@ def checksum(st):
         cs = cs ^ ord(c)
     return cs & 0xff
 
-def od_write_obsolete(st): # To be improved
-    if use_checksum:
-        cmd = "{}*{}\n".format(st, checksum(st))
-    else:
-        cmd = "{}\n".format(st)
-    length = len(cmd)
-    try:
-        if PRINTDEBUG: print (cmd.rstrip())
-        if (odrv0.write(cmd)==length):
-            return 1
-    except:
-        print ("FAILED!!!! "+str(cmd))
-    return 0
-
-def getResponse(st, comment=None): # To be improved
-    result = od_write(st)
-
-    time.sleep(pause_between_commands)
-    line=odrv0.readline().rstrip()
-    if use_checksum:
-        if not "*" in line:
-            print ("Response without checksum '{}'".format(line))
-            return None
-
-        print (line)
-        res, cs = line.split("*")
-        try:
-            if checksum(res) != int(cs):
-                print ("checksum is different {} != {}".format(checksum(res), int(cs)))
-                return None
-        except:
-            print ("checksum is not int")
-            return None
-    else:
-        if PRINTDEBUG: print (line)
-        res = line
-
-    if comment:
-        print ("{}: {:20s} - written[{}]: {}, ".format(comment, res, result, st))
-    return res
-
 
 def sigint_hook(signal_num, frame):
     print(F"shutdown {signal_num} {frame}")
     try:
-        od_writeSpd(0,0)
-        od_writeSpd(1,0)
+        od_writeSpd(0, 0)
+        od_writeSpd(1, 0)
         od_setWatchdogTimer(0)
         od_writeMode(0)
-    except:
+    except:  # noqa: 722
         pass
 
     # workaround with ODrive find_any
@@ -904,6 +868,7 @@ def sigint_hook(signal_num, frame):
         print(thread.name, thread.daemon)
     sys.exit(0)
 
+
 signal.signal(signal.SIGINT, sigint_hook)
 
 
@@ -922,4 +887,3 @@ def sigint_hook(signal_num, frame):
         pass
     while selectMode == 1:
         time.sleep(1)
-
diff --git a/motor_controller/odriver/script/odriver_s1_node.py b/motor_controller/odriver/script/odriver_s1_node.py
index 3d2c9afb..31523f4b 100755
--- a/motor_controller/odriver/script/odriver_s1_node.py
+++ b/motor_controller/odriver/script/odriver_s1_node.py
@@ -1,57 +1,46 @@
 #!/usr/bin/env python3
 
- ###############################################################################
- # Copyright (c) 2019  Carnegie Mellon University
- #
- # Permission is hereby granted, free of charge, to any person obtaining a copy
- # of this software and associated documentation files (the "Software"), to deal
- # in the Software without restriction, including without limitation the rights
- # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- # copies of the Software, and to permit persons to whom the Software is
- # furnished to do so, subject to the following conditions:
- #
- # The above copyright notice and this permission notice shall be included in
- # all copies or substantial portions of the Software.
- #
- # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- # THE SOFTWARE.
- ###############################################################################
-
-
-'''ROS Imports'''
-from odrive.pyfibre import fibre
-import signal
-import sys
+###############################################################################
+# Copyright (c) 2019  Carnegie Mellon University
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+###############################################################################
+
+
+# ROS Imports
 import rclpy
 from rclpy.duration import Duration
-from rclpy.exceptions import ROSInterruptException, InvalidServiceNameException
+from rclpy.exceptions import ROSInterruptException
 import time
 import logging
 import traceback
 from odriver_msgs.msg import MotorStatus
 from odriver_msgs.msg import MotorTarget
-from std_msgs.msg import Header
 
 
-#import serial
+# import serial
 import odrive
-from odrive.utils import dump_errors, format_errors
+from odrive.utils import format_errors
 
-from odrive.enums import ODriveError, AxisState, ControlMode, ProcedureResult, ComponentStatus
+from odrive.enums import ODriveError, AxisState, ControlMode
+from odrive.enums import MOTOR_ERROR_CONTROL_DEADLINE_MISSED
 import odrive.enums
-odrive_error_codes_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ODRIVE_ERROR_" in name]
-axis_error_codes_tup =  [(name, value) for name, value in odrive.enums.__dict__.items() if "AXIS_ERROR_" in name]
-motor_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "MOTOR_ERROR_" in name]
-controller_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "CONTROLLER_ERROR_" in name]
-encoder_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ENCODER_ERROR_" in name]
-sensorless_estimator_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "SENSORLESS_ESTIMATOR_ERROR_" in name]
-
-import time
 import numpy as np
 import threading
 from packaging import version
@@ -65,25 +54,31 @@
 from rcl_interfaces.msg import SetParametersResult
 from rclpy.parameter import Parameter
 
+odrive_error_codes_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ODRIVE_ERROR_" in name]
+axis_error_codes_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "AXIS_ERROR_" in name]
+motor_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "MOTOR_ERROR_" in name]
+controller_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "CONTROLLER_ERROR_" in name]
+encoder_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "ENCODER_ERROR_" in name]
+sensorless_estimator_error_code_tup = [(name, value) for name, value in odrive.enums.__dict__.items() if "SENSORLESS_ESTIMATOR_ERROR_" in name]
 
-PRINTDEBUG=False
+PRINTDEBUG = False
 
-ODRIVE_VERSIONS=[[0,6,5],[0,6,6],[0,6,8],[0,6,9]]
+ODRIVE_VERSIONS = [[0, 6, 5], [0, 6, 6], [0, 6, 8], [0, 6, 9]]
 
 '''Parameter'''
-freq = 40 #Hz
-pause_between_commands = 0.001 #sec
-serialReading_timeout=0.01 #sec
-serialWriting_timeout=0.01 #sec
-lock=threading.Lock()
-use_checksum=False
+freq = 40  # Hz
+pause_between_commands = 0.001  # sec
+serialReading_timeout = 0.01  # sec
+serialWriting_timeout = 0.01  # sec
+lock = threading.Lock()
+use_checksum = False
 max_vel_gain = 20.0
 max_vel_integrator_gain = 100.0
 
 '''Configuarable parameter'''
 meter_per_count = None
-leftIs1 = False # left is axis0, right is axis1
-isClockwise = True # set true if sign = 1 corresponds to the clockwise direction. set false if sign = 1 corresponds to the counter-clockwise direction.
+leftIs1 = False  # left is axis0, right is axis1
+isClockwise = True  # set true if sign = 1 corresponds to the clockwise direction. set false if sign = 1 corresponds to the counter-clockwise direction.
 signLeft = -1.0
 signRight = 1.0
 gainLeft = 1.0
@@ -107,20 +102,21 @@
 
 
 def is_firmware_equal(odrv, od_version):
-    return  (odrv.fw_version_major == od_version[0] and \
-            odrv.fw_version_minor == od_version[1] and \
+    return (odrv.fw_version_major == od_version[0] and
+            odrv.fw_version_minor == od_version[1] and
             odrv.fw_version_revision == od_version[2])
 
 
 def is_firmware_supported(odrv):
-    return any((is_firmware_equal(odrv,x) for x in ODRIVE_VERSIONS))
+    return any((is_firmware_equal(odrv, x) for x in ODRIVE_VERSIONS))
+
 
 def clear_errors(odrv):
     global fw_version
-    
+
     if version.parse("0.5.2") <= fw_version:
         odrv.clear_errors()
-    else: # fw_version <= 0.5.1
+    else:  # fw_version <= 0.5.1
         # The following try block throws an error when an odrv object returns a wrong version number due to a bug related to firmware.
         try:
             odrvs[0].axis0.clear_errors()
@@ -133,7 +129,6 @@ def clear_errors(odrv):
 logger = None
 
 
-
 def find_controller(odrv_index, odrv_serial_number, clear=False, reset_watchdog_error=False):
     '''Hardware Initialization'''
     global odrvs, odrv_is_not_found, version_mismatched, fw_version, channel_termination_token
@@ -149,15 +144,15 @@ def find_controller(odrv_index, odrv_serial_number, clear=False, reset_watchdog_
             logger.info("Finding Odrive controller... (Serial Number: " + str(odrv_serial_number)+")")
             logging.basicConfig(level=logging.DEBUG)
             odrvs[odrv_index] = odrive.find_any(timeout=5, serial_number=odrv_serial_number)
-        except:
+        except:  # noqa: 722
             logger.error(traceback.format_exc())
-            logger.error("Check Odrive connection: " + str(odrv_serial_number)+ " (Serial Number) doesn't exist! ")
+            logger.error("Check Odrive connection: " + str(odrv_serial_number) + " (Serial Number) doesn't exist! ")
             time.sleep(1)
             continue
         else:
             if odrvs[odrv_index] is None:
                 return
-    
+
     odrv_is_not_found[odrv_index] = False
     version_mismatched = False
     use_index = False
@@ -167,21 +162,21 @@ def find_controller(odrv_index, odrv_serial_number, clear=False, reset_watchdog_
         logger.error("Odriver version is mismatched!")
         version_mismatched = True
         return
-    
+
     if odrvs[odrv_index].axis0.commutation_mapper.config.use_index_gpio:
         use_index = True
     if odrvs[odrv_index].axis0.commutation_mapper.config.index_gpio != 10:
         index_not_found = True
     if use_index and index_not_found:
         return
-    
+
     if fw_version_str != "":
         fw_version = version.parse(fw_version_str)
     else:
-        fw_version = version.parse(".".join(map(str,[odrvs[odrv_index].fw_version_major, odrvs[odrv_index].fw_version_minor, odrvs[odrv_index].fw_version_revision])))
+        fw_version = version.parse(".".join(map(str, [odrvs[odrv_index].fw_version_major, odrvs[odrv_index].fw_version_minor, odrvs[odrv_index].fw_version_revision])))
 
     clear_errors(odrvs[odrv_index])
-    logger.info("Connected to Odrive S1 " + str(odrv_serial_number) + " as odrv" + str(odrv_index))    
+    logger.info("Connected to Odrive S1 " + str(odrv_serial_number) + " as odrv" + str(odrv_index))
 
     od_setWatchdogTimer(0, odrv_index)
     # if an axis is stopped by watchdog timeout, reset the error status.
@@ -192,23 +187,24 @@ def find_controller(odrv_index, odrv_serial_number, clear=False, reset_watchdog_
 def reset_error_watchdog_timer_expired(odrv_index):
     # clear errors only if error exactly match with ODriveError.WATCHDOG_TIMER_EXPIRED
     if odrvs[odrv_index].axis0.active_errors == ODriveError.WATCHDOG_TIMER_EXPIRED or \
-         odrvs[odrv_index].axis0.disarm_reason == ODriveError.WATCHDOG_TIMER_EXPIRED:
+            odrvs[odrv_index].axis0.disarm_reason == ODriveError.WATCHDOG_TIMER_EXPIRED:
         clear_errors(odrvs[odrv_index])
         logger.info("Clear odrv"+str(odrv_index)+".axis0 errors in reset_error_watchdog_timer_expired.")
     elif odrvs[odrv_index].axis0.active_errors == ODriveError.NONE and \
-         odrvs[odrv_index].axis0.disarm_reason == ODriveError.NONE:
-         pass # pass if no error
+            odrvs[odrv_index].axis0.disarm_reason == ODriveError.NONE:
+        pass  # pass if no error
     else:
         logger.info("odrv"+str(odrv_index)+" watchdog error was not cleared.")
 
 
 def _odrv_has_error(odrv):
     return (odrv.axis0.active_errors != ODriveError.NONE
-      or odrv.axis0.disarm_reason != ODriveError.NONE)
+            or odrv.axis0.disarm_reason != ODriveError.NONE)
     #  or odrv.spi_encoder0.status != ComponentStatus.NOMINAL)
 
-'''Subscriber Routine'''
+
 def MotorTargetRoutine(data):
+    '''Subscriber Routine'''
     global spd0_c
     global spd1_c
     global loopCtrl_on
@@ -226,6 +222,7 @@ def MotorTargetRoutine(data):
         spd0_c, spd1_c = spd1_c, spd0_c
     lock.release()
 
+
 class OdriveDeviceTask(DiagnosticTask):
     def __init__(self, name):
         super().__init__(name)
@@ -244,7 +241,7 @@ def run(self, stat):
 
             if not is_firmware_supported(odrvs[0]):
                 stat.summary(DiagnosticStatus.ERROR,
-                             "version %d.%d.%d is not matched with required version"%(
+                             "version %d.%d.%d is not matched with required version" % (
                                  odrvs[0].fw_version_major, odrvs[0].fw_version_minor, odrvs[0].fw_version_revision
                                  ))
                 lock.release()
@@ -272,10 +269,10 @@ def run(self, stat):
                 return stat
 
             stat.summary(DiagnosticStatus.OK,
-                         "version: %d.%d.%d"%(odrvs[0].fw_version_major, odrvs[0].fw_version_minor, odrvs[0].fw_version_revision))
-            
+                         "version: %d.%d.%d" % (odrvs[0].fw_version_major, odrvs[0].fw_version_minor, odrvs[0].fw_version_revision))
+
             lock.release()
-        except:
+        except:  # noqa: 722
             logger.error("Error in OdriveDeviceTask.", throttle_duration_sec=5.0)
             lock.release()
 
@@ -283,7 +280,7 @@ def run(self, stat):
 
 
 class TopicCheckTask(DiagnosticTask):
-    def __init__(self, name, topic, topic_type, callback=lambda x:x):
+    def __init__(self, name, topic, topic_type, callback=lambda x: x):
         DiagnosticTask.__init__(self, name)
         self.sub = node.create_subscription(topic_type, topic, self.topic_callback, 10)
         self.callback = callback
@@ -294,7 +291,7 @@ def topic_callback(self, msg):
         self.topic_count += 1
 
     def run(self, stat):
-        now = node.get_clock().now()
+        # now = node.get_clock().now()
 
         if self.topic_count == 0:
             stat.summary(DiagnosticStatus.ERROR, "not working")
@@ -321,18 +318,21 @@ def _relaunch_odrive():
 def _need_relaunch_error_motor(axis):
     return (axis.motor.error & MOTOR_ERROR_CONTROL_DEADLINE_MISSED) != 0
 
+
 def _need_relaunch_error(odrv):
     return _need_relaunch_error_motor(odrv.axis0)
 
-def _error_recovery(relaunch = True):
+
+def _error_recovery(relaunch=True):
     if _need_relaunch_error(odrvs[0]) or _need_relaunch_error(odrvs[1]):
         _relaunch_odrive()
     else:
         clear_errors(odrvs[0])
         clear_errors(odrvs[1])
-        if (_odrv_has_error(odrvs[0]) and relaunch) or (_odrv_has_error(odrvs[1]) and relaunch) :
+        if (_odrv_has_error(odrvs[0]) and relaunch) or (_odrv_has_error(odrvs[1]) and relaunch):
             _relaunch_odrive()
 
+
 # Refer to https://roboticsbackend.com/ros2-rclpy-parameter-callback/
 def parameters_callback(params):
     global vel_gain, vel_integrator_gain
@@ -343,21 +343,21 @@ def parameters_callback(params):
         if param.name == "vel_gain":
             if param.type_ in [Parameter.Type.DOUBLE, Parameter.Type.INTEGER]:
                 if param.value >= 0.0 and param.value < max_vel_gain:
-                        vel_gain = param.value
-                        logger.info(f"Set vel_gain: {vel_gain}")
-                        success = True
+                    vel_gain = param.value
+                    logger.info(f"Set vel_gain: {vel_gain}")
+                    success = True
         if param.name == "vel_integrator_gain":
             if param.type_ in [Parameter.Type.DOUBLE, Parameter.Type.INTEGER]:
                 if param.value >= 0.0 and param.value < max_vel_integrator_gain:
-                        vel_integrator_gain = param.value
-                        logger.info(f"Set vel_integrator_gain: {vel_integrator_gain}")
-                        success = True
+                    vel_integrator_gain = param.value
+                    logger.info(f"Set vel_integrator_gain: {vel_integrator_gain}")
+                    success = True
     lock.release()
     return SetParametersResult(successful=success)
 
 
-'''Main()'''
 def main():
+    '''Main()'''
     pub = node.create_publisher(MotorStatus, 'motorStatus', 10)
     rate = node.create_rate(freq)
 
@@ -387,27 +387,27 @@ def main():
     encoder_bandwidth = node.declare_parameter("encoder_bandwidth", 200).value
     motor_bandwidth = node.declare_parameter("motor_bandwidth", 200).value
 
-    wtimer =node.declare_parameter("wd_timeout", 1.0).value
+    wtimer = node.declare_parameter("wd_timeout", 1.0).value
     wait_first_command = node.declare_parameter("wait_first_command", True).value  # does not set watchdog timer before receiving first motorTarget input.
     reset_watchdog_error = node.declare_parameter("reset_watchdog", True).value  # reset watchdog timeout error at start-up.
     connection_timeout = Duration(seconds=node.declare_parameter("connection_timeout", 5.0).value)
     fw_version_str = node.declare_parameter("fw_version", "").value
-    
+
     odrive_left_serial_str = None
     odrive_right_serial_str = None
     try:
         odrive_left_serial_str = node.declare_parameter("odrive_left_serial_number", rclpy.Parameter.Type.STRING).value
-    except:
+    except:  # noqa: 722
         odrive_left_serial_str = str(node.declare_parameter("odrive_left_serial_number", rclpy.Parameter.Type.INTEGER).value)
     try:
         odrive_right_serial_str = node.declare_parameter("odrive_right_serial_number", rclpy.Parameter.Type.STRING).value
-    except:
+    except:  # noqa: 722
         odrive_right_serial_str = str(node.declare_parameter("odrive_right_serial_number", rclpy.Parameter.Type.INTEGER).value)
     if odrive_left_serial_str == "" or odrive_right_serial_str == "":
         logger.error("CABOT_ODRIVER_SERIAL_0 and 1 should be specified in .env file!!")
         return
 
-    ## Diagnostic Updater
+    # Diagnostic Updater
     updater = Updater(node)
     updater.add(TopicCheckTask("Motor Target", "/cabot/motorTarget", MotorTarget, MotorTargetRoutine))
     updater.add(OdriveDeviceTask("ODrive"))
@@ -423,26 +423,26 @@ def set_config(odrv_index):
             odrvs[odrv_index].axis0.config.encoder_bandwidth = encoder_bandwidth
             odrvs[odrv_index].axis0.config.motor.current_control_bandwidth = motor_bandwidth
             return 1
-        except:
+        except:  # noqa: 722
             logger.error("Can not set motor configuration!!")
             return 0
     set_config(0)
     set_config(1)
 
-    last_feed = 0
+    # last_feed = 0
 
     rate = node.create_rate(freq)
     ms = MotorStatus()
 
-    mode_written=None
-    spd0_c_written,spd1_c_written=None,None
-    vel_gain_written,vel_integrator_gain_written=None,None
+    mode_written = None
+    spd0_c_written, spd1_c_written = None, None
+    vel_gain_written, vel_integrator_gain_written = None, None
 
     def stop_control():
-        od_writeSpd(0,0)
-        od_writeSpd(1,0)
-        od_setWatchdogTimer(0,0)
-        od_setWatchdogTimer(0,1)
+        od_writeSpd(0, 0)
+        od_writeSpd(1, 0)
+        od_setWatchdogTimer(0, 0)
+        od_setWatchdogTimer(0, 1)
         od_writeMode(0)
 
     # check param of vel_gain and vel_integratior_gain
@@ -472,8 +472,8 @@ def stop_control():
         # check odrv remote object
         try:
             odrvs[0].axis0
-            odrvs[1].axis0 
-        except:
+            odrvs[1].axis0
+        except:  # noqa: 722
             # if changes from True to False
             if odrv_is_active:
                 time_disconnect = node.get_clock().now()
@@ -482,7 +482,7 @@ def stop_control():
 
             # reset written values
             mode_written = None
-            spd0_c_written , spd1_c_written = None , None
+            spd0_c_written, spd1_c_written = None, None
 
             import traceback
             logger.error("Failed to access odrv axes.", throttle_duration_sec=5.0)
@@ -499,7 +499,7 @@ def stop_control():
                         reset_error_watchdog_timer_expired(0)
                         reset_error_watchdog_timer_expired(1)
                         rate.sleep()
-                except:
+                except:  # noqa: 722
                     import traceback
                     logger.error("Failed to reset odrv control.")
                     logger.error(traceback.format_exc())
@@ -509,52 +509,54 @@ def stop_control():
                     odrv_is_active = True
 
         # error check
-        try: 
+        try:
             if _odrv_has_error(odrvs[0]) or _odrv_has_error(odrvs[1]):
                 odrive_error_str = \
                     "\nErrors of odrv0 \n" + str(format_errors(odrvs[0])) + \
-                    "\nErrors of odrv1 \n" + str(format_errors(odrvs[1])) 
+                    "\nErrors of odrv1 \n" + str(format_errors(odrvs[1]))
                 logger.error(odrive_error_str, throttle_duration_sec=1.0)
                 time_disconnect = node.get_clock().now()
                 odrv_is_active = False
                 od_writeMode(0)
                 mode_written = None
-                spd0_c_written , spd1_c_written = None , None
+                spd0_c_written, spd1_c_written = None, None
                 rate.sleep()
                 continue
-        except:
+        except:  # noqa: 722
             import traceback
             exception_string = traceback.format_exc()
             logger.error(exception_string)
             rate.sleep()
             continue
 
-
         # send new velocity command
         global lock
         lock.acquire()
         try:
-            if(mode_written!=loopCtrl_on):
-
-                if PRINTDEBUG: print('w m ', loopCtrl_on)
+            if (mode_written != loopCtrl_on):
+                if PRINTDEBUG:
+                    print('w m ', loopCtrl_on)
                 if od_writeMode(loopCtrl_on):
-                    mode_written=loopCtrl_on
-
-            if(spd0_c_written!=spd0_c):
-                if PRINTDEBUG: print('w 0 {:0.2f}'.format(spd0_c))
-                if od_writeSpd(0,spd0_c):
-                    spd0_c_written=spd0_c
-
-            if(spd1_c_written!=spd1_c):
-                if PRINTDEBUG: print('w 1 {:0.2f}'.format(spd1_c))
-                if od_writeSpd(1,spd1_c):
-                    spd1_c_written=spd1_c
-            if(vel_gain_written!=vel_gain or vel_integrator_gain_written!=vel_integrator_gain):
+                    mode_written = loopCtrl_on
+
+            if(spd0_c_written != spd0_c):
+                if PRINTDEBUG:
+                    print('w 0 {:0.2f}'.format(spd0_c))
+                if od_writeSpd(0, spd0_c):
+                    spd0_c_written = spd0_c
+
+            if(spd1_c_written != spd1_c):
+                if PRINTDEBUG:
+                    print('w 1 {:0.2f}'.format(spd1_c))
+                if od_writeSpd(1, spd1_c):
+                    spd1_c_written = spd1_c
+
+            if(vel_gain_written != vel_gain or vel_integrator_gain_written != vel_integrator_gain):
                 if set_config(0) and set_config(1):
-                    vel_gain_written=vel_gain
-                    vel_integrator_gain_written=vel_integrator_gain
+                    vel_gain_written = vel_gain
+                    vel_integrator_gain_written = vel_integrator_gain
             lock.release()
-        except:
+        except:  # noqa: 722
             lock.release()
             import traceback
             exception_string = traceback.format_exc()
@@ -562,18 +564,17 @@ def stop_control():
             logger.error(exception_string)
             rate.sleep()
             continue
-        
+
         # set watchdog timer
         try:
             # enable watchdog timer after receiving at least one motorTarget
             if wait_first_command:
                 if count_motorTarget is not None:
-                    od_setWatchdogTimer(wtimer,0)
-                    od_setWatchdogTimer(wtimer,1)
+                    od_setWatchdogTimer(wtimer, 0)
+                    od_setWatchdogTimer(wtimer, 1)
             else:
-                od_setWatchdogTimer(wtimer,0)
-                od_setWatchdogTimer(wtimer,1)
-
+                od_setWatchdogTimer(wtimer, 0)
+                od_setWatchdogTimer(wtimer, 1)
 
             # feed watchdog timer
             if count_motorTarget is not None:
@@ -582,7 +583,7 @@ def stop_control():
                     od_feedWatchdogTimer(0)
                     od_feedWatchdogTimer(1)
                     previous_count_motorTarget = count_motorTarget
-        except:
+        except:  # noqa: 722
             import traceback
             exception_string = traceback.format_exc()
             logger.error("Failed to set watchdog timer")
@@ -595,13 +596,13 @@ def stop_control():
         current_measured_0 = current_measured_1 = None
         # update encoder counts and speed
         try:
-            enc0, spd0 = odrvs[0].axis0.pos_vel_mapper.pos_rel, odrvs[0].axis0.pos_vel_mapper.vel#getFloats(getResponse("f 0"))
-            enc1, spd1 = odrvs[1].axis0.pos_vel_mapper.pos_rel, odrvs[1].axis0.pos_vel_mapper.vel#getFloats(getResponse("f 1"))
+            enc0, spd0 = odrvs[0].axis0.pos_vel_mapper.pos_rel, odrvs[0].axis0.pos_vel_mapper.vel  # getFloats(getResponse("f 0"))
+            enc1, spd1 = odrvs[1].axis0.pos_vel_mapper.pos_rel, odrvs[1].axis0.pos_vel_mapper.vel  # getFloats(getResponse("f 1"))
             current_setpoint_0 = odrvs[0].axis0.motor.foc.Iq_setpoint
             current_setpoint_1 = odrvs[1].axis0.motor.foc.Iq_setpoint
             current_measured_0 = odrvs[0].axis0.motor.foc.Iq_measured
             current_measured_1 = odrvs[1].axis0.motor.foc.Iq_measured
-        except:
+        except:  # noqa: 722
             print("Reading TRY failed!")
             rate.sleep()
             import traceback
@@ -614,58 +615,61 @@ def stop_control():
             ms.header.stamp = node.get_clock().now().to_msg()
 
             if leftIs1:
-                ms.dist_left_c  = enc1
+                ms.dist_left_c = enc1
                 ms.dist_right_c = enc0
-                ms.spd_left_c   = spd1
-                ms.spd_right_c  = spd0
+                ms.spd_left_c = spd1
+                ms.spd_right_c = spd0
 
                 ms.current_setpoint_left = current_setpoint_1 * signLeft
                 ms.current_setpoint_right = current_setpoint_0 * signRight
                 ms.current_measured_left = current_measured_1 * signLeft
                 ms.current_measured_right = current_measured_0 * signRight
             else:
-                ms.dist_left_c  = enc0
+                ms.dist_left_c = enc0
                 ms.dist_right_c = enc1
-                ms.spd_left_c   = spd0
-                ms.spd_right_c  = spd1
+                ms.spd_left_c = spd0
+                ms.spd_right_c = spd1
 
                 ms.current_setpoint_left = current_setpoint_0 * signLeft
                 ms.current_setpoint_right = current_setpoint_1 * signRight
                 ms.current_measured_left = current_measured_0 * signLeft
                 ms.current_measured_right = current_measured_1 * signRight
 
-            ms.dist_left_c  *= signLeft / gainLeft
+            ms.dist_left_c *= signLeft / gainLeft
             ms.dist_right_c *= signRight / gainRight
-            ms.spd_left_c  *= signLeft / gainLeft
+            ms.spd_left_c *= signLeft / gainLeft
             ms.spd_right_c *= signRight / gainRight
 
-            ms.dist_left  = ms.dist_left_c  * meter_per_round
+            ms.dist_left = ms.dist_left_c * meter_per_round
             ms.dist_right = ms.dist_right_c * meter_per_round
-            ms.spd_left  = ms.spd_left_c  * meter_per_round
+            ms.spd_left = ms.spd_left_c * meter_per_round
             ms.spd_right = ms.spd_right_c * meter_per_round
             pub.publish(ms)
         rate.sleep()
 
     # stop motor before exit
-    od_writeSpd(0,0)
-    od_writeSpd(1,0)
-    od_setWatchdogTimer(0,0)
-    od_setWatchdogTimer(0,1)
+    od_writeSpd(0, 0)
+    od_writeSpd(1, 0)
+    od_setWatchdogTimer(0, 0)
+    od_setWatchdogTimer(0, 1)
     od_writeMode(0)
 
+
 def od_reboot():
     odrvs[0].reboot()
     odrvs[1].reboot()
 
+
 def od_writeSpd(ch, spd):
     try:
         ctrl = odrvs[ch].axis0.controller
         ctrl.config.control_mode = ControlMode.VELOCITY_CONTROL
         ctrl.input_vel = spd
         return 1
-    except:
+    except:  # noqa: 722
         raise
 
+
 def od_setWatchdogTimer(sec, odrv_index):
     if 0 < sec:
         # store previous watchdog_timeout values to use them later
@@ -684,13 +688,15 @@ def od_setWatchdogTimer(sec, odrv_index):
         odrvs[odrv_index].axis0.config.enable_watchdog = False
         odrvs[odrv_index].axis0.config.watchdog_timeout = sec
 
+
 def od_feedWatchdogTimer(odrv_index):
     odrvs[odrv_index].axis0.watchdog_feed()
 
+
 def od_writeMode(loopCtrl_on):
     global selectMode
     try:
-        if (loopCtrl_on==1):
+        if (loopCtrl_on == 1):
             odrvs[0].axis0.requested_state = AxisState.CLOSED_LOOP_CONTROL
             odrvs[1].axis0.requested_state = AxisState.CLOSED_LOOP_CONTROL
             selectMode = loopCtrl_on
@@ -700,9 +706,10 @@ def od_writeMode(loopCtrl_on):
             odrvs[1].axis0.requested_state = AxisState.IDLE
             selectMode = loopCtrl_on
             return 1
-    except:
+    except:  # noqa: 722
         raise
 
+
 '''Run'''
 if __name__ == '__main__':
     try:
@@ -717,4 +724,4 @@ def od_writeMode(loopCtrl_on):
     except ROSInterruptException:
         pass
     while selectMode == 1:
-        time.sleep(1)
\ No newline at end of file
+        time.sleep(1)