From be1bf014ed0e3ad6672c9e342822e9b86fae50e5 Mon Sep 17 00:00:00 2001 From: Jacob Koziej Date: Fri, 20 Oct 2023 02:05:26 -0400 Subject: [PATCH] Apply `ruff` `pre-commit` hook --- common/igvcutils/src/igvcutils/__init__.py | 8 ++- dbw/cmd/cmd.py | 72 ------------------- dbw/cmd/cmd_can.py | 54 -------------- dbw/cmd/node.py | 68 ------------------ dbw/ember_bl/ember_identity.py | 1 + dbw/ember_bl/update.py | 2 +- dbw/node_fw/ember_identity.py | 1 + dbw/node_fw/ember_update_pio.py | 2 + dbw/node_fw/opencan-pio.py | 4 +- dbw/node_fw/updater.py | 2 - dbw/node_sim/node.py | 1 - dbw/node_sim/node_sim.py | 3 - dbw/safed/tests.py | 6 +- ros/docker/detection/src/detection.py | 12 ++-- .../encoder_odom/src/encoder_arduino.py | 22 +++--- ros/docker/encoder_odom/src/encoder_odom.py | 18 +++-- .../tests/spawn_model_test.py | 2 +- .../navigation/scooter_launch/convert_msg.py | 10 +-- ros/docker/pure_pursuit/src/global_ros_pp.py | 13 ++-- ros/docker/pure_pursuit/src/path_test_gen.py | 6 +- ros/docker/pure_pursuit/src/ros_pp.py | 2 +- .../rtabmap/rtabmap_launch/convert_odom.py | 8 +-- ros/docker/techbus/src/techbus.py | 9 ++- tests/test_steering.py | 2 +- tests/test_template.py | 3 +- tests/test_throttle.py | 2 +- 26 files changed, 80 insertions(+), 253 deletions(-) delete mode 100755 dbw/cmd/cmd.py delete mode 100644 dbw/cmd/cmd_can.py delete mode 100644 dbw/cmd/node.py diff --git a/common/igvcutils/src/igvcutils/__init__.py b/common/igvcutils/src/igvcutils/__init__.py index 2a506bb7d..f6517585c 100644 --- a/common/igvcutils/src/igvcutils/__init__.py +++ b/common/igvcutils/src/igvcutils/__init__.py @@ -1,2 +1,6 @@ -from . import can -from . import ctrl +# ruff: noqa: F401 + +from . import ( + can, + ctrl, +) diff --git a/dbw/cmd/cmd.py b/dbw/cmd/cmd.py deleted file mode 100755 index 9c0fcc4f6..000000000 --- a/dbw/cmd/cmd.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/env python3 - -import can -import cantools -import time -from pprint import pprint -import asyncio - -from cmd_can import CanListener -from node import DBWNode - -estop_trigger = False - - -def do_estop_all(nodes, reason): - for node in nodes: - node.send_estop() - print(f"\nDOING ESTOP FOR {reason}\n") - estop_trigger = True - - -def main(): - pprint("DBW Test Command and Control") - bus = can.interface.Bus('can0', bustype='socketcan') - - db = cantools.database.load_file('/selfdrive/can/igvc_can.dbc') - pprint(db.messages) - - print("Bus set up.") - - nodes = [ - DBWNode("Encoder", 4, db, bus), - DBWNode("Brake", 3, db, bus), - DBWNode("Throttle", 2, db, bus), - DBWNode("Blink", 1, db, bus), - ] - - listener = CanListener(bus, nodes, db) - - while True: - if estop_trigger: - do_estop_all(nodes, "estop latched") - - for node in nodes: - if node.get_status == "ESTOP": - do_estop_all(nodes, "detected single node ESTOP") - - print("Checking speed...") - pprint(listener.encoder_counts) - - if listener.time_since_last_enc_counts > 3: - do_estop_all(nodes, "encoder timeout") - - countL, countR = listener.encoder_counts - print(f"Left Encoder: {countL}, Right Encoder: {countR}") - if countL > 80 or countR > 80: - do_estop_all(nodes, f"speed violation: {countL}, {countR}") - - if abs(countL - countR): - do_estop_all( - nodes, f"encoder difference violation: {countL}, {countR}" - ) - - for node in nodes: - print(f"{node.name}: {node.get_status()}") - - time.sleep(0.1) - print("") - - -if __name__ == "__main__": - main() diff --git a/dbw/cmd/cmd_can.py b/dbw/cmd/cmd_can.py deleted file mode 100644 index 73ba018e4..000000000 --- a/dbw/cmd/cmd_can.py +++ /dev/null @@ -1,54 +0,0 @@ -from base64 import encode -from os import EX_CANTCREAT -from threading import Thread - -from pprint import pprint - -from node import DBWNode - -import time - - -class CanListener(Thread): - def __init__(self, bus, nodes: list[DBWNode], db): - self.bus = bus - self.nodes = nodes - self.db = db - self.encoder_counts = [0, 0] - self.time_at_last_enc_counts = 0 - self.time_since_last_enc_counts = 0 - - super().__init__(target=self.listener) - self.daemon = True - self.start() - - pprint("Created CAN listener.") - - def listener(self): - encoder_msg = self.db.get_message_by_name('dbwNode_Encoder_Data') - self.time_at_last_enc_counts = time.time() - while True: - try: - for msg in self.bus: - for node in self.nodes: - if node.status_msg.frame_id == msg.arbitration_id: - status = self.db.decode_message( - msg.arbitration_id, msg.data - )['SystemStatus'] - node.set_status(status) - - time_now = time.time() - self.time_since_last_enc_counts = ( - time_now - self.time_at_last_enc_counts - ) - # print(self.time_since_last_enc_counts) - - if encoder_msg.frame_id == msg.arbitration_id: - self.encoder_counts = self.db.decode_message( - msg.arbitration_id, msg.data - ) - self.time_at_last_enc_counts = time_now - - except Exception as e: - print("CAN Listener error!") - print(e) diff --git a/dbw/cmd/node.py b/dbw/cmd/node.py deleted file mode 100644 index 7b37e36ff..000000000 --- a/dbw/cmd/node.py +++ /dev/null @@ -1,68 +0,0 @@ -from pprint import pprint - -import cantools -import can - - -class DBWNode: - name = "UNDEF" - status = "UNDEF" # should correspond to undefined - - def __init__(self, name: str, id: int, db, bus): - self.name = name - self.id = id - self.db = db - self.bus = bus - self.status_msg = db.get_message_by_name(f"dbwNode_Status_{self.name}") - self.syscmd_msg = db.get_message_by_name(f"dbwNode_SysCmd_{self.name}") - - def set_status(self, status): - self.status = status - - def get_status(self): - return self.status - - def send_msg(self, msg, data): - cmd = msg.encode(data) - msg = can.Message( - arbitration_id=msg.frame_id, data=cmd, is_extended_id=False - ) - self.bus.send(msg) - - def send_syscmd(self, data): - cmd = self.syscmd_msg.encode(data) - msg = can.Message( - arbitration_id=self.syscmd_msg.frame_id, - data=cmd, - is_extended_id=False, - ) - self.bus.send(msg) - - def send_estop(self): - self.send_syscmd({'DbwActive': 0, 'ESTOP': 1}) - - def send_active(self): - self.send_syscmd({'DbwActive': 1, "ESTOP": 0}) - - -class Accel(DBWNode): - modectrl_status = 0 - - def __init__(self, name: str, id: int, db, bus, custom_msgs): - self.modectrl_msg = self.db.get_messsage_by_name(f"dbwNode_Accel_Cmd") - self.disable_mode_control() - super.__init__(name, id, db, bus, custom_msgs) - - def enable_mode_control(self): - self.send_msg(self.modectrl_msg, {"ThrottleCmd": 0, "ModeCtrl": 1}) - self.modectrl_status = 1 - - def disable_mode_control(self): - self.send_msg(self.modectrl_msg, {"ThrottleCmd": 0, "ModeCtrl": 0}) - self.modectrl_status = 0 - - def send_throttle_command(self, cmd): - self.send_msg( - self.modectrl_msg, - {"ThrottleCmd": cmd, "ModeCtrl": self.modectrl_status}, - ) diff --git a/dbw/ember_bl/ember_identity.py b/dbw/ember_bl/ember_identity.py index 78dc2e418..b4ae0a7b2 100644 --- a/dbw/ember_bl/ember_identity.py +++ b/dbw/ember_bl/ember_identity.py @@ -1,3 +1,4 @@ +# ruff: noqa: F821 # Simple script to add a #define for EMBER_NODE_IDENTITY. Import('env') diff --git a/dbw/ember_bl/update.py b/dbw/ember_bl/update.py index b75c93a90..207d28e39 100644 --- a/dbw/ember_bl/update.py +++ b/dbw/ember_bl/update.py @@ -138,7 +138,7 @@ def main(): sleep(SLEEP_WAIT) - log.info(f"Waiting for node to be in RECV_CHUNK....") + log.info("Waiting for node to be in RECV_CHUNK....") while node.state() != 'RECV_CHUNK': sleep(SLEEP_WAIT) diff --git a/dbw/node_fw/ember_identity.py b/dbw/node_fw/ember_identity.py index bad8598fc..b1f8b2061 100644 --- a/dbw/node_fw/ember_identity.py +++ b/dbw/node_fw/ember_identity.py @@ -1,3 +1,4 @@ +# ruff: noqa: F821 # Simple script to add a #define for EMBER_NODE_IDENTITY. Import('env') diff --git a/dbw/node_fw/ember_update_pio.py b/dbw/node_fw/ember_update_pio.py index 8ee8134c1..bec0b503b 100644 --- a/dbw/node_fw/ember_update_pio.py +++ b/dbw/node_fw/ember_update_pio.py @@ -1,3 +1,5 @@ +# ruff: noqa: F821 + Import("env") identity = env.GetProjectOption('board_node_identity', default=None) diff --git a/dbw/node_fw/opencan-pio.py b/dbw/node_fw/opencan-pio.py index defe4ee7d..e3d0b543c 100644 --- a/dbw/node_fw/opencan-pio.py +++ b/dbw/node_fw/opencan-pio.py @@ -1,3 +1,5 @@ +# ruff: noqa: F821 + Import("env") node = env.GetProjectOption("board_can_node") @@ -11,7 +13,7 @@ # These Execute calls call opencan-cli every time. # They could be replaced with a proper Command, but it was tricky getting # PlatformIO to reliably execute them. You could ask them on GitHub or similar. -if 0 != env.Execute(f'scons -QD deps-opencan'): +if 0 != env.Execute('scons -QD deps-opencan'): print("Error making sure opencan-cli is installed; stopping build.") exit(-1) diff --git a/dbw/node_fw/updater.py b/dbw/node_fw/updater.py index 7a400581e..eb447e785 100755 --- a/dbw/node_fw/updater.py +++ b/dbw/node_fw/updater.py @@ -3,9 +3,7 @@ import argparse import can import cantools -import re -from pprint import pprint from time import sleep from tqdm import trange diff --git a/dbw/node_sim/node.py b/dbw/node_sim/node.py index d8c43e5fe..03e5f19ab 100644 --- a/dbw/node_sim/node.py +++ b/dbw/node_sim/node.py @@ -1,4 +1,3 @@ -import logging import time from cand.client import Bus diff --git a/dbw/node_sim/node_sim.py b/dbw/node_sim/node_sim.py index 21ff720ef..90df25b6e 100755 --- a/dbw/node_sim/node_sim.py +++ b/dbw/node_sim/node_sim.py @@ -1,11 +1,8 @@ #!/usr/bin/env python3 # node simulator -import logging import multiprocessing as mp -from cand.client import Bus - from node import Node diff --git a/dbw/safed/tests.py b/dbw/safed/tests.py index 8bb17a099..f846aa0b6 100644 --- a/dbw/safed/tests.py +++ b/dbw/safed/tests.py @@ -41,9 +41,9 @@ def test(self): data_time, data_data = data_rec cmd_time, cmd_data = cmd_rec - if cur_time - data_time >= THROTTLE_ACCELDATA_TIMEOUT_NS: + if cur_time - data_time >= DBWNODE_ACCEL_DATA_TIMEOUT_NS: return self.abort('TIMEOUT') - if cur_time - cmd_time >= DBW_VELCMD_TIMEOUT_NS: + if cur_time - cmd_time >= DBWNODE_VEL_CMD_TIMEOUT_NS: return self.abort('TIMEOUT') perc_diff = abs(data_data['percent'] - cmd_data['throttlePercent']) @@ -76,7 +76,7 @@ def test(self): encoder_time, encoder_data = encoder_rec - if cur_time - encoder_time >= ENCF_ENCODERDATA_TIMEOUT_NS: + if cur_time - encoder_time >= DBWNODE_ENCODER_DATA_TIMEOUT_NS: return self.abort('TIMEOUT') encoder_max = max( diff --git a/ros/docker/detection/src/detection.py b/ros/docker/detection/src/detection.py index 3bbb9a379..93e995a44 100755 --- a/ros/docker/detection/src/detection.py +++ b/ros/docker/detection/src/detection.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 import cv2 as cv -from cv_bridge import CvBridge, CvBridgeError +import numpy as np import rospy -import os + + +from cv_bridge import CvBridge from sensor_msgs.msg import Image from std_msgs.msg import Bool -import numpy as np -from matplotlib import pyplot as plt # DEFINES @@ -142,7 +142,7 @@ def detection(self): ) hsv = cv.cvtColor(barrel_jpg, cv.COLOR_BGR2HSV) - # frame_thresh = cv2.inRange(hsv,orange_min,orange_max) + # frame_thresh = cv.inRange(hsv,orange_min,orange_max) orange_min = (0, 97, 4) orange_max = (18, 255, 255) @@ -156,7 +156,7 @@ def detection(self): keypoints, np.zeros((1, 1)), (0, 0, 255), - cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS, + cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS, ) cv.imshow("Keypoints", im_with_keypoints) cv.waitKey(0) diff --git a/ros/docker/encoder_odom/src/encoder_arduino.py b/ros/docker/encoder_odom/src/encoder_arduino.py index 4a7ed0f88..9e448b731 100755 --- a/ros/docker/encoder_odom/src/encoder_arduino.py +++ b/ros/docker/encoder_odom/src/encoder_arduino.py @@ -1,16 +1,22 @@ #!/usr/bin/env python3 +import pyfirmata import rospy import tf2_ros -import math -from math import sin, cos, pi -# Used for Arudino -import pyfirmata -import time -from geometry_msgs.msg import Point, Pose, Quaternion, Vector3, Twist -from std_msgs.msg import Float32 +from math import ( + cos, + sin, +) + +from geometry_msgs.msg import ( + Point, + Pose, + Quaternion, + Twist, + Vector3, +) from nav_msgs.msg import Odometry @@ -35,7 +41,7 @@ def publish(self, data): # vy might always be zero class Encoder_Odom: def __init__(self): - self.sub = Subscribe() + self.sub = Subscribe() # noqa: F821 self.broadcaster = tf2_ros.TransformBroadcaster() self.x = 0.0 diff --git a/ros/docker/encoder_odom/src/encoder_odom.py b/ros/docker/encoder_odom/src/encoder_odom.py index 3552309fb..a0f8da4e8 100755 --- a/ros/docker/encoder_odom/src/encoder_odom.py +++ b/ros/docker/encoder_odom/src/encoder_odom.py @@ -1,20 +1,26 @@ #!/usr/bin/env python3 +import ctypes + import rospy import tf2_ros import tf_conversions -import ctypes -import math -from math import sin, cos, pi + + +from math import ( + cos, + sin, +) from geometry_msgs.msg import ( Point, Pose, Quaternion, - Vector3, - Twist, TransformStamped, + Twist, + Vector3, ) + from std_msgs.msg import UInt16MultiArray, String from nav_msgs.msg import Odometry @@ -87,7 +93,7 @@ def send_transform(self, current_time): def calc_odom(self, current_time, last_time): # Car Variables - wheel_radius = 0.3302 # 13 inch ~= 0.3302 meter + wheel_radius = 0.3302 # 13 inch ~= 0.3302 meter # noqa: F841 circumference = 1.899156 enc_res = 65536 tick_distance = circumference / enc_res diff --git a/ros/docker/gazebo/igvc_self_drive_sim/igvc_self_drive_gazebo_plugins/tests/spawn_model_test.py b/ros/docker/gazebo/igvc_self_drive_sim/igvc_self_drive_gazebo_plugins/tests/spawn_model_test.py index 39caaa2c6..cca7b5a3b 100755 --- a/ros/docker/gazebo/igvc_self_drive_sim/igvc_self_drive_gazebo_plugins/tests/spawn_model_test.py +++ b/ros/docker/gazebo/igvc_self_drive_sim/igvc_self_drive_gazebo_plugins/tests/spawn_model_test.py @@ -2,7 +2,7 @@ import unittest import rospy import rospkg -from tf import LookupException, ExtrapolationException, TransformListener +from tf import LookupException, TransformListener import os import sys import shlex diff --git a/ros/docker/navigation/scooter_launch/convert_msg.py b/ros/docker/navigation/scooter_launch/convert_msg.py index 7a2f6d125..e07e0c376 100755 --- a/ros/docker/navigation/scooter_launch/convert_msg.py +++ b/ros/docker/navigation/scooter_launch/convert_msg.py @@ -30,10 +30,10 @@ def main(): initial.append(out.group().strip()) if len(initial) >= 2: - if form == True: + if form: print('form: [') print(f'{{{initial[0]}, {initial[1]}, z: 0.0}},') - if form == False: + if not form: print(f'{{{initial[0]}, {initial[1]}, z: 0.0}}') print("],") form = not form @@ -49,16 +49,16 @@ def main(): coordinates.append(out.group().strip()) if len(coordinates) >= 2: - if form == True: + if form: print('form: [') print(f'{{{coordinates[0]}, {coordinates[1]}, z: 0.0}},') - if form == False: + if not form: print(f'{{{coordinates[0]}, {coordinates[1]}, z: 0.0}}') print("],") form = not form coordinates = [] - except: + except Exception: pass if close: diff --git a/ros/docker/pure_pursuit/src/global_ros_pp.py b/ros/docker/pure_pursuit/src/global_ros_pp.py index 6a5c10319..46887cd67 100755 --- a/ros/docker/pure_pursuit/src/global_ros_pp.py +++ b/ros/docker/pure_pursuit/src/global_ros_pp.py @@ -1,12 +1,15 @@ #!/usr/bin/env python3 +import math + + +import numpy as np import rospy -from std_msgs.msg import Float32MultiArray -from geometry_msgs.msg import PoseStamped + + from nav_msgs.msg import Odometry, Path +from std_msgs.msg import Float32MultiArray -import math -import numpy as np ld = 1.18 # define lookahead distance wheel_base = 1.75 # double check @@ -116,7 +119,7 @@ def lookahead(curr_x, curr_y): else: return 0, 0 - except NoIntersectionException as e: + except NoIntersectionException: print("Path lookahead no intersection") return -999, -999 diff --git a/ros/docker/pure_pursuit/src/path_test_gen.py b/ros/docker/pure_pursuit/src/path_test_gen.py index 46c35a0d3..aca05b49f 100755 --- a/ros/docker/pure_pursuit/src/path_test_gen.py +++ b/ros/docker/pure_pursuit/src/path_test_gen.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 +import numpy as np import rospy + + from nav_msgs.msg import Path -import math -import numpy as np -import turtle if __name__ == "__main__": rospy.init_node('path_gen', anonymous=True) diff --git a/ros/docker/pure_pursuit/src/ros_pp.py b/ros/docker/pure_pursuit/src/ros_pp.py index 792754414..16c0e50fd 100755 --- a/ros/docker/pure_pursuit/src/ros_pp.py +++ b/ros/docker/pure_pursuit/src/ros_pp.py @@ -127,7 +127,7 @@ def lookahead(curr_x, curr_y): else: return 0, 0 - except NoIntersectionException as e: + except NoIntersectionException: print("Path lookahead no intersection") return -999, -999 diff --git a/ros/docker/rtabmap/rtabmap_launch/convert_odom.py b/ros/docker/rtabmap/rtabmap_launch/convert_odom.py index 4cdaa51b1..a50415a33 100755 --- a/ros/docker/rtabmap/rtabmap_launch/convert_odom.py +++ b/ros/docker/rtabmap/rtabmap_launch/convert_odom.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 -import roslib import rospy -from geometry_msgs.msg import PoseWithCovarianceStamped + + from nav_msgs.msg import Odometry @@ -53,7 +53,7 @@ def pub_ekf_odom(self, msg): if __name__ == '__main__': print("I LIVE") try: - O = OdomEKF() + odom = OdomEKF() rospy.spin() - except: + except Exception: pass diff --git a/ros/docker/techbus/src/techbus.py b/ros/docker/techbus/src/techbus.py index b2280c712..39fec1441 100755 --- a/ros/docker/techbus/src/techbus.py +++ b/ros/docker/techbus/src/techbus.py @@ -1,12 +1,15 @@ #!/usr/bin/env python3 -import rospy import cand -import math +import rospy + + from math import atan2 + from geometry_msgs.msg import Twist -from std_msgs.msg import UInt16MultiArray, String +from std_msgs.msg import UInt16MultiArray + # CAN Messages EncoderCAN = 'CTRL_EncoderData' diff --git a/tests/test_steering.py b/tests/test_steering.py index d2347907f..e3f5b891a 100755 --- a/tests/test_steering.py +++ b/tests/test_steering.py @@ -78,7 +78,7 @@ def main(): try: test.run(args.percent, args.time) - except: + except Exception: pass test.end() diff --git a/tests/test_template.py b/tests/test_template.py index 2ef411aa6..04241a766 100755 --- a/tests/test_template.py +++ b/tests/test_template.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import argparse -import time import cand @@ -62,7 +61,7 @@ def main(): try: test.run() - except: + except Exception: pass test.end() diff --git a/tests/test_throttle.py b/tests/test_throttle.py index c468650f9..9171292c8 100755 --- a/tests/test_throttle.py +++ b/tests/test_throttle.py @@ -85,7 +85,7 @@ def main(): try: test.run(args.percent, args.time) - except: + except Exception: pass test.end()