mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[robotpy] Mirror most other subprojects (#8208)
GitOrigin-RevId: ac60fd3cf4a24023184376687da28373d14b781a This mirrors the robotpy files for the following projects: - apriltag - datalog - hal - ntcore - romiVendordep - wpilibc - wpimath - xrpVendordep This excludes cscore and the halsim wrappers for at this time. NOTE: This does not hook these projects up to the build system, just simply mirrors the files. The building will take place in a follow up PR to make it easier to review the changes necessary to build.
This commit is contained in:
210
wpilibc/src/main/python/wpilib/__init__.py
Normal file
210
wpilibc/src/main/python/wpilib/__init__.py
Normal file
@@ -0,0 +1,210 @@
|
||||
from . import _init__wpilib
|
||||
|
||||
# TODO: robotpy-build subpackage bug
|
||||
from wpimath._controls._controls import trajectory as _
|
||||
|
||||
# autogenerated by 'semiwrap create-imports wpilib wpilib._wpilib'
|
||||
from ._wpilib import (
|
||||
ADXL345_I2C,
|
||||
AddressableLED,
|
||||
Alert,
|
||||
AnalogAccelerometer,
|
||||
AnalogEncoder,
|
||||
AnalogGyro,
|
||||
AnalogInput,
|
||||
AnalogPotentiometer,
|
||||
CAN,
|
||||
CANStatus,
|
||||
Color,
|
||||
Color8Bit,
|
||||
Compressor,
|
||||
CompressorConfigType,
|
||||
DMC60,
|
||||
DSControlWord,
|
||||
DataLogManager,
|
||||
DigitalInput,
|
||||
DigitalOutput,
|
||||
DoubleSolenoid,
|
||||
DriverStation,
|
||||
DutyCycle,
|
||||
DutyCycleEncoder,
|
||||
Encoder,
|
||||
Field2d,
|
||||
FieldObject2d,
|
||||
I2C,
|
||||
IterativeRobotBase,
|
||||
Jaguar,
|
||||
Joystick,
|
||||
Koors40,
|
||||
LEDPattern,
|
||||
Mechanism2d,
|
||||
MechanismLigament2d,
|
||||
MechanismObject2d,
|
||||
MechanismRoot2d,
|
||||
MotorControllerGroup,
|
||||
MotorSafety,
|
||||
Notifier,
|
||||
OnboardIMU,
|
||||
PS4Controller,
|
||||
PS5Controller,
|
||||
PWM,
|
||||
PWMMotorController,
|
||||
PWMSparkFlex,
|
||||
PWMSparkMax,
|
||||
PWMTalonFX,
|
||||
PWMTalonSRX,
|
||||
PWMVenom,
|
||||
PWMVictorSPX,
|
||||
PneumaticHub,
|
||||
PneumaticsBase,
|
||||
PneumaticsControlModule,
|
||||
PneumaticsModuleType,
|
||||
PowerDistribution,
|
||||
Preferences,
|
||||
RobotBase,
|
||||
RobotController,
|
||||
RobotState,
|
||||
RuntimeType,
|
||||
SD540,
|
||||
SendableBuilderImpl,
|
||||
SendableChooser,
|
||||
SendableChooserBase,
|
||||
SensorUtil,
|
||||
SerialPort,
|
||||
Servo,
|
||||
SharpIR,
|
||||
SmartDashboard,
|
||||
Solenoid,
|
||||
Spark,
|
||||
SparkMini,
|
||||
StadiaController,
|
||||
SystemServer,
|
||||
Talon,
|
||||
TimedRobot,
|
||||
Timer,
|
||||
TimesliceRobot,
|
||||
Tracer,
|
||||
Victor,
|
||||
VictorSP,
|
||||
Watchdog,
|
||||
XboxController,
|
||||
getCurrentThreadPriority,
|
||||
getDeployDirectory,
|
||||
getErrorMessage,
|
||||
getOperatingDirectory,
|
||||
getTime,
|
||||
setCurrentThreadPriority,
|
||||
wait,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"ADXL345_I2C",
|
||||
"AddressableLED",
|
||||
"Alert",
|
||||
"AnalogAccelerometer",
|
||||
"AnalogEncoder",
|
||||
"AnalogGyro",
|
||||
"AnalogInput",
|
||||
"AnalogPotentiometer",
|
||||
"CAN",
|
||||
"CANStatus",
|
||||
"Color",
|
||||
"Color8Bit",
|
||||
"Compressor",
|
||||
"CompressorConfigType",
|
||||
"DMC60",
|
||||
"DSControlWord",
|
||||
"DataLogManager",
|
||||
"DigitalInput",
|
||||
"DigitalOutput",
|
||||
"DoubleSolenoid",
|
||||
"DriverStation",
|
||||
"DutyCycle",
|
||||
"DutyCycleEncoder",
|
||||
"Encoder",
|
||||
"Field2d",
|
||||
"FieldObject2d",
|
||||
"I2C",
|
||||
"IterativeRobotBase",
|
||||
"Jaguar",
|
||||
"Joystick",
|
||||
"Koors40",
|
||||
"LEDPattern",
|
||||
"Mechanism2d",
|
||||
"MechanismLigament2d",
|
||||
"MechanismObject2d",
|
||||
"MechanismRoot2d",
|
||||
"MotorControllerGroup",
|
||||
"MotorSafety",
|
||||
"Notifier",
|
||||
"OnboardIMU",
|
||||
"PS4Controller",
|
||||
"PS5Controller",
|
||||
"PWM",
|
||||
"PWMMotorController",
|
||||
"PWMSparkFlex",
|
||||
"PWMSparkMax",
|
||||
"PWMTalonFX",
|
||||
"PWMTalonSRX",
|
||||
"PWMVenom",
|
||||
"PWMVictorSPX",
|
||||
"PneumaticHub",
|
||||
"PneumaticsBase",
|
||||
"PneumaticsControlModule",
|
||||
"PneumaticsModuleType",
|
||||
"PowerDistribution",
|
||||
"Preferences",
|
||||
"RobotBase",
|
||||
"RobotController",
|
||||
"RobotState",
|
||||
"RuntimeType",
|
||||
"SD540",
|
||||
"SendableBuilderImpl",
|
||||
"SendableChooser",
|
||||
"SendableChooserBase",
|
||||
"SensorUtil",
|
||||
"SerialPort",
|
||||
"Servo",
|
||||
"SharpIR",
|
||||
"SmartDashboard",
|
||||
"Solenoid",
|
||||
"Spark",
|
||||
"SparkMini",
|
||||
"StadiaController",
|
||||
"SystemServer",
|
||||
"Talon",
|
||||
"TimedRobot",
|
||||
"Timer",
|
||||
"TimesliceRobot",
|
||||
"Tracer",
|
||||
"Victor",
|
||||
"VictorSP",
|
||||
"Watchdog",
|
||||
"XboxController",
|
||||
"getCurrentThreadPriority",
|
||||
"getDeployDirectory",
|
||||
"getErrorMessage",
|
||||
"getOperatingDirectory",
|
||||
"getTime",
|
||||
"setCurrentThreadPriority",
|
||||
"wait",
|
||||
]
|
||||
|
||||
# Error reporting
|
||||
from ._impl.report_error import reportError, reportWarning
|
||||
|
||||
__all__ += ["reportError", "reportWarning"]
|
||||
|
||||
del _init__wpilib
|
||||
|
||||
from .cameraserver import CameraServer
|
||||
from .deployinfo import getDeployData
|
||||
|
||||
try:
|
||||
from .version import version as __version__
|
||||
except ImportError:
|
||||
__version__ = "master"
|
||||
|
||||
from ._impl.main import run
|
||||
|
||||
__all__ += ["CameraServer", "run"]
|
||||
0
wpilibc/src/main/python/wpilib/_impl/__init__.py
Normal file
0
wpilibc/src/main/python/wpilib/_impl/__init__.py
Normal file
18
wpilibc/src/main/python/wpilib/_impl/main.py
Normal file
18
wpilibc/src/main/python/wpilib/_impl/main.py
Normal file
@@ -0,0 +1,18 @@
|
||||
import inspect
|
||||
import sys
|
||||
|
||||
|
||||
def run(robot_class, **kwargs):
|
||||
"""
|
||||
``wpilib.run`` is no longer used. You should run your robot code via one of
|
||||
the following methods instead:
|
||||
|
||||
* Windows: ``py -m robotpy [arguments]``
|
||||
* Linux/macOS: ``python -m robotpy [arguments]``
|
||||
|
||||
In a virtualenv the ``robotpy`` command can be used directly.
|
||||
"""
|
||||
|
||||
msg = inspect.cleandoc(inspect.getdoc(run) or "`wpilib.run` is no longer used")
|
||||
print(msg, file=sys.stderr)
|
||||
sys.exit(1)
|
||||
91
wpilibc/src/main/python/wpilib/_impl/report_error.py
Normal file
91
wpilibc/src/main/python/wpilib/_impl/report_error.py
Normal file
@@ -0,0 +1,91 @@
|
||||
import hal
|
||||
import sys
|
||||
import traceback
|
||||
import logging
|
||||
import inspect
|
||||
|
||||
robotpy_logger = logging.getLogger("robotpy")
|
||||
user_logger = logging.getLogger("your.robot")
|
||||
|
||||
|
||||
def reportErrorInternal(
|
||||
error: str,
|
||||
printTrace: bool = False,
|
||||
fromUser: bool = False,
|
||||
isWarning: bool = True,
|
||||
code: int = 1,
|
||||
) -> None:
|
||||
traceString = ""
|
||||
|
||||
if fromUser:
|
||||
log = user_logger
|
||||
else:
|
||||
log = robotpy_logger
|
||||
|
||||
if printTrace:
|
||||
exc_info = sys.exc_info()
|
||||
|
||||
exc = exc_info[0]
|
||||
if exc is None:
|
||||
tb = traceback.extract_stack()[:-2]
|
||||
else:
|
||||
tb = traceback.extract_tb(exc_info[2])
|
||||
|
||||
locString = "%s.%s:%s" % (tb[-1][0], tb[-1][1], tb[-1][2])
|
||||
|
||||
trc = "Traceback (most recent call last):\n"
|
||||
stackstr = trc + "".join(traceback.format_list(tb))
|
||||
if exc is not None:
|
||||
stackstr += " " + ("".join(traceback.format_exception(*exc_info))).lstrip(
|
||||
trc
|
||||
)
|
||||
traceString += "\n" + stackstr
|
||||
|
||||
if exc is None:
|
||||
log.error(error + "\n" + traceString)
|
||||
else:
|
||||
log.error(error, exc_info=exc_info)
|
||||
else:
|
||||
if isWarning:
|
||||
log.warning(error)
|
||||
else:
|
||||
log.error(error)
|
||||
|
||||
try:
|
||||
frame = inspect.stack(context=2)[-1]
|
||||
locString = f"{frame.filename}:{frame.lineno}"
|
||||
except Exception:
|
||||
locString = "<unknown location>"
|
||||
|
||||
if not hal.__hal_simulation__:
|
||||
hal.sendError(
|
||||
not isWarning,
|
||||
code,
|
||||
False,
|
||||
error,
|
||||
locString,
|
||||
traceString,
|
||||
True,
|
||||
)
|
||||
|
||||
|
||||
def reportError(error: str, printTrace: bool = False) -> None:
|
||||
"""
|
||||
Report error to Driver Station, and also prints error to ``sys.stderr``.
|
||||
Optionally appends stack trace to error message.
|
||||
|
||||
:param error: message to show
|
||||
:param printTrace: If True, appends stack trace to error string
|
||||
"""
|
||||
reportErrorInternal(error, printTrace, fromUser=True)
|
||||
|
||||
|
||||
def reportWarning(error: str, printTrace: bool = False) -> None:
|
||||
"""
|
||||
Report warning to Driver Station, and also prints error to ``sys.stderr``.
|
||||
Optionally appends stack trace to error message.
|
||||
|
||||
:param error: message to show
|
||||
:param printTrace: If True, appends stack trace to error string
|
||||
"""
|
||||
reportErrorInternal(error, printTrace, fromUser=True, isWarning=True)
|
||||
250
wpilibc/src/main/python/wpilib/_impl/start.py
Normal file
250
wpilibc/src/main/python/wpilib/_impl/start.py
Normal file
@@ -0,0 +1,250 @@
|
||||
import hal
|
||||
import wpilib
|
||||
import logging
|
||||
import os.path
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
import typing
|
||||
|
||||
import importlib.metadata
|
||||
|
||||
if sys.version_info < (3, 10):
|
||||
|
||||
def entry_points(group):
|
||||
eps = importlib.metadata.entry_points()
|
||||
return eps.get(group, [])
|
||||
|
||||
else:
|
||||
entry_points = importlib.metadata.entry_points
|
||||
|
||||
|
||||
from .report_error import reportError, reportErrorInternal
|
||||
|
||||
|
||||
def _log_versions(robotpy_version: typing.Optional[str]):
|
||||
import wpilib
|
||||
import wpilib.deployinfo
|
||||
|
||||
import logging
|
||||
|
||||
data = wpilib.deployinfo.getDeployData()
|
||||
if data:
|
||||
logger = logging.getLogger("deploy-info")
|
||||
logger.info(
|
||||
"%s@%s at %s",
|
||||
data.get("deploy-user", "<unknown>"),
|
||||
data.get("deploy-host", "<unknown>"),
|
||||
data.get("deploy-date", "<unknown>"),
|
||||
)
|
||||
if "git-hash" in data:
|
||||
logger.info(
|
||||
"- git info: %s (branch=%s)",
|
||||
data.get("git-desc", "<unknown>"),
|
||||
data.get("git-branch", "<unknown>"),
|
||||
)
|
||||
|
||||
logger = logging.getLogger("wpilib")
|
||||
|
||||
if robotpy_version:
|
||||
logger.info("RobotPy version %s", robotpy_version)
|
||||
|
||||
logger.info("WPILib version %s", wpilib.__version__)
|
||||
|
||||
if wpilib.RobotBase.isSimulation():
|
||||
logger.info("Running with simulated HAL.")
|
||||
|
||||
# check to see if we're on a RoboRIO
|
||||
# NOTE: may have false positives, but it should work well enough
|
||||
if os.path.exists("/etc/natinst/share/scs_imagemetadata.ini"):
|
||||
logger.warning(
|
||||
"Running simulation HAL on actual roboRIO! This probably isn't what you want, and will probably cause difficult-to-debug issues!"
|
||||
)
|
||||
|
||||
if logger.isEnabledFor(logging.DEBUG):
|
||||
versions = {}
|
||||
|
||||
# Log third party versions
|
||||
for group in ("robotpylib", "robotpybuild"):
|
||||
for entry_point in entry_points(group=group):
|
||||
# Don't actually load the entry points -- just print the
|
||||
# packages unless we need to load them
|
||||
dist = entry_point.dist
|
||||
versions[dist.name] = dist.version
|
||||
|
||||
for k, v in versions.items():
|
||||
if k != "wpilib":
|
||||
logger.debug("%s version %s", k, v)
|
||||
|
||||
|
||||
class Main:
|
||||
"""
|
||||
Executes the robot code using the currently installed HAL (this is probably not what you want unless you're on the roboRIO)
|
||||
"""
|
||||
|
||||
def __init__(self, parser):
|
||||
pass
|
||||
|
||||
def run(self, options, robot_class, **static_options):
|
||||
return robot_class.main(robot_class)
|
||||
|
||||
|
||||
class RobotStarter:
|
||||
def __init__(self):
|
||||
self.logger = logging.getLogger("robotpy")
|
||||
self.robot = None
|
||||
self.suppressExitWarning = False
|
||||
self._robotpy_version = None
|
||||
|
||||
@property
|
||||
def robotpy_version(self) -> typing.Optional[str]:
|
||||
if not self._robotpy_version:
|
||||
try:
|
||||
pkg = importlib.metadata.metadata("robotpy")
|
||||
except importlib.metadata.PackageNotFoundError:
|
||||
pass
|
||||
else:
|
||||
self._robotpy_version = pkg.get("Version", None)
|
||||
|
||||
return self._robotpy_version
|
||||
|
||||
def run(self, robot_cls: wpilib.RobotBase) -> bool:
|
||||
_log_versions(self.robotpy_version)
|
||||
|
||||
retval = False
|
||||
if hal.hasMain():
|
||||
rval = [False]
|
||||
|
||||
def _start():
|
||||
try:
|
||||
rval[0] = self.start(robot_cls)
|
||||
finally:
|
||||
hal.exitMain()
|
||||
|
||||
th = threading.Thread(target=_start, name="RobotThread", daemon=True)
|
||||
th.start()
|
||||
try:
|
||||
hal.runMain()
|
||||
except KeyboardInterrupt:
|
||||
self.logger.exception(
|
||||
"THIS IS NOT AN ERROR: The user hit CTRL-C to kill the robot"
|
||||
)
|
||||
self.logger.info("Exiting because of keyboard interrupt")
|
||||
|
||||
self.suppressExitWarning = True
|
||||
robot = self.robot
|
||||
if robot:
|
||||
try:
|
||||
robot.endCompetition()
|
||||
except:
|
||||
self.logger.warning("endCompetition raised an exception")
|
||||
|
||||
th.join(1)
|
||||
if th.is_alive():
|
||||
self.logger.warning("robot thread didn't die, crash may occur next!")
|
||||
retval = rval[0]
|
||||
else:
|
||||
retval = self.start(robot_cls)
|
||||
|
||||
from wpilib import RobotBase
|
||||
|
||||
if RobotBase.isSimulation():
|
||||
import wpilib.simulation
|
||||
|
||||
wpilib.simulation._simulation._resetMotorSafety()
|
||||
|
||||
return retval
|
||||
|
||||
def start(self, robot_cls: wpilib.RobotBase) -> bool:
|
||||
try:
|
||||
return self._start(robot_cls)
|
||||
except:
|
||||
reportErrorInternal(
|
||||
"The robot program quit unexpectedly. This is usually due to a code error.\n"
|
||||
"The above stacktrace can help determine where the error occurred.\n",
|
||||
True,
|
||||
)
|
||||
return False
|
||||
|
||||
def _start(self, robot_cls: wpilib.RobotBase) -> bool:
|
||||
hal.reportUsage("Language", "Python")
|
||||
|
||||
if not wpilib.Notifier.setHALThreadPriority(True, 40):
|
||||
reportErrorInternal(
|
||||
"Setting HAL Notifier RT priority to 40 failed", isWarning=True
|
||||
)
|
||||
|
||||
isSimulation = wpilib.RobotBase.isSimulation()
|
||||
|
||||
# hack: initialize networktables before creating the robot
|
||||
# class, otherwise our logger doesn't get created
|
||||
import ntcore
|
||||
|
||||
inst = ntcore.NetworkTableInstance.getDefault()
|
||||
|
||||
# subscribe to "" to force persistent values to progagate to local
|
||||
msub = ntcore.MultiSubscriber(inst, [""])
|
||||
|
||||
if not isSimulation:
|
||||
inst.startServer("/home/systemcore/networktables.ini")
|
||||
else:
|
||||
inst.startServer()
|
||||
|
||||
# wait for the NT server to actually start
|
||||
for i in range(100):
|
||||
if (
|
||||
inst.getNetworkMode()
|
||||
& ntcore.NetworkTableInstance.NetworkMode.kNetModeStarting
|
||||
) == 0:
|
||||
break
|
||||
# real sleep since we're waiting for the server, not simulated sleep
|
||||
time.sleep(0.010)
|
||||
else:
|
||||
reportErrorInternal(
|
||||
"timed out while waiting for NT server to start", isWarning=True
|
||||
)
|
||||
|
||||
wpilib.SmartDashboard.init()
|
||||
|
||||
# Call DriverStation.refreshData() to kick things off
|
||||
wpilib.DriverStation.refreshData()
|
||||
|
||||
try:
|
||||
self.robot = robot_cls()
|
||||
except:
|
||||
reportError(
|
||||
f"Unhandled exception instantiating robot {robot_cls.__name__}", True
|
||||
)
|
||||
reportErrorInternal(f"Could not instantiate robot {robot_cls.__name__}!")
|
||||
raise
|
||||
|
||||
# TODO: Add a check to see if the user forgot to call super().__init__()
|
||||
# if not hasattr(robot, "_RobotBase__initialized"):
|
||||
# logger.error(
|
||||
# "If your robot class has an __init__ function, it must call super().__init__()!"
|
||||
# )
|
||||
# return False
|
||||
|
||||
try:
|
||||
self.robot.startCompetition()
|
||||
except KeyboardInterrupt:
|
||||
self.robot = None
|
||||
self.logger.exception(
|
||||
"THIS IS NOT AN ERROR: The user hit CTRL-C to kill the robot"
|
||||
)
|
||||
self.logger.info("Exiting because of keyboard interrupt")
|
||||
return True
|
||||
except:
|
||||
self.robot = None
|
||||
|
||||
reportError("Unhandled exception", True)
|
||||
raise
|
||||
else:
|
||||
self.robot = None
|
||||
if self.suppressExitWarning:
|
||||
self.logger.info("Robot code exited")
|
||||
return True
|
||||
else:
|
||||
# startCompetition never returns unless exception occurs....
|
||||
reportError("Unexpected return from startCompetition() method.", False)
|
||||
return False
|
||||
231
wpilibc/src/main/python/wpilib/_impl/utils.py
Normal file
231
wpilibc/src/main/python/wpilib/_impl/utils.py
Normal file
@@ -0,0 +1,231 @@
|
||||
# novalidate
|
||||
|
||||
import sys
|
||||
import inspect
|
||||
|
||||
|
||||
def match_arglist(name, args, kwargs, templates, allow_extra_kwargs=False):
|
||||
"""
|
||||
This compares args and kwargs against the argument templates in templates.
|
||||
|
||||
:param name: Name of the function being checked
|
||||
:param args: The list of positional arguments
|
||||
:param kwargs: The list of keyword arguments
|
||||
:param templates: A list of dictionaries corresponding to possible
|
||||
argument list formats.
|
||||
:param allow_extra_kwargs: Whether or not to allow extra keyword arguments. If this
|
||||
is true, then extra keyword arguments will be added to the result dictionary.
|
||||
|
||||
An argument template is structured as follows:
|
||||
|
||||
Each element in the list should be a tuple, corresponding to an argument name,
|
||||
and argument type condition. See types_match() for argument type condition structures.
|
||||
|
||||
:returns The id of the selected template
|
||||
:returns A dictionary of argument name, value tuples.
|
||||
:
|
||||
"""
|
||||
return __match_arglist(name, args, kwargs, templates, False, allow_extra_kwargs)
|
||||
|
||||
|
||||
def _print(*args, **kwargs):
|
||||
print(*args, **kwargs)
|
||||
|
||||
|
||||
def __match_arglist(name, args, kwargs, templates, err, allow_extra_kwargs=False):
|
||||
# TODO: we can do better at giving the user an error message...
|
||||
|
||||
if err:
|
||||
_print("*" * 50)
|
||||
_print("ERROR: Invalid arguments passed to %s()!!" % name)
|
||||
_print(" checking args against %s possible templates" % len(templates))
|
||||
_print("*" * 50)
|
||||
if len(args):
|
||||
_print("Your non-keyword arguments: ")
|
||||
for i, arg in enumerate(args):
|
||||
_print(" #%d: value %s, type %s" % (i, arg, type(arg)))
|
||||
if len(kwargs):
|
||||
_print("Your keyword args: ")
|
||||
for k, v in kwargs.items():
|
||||
_print(" %s: value %s, type %s" % (k, v, type(v)))
|
||||
_print("*" * 50)
|
||||
|
||||
# Try each template until we find one that works
|
||||
for i, template in enumerate(templates):
|
||||
# List copies of the arguments
|
||||
args_copy = list(reversed(args))
|
||||
kwargs_copy = list(kwargs.copy())
|
||||
kwarg_found = False
|
||||
results = dict()
|
||||
|
||||
if err:
|
||||
_print(
|
||||
"Checking template %s: %s" % (i, ", ".join(an for an, _ in template))
|
||||
)
|
||||
showed_error = False
|
||||
|
||||
# Scan through all arguments and set valid to false if we find an issue.
|
||||
for j, (arg_name, arg_type_condition) in enumerate(template):
|
||||
# Check kwargs first, then check args
|
||||
if arg_name in kwargs_copy:
|
||||
kwargs_copy.remove(arg_name)
|
||||
value = kwargs[arg_name]
|
||||
match_type = "keyword"
|
||||
kwarg_found = True
|
||||
elif not kwarg_found and len(args_copy) > 0:
|
||||
value = args_copy.pop()
|
||||
match_type = "non-keyword"
|
||||
else:
|
||||
value = None
|
||||
match_type = "optional"
|
||||
|
||||
results[arg_name] = value
|
||||
|
||||
# Check to see if identities match:
|
||||
if not types_match(value, arg_type_condition):
|
||||
if err:
|
||||
_print(
|
||||
"- Error at arg %d: %s != %s"
|
||||
% (j, arg_name, typematch_to_str(arg_type_condition))
|
||||
)
|
||||
_print(
|
||||
" your arg: %s; value %s %s"
|
||||
% (match_type, value, type(value))
|
||||
)
|
||||
_print()
|
||||
showed_error = True
|
||||
break
|
||||
|
||||
else:
|
||||
# If the results are valid and the argument lists are empty, return the results.
|
||||
if len(args_copy) == 0 and (len(kwargs_copy) == 0 or allow_extra_kwargs):
|
||||
output = kwargs.copy()
|
||||
output.update(results)
|
||||
return templates.index(template), output
|
||||
|
||||
if err and not showed_error:
|
||||
if len(args_copy) != 0:
|
||||
_print("- Error: too many arguments")
|
||||
elif len(kwargs_copy):
|
||||
_print(
|
||||
"- Error: unused parameters: %s"
|
||||
% ", ".join("%s" % s for s in kwargs_copy)
|
||||
)
|
||||
|
||||
_print()
|
||||
# _print("Template %s unmatched:" % i)
|
||||
# if len(args_copy):
|
||||
# _print("- Args: %s" % (' '.join('%s' % s for s in args_copy)))
|
||||
# if len(kwargs_copy):
|
||||
# _print("- Kwargs: %s" % (' '.join('%s' % s for s in kwargs_copy)))
|
||||
|
||||
# We found nothing, then... but we need to give the user a good
|
||||
# error message, so run it again in verbose mode, then raise the error!
|
||||
if not err:
|
||||
__match_arglist(name, args, kwargs, templates, True)
|
||||
else:
|
||||
_print("*" * 50)
|
||||
raise ValueError(
|
||||
"Attribute error, attributes given did not match any argument templates. See messages above for more info"
|
||||
)
|
||||
|
||||
|
||||
def types_match(object, type_structure):
|
||||
"""
|
||||
:param object: the object to check
|
||||
:param type_structure: The structure, composed of lists, strings, and types,
|
||||
representing conditions that object must meet.
|
||||
|
||||
Here are the possibilities for type_structure:
|
||||
- HasAttribute
|
||||
- type_condition
|
||||
- list of type_structures
|
||||
- None
|
||||
|
||||
If type_structure is an HasAttribute, this will return False if
|
||||
any contained attribute is not present in object
|
||||
|
||||
If type_structure is a type_condition, then object must be of the same type
|
||||
|
||||
If type_structure is a list of type_structures, it is handled by running
|
||||
types_match on each item, and returning true if at least one matches.
|
||||
|
||||
If type_structure is None, than everything matches it, and true is returned.
|
||||
"""
|
||||
|
||||
if type_structure is None:
|
||||
return True
|
||||
|
||||
# Is it an attribute list?
|
||||
elif hasattr(type_structure, "matches"):
|
||||
return type_structure.matches(object)
|
||||
|
||||
elif isinstance(type_structure, list) and len(type_structure) != 0:
|
||||
# If the list is not an attribute condition, check each element
|
||||
# and return true if a match is found
|
||||
for type_condition in type_structure:
|
||||
if types_match(object, type_condition):
|
||||
return True
|
||||
else:
|
||||
return isinstance(object, type_structure)
|
||||
|
||||
|
||||
def typematch_to_str(type_structure):
|
||||
"""Only used for debugging"""
|
||||
|
||||
if type_structure is None:
|
||||
return "always matches"
|
||||
|
||||
elif isinstance(type_structure, HasAttribute):
|
||||
return "hasattr(%s)" % " or ".join(type_structure.conditions)
|
||||
|
||||
elif isinstance(type_structure, list):
|
||||
return " or ".join(typematch_to_str(tc) for tc in type_structure)
|
||||
|
||||
else:
|
||||
return "%s" % type_structure
|
||||
|
||||
|
||||
class HasAttribute:
|
||||
def __init__(self, *args):
|
||||
self.conditions = args[:]
|
||||
|
||||
def matches(self, object):
|
||||
for attribute in self.conditions:
|
||||
if not hasattr(object, attribute):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
|
||||
def reset_wpilib():
|
||||
"""
|
||||
Clears all devices from WPILib, and resets the hal data
|
||||
|
||||
.. warning:: This is only intended to be used by test frameworks and
|
||||
other debugging tools with the simulated HAL!
|
||||
"""
|
||||
|
||||
raise NotImplementedError("Not supported yet for 2020")
|
||||
|
||||
modules = [
|
||||
"wpilib",
|
||||
"wpilib.buttons",
|
||||
"wpilib.command",
|
||||
"wpilib.interfaces",
|
||||
"wpilib.shuffleboard",
|
||||
]
|
||||
|
||||
for modname in modules:
|
||||
try:
|
||||
module = sys.modules[modname]
|
||||
except KeyError:
|
||||
continue
|
||||
|
||||
for _, cls in inspect.getmembers(module, inspect.isclass):
|
||||
if hasattr(cls, "_reset"):
|
||||
cls._reset()
|
||||
|
||||
import hal_impl.functions
|
||||
|
||||
hal_impl.functions.reset_hal()
|
||||
94
wpilibc/src/main/python/wpilib/cameraserver.py
Normal file
94
wpilibc/src/main/python/wpilib/cameraserver.py
Normal file
@@ -0,0 +1,94 @@
|
||||
from typing import Optional
|
||||
|
||||
import hal
|
||||
import subprocess
|
||||
import threading
|
||||
|
||||
import logging
|
||||
|
||||
logger = logging.getLogger("wpilib.cs")
|
||||
|
||||
__all__ = ["CameraServer"]
|
||||
|
||||
|
||||
class CameraServer:
|
||||
"""
|
||||
Provides a way to launch an out of process cscore-based camera
|
||||
service instance, for streaming or for image processing.
|
||||
|
||||
.. note:: This does not correspond directly to the wpilib
|
||||
CameraServer object; that can be found as
|
||||
:class:`cscore.CameraServer`. However, you should
|
||||
not use cscore directly from your robot code, see
|
||||
the documentation for details
|
||||
"""
|
||||
|
||||
_alive = False
|
||||
_launched = False
|
||||
|
||||
@classmethod
|
||||
def is_alive(cls) -> bool:
|
||||
""":returns: True if the CameraServer is still alive"""
|
||||
return cls._alive
|
||||
|
||||
@classmethod
|
||||
def launch(cls, vision_py: Optional[str] = None) -> None:
|
||||
"""
|
||||
Launches the CameraServer process in autocapture mode or
|
||||
using a user-specified python script
|
||||
|
||||
:param vision_py: If specified, this is the relative path to
|
||||
a filename with a function in it
|
||||
|
||||
Example usage::
|
||||
|
||||
wpilib.CameraServer.launch("vision.py:main")
|
||||
|
||||
.. warning:: You must have robotpy-cscore installed, or this
|
||||
function will fail without returning an error
|
||||
(you will see an error in the console).
|
||||
|
||||
"""
|
||||
|
||||
if cls._launched:
|
||||
return
|
||||
|
||||
cls._launched = True
|
||||
from ._wpilib import RobotBase
|
||||
|
||||
if RobotBase.isSimulation():
|
||||
logger.info("Would launch CameraServer with vision_py=%s", vision_py)
|
||||
cls._alive = True
|
||||
else:
|
||||
logger.info("Launching CameraServer process")
|
||||
|
||||
# Launch the cscore launcher in a separate process
|
||||
import sys
|
||||
|
||||
args = [sys.executable, "-m", "cscore"]
|
||||
|
||||
# TODO: Get accurate reporting data from the other cscore process. For
|
||||
# now, just differentiate between users with a custom py file and those
|
||||
# who do not. cscore handle values indicate type with bits 24-30
|
||||
|
||||
if vision_py:
|
||||
hal.reportUsage("RobotPy/CameraServer", vision_py)
|
||||
if not vision_py.startswith("/"):
|
||||
vision_py = "/home/systemcore/py/" + vision_py
|
||||
args.append(vision_py)
|
||||
else:
|
||||
hal.reportUsage("RobotPy/CameraServer", "")
|
||||
|
||||
# We open a pipe to it so that when this process exits, it dies
|
||||
proc = subprocess.Popen(
|
||||
args, close_fds=True, stdin=subprocess.PIPE, cwd="/home/systemcore/py"
|
||||
)
|
||||
th = threading.Thread(target=cls._monitor_child, args=(proc,))
|
||||
th.daemon = True
|
||||
th.start()
|
||||
|
||||
@classmethod
|
||||
def _monitor_child(cls, proc: subprocess.Popen) -> None:
|
||||
proc.wait()
|
||||
logger.warning("CameraServer process exited with exitcode %s", proc.returncode)
|
||||
cls._alive = False
|
||||
6
wpilibc/src/main/python/wpilib/counter/__init__.py
Normal file
6
wpilibc/src/main/python/wpilib/counter/__init__.py
Normal file
@@ -0,0 +1,6 @@
|
||||
from . import _init__counter
|
||||
|
||||
# autogenerated by 'semiwrap create-imports wpilib.counter wpilib.counter._counter'
|
||||
from ._counter import EdgeConfiguration, Tachometer, UpDownCounter
|
||||
|
||||
__all__ = ["EdgeConfiguration", "Tachometer", "UpDownCounter"]
|
||||
7
wpilibc/src/main/python/wpilib/counter/counter.cpp
Normal file
7
wpilibc/src/main/python/wpilib/counter/counter.cpp
Normal file
@@ -0,0 +1,7 @@
|
||||
|
||||
#include "semiwrap_init.wpilib.counter._counter.hpp"
|
||||
|
||||
SEMIWRAP_PYBIND11_MODULE(m)
|
||||
{
|
||||
initWrapper(m);
|
||||
}
|
||||
33
wpilibc/src/main/python/wpilib/deployinfo.py
Normal file
33
wpilibc/src/main/python/wpilib/deployinfo.py
Normal file
@@ -0,0 +1,33 @@
|
||||
from ._wpilib import RobotBase
|
||||
import json
|
||||
import typing
|
||||
|
||||
|
||||
def getDeployData() -> typing.Optional[typing.Dict[str, str]]:
|
||||
"""
|
||||
Utility function useful for retrieving deploy-related information
|
||||
that pyfrc stores with your robot code. The dictionary has the
|
||||
following keys:
|
||||
|
||||
* deploy-host
|
||||
* deploy-user
|
||||
* deploy-date
|
||||
* code-path
|
||||
|
||||
If the code is deployed from a git repo, and the git program is in
|
||||
your path, the following keys will also be present:
|
||||
|
||||
* git-hash
|
||||
* git-desc
|
||||
* git-branch
|
||||
|
||||
:returns: None in simulation, or a dictionary
|
||||
"""
|
||||
if not RobotBase.isReal():
|
||||
return None
|
||||
|
||||
try:
|
||||
with open("/home/systemcore/py/deploy.json") as fp:
|
||||
return json.load(fp)
|
||||
except FileNotFoundError:
|
||||
return {}
|
||||
8
wpilibc/src/main/python/wpilib/drive/__init__.py
Normal file
8
wpilibc/src/main/python/wpilib/drive/__init__.py
Normal file
@@ -0,0 +1,8 @@
|
||||
from . import _init__drive
|
||||
|
||||
# autogenerated by 'semiwrap create-imports wpilib.drive wpilib.drive._drive'
|
||||
from ._drive import DifferentialDrive, MecanumDrive, RobotDriveBase
|
||||
|
||||
__all__ = ["DifferentialDrive", "MecanumDrive", "RobotDriveBase"]
|
||||
|
||||
del _init__drive
|
||||
7
wpilibc/src/main/python/wpilib/drive/drive.cpp
Normal file
7
wpilibc/src/main/python/wpilib/drive/drive.cpp
Normal file
@@ -0,0 +1,7 @@
|
||||
|
||||
#include "semiwrap_init.wpilib.drive._drive.hpp"
|
||||
|
||||
SEMIWRAP_PYBIND11_MODULE(m)
|
||||
{
|
||||
initWrapper(m);
|
||||
}
|
||||
6
wpilibc/src/main/python/wpilib/event/__init__.py
Normal file
6
wpilibc/src/main/python/wpilib/event/__init__.py
Normal file
@@ -0,0 +1,6 @@
|
||||
from . import _init__event
|
||||
|
||||
# autogenerated by 'semiwrap create-imports wpilib.event wpilib.event._event'
|
||||
from ._event import BooleanEvent, EventLoop, NetworkBooleanEvent
|
||||
|
||||
__all__ = ["BooleanEvent", "EventLoop", "NetworkBooleanEvent"]
|
||||
7
wpilibc/src/main/python/wpilib/event/event.cpp
Normal file
7
wpilibc/src/main/python/wpilib/event/event.cpp
Normal file
@@ -0,0 +1,7 @@
|
||||
|
||||
#include "semiwrap_init.wpilib.event._event.hpp"
|
||||
|
||||
SEMIWRAP_PYBIND11_MODULE(m)
|
||||
{
|
||||
initWrapper(m);
|
||||
}
|
||||
4
wpilibc/src/main/python/wpilib/interfaces/__init__.py
Normal file
4
wpilibc/src/main/python/wpilib/interfaces/__init__.py
Normal file
@@ -0,0 +1,4 @@
|
||||
# autogenerated by 'semiwrap create-imports wpilib.interfaces wpilib._wpilib.interfaces'
|
||||
from .._wpilib.interfaces import CounterBase, GenericHID, MotorController
|
||||
|
||||
__all__ = ["CounterBase", "GenericHID", "MotorController"]
|
||||
0
wpilibc/src/main/python/wpilib/py.typed
Normal file
0
wpilibc/src/main/python/wpilib/py.typed
Normal file
117
wpilibc/src/main/python/wpilib/simulation/__init__.py
Normal file
117
wpilibc/src/main/python/wpilib/simulation/__init__.py
Normal file
@@ -0,0 +1,117 @@
|
||||
from . import _init__simulation
|
||||
|
||||
# needed for dcmotor return value, TODO fix in robotpy-build
|
||||
from wpimath._controls._controls import plant as _
|
||||
|
||||
# autogenerated by 'semiwrap create-imports wpilib.simulation wpilib.simulation._simulation'
|
||||
from ._simulation import (
|
||||
ADXL345Sim,
|
||||
AddressableLEDSim,
|
||||
AnalogEncoderSim,
|
||||
AnalogInputSim,
|
||||
BatterySim,
|
||||
CTREPCMSim,
|
||||
CallbackStore,
|
||||
DCMotorSim,
|
||||
DIOSim,
|
||||
DifferentialDrivetrainSim,
|
||||
DigitalPWMSim,
|
||||
DoubleSolenoidSim,
|
||||
DriverStationSim,
|
||||
DutyCycleEncoderSim,
|
||||
DutyCycleSim,
|
||||
ElevatorSim,
|
||||
EncoderSim,
|
||||
FlywheelSim,
|
||||
GenericHIDSim,
|
||||
JoystickSim,
|
||||
LinearSystemSim_1_1_1,
|
||||
LinearSystemSim_1_1_2,
|
||||
LinearSystemSim_2_1_1,
|
||||
LinearSystemSim_2_1_2,
|
||||
LinearSystemSim_2_2_1,
|
||||
LinearSystemSim_2_2_2,
|
||||
PS4ControllerSim,
|
||||
PS5ControllerSim,
|
||||
PWMMotorControllerSim,
|
||||
PWMSim,
|
||||
PneumaticsBaseSim,
|
||||
PowerDistributionSim,
|
||||
REVPHSim,
|
||||
RoboRioSim,
|
||||
SendableChooserSim,
|
||||
ServoSim,
|
||||
SharpIRSim,
|
||||
SimDeviceSim,
|
||||
SingleJointedArmSim,
|
||||
SolenoidSim,
|
||||
StadiaControllerSim,
|
||||
XboxControllerSim,
|
||||
getProgramStarted,
|
||||
isTimingPaused,
|
||||
pauseTiming,
|
||||
restartTiming,
|
||||
resumeTiming,
|
||||
setProgramStarted,
|
||||
setRuntimeType,
|
||||
stepTiming,
|
||||
stepTimingAsync,
|
||||
waitForProgramStart,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"ADXL345Sim",
|
||||
"AddressableLEDSim",
|
||||
"AnalogEncoderSim",
|
||||
"AnalogInputSim",
|
||||
"BatterySim",
|
||||
"CTREPCMSim",
|
||||
"CallbackStore",
|
||||
"DCMotorSim",
|
||||
"DIOSim",
|
||||
"DifferentialDrivetrainSim",
|
||||
"DigitalPWMSim",
|
||||
"DoubleSolenoidSim",
|
||||
"DriverStationSim",
|
||||
"DutyCycleEncoderSim",
|
||||
"DutyCycleSim",
|
||||
"ElevatorSim",
|
||||
"EncoderSim",
|
||||
"FlywheelSim",
|
||||
"GenericHIDSim",
|
||||
"JoystickSim",
|
||||
"LinearSystemSim_1_1_1",
|
||||
"LinearSystemSim_1_1_2",
|
||||
"LinearSystemSim_2_1_1",
|
||||
"LinearSystemSim_2_1_2",
|
||||
"LinearSystemSim_2_2_1",
|
||||
"LinearSystemSim_2_2_2",
|
||||
"PS4ControllerSim",
|
||||
"PS5ControllerSim",
|
||||
"PWMMotorControllerSim",
|
||||
"PWMSim",
|
||||
"PneumaticsBaseSim",
|
||||
"PowerDistributionSim",
|
||||
"REVPHSim",
|
||||
"RoboRioSim",
|
||||
"SendableChooserSim",
|
||||
"ServoSim",
|
||||
"SharpIRSim",
|
||||
"SimDeviceSim",
|
||||
"SingleJointedArmSim",
|
||||
"SolenoidSim",
|
||||
"StadiaControllerSim",
|
||||
"XboxControllerSim",
|
||||
"getProgramStarted",
|
||||
"isTimingPaused",
|
||||
"pauseTiming",
|
||||
"restartTiming",
|
||||
"resumeTiming",
|
||||
"setProgramStarted",
|
||||
"setRuntimeType",
|
||||
"stepTiming",
|
||||
"stepTimingAsync",
|
||||
"waitForProgramStart",
|
||||
]
|
||||
|
||||
del _init__simulation
|
||||
36
wpilibc/src/main/python/wpilib/simulation/simulation.cpp
Normal file
36
wpilibc/src/main/python/wpilib/simulation/simulation.cpp
Normal file
@@ -0,0 +1,36 @@
|
||||
|
||||
#include "semiwrap_init.wpilib.simulation._simulation.hpp"
|
||||
|
||||
#ifndef __FRC_SYSTEMCORE__
|
||||
|
||||
namespace frc::impl {
|
||||
void ResetSmartDashboardInstance();
|
||||
void ResetMotorSafety();
|
||||
} // namespace frc::impl
|
||||
|
||||
namespace wpi::impl {
|
||||
void ResetSendableRegistry();
|
||||
} // namespace wpi::impl
|
||||
|
||||
void resetWpilibSimulationData() {
|
||||
frc::impl::ResetSmartDashboardInstance();
|
||||
frc::impl::ResetMotorSafety();
|
||||
wpi::impl::ResetSendableRegistry();
|
||||
}
|
||||
|
||||
void resetMotorSafety() {
|
||||
frc::impl::ResetMotorSafety();
|
||||
}
|
||||
|
||||
#else
|
||||
void resetWpilibSimulationData() {}
|
||||
void resetMotorSafety() {}
|
||||
#endif
|
||||
|
||||
SEMIWRAP_PYBIND11_MODULE(m) {
|
||||
initWrapper(m);
|
||||
|
||||
m.def("_resetWpilibSimulationData", &resetWpilibSimulationData,
|
||||
release_gil());
|
||||
m.def("_resetMotorSafety", &resetMotorSafety, release_gil());
|
||||
}
|
||||
4
wpilibc/src/main/python/wpilib/src/main.cpp
Normal file
4
wpilibc/src/main/python/wpilib/src/main.cpp
Normal file
@@ -0,0 +1,4 @@
|
||||
|
||||
#include "semiwrap_init.wpilib._wpilib.hpp"
|
||||
|
||||
SEMIWRAP_PYBIND11_MODULE(m) { initWrapper(m); }
|
||||
18
wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp
Normal file
18
wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp
Normal file
@@ -0,0 +1,18 @@
|
||||
#include "rpy/ControlWord.h"
|
||||
|
||||
#include <hal/DriverStation.h>
|
||||
|
||||
namespace rpy {
|
||||
|
||||
std::tuple<bool, bool, bool> GetControlState() {
|
||||
HAL_ControlWord controlWord;
|
||||
HAL_GetControlWord(&controlWord);
|
||||
|
||||
bool enable = controlWord.enabled != 0 && controlWord.dsAttached != 0;
|
||||
bool auton = controlWord.autonomous != 0;
|
||||
bool test = controlWord.test != 0;
|
||||
|
||||
return std::make_tuple(enable, auton, test);
|
||||
}
|
||||
|
||||
} // namespace rpy
|
||||
7
wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h
Normal file
7
wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h
Normal file
@@ -0,0 +1,7 @@
|
||||
#include <tuple>
|
||||
|
||||
namespace rpy {
|
||||
|
||||
std::tuple<bool, bool, bool> GetControlState();
|
||||
|
||||
} // namespace rpy
|
||||
33
wpilibc/src/main/python/wpilib/src/rpy/Filesystem.h
Normal file
33
wpilibc/src/main/python/wpilib/src/rpy/Filesystem.h
Normal file
@@ -0,0 +1,33 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/fs.h>
|
||||
|
||||
namespace robotpy::filesystem {
|
||||
|
||||
/**
|
||||
* Obtains the operating directory of the program. On the robot, this
|
||||
* is /home/systemcore/py. In simulation, it is the location of robot.py
|
||||
*
|
||||
* @return The result of the operating directory lookup.
|
||||
*/
|
||||
std::string GetOperatingDirectory();
|
||||
|
||||
/**
|
||||
* Obtains the deploy directory of the program, which is the remote location
|
||||
* the deploy directory is deployed to by default. On the robot, this is
|
||||
* /home/systemcore/py/deploy. In simulation, it is where the simulation was launched
|
||||
* from, in the subdirectory "deploy" (`dirname(robot.py)`/deploy).
|
||||
*
|
||||
* @return The result of the operating directory lookup
|
||||
*/
|
||||
std::string GetDeployDirectory();
|
||||
|
||||
// intended to be used by C++ bindings, returns same as GetOperatingDirectory
|
||||
fs::path GetOperatingDirectoryFs();
|
||||
// intended to be used by C++ bindings, returns same as GetDeployDirectory
|
||||
fs::path GetDeployDirectoryFs();
|
||||
|
||||
} // namespace robotpy::filesystem
|
||||
|
||||
#include "Filesystem.inc"
|
||||
54
wpilibc/src/main/python/wpilib/src/rpy/Filesystem.inc
Normal file
54
wpilibc/src/main/python/wpilib/src/rpy/Filesystem.inc
Normal file
@@ -0,0 +1,54 @@
|
||||
|
||||
// TODO: this should be in a shared library, but robotpy-build does not support that
|
||||
|
||||
#include <pybind11/eval.h>
|
||||
#include <semiwrap.h>
|
||||
|
||||
namespace robotpy::filesystem {
|
||||
|
||||
static fs::path getMainPath() {
|
||||
py::gil_scoped_acquire gil;
|
||||
py::dict locals;
|
||||
py::exec(R"(
|
||||
found = False
|
||||
try:
|
||||
from robotpy.main import robot_py_path
|
||||
if robot_py_path:
|
||||
main_path = str(robot_py_path.parent.absolute())
|
||||
found = True
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
if not found:
|
||||
import sys, os.path
|
||||
main = sys.modules['__main__'];
|
||||
if hasattr(main, '__file__'):
|
||||
main_path = os.path.abspath(os.path.dirname(main.__file__))
|
||||
)",
|
||||
py::globals(), locals);
|
||||
|
||||
if (locals.contains("main_path")) {
|
||||
return fs::path(py::cast<std::string>(locals["main_path"]));
|
||||
} else {
|
||||
#ifdef __FRC_SYSTEMCORE__
|
||||
return fs::path("/home/systemcore/py");
|
||||
#else
|
||||
return fs::current_path();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
inline std::string GetOperatingDirectory() {
|
||||
return GetOperatingDirectoryFs().string();
|
||||
}
|
||||
|
||||
inline std::string GetDeployDirectory() { return GetDeployDirectoryFs().string(); }
|
||||
|
||||
inline fs::path GetOperatingDirectoryFs() {
|
||||
static fs::path operatingPath = getMainPath();
|
||||
return operatingPath;
|
||||
}
|
||||
|
||||
inline fs::path GetDeployDirectoryFs() { return GetOperatingDirectoryFs() / "deploy"; }
|
||||
|
||||
} // namespace robotpy::filesystem
|
||||
@@ -0,0 +1,62 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "MotorControllerGroup.h"
|
||||
|
||||
#include "wpi/sendable/SendableBuilder.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
void PyMotorControllerGroup::Initialize() {
|
||||
for (auto motorController : m_motorControllers) {
|
||||
wpi::SendableRegistry::AddChild(this, motorController.get());
|
||||
}
|
||||
static int instances = 0;
|
||||
++instances;
|
||||
wpi::SendableRegistry::Add(this, "MotorControllerGroup", instances);
|
||||
}
|
||||
|
||||
void PyMotorControllerGroup::Set(double speed) {
|
||||
for (auto motorController : m_motorControllers) {
|
||||
motorController->Set(m_isInverted ? -speed : speed);
|
||||
}
|
||||
}
|
||||
|
||||
void PyMotorControllerGroup::SetVoltage(units::volt_t output) {
|
||||
for (auto motorController : m_motorControllers) {
|
||||
motorController->SetVoltage(m_isInverted ? -output : output);
|
||||
}
|
||||
}
|
||||
|
||||
double PyMotorControllerGroup::Get() const {
|
||||
if (!m_motorControllers.empty()) {
|
||||
return m_motorControllers.front()->Get() * (m_isInverted ? -1 : 1);
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
void PyMotorControllerGroup::SetInverted(bool isInverted) {
|
||||
m_isInverted = isInverted;
|
||||
}
|
||||
|
||||
bool PyMotorControllerGroup::GetInverted() const { return m_isInverted; }
|
||||
|
||||
void PyMotorControllerGroup::Disable() {
|
||||
for (auto motorController : m_motorControllers) {
|
||||
motorController->Disable();
|
||||
}
|
||||
}
|
||||
|
||||
void PyMotorControllerGroup::StopMotor() {
|
||||
for (auto motorController : m_motorControllers) {
|
||||
motorController->StopMotor();
|
||||
}
|
||||
}
|
||||
|
||||
void PyMotorControllerGroup::InitSendable(wpi::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("Motor Controller");
|
||||
builder.SetActuator(true);
|
||||
builder.AddDoubleProperty("Value", [=, this]() { return Get(); },
|
||||
[=, this](double value) { Set(value); });
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
|
||||
#include "frc/motorcontrol/MotorController.h"
|
||||
#include "wpi/sendable/Sendable.h"
|
||||
#include "wpi/sendable/SendableHelper.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
class PyMotorControllerGroup : public wpi::Sendable,
|
||||
public MotorController,
|
||||
public wpi::SendableHelper<PyMotorControllerGroup> {
|
||||
public:
|
||||
PyMotorControllerGroup(std::vector<std::shared_ptr<frc::MotorController>> &&args) :
|
||||
m_motorControllers(args) {}
|
||||
~PyMotorControllerGroup() override = default;
|
||||
|
||||
PyMotorControllerGroup(PyMotorControllerGroup&&) = default;
|
||||
PyMotorControllerGroup& operator=(PyMotorControllerGroup&&) = default;
|
||||
|
||||
void Set(double speed) override;
|
||||
void SetVoltage(units::volt_t output) override;
|
||||
double Get() const override;
|
||||
void SetInverted(bool isInverted) override;
|
||||
bool GetInverted() const override;
|
||||
void Disable() override;
|
||||
void StopMotor() override;
|
||||
|
||||
void InitSendable(wpi::SendableBuilder& builder) override;
|
||||
|
||||
private:
|
||||
void Initialize();
|
||||
|
||||
bool m_isInverted = false;
|
||||
std::vector<std::shared_ptr<frc::MotorController>> m_motorControllers;
|
||||
};
|
||||
|
||||
} // namespace rpy
|
||||
189
wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp
Normal file
189
wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp
Normal file
@@ -0,0 +1,189 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "rpy/Notifier.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <hal/Notifier.h>
|
||||
#include <hal/Threads.h>
|
||||
|
||||
#include "frc/Errors.h"
|
||||
#include "frc/Timer.h"
|
||||
|
||||
#include <pybind11/functional.h>
|
||||
#include <gilsafe_object.h>
|
||||
|
||||
using namespace frc;
|
||||
using namespace pybind11::literals;
|
||||
|
||||
// Hang the thread since returning to the caller is going to crash when we try
|
||||
// to obtain the GIL again
|
||||
// - this is a daemon thread so it's fine?
|
||||
// - Python 3.14 does this too
|
||||
static void _hang_thread_if_finalizing() {
|
||||
if (Py_IsFinalizing()) {
|
||||
while (true) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1000));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PyNotifier::PyNotifier(std::function<void()> handler) {
|
||||
if (!handler) {
|
||||
throw FRC_MakeError(err::NullParameter, "handler");
|
||||
}
|
||||
m_handler = handler;
|
||||
int32_t status = 0;
|
||||
m_notifier = HAL_InitializeNotifier(&status);
|
||||
FRC_CheckErrorStatus(status, "InitializeNotifier");
|
||||
|
||||
std::function<void()> target([this] {
|
||||
py::gil_scoped_release release;
|
||||
|
||||
try {
|
||||
for (;;) {
|
||||
int32_t status = 0;
|
||||
HAL_NotifierHandle notifier = m_notifier.load();
|
||||
if (notifier == 0) {
|
||||
break;
|
||||
}
|
||||
uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
|
||||
if (curTime == 0 || status != 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
std::function<void()> handler;
|
||||
{
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
handler = m_handler;
|
||||
if (m_periodic) {
|
||||
m_expirationTime += m_period;
|
||||
UpdateAlarm();
|
||||
} else {
|
||||
// need to update the alarm to cause it to wait again
|
||||
UpdateAlarm(UINT64_MAX);
|
||||
}
|
||||
}
|
||||
|
||||
// call callback
|
||||
if (handler) {
|
||||
if (Py_IsFinalizing()) {
|
||||
break;
|
||||
}
|
||||
|
||||
handler();
|
||||
}
|
||||
}
|
||||
} catch (...) {
|
||||
_hang_thread_if_finalizing();
|
||||
throw;
|
||||
}
|
||||
|
||||
_hang_thread_if_finalizing();
|
||||
});
|
||||
|
||||
py::gil_scoped_acquire acquire;
|
||||
|
||||
// create a python thread and start it
|
||||
auto Thread = py::module::import("threading").attr("Thread");
|
||||
m_thread = Thread("target"_a = target, "daemon"_a = true,
|
||||
"name"_a = "notifier-thread");
|
||||
m_thread.attr("start")();
|
||||
}
|
||||
|
||||
PyNotifier::~PyNotifier() {
|
||||
int32_t status = 0;
|
||||
// atomically set handle to 0, then clean
|
||||
HAL_NotifierHandle handle = m_notifier.exchange(0);
|
||||
HAL_StopNotifier(handle, &status);
|
||||
FRC_ReportError(status, "StopNotifier");
|
||||
|
||||
// Join the thread to ensure the handler has exited.
|
||||
if (m_thread) {
|
||||
m_thread.attr("join")();
|
||||
}
|
||||
|
||||
HAL_CleanNotifier(handle);
|
||||
}
|
||||
|
||||
PyNotifier::PyNotifier(PyNotifier &&rhs)
|
||||
: m_thread(std::move(rhs.m_thread)),
|
||||
m_notifier(rhs.m_notifier.load()),
|
||||
m_handler(std::move(rhs.m_handler)),
|
||||
m_expirationTime(std::move(rhs.m_expirationTime)),
|
||||
m_period(std::move(rhs.m_period)),
|
||||
m_periodic(std::move(rhs.m_periodic)) {
|
||||
rhs.m_notifier = HAL_kInvalidHandle;
|
||||
}
|
||||
|
||||
PyNotifier &PyNotifier::operator=(PyNotifier &&rhs) {
|
||||
m_thread = std::move(rhs.m_thread);
|
||||
m_notifier = rhs.m_notifier.load();
|
||||
rhs.m_notifier = HAL_kInvalidHandle;
|
||||
m_handler = std::move(rhs.m_handler);
|
||||
m_expirationTime = std::move(rhs.m_expirationTime);
|
||||
m_period = std::move(rhs.m_period);
|
||||
m_periodic = std::move(rhs.m_periodic);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void PyNotifier::SetName(std::string_view name) {
|
||||
fmt::memory_buffer buf;
|
||||
fmt::format_to(fmt::appender{buf}, "{}", name);
|
||||
buf.push_back('\0'); // null terminate
|
||||
int32_t status = 0;
|
||||
HAL_SetNotifierName(m_notifier, buf.data(), &status);
|
||||
}
|
||||
|
||||
void PyNotifier::SetCallback(std::function<void()> handler) {
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
m_handler = handler;
|
||||
}
|
||||
|
||||
void PyNotifier::StartSingle(units::second_t delay) {
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
m_periodic = false;
|
||||
m_period = delay;
|
||||
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
|
||||
UpdateAlarm();
|
||||
}
|
||||
|
||||
void PyNotifier::StartPeriodic(units::second_t period) {
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
m_periodic = true;
|
||||
m_period = period;
|
||||
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
|
||||
UpdateAlarm();
|
||||
}
|
||||
|
||||
void PyNotifier::Stop() {
|
||||
std::scoped_lock lock(m_processMutex);
|
||||
m_periodic = false;
|
||||
int32_t status = 0;
|
||||
HAL_CancelNotifierAlarm(m_notifier, &status);
|
||||
FRC_CheckErrorStatus(status, "CancelNotifierAlarm");
|
||||
}
|
||||
|
||||
void PyNotifier::UpdateAlarm(uint64_t triggerTime) {
|
||||
int32_t status = 0;
|
||||
// Return if we are being destructed, or were not created successfully
|
||||
auto notifier = m_notifier.load();
|
||||
if (notifier == 0) {
|
||||
return;
|
||||
}
|
||||
HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
|
||||
FRC_CheckErrorStatus(status, "UpdateNotifierAlarm");
|
||||
}
|
||||
|
||||
void PyNotifier::UpdateAlarm() {
|
||||
UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
|
||||
}
|
||||
|
||||
bool PyNotifier::SetHALThreadPriority(bool realTime, int32_t priority) {
|
||||
int32_t status = 0;
|
||||
return HAL_SetNotifierThreadPriority(realTime, priority, &status);
|
||||
}
|
||||
146
wpilibc/src/main/python/wpilib/src/rpy/Notifier.h
Normal file
146
wpilibc/src/main/python/wpilib/src/rpy/Notifier.h
Normal file
@@ -0,0 +1,146 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <string_view>
|
||||
#include <thread>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include <hal/Types.h>
|
||||
#include <units/time.h>
|
||||
#include <wpi/mutex.h>
|
||||
|
||||
#include <semiwrap.h>
|
||||
|
||||
namespace frc {
|
||||
|
||||
class PyNotifier {
|
||||
public:
|
||||
/**
|
||||
* Create a Notifier for timer event notification.
|
||||
*
|
||||
* @param handler The handler is called at the notification time which is set
|
||||
* using StartSingle or StartPeriodic.
|
||||
*/
|
||||
explicit PyNotifier(std::function<void()> handler);
|
||||
|
||||
// template <typename Callable, typename Arg, typename... Args>
|
||||
// PyNotifier(Callable &&f, Arg &&arg, Args &&... args)
|
||||
// : PyNotifier(std::bind(std::forward<Callable>(f),
|
||||
// std::forward<Arg>(arg),
|
||||
// std::forward<Args>(args)...)) {}
|
||||
|
||||
/**
|
||||
* Free the resources for a timer event.
|
||||
*/
|
||||
virtual ~PyNotifier();
|
||||
|
||||
PyNotifier(PyNotifier &&rhs);
|
||||
PyNotifier &operator=(PyNotifier &&rhs);
|
||||
|
||||
/**
|
||||
* Sets the name of the notifier. Used for debugging purposes only.
|
||||
*
|
||||
* @param name Name
|
||||
*/
|
||||
void SetName(std::string_view name);
|
||||
|
||||
/**
|
||||
* Change the handler function.
|
||||
*
|
||||
* @param handler Handler
|
||||
*/
|
||||
void SetCallback(std::function<void()> handler);
|
||||
|
||||
/**
|
||||
* Register for single event notification.
|
||||
*
|
||||
* A timer event is queued for a single event after the specified delay.
|
||||
*
|
||||
* @param delay Amount of time to wait before the handler is called.
|
||||
*/
|
||||
void StartSingle(units::second_t delay);
|
||||
|
||||
/**
|
||||
* Register for periodic event notification.
|
||||
*
|
||||
* A timer event is queued for periodic event notification. Each time the
|
||||
* interrupt occurs, the event will be immediately requeued for the same time
|
||||
* interval.
|
||||
*
|
||||
* @param period Period to call the handler starting one period
|
||||
* after the call to this method.
|
||||
*/
|
||||
void StartPeriodic(units::second_t period);
|
||||
|
||||
/**
|
||||
* Stop timer events from occurring.
|
||||
*
|
||||
* Stop any repeating timer events from occurring. This will also remove any
|
||||
* single notification events from the queue.
|
||||
*
|
||||
* If a timer-based call to the registered handler is in progress, this
|
||||
* function will block until the handler call is complete.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* Sets the HAL notifier thread priority.
|
||||
*
|
||||
* The HAL notifier thread is responsible for managing the FPGA's notifier
|
||||
* interrupt and waking up user's Notifiers when it's their time to run.
|
||||
* Giving the HAL notifier thread real-time priority helps ensure the user's
|
||||
* real-time Notifiers, if any, are notified to run in a timely manner.
|
||||
*
|
||||
* @param realTime Set to true to set a real-time priority, false for standard
|
||||
* priority.
|
||||
* @param priority Priority to set the thread to. For real-time, this is 1-99
|
||||
* with 99 being highest. For non-real-time, this is forced to
|
||||
* 0. See "man 7 sched" for more details.
|
||||
* @return True on success.
|
||||
*/
|
||||
static bool SetHALThreadPriority(bool realTime, int32_t priority);
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update the HAL alarm time.
|
||||
*
|
||||
* @param triggerTime the time at which the next alarm will be triggered
|
||||
*/
|
||||
void UpdateAlarm(uint64_t triggerTime);
|
||||
|
||||
/**
|
||||
* Update the HAL alarm time based on m_expirationTime.
|
||||
*/
|
||||
void UpdateAlarm();
|
||||
|
||||
// The thread waiting on the HAL alarm
|
||||
py::object m_thread;
|
||||
|
||||
// Held while updating process information
|
||||
wpi::mutex m_processMutex;
|
||||
|
||||
// HAL handle, atomic for proper destruction
|
||||
std::atomic<HAL_NotifierHandle> m_notifier{0};
|
||||
|
||||
// Address of the handler
|
||||
std::function<void()> m_handler;
|
||||
|
||||
// The absolute expiration time
|
||||
units::second_t m_expirationTime = 0_s;
|
||||
|
||||
// The relative time (either periodic or single)
|
||||
units::second_t m_period = 0_s;
|
||||
|
||||
// True if this is a periodic event
|
||||
bool m_periodic = false;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,43 @@
|
||||
|
||||
#include "SmartDashboardData.h"
|
||||
|
||||
namespace rpy {
|
||||
|
||||
//
|
||||
// Ensures that python objects added to the SmartDashboard have at least one
|
||||
// reference to them
|
||||
//
|
||||
// All functions here must be called with the GIL held
|
||||
//
|
||||
|
||||
static py::dict &getSmartDashboardData() {
|
||||
static py::dict data;
|
||||
return data;
|
||||
}
|
||||
|
||||
void addSmartDashboardData(py::str &key, std::shared_ptr<wpi::Sendable> data) {
|
||||
auto &sdData = getSmartDashboardData();
|
||||
sdData[key] = py::cast(data);
|
||||
}
|
||||
|
||||
void clearSmartDashboardData() {
|
||||
auto &sdData = getSmartDashboardData();
|
||||
if (sdData) {
|
||||
sdData.clear();
|
||||
}
|
||||
}
|
||||
|
||||
void destroySmartDashboardData() {
|
||||
auto &sdData = getSmartDashboardData();
|
||||
if (sdData) {
|
||||
sdData.clear();
|
||||
// force the dictionary to be deleted otherwise it'll crash when libc++
|
||||
// is unwinding static objects after interpreter destruction
|
||||
sdData.dec_ref();
|
||||
// release our reference to this otherwise the destructor will try to
|
||||
// delete a non-existant PyObject* after interpreter destruction
|
||||
sdData.release();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rpy
|
||||
17
wpilibc/src/main/python/wpilib/src/rpy/SmartDashboardData.h
Normal file
17
wpilibc/src/main/python/wpilib/src/rpy/SmartDashboardData.h
Normal file
@@ -0,0 +1,17 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <semiwrap.h>
|
||||
|
||||
namespace rpy {
|
||||
|
||||
//
|
||||
// These functions must be called with the GIL held
|
||||
//
|
||||
|
||||
void addSmartDashboardData(py::str &key, std::shared_ptr<wpi::Sendable> data);
|
||||
void clearSmartDashboardData();
|
||||
void destroySmartDashboardData();
|
||||
|
||||
} // namespace rpy
|
||||
4
wpilibc/src/main/python/wpilib/sysid/__init__.py
Normal file
4
wpilibc/src/main/python/wpilib/sysid/__init__.py
Normal file
@@ -0,0 +1,4 @@
|
||||
# autogenerated by 'semiwrap create-imports wpilib.sysid wpilib._wpilib.sysid'
|
||||
from .._wpilib.sysid import State, SysIdRoutineLog
|
||||
|
||||
__all__ = ["State", "SysIdRoutineLog"]
|
||||
Reference in New Issue
Block a user