diff --git a/crazyflie_py/crazyflie_py/__init__.py b/crazyflie_py/crazyflie_py/__init__.py index 6184517df..44fbd53e3 100644 --- a/crazyflie_py/crazyflie_py/__init__.py +++ b/crazyflie_py/crazyflie_py/__init__.py @@ -1,3 +1,3 @@ from .crazyswarm_py import Crazyswarm -__all__ = ["Crazyswarm"] +__all__ = ['Crazyswarm'] diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index c679e6c62..03055a8bc 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -1,9 +1,9 @@ #!/usr/bin/env python +from collections import defaultdict # import sys # import rospy -import numpy as np # import time # import tf_conversions # from std_srvs.srv import Empty @@ -13,18 +13,19 @@ # from tf import TransformListener # from .visualizer import visNull -from collections import defaultdict + +from crazyflie_interfaces.msg import FullState, Position, TrajectoryPolynomialPiece +from crazyflie_interfaces.srv import GoTo, Land,\ + NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory +from geometry_msgs.msg import Point +import numpy as np +from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue +from rcl_interfaces.srv import DescribeParameters, GetParameters, ListParameters, SetParameters import rclpy import rclpy.node import rowan from std_srvs.srv import Empty -from geometry_msgs.msg import Point -from rcl_interfaces.srv import GetParameters, SetParameters, ListParameters, DescribeParameters -from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType -from crazyflie_interfaces.srv import Takeoff, Land, GoTo, \ - UploadTrajectory, StartTrajectory, NotifySetpointsStop -from crazyflie_interfaces.msg import TrajectoryPolynomialPiece, FullState, Position def arrayToGeometryPoint(a): @@ -109,42 +110,42 @@ def __init__(self, node, cfname, paramTypeDict): paramTypeDict: dictionary of the parameter types """ - prefix = "/" + cfname + prefix = '/' + cfname self.prefix = prefix self.node = node # self.tf = tf - # rospy.wait_for_service(prefix + "/set_group_mask") - # self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask) - self.emergencyService = node.create_client(Empty, prefix + "/emergency") + # rospy.wait_for_service(prefix + '/set_group_mask') + # self.setGroupMaskService = rospy.ServiceProxy(prefix + '/set_group_mask', SetGroupMask) + self.emergencyService = node.create_client(Empty, prefix + '/emergency') self.emergencyService.wait_for_service() - self.takeoffService = node.create_client(Takeoff, prefix + "/takeoff") + self.takeoffService = node.create_client(Takeoff, prefix + '/takeoff') self.takeoffService.wait_for_service() - self.landService = node.create_client(Land, prefix + "/land") + self.landService = node.create_client(Land, prefix + '/land') self.landService.wait_for_service() - # # rospy.wait_for_service(prefix + "/stop") - # # self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop) - self.goToService = node.create_client(GoTo, prefix + "/go_to") + # # rospy.wait_for_service(prefix + '/stop') + # # self.stopService = rospy.ServiceProxy(prefix + '/stop', Stop) + self.goToService = node.create_client(GoTo, prefix + '/go_to') self.goToService.wait_for_service() self.uploadTrajectoryService = node.create_client( - UploadTrajectory, prefix + "/upload_trajectory") + UploadTrajectory, prefix + '/upload_trajectory') self.uploadTrajectoryService.wait_for_service() self.startTrajectoryService = node.create_client( - StartTrajectory, prefix + "/start_trajectory") + StartTrajectory, prefix + '/start_trajectory') self.startTrajectoryService.wait_for_service() self.notifySetpointsStopService = node.create_client( - NotifySetpointsStop, prefix + "/notify_setpoints_stop") + NotifySetpointsStop, prefix + '/notify_setpoints_stop') self.notifySetpointsStopService.wait_for_service() self.setParamsService = node.create_client( - SetParameters, "/crazyflie_server/set_parameters") + SetParameters, '/crazyflie_server/set_parameters') self.setParamsService.wait_for_service() # Query some settings - getParamsService = node.create_client(GetParameters, "/crazyflie_server/get_parameters") + getParamsService = node.create_client(GetParameters, '/crazyflie_server/get_parameters') getParamsService.wait_for_service() req = GetParameters.Request() - req.names = ["robots.{}.initial_position".format(cfname), "robots.{}.uri".format(cfname)] + req.names = ['robots.{}.initial_position'.format(cfname), 'robots.{}.uri'.format(cfname)] future = getParamsService.call_async(req) while rclpy.ok(): rclpy.spin_once(node) @@ -166,26 +167,26 @@ def __init__(self, node, cfname, paramTypeDict): self.paramTypeDict = paramTypeDict self.cmdFullStatePublisher = node.create_publisher( - FullState, prefix + "/cmd_full_state", 1) + FullState, prefix + '/cmd_full_state', 1) self.cmdFullStateMsg = FullState() - self.cmdFullStateMsg.header.frame_id = "/world" + self.cmdFullStateMsg.header.frame_id = '/world' # self.cmdStopPublisher = rospy.Publisher( - # prefix + "/cmd_stop", std_msgs.msg.Empty, queue_size=1) + # prefix + '/cmd_stop', std_msgs.msg.Empty, queue_size=1) # self.cmdVelPublisher = rospy.Publisher( - # prefix + "/cmd_vel", geometry_msgs.msg.Twist, queue_size=1) + # prefix + '/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) self.cmdPositionPublisher = node.create_publisher( - Position, prefix + "/cmd_position", 1) + Position, prefix + '/cmd_position', 1) self.cmdPositionMsg = Position() - self.cmdPositionMsg.header.frame_id = "/world" + self.cmdPositionMsg.header.frame_id = '/world' # self.cmdVelocityWorldPublisher = rospy.Publisher( - # prefix + "/cmd_velocity_world", VelocityWorld, queue_size=1) + # prefix + '/cmd_velocity_world', VelocityWorld, queue_size=1) # self.cmdVelocityWorldMsg = VelocityWorld() # self.cmdVelocityWorldMsg.header.seq = 0 - # self.cmdVelocityWorldMsg.header.frame_id = "/world" + # self.cmdVelocityWorldMsg.header.frame_id = '/world' # def setGroupMask(self, groupMask): # """Sets the group mask bits for this robot. @@ -193,7 +194,7 @@ def __init__(self, node, cfname, paramTypeDict): # The purpose of groups is to make it possible to trigger an action # (for example, executing a previously-uploaded trajectory) on a subset # of all robots without needing to send more than one radio packet. - # This is important to achieve tight, synchronized "choreography". + # This is important to achieve tight, synchronized 'choreography'. # Up to 8 groups may exist, corresponding to bits in the groupMask byte. # When a broadcast command is triggered on the :obj:`CrazyflieServer` object @@ -240,15 +241,15 @@ def __init__(self, node, cfname, paramTypeDict): # # Set radii before enabling to ensure collision avoidance never # # observes a wrong radius value. # self.setParams({ - # "colAv/ellipsoidX": float(ellipsoidRadii[0]), - # "colAv/ellipsoidY": float(ellipsoidRadii[1]), - # "colAv/ellipsoidZ": float(ellipsoidRadii[2]), + # 'colAv/ellipsoidX': float(ellipsoidRadii[0]), + # 'colAv/ellipsoidY': float(ellipsoidRadii[1]), + # 'colAv/ellipsoidZ': float(ellipsoidRadii[2]), # }) - # self.setParam("colAv/enable", 1) + # self.setParam('colAv/enable', 1) # def disableCollisionAvoidance(self): # """Disables onboard collision avoidance.""" - # self.setParam("colAv/enable", 0) + # self.setParam('colAv/enable', 0) def emergency(self): """ @@ -469,9 +470,9 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): # position (np.array[3]): Current position. Meters. # """ # self.tf.waitForTransform( - # "/world", "/cf" + str(self.id), rospy.Time(0), rospy.Duration(10)) + # '/world', '/cf' + str(self.id), rospy.Time(0), rospy.Duration(10)) # position, quaternion = self.tf.lookupTransform( - # "/world", "/cf" + str(self.id), rospy.Time(0)) + # '/world', '/cf' + str(self.id), rospy.Time(0)) # return np.array(position) # def getParam(self, name): @@ -493,7 +494,7 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): # Returns: # value (Any): The parameter's value. # """ - # return rospy.get_param(self.prefix + "/" + name) + # return rospy.get_param(self.prefix + '/' + name) def setParam(self, name, value): """ @@ -507,7 +508,7 @@ def setParam(self, name, value): value (Any): The parameter's value. """ - param_name = self.prefix[1:] + ".params." + name + param_name = self.prefix[1:] + '.params.' + name param_type = self.paramTypeDict[name] if param_type == ParameterType.PARAMETER_INTEGER: param_value = ParameterValue(type=param_type, integer_value=int(value)) @@ -526,7 +527,7 @@ def setParam(self, name, value): # params (Dict[str, Any]): Dict of parameter names/values. # """ # for name, value in params.items(): - # rospy.set_param(self.prefix + "/" + name, value) + # rospy.set_param(self.prefix + '/' + name, value) # self.updateParamsService(params.keys()) def cmdFullState(self, pos, vel, acc, yaw, omega): @@ -614,11 +615,11 @@ def cmdFullState(self, pos, vel, acc, yaw, omega): # self.cmdStopPublisher.publish(std_msgs.msg.Empty()) # def cmdVel(self, roll, pitch, yawrate, thrust): - # """Sends a streaming command of the "easy mode" manual control inputs. + # """Sends a streaming command of the 'easy mode' manual control inputs. # The (absolute roll & pitch, yaw rate, thrust) inputs are typically # used for manual flying by beginners or causal pilots, as opposed to the - # "acrobatic mode" inputs where roll and pitch rates are controlled + # 'acrobatic mode' inputs where roll and pitch rates are controlled # instead of absolute angles. This mode limits the possible maneuvers, # e.g. it is not possible to do a flip because the controller joystick # would need to rotate 360 degrees. @@ -678,7 +679,7 @@ def cmdPosition(self, pos, yaw=0.): # common to change the LED ring color many times during a flight, e.g. # as some kind of status indicator. This method makes it convenient. - # PRECONDITION: The param "ring/effect" must be set to 7 (solid color) + # PRECONDITION: The param 'ring/effect' must be set to 7 (solid color) # for this command to have any effect. The default mode uses the ring # color to indicate radio connection quality. @@ -690,9 +691,9 @@ def cmdPosition(self, pos, yaw=0.): # g (float): Green component of color, in range [0, 1]. # b (float): Blue component of color, in range [0, 1]. # """ - # self.setParam("ring/solidRed", int(r * 255)) - # self.setParam("ring/solidGreen", int(g * 255)) - # self.setParam("ring/solidBlue", int(b * 255)) + # self.setParam('ring/solidRed', int(r * 255)) + # self.setParam('ring/solidGreen', int(g * 255)) + # self.setParam('ring/solidBlue', int(b * 255)) class CrazyflieServer(rclpy.node.Node): @@ -712,37 +713,37 @@ class CrazyflieServer(rclpy.node.Node): def __init__(self): """Initialize the server. Waits for all ROS services before returning.""" - super().__init__("CrazyflieAPI") - self.emergencyService = self.create_client(Empty, "all/emergency") + super().__init__('CrazyflieAPI') + self.emergencyService = self.create_client(Empty, 'all/emergency') self.emergencyService.wait_for_service() - self.takeoffService = self.create_client(Takeoff, "all/takeoff") + self.takeoffService = self.create_client(Takeoff, 'all/takeoff') self.takeoffService.wait_for_service() - self.landService = self.create_client(Land, "all/land") + self.landService = self.create_client(Land, 'all/land') self.landService.wait_for_service() - self.goToService = self.create_client(GoTo, "all/go_to") + self.goToService = self.create_client(GoTo, 'all/go_to') self.goToService.wait_for_service() - self.startTrajectoryService = self.create_client(StartTrajectory, "all/start_trajectory") + self.startTrajectoryService = self.create_client(StartTrajectory, 'all/start_trajectory') self.startTrajectoryService.wait_for_service() self.setParamsService = self.create_client( - SetParameters, "/crazyflie_server/set_parameters") + SetParameters, '/crazyflie_server/set_parameters') self.setParamsService.wait_for_service() self.cmdFullStatePublisher = self.create_publisher( - FullState, "all/cmd_full_state", 1) + FullState, 'all/cmd_full_state', 1) self.cmdFullStateMsg = FullState() - self.cmdFullStateMsg.header.frame_id = "/world" + self.cmdFullStateMsg.header.frame_id = '/world' cfnames = [] for srv_name, srv_types in self.get_service_names_and_types(): - if "crazyflie_interfaces/srv/StartTrajectory" in srv_types: - # remove "/" and "/start_trajectory" + if 'crazyflie_interfaces/srv/StartTrajectory' in srv_types: + # remove '/' and '/start_trajectory' cfname = srv_name[1:-17] - if cfname != "all": + if cfname != 'all': cfnames.append(cfname) # Query all parameters - listParamsService = self.create_client(ListParameters, "/crazyflie_server/list_parameters") + listParamsService = self.create_client(ListParameters, '/crazyflie_server/list_parameters') listParamsService.wait_for_service() req = ListParameters.Request() req.depth = ListParameters.Request.DEPTH_RECURSIVE @@ -755,13 +756,13 @@ def __init__(self): # Filter the parameters that belong to this Crazyflie response = future.result() for p in response.result.names: - if ".params." in p: + if '.params.' in p: params.append(p) break # Find the types for the parameters and store them describeParametersService = self.create_client( - DescribeParameters, "/crazyflie_server/describe_parameters") + DescribeParameters, '/crazyflie_server/describe_parameters') describeParametersService.wait_for_service() req = DescribeParameters.Request() req.names = params @@ -773,7 +774,7 @@ def __init__(self): # Filter the parameters that belong to this Crazyflie response = future.result() for p, d in zip(params, response.descriptors): - idx = p.index(".params.") + idx = p.index('.params.') cf_name = p[0:idx] param_name = p[idx+8:] t = d.type @@ -782,11 +783,11 @@ def __init__(self): else: allParamTypeDicts[cf_name] = {param_name: t} break - self.paramTypeDict = allParamTypeDicts["all"] + self.paramTypeDict = allParamTypeDicts['all'] self.crazyflies = [] - self.crazyfliesById = dict() - self.crazyfliesByName = dict() + self.crazyfliesById = {} + self.crazyfliesByName = {} for cfname in cfnames: cf = Crazyflie(self, cfname, allParamTypeDicts[cfname]) self.crazyflies.append(cf) @@ -918,7 +919,7 @@ def startTrajectory(self, trajectoryId, def setParam(self, name, value): """Set parameter via broadcasts. See Crazyflie.setParam for details.""" - param_name = "all.params." + name + param_name = 'all.params.' + name param_type = self.paramTypeDict[name] if param_type == ParameterType.PARAMETER_INTEGER: param_value = ParameterValue(type=param_type, integer_value=int(value)) diff --git a/crazyflie_py/crazyflie_py/crazyswarm_py.py b/crazyflie_py/crazyflie_py/crazyswarm_py.py index 959ac44a6..ad3280639 100644 --- a/crazyflie_py/crazyflie_py/crazyswarm_py.py +++ b/crazyflie_py/crazyflie_py/crazyswarm_py.py @@ -1,10 +1,11 @@ import rclpy from . import genericJoystick -from .crazyflie import TimeHelper, CrazyflieServer +from .crazyflie import CrazyflieServer, TimeHelper class Crazyswarm: + def __init__(self): rclpy.init() diff --git a/crazyflie_py/crazyflie_py/genericJoystick.py b/crazyflie_py/crazyflie_py/genericJoystick.py index 32364b203..1fe1651a3 100644 --- a/crazyflie_py/crazyflie_py/genericJoystick.py +++ b/crazyflie_py/crazyflie_py/genericJoystick.py @@ -1,5 +1,6 @@ import copy # import pyglet + from . import keyboard # class JoyStickHandler: @@ -21,6 +22,7 @@ class Joystick: + def __init__(self, timeHelper): # joysticks = pyglet.input.get_joysticks() # joystick = joysticks[0] @@ -35,14 +37,14 @@ def __init__(self, timeHelper): self.js = linuxjsdev.Joystick() devices = self.js.devices() if len(devices) == 0: - print("Warning: No joystick found!") + print('Warning: No joystick found!') else: - ids = [dev["id"] for dev in devices] + ids = [dev['id'] for dev in devices] # For backwards compatibility, always choose device 0 if available. - self.joyID = 0 if 0 in ids else devices[0]["id"] + self.joyID = 0 if 0 in ids else devices[0]['id'] self.js.open(self.joyID) except ImportError: - print("Warning: Joystick only supported on Linux.") + print('Warning: Joystick only supported on Linux.') # def on_joybutton_press(joystick, button): # print(button) diff --git a/crazyflie_py/crazyflie_py/joystick.py b/crazyflie_py/crazyflie_py/joystick.py index f6ae10500..bdc1a096a 100644 --- a/crazyflie_py/crazyflie_py/joystick.py +++ b/crazyflie_py/crazyflie_py/joystick.py @@ -1,13 +1,15 @@ import time + import rospy from sensor_msgs.msg import Joy class Joystick: + def __init__(self): self.lastButtonState = 0 self.buttonWasPressed = False - rospy.Subscriber("/joy", Joy, self.joyChanged) + rospy.Subscriber('/joy', Joy, self.joyChanged) def joyChanged(self, data): if (not self.buttonWasPressed and diff --git a/crazyflie_py/crazyflie_py/keyboard.py b/crazyflie_py/crazyflie_py/keyboard.py index b4c92992f..d91cd1c25 100644 --- a/crazyflie_py/crazyflie_py/keyboard.py +++ b/crazyflie_py/crazyflie_py/keyboard.py @@ -1,9 +1,10 @@ -import sys import select +import sys import termios class KeyPoller(): + def __enter__(self): # Save the terminal settings self.fd = sys.stdin.fileno() @@ -16,7 +17,7 @@ def __enter__(self): return self - def __exit__(self, type, value, traceback): + def __exit__(self, type, value, traceback): # noqa A002 termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old_term) def poll(self): diff --git a/crazyflie_py/crazyflie_py/linuxjsdev.py b/crazyflie_py/crazyflie_py/linuxjsdev.py index c9ebf50a3..8bea8def8 100644 --- a/crazyflie_py/crazyflie_py/linuxjsdev.py +++ b/crazyflie_py/crazyflie_py/linuxjsdev.py @@ -37,19 +37,19 @@ import sys if not sys.platform.startswith('linux'): - raise ImportError("Only supported on Linux") + raise ImportError('Only supported on Linux') try: import fcntl except ImportError as e: - raise Exception("fcntl library probably not installed ({})".format(e)) + raise Exception('fcntl library probably not installed ({})'.format(e)) __author__ = 'Bitcraze AB' __all__ = ['Joystick'] logger = logging.getLogger(__name__) -JS_EVENT_FMT = "@IhBB" +JS_EVENT_FMT = '@IhBB' JE_TIME = 0 JE_VALUE = 1 JE_TYPE = 2 @@ -63,8 +63,8 @@ JSIOCGAXES = 0x80016a11 JSIOCGBUTTONS = 0x80016a12 -MODULE_MAIN = "Joystick" -MODULE_NAME = "linuxjsdev" +MODULE_MAIN = 'Joystick' +MODULE_NAME = 'linuxjsdev' class JEvent(object): @@ -76,7 +76,7 @@ def __init__(self, evt_type, number, value): self.value = value def __repr__(self): - return "JEvent(type={}, number={}, value={})".format( + return 'JEvent(type={}, number={}, value={})'.format( self.type, self.number, self.value) @@ -90,7 +90,7 @@ class _JS(): def __init__(self, num, name): self.num = num self.name = name - self._f_name = "/dev/input/js{}".format(num) + self._f_name = '/dev/input/js{}'.format(num) self._f = None self.opened = False @@ -98,12 +98,12 @@ def __init__(self, num, name): self.axes = [] self._prev_pressed = {} - def open(self): + def open(self): # noqa: A003 if self._f: - raise Exception("{} at {} is already " - "opened".format(self.name, self._f_name)) + raise Exception('{} at {} is already ' + 'opened'.format(self.name, self._f_name)) - self._f = open("/dev/input/js{}".format(self.num), "rb") + self._f = open('/dev/input/js{}'.format(self.num), 'rb') fcntl.fcntl(self._f.fileno(), fcntl.F_SETFL, os.O_NONBLOCK) # Get number of axis and button @@ -111,15 +111,15 @@ def open(self): if fcntl.ioctl(self._f.fileno(), JSIOCGAXES, val) != 0: self._f.close() self._f = None - raise Exception("Failed to read number of axes") + raise Exception('Failed to read number of axes') - self.axes = list(0 for i in range(val.value)) + self.axes = [0 for i in range(val.value)] if fcntl.ioctl(self._f.fileno(), JSIOCGBUTTONS, val) != 0: self._f.close() self._f = None - raise Exception("Failed to read number of axes") + raise Exception('Failed to read number of axes') - self.buttons = list(0 for i in range(val.value)) + self.buttons = [0 for i in range(val.value)] self.__initvalues() def close(self): @@ -127,7 +127,7 @@ def close(self): if not self._f: return - logger.info("Closed {} ({})".format(self.name, self.num)) + logger.info('Closed {} ({})'.format(self.name, self.num)) self._f.close() self._f = None @@ -170,7 +170,7 @@ def _read_all_events(self): logger.info(str(e)) self._f.close() self._f = None - raise IOError("Device has been disconnected") + raise IOError('Device has been disconnected') except TypeError: pass except ValueError: @@ -184,7 +184,7 @@ def _read_all_events(self): def read(self): """Return a list of all joystick event since the last call.""" if not self._f: - raise Exception("Joystick device not opened") + raise Exception('Joystick device not opened') self._read_all_events() @@ -201,23 +201,23 @@ def __init__(self): def devices(self): """ - Return a list containing an {"id": id, "name": name} dict for each detected device. + Return a list containing an {'id': id, 'name': name} dict for each detected device. Result is cached once one or more devices are found. """ if len(self._devices) == 0: - syspaths = glob.glob("/sys/class/input/js*") + syspaths = glob.glob('/sys/class/input/js*') for path in syspaths: device_id = int(os.path.basename(path)[2:]) - with open(path + "/device/name") as namefile: + with open(path + '/device/name') as namefile: name = namefile.read().strip() self._js[device_id] = _JS(device_id, name) - self._devices.append({"id": device_id, "name": name}) + self._devices.append({'id': device_id, 'name': name}) return self._devices - def open(self, device_id): + def open(self, device_id): # noqa: A003 """Open the joystick device. The device_id is given by available_devices.""" self._js[device_id].open() diff --git a/crazyflie_py/crazyflie_py/uav_trajectory.py b/crazyflie_py/crazyflie_py/uav_trajectory.py index b75af3585..39e0db1d2 100644 --- a/crazyflie_py/crazyflie_py/uav_trajectory.py +++ b/crazyflie_py/crazyflie_py/uav_trajectory.py @@ -10,11 +10,12 @@ def normalize(v): class Polynomial: + def __init__(self, p): self.p = p # evaluate a polynomial using horner's rule - def eval(self, t): + def eval(self, t): # noqa: A003 assert t >= 0 x = 0.0 for i in range(0, len(self.p)): @@ -27,6 +28,7 @@ def derivative(self): class TrajectoryOutput: + def __init__(self): self.pos = None # position [m] self.vel = None # velocity [m/s] @@ -37,6 +39,7 @@ def __init__(self): # 4d single polynomial piece for x-y-z-yaw, includes duration. class Polynomial4D: + def __init__(self, duration, px, py, pz, pyaw): self.duration = duration self.px = Polynomial(px) @@ -53,7 +56,7 @@ def derivative(self): self.pz.derivative().p, self.pyaw.derivative().p) - def eval(self, t): + def eval(self, t): # noqa: A003 result = TrajectoryOutput() # flat variables result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)]) @@ -93,6 +96,7 @@ def eval(self, t): class Trajectory: + def __init__(self): self.polynomials = None self.duration = None @@ -101,12 +105,12 @@ def n_pieces(self): return len(self.polynomials) def loadcsv(self, filename): - data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33)) + data = np.loadtxt(filename, delimiter=',', skiprows=1, usecols=range(33)) self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data] self.duration = np.sum(data[:, 0]) - def eval(self, t): + def eval(self, t): # noqa: A003 assert t >= 0 assert t <= self.duration