diff --git a/.github/workflows/ci-ros2.yml b/.github/workflows/ci-ros2.yml index dfb7ba1d6..315d94ba1 100644 --- a/.github/workflows/ci-ros2.yml +++ b/.github/workflows/ci-ros2.yml @@ -42,14 +42,21 @@ jobs: sudo apt update sudo apt install -y libusb-1.0-0-dev + - name: install pip dependencies + # TODO: would be better to follow https://answers.ros.org/question/370666/how-to-declare-an-external-python-dependency-in-ros2/ + # but this requires some upstream changes + run: | + pip install rowan + - uses: actions/checkout@v2 - name: build and test ROS 2 uses: ros-tooling/action-ros-ci@v0.3 with: - # TODO: removed crazyflie_interfaces since it causes some test failures package-name: | crazyflie crazyflie_examples + crazyflie_interfaces + crazyflie_py crazyflie_sim target-ros2-distro: ${{ matrix.ros_distribution }} vcs-repo-file-url: rosinstall \ No newline at end of file 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 174877303..025cac736 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,17 +13,20 @@ # 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, Twist -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): result = Point() @@ -32,8 +35,10 @@ def arrayToGeometryPoint(a): result.z = a[2] return result + class TimeHelper: - """Object containing all time-related functionality. + """ + Object containing all time-related functionality. This class mainly exists to support both real hardware and (potentially faster or slower than realtime) simulation with the same script. @@ -44,7 +49,9 @@ class TimeHelper: visualizer: No-op object conforming to the Visualizer API used in simulation scripts. Maintains the property that scripts should not know/care if they are running in simulation or not. + """ + def __init__(self, node): self.node = node # self.rosRate = None @@ -53,7 +60,7 @@ def __init__(self, node): # self.visualizer = visNull.VisNull() def time(self): - """Returns the current time in seconds.""" + """Return current time in seconds.""" return self.node.get_clock().now().nanoseconds / 1e9 def sleep(self, duration): @@ -64,7 +71,7 @@ def sleep(self, duration): rclpy.spin_once(self.node, timeout_sec=0) def sleepForRate(self, rateHz): - """Sleeps so that, if called in a loop, executes at specified rate.""" + """Sleep so that, if called in a loop, executes at specified rate.""" # Note: The following ROS 2 construct cannot easily be used, because in ROS 2 # there is no implicit threading anymore. Thus, the rosRate.sleep() call # is blocking. Instead, we simulate the rate behavior ourselves. @@ -80,54 +87,63 @@ def sleepForRate(self, rateHz): self.nextTime += 1.0 / rateHz def isShutdown(self): - """Returns true if the script should abort, e.g. from Ctrl-C.""" + """Return True if the script should abort, e.g. from Ctrl-C.""" return not rclpy.ok() class Crazyflie: - """Object representing a single robot. + """ + Object representing a single robot. The bulk of the module's functionality is contained in this class. """ def __init__(self, node, cfname, paramTypeDict): - """Constructor. + """ + Construct Crazyflie. Args: - cfname (string): Name of the robot names[ace] + node: ROS node reference. + cfname (string): Name of the robot names[ace]. + 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") + self.uploadTrajectoryService = node.create_client( + UploadTrajectory, prefix + '/upload_trajectory') self.uploadTrajectoryService.wait_for_service() - self.startTrajectoryService = node.create_client(StartTrajectory, prefix + "/start_trajectory") + self.startTrajectoryService = node.create_client( + StartTrajectory, prefix + '/start_trajectory') self.startTrajectoryService.wait_for_service() - self.notifySetpointsStopService = node.create_client(NotifySetpointsStop, prefix + "/notify_setpoints_stop") + self.notifySetpointsStopService = node.create_client( + NotifySetpointsStop, prefix + '/notify_setpoints_stop') self.notifySetpointsStopService.wait_for_service() - self.setParamsService = node.create_client(SetParameters, "/crazyflie_server/set_parameters") + self.setParamsService = node.create_client( + 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) @@ -148,23 +164,27 @@ def __init__(self, node, cfname, paramTypeDict): self.paramTypeDict = paramTypeDict - self.cmdFullStatePublisher = node.create_publisher(FullState, prefix + "/cmd_full_state", 1) + self.cmdFullStatePublisher = node.create_publisher( + 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) + # self.cmdStopPublisher = rospy.Publisher( + # prefix + '/cmd_stop', std_msgs.msg.Empty, queue_size=1) - # self.cmdVelPublisher = rospy.Publisher(prefix + "/cmd_vel", geometry_msgs.msg.Twist, queue_size=1) + # self.cmdVelPublisher = rospy.Publisher( + # prefix + '/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) - self.cmdPositionPublisher = node.create_publisher(Position, prefix + "/cmd_position", 1) + self.cmdPositionPublisher = node.create_publisher( + 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) + # self.cmdVelocityWorldPublisher = rospy.Publisher( + # 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. @@ -172,7 +192,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 @@ -202,7 +222,8 @@ def __init__(self, node, cfname, paramTypeDict): # of this list. With real hardware, this list is **ignored**, and # collision avoidance is checked with all other Crazyflies on the # same radio channel. - # ellipsoidRadii (array-like of float[3]): Radii of collision volume ellipsoid in meters. + # ellipsoidRadii (array-like of float[3]): Radii of collision volume + # ellipsoid in meters. # The Crazyflie's boundary for collision checking is a tall # ellipsoid. This accounts for the downwash effect: Due to the # fast-moving stream of air produced by the rotors, the safe @@ -218,19 +239,19 @@ 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): - """Emergency stop. Cuts power; causes future commands to be ignored. + """ + Emergency stop. Cuts power; causes future commands to be ignored. This command is useful if the operator determines that the control script is flawed, and that continuing to follow it will cause wrong/ @@ -244,15 +265,18 @@ def emergency(self): req = Empty.Request() self.emergencyService.call_async(req) - def takeoff(self, targetHeight, duration, groupMask = 0): - """Execute a takeoff - fly straight up, then hover indefinitely. + def takeoff(self, targetHeight, duration, groupMask=0): + """ + Execute a takeoff - fly straight up, then hover indefinitely. Asynchronous command; returns immediately. Args: + ---- targetHeight (float): The z-coordinate at which to hover. duration (float): How long until the height is reached. Seconds. groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc. + """ req = Takeoff.Request() req.group_mask = groupMask @@ -260,18 +284,21 @@ def takeoff(self, targetHeight, duration, groupMask = 0): req.duration = rclpy.duration.Duration(seconds=duration).to_msg() self.takeoffService.call_async(req) - def land(self, targetHeight, duration, groupMask = 0): - """Execute a landing - fly straight down. User must cut power after. + def land(self, targetHeight, duration, groupMask=0): + """ + Execute a landing - fly straight down. User must cut power after. Asynchronous command; returns immediately. Args: + ---- targetHeight (float): The z-coordinate at which to land. Meters. Usually should be a few centimeters above the initial position to ensure that the controller does not try to penetrate the floor if the mocap coordinate origin is not perfect. duration (float): How long until the height is reached. Seconds. groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc. + """ req = Land.Request() req.group_mask = groupMask @@ -291,8 +318,9 @@ def land(self, targetHeight, duration, groupMask = 0): # """ # self.stopService(groupMask) - def goTo(self, goal, yaw, duration, relative = False, groupMask = 0): - """Move smoothly to the goal, then hover indefinitely. + def goTo(self, goal, yaw, duration, relative=False, groupMask=0): + """ + Move smoothly to the goal, then hover indefinitely. Asynchronous command; returns immediately. @@ -323,6 +351,7 @@ def goTo(self, goal, yaw, duration, relative = False, groupMask = 0): position is interpreted as absolute coordintates in the global reference frame. groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc. + """ req = GoTo.Request() req.group_mask = groupMask @@ -333,24 +362,27 @@ def goTo(self, goal, yaw, duration, relative = False, groupMask = 0): self.goToService.call_async(req) def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory): - """Uploads a piecewise polynomial trajectory for later execution. + """ + Upload a piecewise polynomial trajectory for later execution. See uav_trajectory.py for more information about piecewise polynomial trajectories. Args: + ---- trajectoryId (int): ID number of this trajectory. Multiple trajectories can be uploaded. TODO: what is the maximum ID? pieceOffset (int): TODO(whoenig): explain this. trajectory (:obj:`pycrazyswarm.uav_trajectory.Trajectory`): Trajectory object. + """ pieces = [] for poly in trajectory.polynomials: piece = TrajectoryPolynomialPiece() piece.duration = rclpy.duration.Duration(seconds=poly.duration).to_msg() - piece.poly_x = poly.px.p.tolist() - piece.poly_y = poly.py.p.tolist() - piece.poly_z = poly.pz.p.tolist() + piece.poly_x = poly.px.p.tolist() + piece.poly_y = poly.py.p.tolist() + piece.poly_z = poly.pz.p.tolist() piece.poly_yaw = poly.pyaw.p.tolist() pieces.append(piece) req = UploadTrajectory.Request() @@ -359,12 +391,16 @@ def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory): req.pieces = pieces self.uploadTrajectoryService.call_async(req) - def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0): - """Begins executing a previously uploaded trajectory. + def startTrajectory(self, trajectoryId, + timescale=1.0, reverse=False, + relative=True, groupMask=0): + """ + Begin executing a previously uploaded trajectory. Asynchronous command; returns immediately. Args: + ---- trajectoryId (int): ID number as given to :meth:`uploadTrajectory()`. timescale (float): Scales the trajectory duration by this factor. For example if timescale == 2.0, the trajectory will take twice @@ -374,6 +410,7 @@ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relati is shifted such that it begins at the current position setpoint. This is usually the desired behavior. groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc. + """ req = StartTrajectory.Request() req.group_mask = groupMask @@ -384,7 +421,8 @@ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relati self.startTrajectoryService.call_async(req) def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): - """Informs that streaming low-level setpoint packets are about to stop. + """ + Informs that streaming low-level setpoint packets are about to stop. Streaming setpoints are :meth:`cmdVelocityWorld`, :meth:`cmdFullState`, and so on. For safety purposes, they normally preempt onboard high-level @@ -407,6 +445,8 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): streaming setpoint should be followed before reverting to the onboard-determined behavior. May be longer e.g. if one radio is controlling many robots. + groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc. + """ req = NotifySetpointsStop.Request() req.remain_valid_millisecs = remainValidMillisecs @@ -425,8 +465,10 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): # Returns: # position (np.array[3]): Current position. Meters. # """ - # self.tf.waitForTransform("/world", "/cf" + str(self.id), rospy.Time(0), rospy.Duration(10)) - # position, quaternion = self.tf.lookupTransform("/world", "/cf" + str(self.id), rospy.Time(0)) + # self.tf.waitForTransform( + # '/world', '/cf' + str(self.id), rospy.Time(0), rospy.Duration(10)) + # position, quaternion = self.tf.lookupTransform( + # '/world', '/cf' + str(self.id), rospy.Time(0)) # return np.array(position) # def getParam(self, name): @@ -448,18 +490,20 @@ 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): - """Changes the value of the given parameter. + """ + Change the value of the given parameter. See :meth:`getParam()` docs for overview of the parameter system. Args: name (str): The parameter's name. 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)) @@ -478,11 +522,12 @@ 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): - """Sends a streaming full-state controller setpoint command. + """ + Send a streaming full-state controller setpoint command. The full-state setpoint is most useful for aggressive maneuvers where feedforward inputs for acceleration and angular velocity are critical @@ -503,25 +548,26 @@ def cmdFullState(self, pos, vel, acc, yaw, omega): yaw (float): Yaw angle. Radians. omega (array-like of float[3]): Angular velocity in body frame. Radians / sec. + """ self.cmdFullStateMsg.header.stamp = self.node.get_clock().now().to_msg() - self.cmdFullStateMsg.pose.position.x = pos[0] - self.cmdFullStateMsg.pose.position.y = pos[1] - self.cmdFullStateMsg.pose.position.z = pos[2] - self.cmdFullStateMsg.twist.linear.x = vel[0] - self.cmdFullStateMsg.twist.linear.y = vel[1] - self.cmdFullStateMsg.twist.linear.z = vel[2] - self.cmdFullStateMsg.acc.x = acc[0] - self.cmdFullStateMsg.acc.y = acc[1] - self.cmdFullStateMsg.acc.z = acc[2] + self.cmdFullStateMsg.pose.position.x = pos[0] + self.cmdFullStateMsg.pose.position.y = pos[1] + self.cmdFullStateMsg.pose.position.z = pos[2] + self.cmdFullStateMsg.twist.linear.x = vel[0] + self.cmdFullStateMsg.twist.linear.y = vel[1] + self.cmdFullStateMsg.twist.linear.z = vel[2] + self.cmdFullStateMsg.acc.x = acc[0] + self.cmdFullStateMsg.acc.y = acc[1] + self.cmdFullStateMsg.acc.z = acc[2] q = rowan.from_euler(0, 0, yaw) self.cmdFullStateMsg.pose.orientation.w = q[0] self.cmdFullStateMsg.pose.orientation.x = q[1] self.cmdFullStateMsg.pose.orientation.y = q[2] self.cmdFullStateMsg.pose.orientation.z = q[3] - self.cmdFullStateMsg.twist.angular.x = omega[0] - self.cmdFullStateMsg.twist.angular.y = omega[1] - self.cmdFullStateMsg.twist.angular.z = omega[2] + self.cmdFullStateMsg.twist.angular.x = omega[0] + self.cmdFullStateMsg.twist.angular.y = omega[1] + self.cmdFullStateMsg.twist.angular.z = omega[2] self.cmdFullStatePublisher.publish(self.cmdFullStateMsg) # def cmdVelocityWorld(self, vel, yawRate): @@ -563,11 +609,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. @@ -597,22 +643,26 @@ def cmdFullState(self, pos, vel, acc, yaw, omega): # msg.linear.z = thrust # self.cmdVelPublisher.publish(msg) - def cmdPosition(self, pos, yaw = 0.): - """Sends a streaming command of absolute position and yaw setpoint. + def cmdPosition(self, pos, yaw=0.): + """ + Send a streaming command of absolute position and yaw setpoint. Useful for slow maneuvers where a high-level planner determines the desired position, and the rest is left to the onboard controller. For more information on streaming setpoint commands, see the :meth:`cmdFullState()` documentation. + Args: + ---- pos (array-like of float[3]): Position. Meters. yaw (float): Yaw angle. Radians. + """ self.cmdPositionMsg.header.stamp = self.node.get_clock().now().to_msg() - self.cmdPositionMsg.x = pos[0] - self.cmdPositionMsg.y = pos[1] - self.cmdPositionMsg.z = pos[2] + self.cmdPositionMsg.x = pos[0] + self.cmdPositionMsg.y = pos[1] + self.cmdPositionMsg.z = pos[2] self.cmdPositionMsg.yaw = yaw self.cmdPositionPublisher.publish(self.cmdPositionMsg) @@ -623,7 +673,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. @@ -635,13 +685,14 @@ 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): - """Object for broadcasting commands to all robots at once. + """ + Object for broadcasting commands to all robots at once. Also is the container for the individual :obj:`Crazyflie` objects. @@ -650,39 +701,42 @@ class CrazyflieServer(rclpy.node.Node): as determined by the crazyflies.yaml config file. crazyfliesById (Dict[int, Crazyflie]): Index to the same Crazyflie objects by their ID number (last byte of radio address). + """ + def __init__(self): - """Initialize the server. Waits for all ROS services before returning. - """ - super().__init__("CrazyflieAPI") - self.emergencyService = self.create_client(Empty, "all/emergency") + """Initialize the server. Waits for all ROS services before returning.""" + 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") + self.setParamsService = self.create_client( + SetParameters, '/crazyflie_server/set_parameters') self.setParamsService.wait_for_service() - self.cmdFullStatePublisher = self.create_publisher(FullState, "all/cmd_full_state", 1) + self.cmdFullStatePublisher = self.create_publisher( + 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 @@ -695,12 +749,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") + describeParametersService = self.create_client( + DescribeParameters, '/crazyflie_server/describe_parameters') describeParametersService.wait_for_service() req = DescribeParameters.Request() req.names = params @@ -712,7 +767,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 @@ -721,11 +776,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) @@ -735,7 +790,8 @@ def __init__(self): self.crazyfliesById[cfid] = cf def emergency(self): - """Emergency stop. Cuts power; causes future commands to be ignored. + """ + Emergency stop. Cuts power; causes future commands to be ignored. This command is useful if the operator determines that the control script is flawed, and that continuing to follow it will cause wrong/ @@ -749,17 +805,20 @@ def emergency(self): req = Empty.Request() self.emergencyService.call_async(req) - def takeoff(self, targetHeight, duration, groupMask = 0): - """Broadcasted takeoff - fly straight up, then hover indefinitely. + def takeoff(self, targetHeight, duration, groupMask=0): + """ + Broadcasted takeoff - fly straight up, then hover indefinitely. Broadcast version of :meth:`Crazyflie.takeoff()`. All robots that match the groupMask take off at exactly the same time. Use for synchronized movement. Asynchronous command; returns immediately. Args: + ---- targetHeight (float): The z-coordinate at which to hover. duration (float): How long until the height is reached. Seconds. groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc. + """ req = Takeoff.Request() req.group_mask = groupMask @@ -767,20 +826,23 @@ def takeoff(self, targetHeight, duration, groupMask = 0): req.duration = rclpy.duration.Duration(seconds=duration).to_msg() self.takeoffService.call_async(req) - def land(self, targetHeight, duration, groupMask = 0): - """Broadcasted landing - fly straight down. User must cut power after. + def land(self, targetHeight, duration, groupMask=0): + """ + Broadcasted landing - fly straight down. User must cut power after. Broadcast version of :meth:`Crazyflie.land()`. All robots that match the groupMask land at exactly the same time. Use for synchronized movement. Asynchronous command; returns immediately. Args: + ---- targetHeight (float): The z-coordinate at which to land. Meters. Usually should be a few centimeters above the initial position to ensure that the controller does not try to penetrate the floor if the mocap coordinate origin is not perfect. duration (float): How long until the height is reached. Seconds. groupMask (int): Group mask bits. See :meth:`Crazyflie.setGroupMask()` doc. + """ req = Land.Request() req.group_mask = groupMask @@ -788,8 +850,9 @@ def land(self, targetHeight, duration, groupMask = 0): req.duration = rclpy.duration.Duration(seconds=duration).to_msg() self.landService.call_async(req) - def goTo(self, goal, yaw, duration, groupMask = 0): - """Broadcasted goTo - Move smoothly to goal, then hover indefinitely. + def goTo(self, goal, yaw, duration, groupMask=0): + """ + Broadcasted goTo - Move smoothly to goal, then hover indefinitely. Broadcast version of :meth:`Crazyflie.goTo()`. All robots that match the groupMask start moving at exactly the same time. Use for synchronized @@ -807,6 +870,7 @@ def goTo(self, goal, yaw, duration, groupMask = 0): yaw (float): The goal yaw angle (heading). Radians. duration (float): How long until the goal is reached. Seconds. groupMask (int): Group mask bits. See :meth:`Crazyflie.setGroupMask()` doc. + """ req = GoTo.Request() req.group_mask = groupMask @@ -816,8 +880,11 @@ def goTo(self, goal, yaw, duration, groupMask = 0): req.duration = rclpy.duration.Duration(seconds=duration).to_msg() self.goToService.call_async(req) - def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0): - """Broadcasted - begins executing a previously uploaded trajectory. + def startTrajectory(self, trajectoryId, + timescale=1.0, reverse=False, + relative=True, groupMask=0): + """ + Broadcasted - begins executing a previously uploaded trajectory. Broadcast version of :meth:`Crazyflie.startTrajectory()`. Asynchronous command; returns immediately. @@ -831,6 +898,7 @@ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relati relative (bool): If true (default), the position of the trajectory is shifted such that it begins at the current position setpoint. groupMask (int): Group mask bits. See :meth:`Crazyflie.setGroupMask()` doc. + """ req = StartTrajectory.Request() req.group_mask = groupMask @@ -841,8 +909,8 @@ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relati self.startTrajectoryService.call_async(req) def setParam(self, name, value): - """Broadcasted setParam. See Crazyflie.setParam() for details.""" - param_name = "all.params." + name + """Set parameter via broadcasts. See Crazyflie.setParam for details.""" + 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)) @@ -853,7 +921,8 @@ def setParam(self, name, value): self.setParamsService.call_async(req) def cmdFullState(self, pos, vel, acc, yaw, omega): - """Sends a streaming full-state controller setpoint command. + """ + Send a streaming full-state controller setpoint command. The full-state setpoint is most useful for aggressive maneuvers where feedforward inputs for acceleration and angular velocity are critical @@ -874,23 +943,24 @@ def cmdFullState(self, pos, vel, acc, yaw, omega): yaw (float): Yaw angle. Radians. omega (array-like of float[3]): Angular velocity in body frame. Radians / sec. + """ self.cmdFullStateMsg.header.stamp = self.get_clock().now().to_msg() - self.cmdFullStateMsg.pose.position.x = pos[0] - self.cmdFullStateMsg.pose.position.y = pos[1] - self.cmdFullStateMsg.pose.position.z = pos[2] - self.cmdFullStateMsg.twist.linear.x = vel[0] - self.cmdFullStateMsg.twist.linear.y = vel[1] - self.cmdFullStateMsg.twist.linear.z = vel[2] - self.cmdFullStateMsg.acc.x = acc[0] - self.cmdFullStateMsg.acc.y = acc[1] - self.cmdFullStateMsg.acc.z = acc[2] + self.cmdFullStateMsg.pose.position.x = pos[0] + self.cmdFullStateMsg.pose.position.y = pos[1] + self.cmdFullStateMsg.pose.position.z = pos[2] + self.cmdFullStateMsg.twist.linear.x = vel[0] + self.cmdFullStateMsg.twist.linear.y = vel[1] + self.cmdFullStateMsg.twist.linear.z = vel[2] + self.cmdFullStateMsg.acc.x = acc[0] + self.cmdFullStateMsg.acc.y = acc[1] + self.cmdFullStateMsg.acc.z = acc[2] q = rowan.from_euler(0, 0, yaw) self.cmdFullStateMsg.pose.orientation.w = q[0] self.cmdFullStateMsg.pose.orientation.x = q[1] self.cmdFullStateMsg.pose.orientation.y = q[2] self.cmdFullStateMsg.pose.orientation.z = q[3] - self.cmdFullStateMsg.twist.angular.x = omega[0] - self.cmdFullStateMsg.twist.angular.y = omega[1] - self.cmdFullStateMsg.twist.angular.z = omega[2] - self.cmdFullStatePublisher.publish(self.cmdFullStateMsg) \ No newline at end of file + self.cmdFullStateMsg.twist.angular.x = omega[0] + self.cmdFullStateMsg.twist.angular.y = omega[1] + self.cmdFullStateMsg.twist.angular.z = omega[2] + self.cmdFullStatePublisher.publish(self.cmdFullStateMsg) diff --git a/crazyflie_py/crazyflie_py/crazyswarm_py.py b/crazyflie_py/crazyflie_py/crazyswarm_py.py index 2775080c4..ad3280639 100644 --- a/crazyflie_py/crazyflie_py/crazyswarm_py.py +++ b/crazyflie_py/crazyflie_py/crazyswarm_py.py @@ -1,13 +1,14 @@ import rclpy from . import genericJoystick -from .crazyflie import TimeHelper, CrazyflieServer +from .crazyflie import CrazyflieServer, TimeHelper class Crazyswarm: + def __init__(self): rclpy.init() - + self.allcfs = CrazyflieServer() self.timeHelper = TimeHelper(self.allcfs) diff --git a/crazyflie_py/crazyflie_py/genericJoystick.py b/crazyflie_py/crazyflie_py/genericJoystick.py index 2dbb08be3..1fe1651a3 100644 --- a/crazyflie_py/crazyflie_py/genericJoystick.py +++ b/crazyflie_py/crazyflie_py/genericJoystick.py @@ -1,6 +1,6 @@ -import time import copy # import pyglet + from . import keyboard # class JoyStickHandler: @@ -20,7 +20,9 @@ # def on_joyhat_motion(joystick, hat_x, hat_y): # pass + 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) @@ -77,7 +79,6 @@ def waitUntilButtonPressed(self): while keyPoller.poll() is not None: self.timeHelper.sleep(0.01) - def checkIfAnyButtonIsPressed(self): if self.joyID is not None: state = self.js.read(self.joyID) diff --git a/crazyflie_py/crazyflie_py/joystick.py b/crazyflie_py/crazyflie_py/joystick.py index ba8207965..bdc1a096a 100644 --- a/crazyflie_py/crazyflie_py/joystick.py +++ b/crazyflie_py/crazyflie_py/joystick.py @@ -1,17 +1,20 @@ 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 - data.buttons[5] == 1 and - self.lastButtonState == 0): + data.buttons[5] == 1 and + self.lastButtonState == 0): self.buttonWasPressed = True self.lastButtonState = data.buttons[5] diff --git a/crazyflie_py/crazyflie_py/keyboard.py b/crazyflie_py/crazyflie_py/keyboard.py index e5f788775..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,11 +17,11 @@ 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): - dr,dw,de = select.select([sys.stdin], [], [], 0) + dr, dw, de = select.select([sys.stdin], [], [], 0) if not dr == []: return sys.stdin.read(1) return None diff --git a/crazyflie_py/crazyflie_py/linuxjsdev.py b/crazyflie_py/crazyflie_py/linuxjsdev.py index ba5df731e..8bea8def8 100644 --- a/crazyflie_py/crazyflie_py/linuxjsdev.py +++ b/crazyflie_py/crazyflie_py/linuxjsdev.py @@ -23,8 +23,9 @@ # along with this program; if not, write to the Free Software Foundation, # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. """ -Linux joystick driver using the Linux input_joystick subsystem. Requires sysfs -to be mounted on /sys and /dev/input/js* to be readable. +Linux joystick driver using the Linux input_joystick subsystem. + +Requires sysfs to be mounted on /sys and /dev/input/js* to be readable. This module is very linux specific but should work on any CPU platform """ @@ -36,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 @@ -62,14 +63,12 @@ JSIOCGAXES = 0x80016a11 JSIOCGBUTTONS = 0x80016a12 -MODULE_MAIN = "Joystick" -MODULE_NAME = "linuxjsdev" +MODULE_MAIN = 'Joystick' +MODULE_NAME = 'linuxjsdev' class JEvent(object): - """ - Joystick event class. Encapsulate single joystick event. - """ + """Joystick event class. Encapsulate single joystick event.""" def __init__(self, evt_type, number, value): self.type = evt_type @@ -77,9 +76,10 @@ 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) + # Constants TYPE_BUTTON = 1 TYPE_AXIS = 2 @@ -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,43 +111,43 @@ 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): - """Open the joystick device""" + """Open the joystick device.""" 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 def __initvalues(self): - """Read the buttons and axes initial values from the js device""" + """Read the buttons and axes initial values from the js device.""" for _ in range(len(self.axes) + len(self.buttons)): data = self._f.read(struct.calcsize(JS_EVENT_FMT)) jsdata = struct.unpack(JS_EVENT_FMT, data) self.__updatestate(jsdata) def __updatestate(self, jsdata): - """Update the internal absolute state of buttons and axes""" + """Update the internal absolute state of buttons and axes.""" if jsdata[JE_TYPE] & JS_EVENT_AXIS != 0: self.axes[jsdata[JE_NUMBER]] = jsdata[JE_VALUE] / 32768.0 elif jsdata[JE_TYPE] & JS_EVENT_BUTTON != 0: self.buttons[jsdata[JE_NUMBER]] = jsdata[JE_VALUE] def __decode_event(self, jsdata): - """ Decode a jsdev event into a dict """ + """Decode a jsdev event into a dict.""" # TODO: Add timestamp? if jsdata[JE_TYPE] & JS_EVENT_AXIS != 0: return JEvent(evt_type=TYPE_AXIS, @@ -159,7 +159,7 @@ def __decode_event(self, jsdata): value=jsdata[JE_VALUE] / 32768.0) def _read_all_events(self): - """Consume all the events queued up in the JS device""" + """Consume all the events queued up in the JS device.""" try: while True: data = self._f.read(struct.calcsize(JS_EVENT_FMT)) @@ -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: @@ -182,9 +182,9 @@ def _read_all_events(self): pass def read(self): - """ Returns a list of all joystick event since the last call """ + """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() @@ -192,9 +192,7 @@ def read(self): class Joystick(): - """ - Linux jsdev implementation of the Joystick class - """ + """Linux jsdev implementation of the Joystick class.""" def __init__(self): self.name = MODULE_NAME @@ -203,31 +201,30 @@ def __init__(self): def devices(self): """ - Returns a list containing an {"id": id, "name": name} dict for each - detected device. Result is cached once one or more devices are found. + 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): - """ - Open the joystick device. The device_id is given by available_devices - """ + def open(self, device_id): # noqa: A003 + """Open the joystick device. The device_id is given by available_devices.""" self._js[device_id].open() def close(self, device_id): - """Open the joystick device""" + """Open the joystick device.""" self._js[device_id].close() def read(self, device_id): - """ Returns a list of all joystick event since the last call """ + """Return a list of all joystick event since the last call.""" return self._js[device_id].read() diff --git a/crazyflie_py/crazyflie_py/uav_trajectory.py b/crazyflie_py/crazyflie_py/uav_trajectory.py index 91939450a..39e0db1d2 100644 --- a/crazyflie_py/crazyflie_py/uav_trajectory.py +++ b/crazyflie_py/crazyflie_py/uav_trajectory.py @@ -2,108 +2,120 @@ import numpy as np + def normalize(v): - norm = np.linalg.norm(v) - assert norm > 0 - return v / norm + norm = np.linalg.norm(v) + assert norm > 0 + return v / norm class Polynomial: - def __init__(self, p): - self.p = p - # evaluate a polynomial using horner's rule - def eval(self, t): - assert t >= 0 - x = 0.0 - for i in range(0, len(self.p)): - x = x * t + self.p[len(self.p) - 1 - i] - return x + def __init__(self, p): + self.p = p + + # evaluate a polynomial using horner's rule + def eval(self, t): # noqa: A003 + assert t >= 0 + x = 0.0 + for i in range(0, len(self.p)): + x = x * t + self.p[len(self.p) - 1 - i] + return x - # compute and return derivative - def derivative(self): - return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)]) + # compute and return derivative + def derivative(self): + return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)]) class TrajectoryOutput: - def __init__(self): - self.pos = None # position [m] - self.vel = None # velocity [m/s] - self.acc = None # acceleration [m/s^2] - self.omega = None # angular velocity [rad/s] - self.yaw = None # yaw angle [rad] + + def __init__(self): + self.pos = None # position [m] + self.vel = None # velocity [m/s] + self.acc = None # acceleration [m/s^2] + self.omega = None # angular velocity [rad/s] + self.yaw = None # yaw angle [rad] # 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) - self.py = Polynomial(py) - self.pz = Polynomial(pz) - self.pyaw = Polynomial(pyaw) - - # compute and return derivative - def derivative(self): - return Polynomial4D( - self.duration, - self.px.derivative().p, - self.py.derivative().p, - self.pz.derivative().p, - self.pyaw.derivative().p) - - def eval(self, t): - result = TrajectoryOutput() - # flat variables - result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)]) - result.yaw = self.pyaw.eval(t) - - # 1st derivative - derivative = self.derivative() - result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)]) - dyaw = derivative.pyaw.eval(t) - - # 2nd derivative - derivative2 = derivative.derivative() - result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)]) - - # 3rd derivative - derivative3 = derivative2.derivative() - jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)]) - - thrust = result.acc + np.array([0, 0, 9.81]) # add gravity - - z_body = normalize(thrust) - x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0]) - y_body = normalize(np.cross(z_body, x_world)) - x_body = np.cross(y_body, z_body) - - jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body) - h_w = jerk_orth_zbody / np.linalg.norm(thrust) - - result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw]) - return result + + def __init__(self, duration, px, py, pz, pyaw): + self.duration = duration + self.px = Polynomial(px) + self.py = Polynomial(py) + self.pz = Polynomial(pz) + self.pyaw = Polynomial(pyaw) + + # compute and return derivative + def derivative(self): + return Polynomial4D( + self.duration, + self.px.derivative().p, + self.py.derivative().p, + self.pz.derivative().p, + self.pyaw.derivative().p) + + 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)]) + result.yaw = self.pyaw.eval(t) + + # 1st derivative + derivative = self.derivative() + result.vel = np.array([derivative.px.eval(t), + derivative.py.eval(t), + derivative.pz.eval(t)]) + dyaw = derivative.pyaw.eval(t) + + # 2nd derivative + derivative2 = derivative.derivative() + result.acc = np.array([derivative2.px.eval(t), + derivative2.py.eval(t), + derivative2.pz.eval(t)]) + + # 3rd derivative + derivative3 = derivative2.derivative() + jerk = np.array([derivative3.px.eval(t), + derivative3.py.eval(t), + derivative3.pz.eval(t)]) + + thrust = result.acc + np.array([0, 0, 9.81]) # add gravity + + z_body = normalize(thrust) + x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0]) + y_body = normalize(np.cross(z_body, x_world)) + x_body = np.cross(y_body, z_body) + + jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body) + h_w = jerk_orth_zbody / np.linalg.norm(thrust) + + result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw]) + return result class Trajectory: - def __init__(self): - self.polynomials = None - self.duration = None - - def n_pieces(self): - return len(self.polynomials) - - def loadcsv(self, filename): - 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): - assert t >= 0 - assert t <= self.duration - - current_t = 0.0 - for p in self.polynomials: - if t <= current_t + p.duration: - return p.eval(t - current_t) - current_t = current_t + p.duration + + def __init__(self): + self.polynomials = None + self.duration = None + + def n_pieces(self): + return len(self.polynomials) + + def loadcsv(self, filename): + 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): # noqa: A003 + assert t >= 0 + assert t <= self.duration + + current_t = 0.0 + for p in self.polynomials: + if t <= current_t + p.duration: + return p.eval(t - current_t) + current_t = current_t + p.duration diff --git a/crazyflie_py/crazyflie_py/util.py b/crazyflie_py/crazyflie_py/util.py index e27dfaa74..2c201ff9c 100644 --- a/crazyflie_py/crazyflie_py/util.py +++ b/crazyflie_py/crazyflie_py/util.py @@ -6,7 +6,8 @@ def check_ellipsoid_collisions(positions, radii): - """Checks for collisions between a set of ellipsoids at given positions. + """ + Check for collisions between a set of ellipsoids at given positions. Args: positions (array float[n, 3]): The ellipsoid centers. @@ -15,6 +16,7 @@ def check_ellipsoid_collisions(positions, radii): Returns: colliding (array bool[n]): True at index i if the i'th ellipsoid intersects any of the other ellipsoids. + """ scaled = positions / radii[None, :] dists = sp.spatial.distance.pdist(scaled) @@ -27,7 +29,8 @@ def check_ellipsoid_collisions(positions, radii): def poisson_disk_sample(n, dim, mindist): - """Generates random points with guaranteed minimum pairwise distance. + """ + Generate random points with guaranteed minimum pairwise distance. Uses extremely naive and slow "dart throwing" algorithm. TODO(jpreiss): find/implement a library with a fast algorithm. @@ -39,21 +42,21 @@ def poisson_disk_sample(n, dim, mindist): Returns: pts (array float[n, dim]): The sampled points. - """ + """ # Select hypercube volume such that n points will not pack it too tightly. # Note: Will be too sparse for dim >> 3, but reasonable for dim == 2 or 3. measure_ratio = 1.25 std = (measure_ratio * n) ** (1.0 / dim) * mindist + def sample(): return std * np.random.uniform(-0.5, 0.5, size=dim) # Sample the points using dart-throwing. - pts = sample()[None,:] + pts = sample()[None, :] while len(pts) < n: pt = sample() dists = np.linalg.norm(pts - pt, axis=1) if np.all(dists >= mindist): - pts = np.concatenate([pts, pt[None,:]], axis=0) + pts = np.concatenate([pts, pt[None, :]], axis=0) return pts - diff --git a/crazyflie_py/test/test_copyright.py b/crazyflie_py/test/test_copyright.py deleted file mode 100644 index cc8ff03f7..000000000 --- a/crazyflie_py/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/crazyflie_py/test/test_pep257.py b/crazyflie_py/test/test_pep257.py index b234a3840..bd90b5f8e 100644 --- a/crazyflie_py/test/test_pep257.py +++ b/crazyflie_py/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) + rc = main(argv=['.', 'test', '--add-ignore', 'D406', 'D407', 'D417']) assert rc == 0, 'Found code style errors / warnings' diff --git a/docs2/howto.rst b/docs2/howto.rst index abdeecc1c..7c214d872 100644 --- a/docs2/howto.rst +++ b/docs2/howto.rst @@ -90,4 +90,15 @@ To close the logblocks again, run: .. code-block:: bash ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'topic_test'}" - ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'pose'}" \ No newline at end of file + ros2 service call /cf2/remove_logging crazyflie_interfaces/srv/RemoveLogging "{topic_name: 'pose'}" + +Run Tests Locally +----------------- + +This requires some updated pip packages for testing, see https://docs.ros.org/en/humble/Installation/Alternatives/Ubuntu-Development-Setup.html, otherwise the reported failures will be inconsistent with CI. + +Then execute: + +``` +colcon test --event-handlers=console_cohesion+ --return-code-on-test-failure --packages-select crazyflie_py +```