def __integrator(self):
"""Thread function for integrating analog stick values.
Inputs from both analog sticks are multiplied by the respective gains
and integrated. This is done in a separate thread so that the
integrators can continue running even when the input thread is blocked
waiting for new events from the controller.
"""
# Make sure this thread does not have realtime priority
os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
while True:
time_loop_start = time.perf_counter()
if not self.running:
return
if self.integrator_mode:
self.int_x += self.left_gain * self.int_loop_period * self.left_x
self.int_y += self.left_gain * self.int_loop_period * self.left_y
self.int_x += self.right_gain * self.int_loop_period * self.right_x
self.int_y += self.right_gain * self.int_loop_period * self.right_y
self.int_x = np.clip(self.int_x, -self.int_limit, self.int_limit)
self.int_y = np.clip(self.int_y, -self.int_limit, self.int_limit)
time_elapsed = time.perf_counter() - time_loop_start
time_sleep = self.int_loop_period - time_elapsed
if time_sleep > 0:
time.sleep(time_sleep)
def priority(level=0, pid=0):
gc.disable() if level > 0 else gc.enable()
libc.munlockall()
if level == 1:
policy = os.SCHED_RR
elif level >= 2:
policy = os.SCHED_FIFO
else:
policy = os.SCHED_OTHER
sched_param = os.sched_param(os.sched_get_priority_max(policy))
try:
os.sched_setscheduler(pid, policy, sched_param)
except PermissionError:
return False
# try to lock memory (and we succeeded in sched already)
if level >= 2:
res = libc.mlockall(MCL_CURRENT | MCL_FUTURE)
if res != 0:
libc.munlockall()
return True
def set_high_priority(logger):
""" Change process scheduler and priority. """
# use "real time" scheduler
done = False
sched = os.SCHED_RR
if os.sched_getscheduler(0) == sched:
# already running with RR scheduler, likely set from init system, don't touch priority
done = True
else:
prio = (os.sched_get_priority_max(sched) - os.sched_get_priority_min(sched)) // 2
param = os.sched_param(prio)
try:
os.sched_setscheduler(0, sched, param)
except OSError:
logger.warning("Failed to set real time process scheduler to %u, priority %u" % (sched, prio))
else:
done = True
logger.info("Process real time scheduler set to %u, priority %u" % (sched, prio))
if not done:
# renice to highest priority
target_niceness = -19
previous_niceness = os.nice(0)
delta_niceness = target_niceness - previous_niceness
try:
new_niceness = os.nice(delta_niceness)
except OSError:
new_niceness = previous_niceness
if new_niceness != target_niceness:
logger.warning("Unable to renice process to %d, current niceness is %d" % (target_niceness, new_niceness))
else:
logger.info("Process reniced from %d to %d" % (previous_niceness, new_niceness))
def set_high_priority(logger):
""" Change process priority to the highest possible. """
# use "real time" scheduler
done = False
sched = os.SCHED_RR
prio = os.sched_get_priority_max(sched)
param = os.sched_param(prio)
try:
os.sched_setscheduler(0, sched, param)
except OSError:
logger.warning("Failed to set real time process scheduler to %u, priority %u" % (sched, prio))
else:
done = True
logger.info("Process real time scheduler set to %u, priority %u" % (sched, prio))
if not done:
# renice to highest priority
target_niceness = -19
previous_niceness = os.nice(0)
delta_niceness = target_niceness - previous_niceness
try:
new_niceness = os.nice(delta_niceness)
except OSError:
new_niceness = previous_niceness
if new_niceness != target_niceness:
logger.warning("Unable to renice process to %d, current niceness is %d" % (target_niceness, new_niceness))
else:
logger.info("Process reniced from %d to %d" % (previous_niceness, new_niceness))
def set_high_priority(logger):
""" Change process priority to the highest possible. """
# use "real time" scheduler
done = False
sched = os.SCHED_RR
prio = os.sched_get_priority_max(sched)
param = os.sched_param(prio)
try:
os.sched_setscheduler(0, sched, param)
except OSError:
logger.warning("Failed to set real time process scheduler to %u, priority %u" % (sched, prio))
else:
done = True
logger.info("Process real time scheduler set to %u, priority %u" % (sched, prio))
if not done:
# renice to highest priority
target_niceness = -19
previous_niceness = os.nice(0)
delta_niceness = target_niceness - previous_niceness
try:
new_niceness = os.nice(delta_niceness)
except OSError:
new_niceness = previous_niceness
if new_niceness != target_niceness:
logger.warning("Unable to renice process to %d, current niceness is %d" % (target_niceness, new_niceness))
else:
logger.info("Process reniced from %d to %d" % (previous_niceness, new_niceness))
def define_priority():
"""."""
# sched = _os.SCHED_FIFO
sched = _os.SCHED_RR
prio = _os.sched_get_priority_max(sched)
param = _os.sched_param(prio)
try:
_os.sched_setscheduler(0, sched, param)
print('High priority set!')
except PermissionError:
print('Could not set priority')
def apply(self, process: psutil.Process) -> None:
try:
if self.nice is not None and process.nice() != self.nice:
process.nice(self.nice)
except psutil.AccessDenied:
pass
try:
if (self.ioclass is not None
and process.ionice().ioclass != self.ioclass
or self.ionice is not None
and process.ionice().value != self.ionice):
process.ionice(self.ioclass, self.ionice)
except psutil.AccessDenied:
pass
if self.sched is not None:
try:
if os.sched_getscheduler(process.pid) != self.sched:
sched_prio: int = 0
if self.sched in (Scheduler.FIFO, Scheduler.RR):
assert self.sched_prio is not None
sched_prio = self.sched_prio
os.sched_setscheduler(
process.pid,
self.sched,
os.sched_param(sched_prio) # type: ignore
)
except PermissionError:
pass
elif self.sched_prio is not None:
try:
if (os.sched_getscheduler(process.pid)
in (Scheduler.FIFO, Scheduler.RR)):
os.sched_setparam(
process.pid,
os.sched_param(sched_prio) # type: ignore
)
except PermissionError:
pass
if self.oom_score_adj is not None:
try:
with open('/proc/%(pid)s/oom_score_adj' % {'pid': process.pid},
'r+') as file:
prev_oom_score_adj: int = int(file.read())
if prev_oom_score_adj != self.oom_score_adj:
file.write(str(self.oom_score_adj))
except PermissionError:
pass
if self.cgroup is not None:
# TODO: implement
pass
def __init__(self,
ag_protocol: AbstractTransport,
magnetometer_protocol: AbstractTransport,
high_priority: bool = False):
self.ag = ag_protocol
self.mag = magnetometer_protocol
# Needs to be a high priority process or it'll drop samples
# when other processes are under heavy load.
if high_priority:
priority = os.sched_get_priority_max(os.SCHED_FIFO)
param = os.sched_param(priority)
os.sched_setscheduler(0, os.SCHED_FIFO, param)
def __get_input(self):
"""Thread for reading input from gamepad"""
# Make sure this thread does not have realtime priority
os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
while True:
if not self.running:
return
# use select() to check if events are waiting to avoid blocking on read()
if self.sel.select(timeout=0.1):
# call to read() is blocking
events = self.gamepad.read()
for event in events:
# cache the raw state of this event
self.state[event.code] = event.state
if event.code == 'ABS_X':
self._update_analog('left')
elif event.code == 'ABS_Y':
self._update_analog('left')
elif event.code == 'ABS_RX':
self._update_analog('right')
elif event.code == 'ABS_RY':
self._update_analog('right')
elif event.code == 'BTN_NORTH' and event.state == 1:
self.int_x = 0.0
elif event.code == 'BTN_WEST' and event.state == 1:
self.int_y = 0.0
elif event.code == 'BTN_TL' and event.state == 1:
self.integrator_mode = False
elif event.code == 'BTN_TR' and event.state == 1:
self.integrator_mode = True
# call any callbacks registered for this event.code
callback = self.callbacks.get(event.code, None)
if callback is not None:
callback(event.state)
if self.debug_prints:
print(event.code + ': ' + str(event.state))
def set_high_priority(logger):
""" Change process scheduler and priority. """
# use "real time" scheduler
done = False
sched = os.SCHED_RR
if os.sched_getscheduler(0) == sched:
# already running with RR scheduler, likely set from init system, don't touch priority
done = True
else:
prio = (os.sched_get_priority_max(sched) -
os.sched_get_priority_min(sched)) // 2
param = os.sched_param(prio)
try:
os.sched_setscheduler(0, sched, param)
except OSError:
logger.warning(
"Failed to set real time process scheduler to %u, priority %u"
% (sched, prio))
else:
done = True
logger.info("Process real time scheduler set to %u, priority %u" %
(sched, prio))
if not done:
# renice to highest priority
target_niceness = -19
previous_niceness = os.nice(0)
delta_niceness = target_niceness - previous_niceness
try:
new_niceness = os.nice(delta_niceness)
except OSError:
new_niceness = previous_niceness
if new_niceness != target_niceness:
logger.warning(
"Unable to renice process to %d, current niceness is %d" %
(target_niceness, new_niceness))
else:
logger.info("Process reniced from %d to %d" %
(previous_niceness, new_niceness))
def wrapper(*args, **kwargs):
did_sched = False
try:
pri = os.sched_get_priority_max(os.SCHED_RR)
sched = os.sched_param(pri)
os.sched_setscheduler(0, os.SCHED_RR, sched)
did_sched = True
except PermissionError as e:
msg = ("Looks like you haven't set scheduling capabilities "
"for your python executable, so you can't run with "
"high priority. To disable this message, either consider "
"running `sudo setcap cap_sys_nice+ep /path/to/python3` or "
"comment out the `@hi_priority` decorator above the "
"rf_send function, since it's not working anyway, or just "
"increase the logging threadshold higher than `info`. NB: "
"`setcap` will *not* work on symlinks, you must provide "
"the path to the actual python3 executable.")
logger.info(e)
logger.info(msg)
func(*args, **kwargs)
if did_sched:
os.sched_yield()
def wrapper(*args, **kwargs):
did_sched = False
try:
pri = os.sched_get_priority_max(os.SCHED_RR)
sched = os.sched_param(pri)
os.sched_setscheduler(0, os.SCHED_RR, sched)
did_sched = True
except PermissionError as e:
msg = ("Looks like you haven't set scheduling capabilities "
"for your python executable, so you can't run with "
"high priority. To disable this message, either consider "
"running `sudo setcap cap_sys_nice+ep /path/to/python3` or "
"comment out the `@hi_priority` decorator above the "
"rf_send function, since it's not working anyway, or just "
"increase the logging threadshold higher than `info`. NB: "
"`setcap` will *not* work on symlinks, you must provide "
"the path to the actual python3 executable.")
logger.info(e)
logger.info(msg)
func(*args, **kwargs)
if did_sched:
os.sched_yield()
def __linux_fifo_scheduler(self):
if python_implementation() == 'CPython':
param = os.sched_param(os.SCHED_FIFO)
os.sched_setscheduler(0, os.SCHED_FIFO, param)
else:
self.__native_linux_fifo_scheduler()
# 14400 seconds = 4 hours
wait_timeout = int(os.environ.get(
"WAIT_TIMEOUT",
14400)) * 1000000000 # In nano second. 1 sec = 1000000000 ns
gc_progress = for_each_git_dir(["/usr/bin/env", "git", "gc", "--aggressive"],
"/var/cache/git")
# Limit CPU usage
os.nice(19)
# Use batch scheduler.
# This means that this process and all its child can be preempted by other schduler scheme.
# This also mean that they will have bigger timeslice to prevent frequent context switching.
#
# Reference:
# https://lwn.net/Articles/3866/
# https://www.mankier.com/7/sched
os.sched_setscheduler(0, os.SCHED_BATCH, os.sched_param(0))
while True:
sleep_until(target_hour, target_min)
start_time = time.time_ns()
for is_finished in gc_progress:
if is_finished or time.time_ns() - start_time >= wait_timeout:
# sleep for 1 min in case of the git gc command finish within one min.
# In this case, sleep_until with return immediately and the whole gc_progress will be started again
time.sleep(60)
break
def run(self):
logger = self.logger
'''
# wtf. python's built in os.sched_setscheduler() just does not do SCHED_FIFO any more. even
# with caps set, it still returns EPERM
# turns out systemd's logind puts ssh users into a cgroup that can't do RT.
# move your shell to the default cpu group with:
# cgclassify -g cpu:/ $$
# then you can do things like the following.
# do it with C instead
c = ctypes.cdll.LoadLibrary('libc.so.6')
SCHED_FIFO = 1
class _SchedParams(ctypes.Structure):
_fields_ = [('sched_priority', ctypes.c_int)]
schedParams = _SchedParams()
schedParams.sched_priority = c.sched_get_priority_max(SCHED_FIFO)
err = c.sched_setscheduler(0, SCHED_FIFO, ctypes.byref(schedParams))
if err:
logger.critical('couldn\'t set scheduler priority: {}'.format(err))
'''
sp_max = os.sched_get_priority_max(os.SCHED_FIFO) # see 'man sched_setscheduler',
sp = os.sched_param(99) # range 1 (low) - 99 (high), for FIFO in linux
try:
os.sched_setscheduler(0, os.SCHED_FIFO, sp) # FIFO or RR are real-time policies (linux)
except PermissionError:
logger.critical('prctl.capbset.sys_nice ={}'.format(prctl.capbset.sys_nice))
logger.critical('prctl.capbset.net_admin ={}'.format(prctl.capbset.net_admin))
logger.critical('prctl.capbset.net_raw ={}'.format(prctl.capbset.net_raw))
logger.critical('[1;31mPermission denied[0m. Does the python binary have cap_net_raw,cap_sys_nice,cap_net_admin=eip? or do you need to run this? cgclassify -g cpu:/ $$')
quit
#os.setpriority(os.PRIO_PROCESS, 0, -20)
in_process = False
session_stats=(0,0,0)
#print('attach debugger within 30 seconds now')
#time.sleep(30)
while not self._shutdown.is_set():
if not (self.online and self.handle):
self.start_handle()
continue
try:
rval = self.handle.next()
if rval:
pkthdr, packet = rval
rval = 1
caplen = pkthdr.getcaplen()
hlen = pkthdr.getlen()
timestamp = pkthdr.getts()
timestamp = timestamp[0] + timestamp[1]/1000000
else:
rval = 0
except:
t,v,tb = sys.exc_info()
self.logger.warning('some bad foo, restarting capture: {}'.format(v))
self.online = False
time.sleep(10)
# timeout expired
if rval == 0:
# this should go in the EventManger
# every 1 second, check the events queue for stalled or 2MSL
# that need to be pushed to the API
#self.events_scan()
continue
if not rval == 1:
logger.critical('unexpected rval: {}'.format(rval))
self.lastevent = datetime.datetime.utcnow()
ev = self.process_packet(hlen, caplen, timestamp, packet)
if ev:
# TCP conversation is finished
logger.info('event {} ready for payload processing; hand off to queue'.format(ev.uuid))
# tell event manager to pop its cherry
self.eventmanager.pending.set()
'''
def __linux_standard_scheduler(self):
if python_implementation() == 'CPython':
param = os.sched_param(os.SCHED_OTHER)
os.sched_setscheduler(0, os.SCHED_OTHER, param)
else:
self.__native_linux_standard_scheduler()
def set_realtime_priority(level):
if not PC:
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
#self.trajectory_queue_init()
q, _ = self.master_board.get_state()
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
self.prev_t = current_time
self.acc_error = np.zeros(2)
# Communication with MasterBoard
while (self._running and not timeout):
attitude = self.master_board.get_attitude()
current_position = self.master_board.get_state()[0]
q = self.balance(attitude, current_position, current_time)
#print(np.degrees(q))
timeout = self.master_board.set_reference(q, np.zeros(8))
timeout = self.master_board.track_reference(None)
self.plotter.add_data("Attitude Reading", attitude)
self.plotter.add_time(current_time)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset process priority
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
'-s',
default='--\nHELLO\nWORLD\n',
help='string to overwrite with')
parser.add_argument('--binary',
'-b',
default='/usr/bin/sum',
help='binary to use sudo with')
args = parser.parse_args()
init()
try:
inotify_thread = Thread(target=f_inotify)
inotify_thread.setDaemon(True)
inotify_thread.start()
os.sched_setscheduler(0, os.SCHED_IDLE, os.sched_getparam(0))
print(os.sched_getaffinity(0))
os.sched_setaffinity(0, os.sched_getaffinity(0))
os.nice(19)
time.sleep(2)
print("starting")
asd = subprocess.Popen(
"/dev/shm/_tmp/\\ \\ \\ \\ \\ 34873\\ -r unconfined_r {} '{}'".format(
args.binary, args.string),
shell=True)
asd_PID = asd.pid
stdout, stderr = asd.communicate()
#print(stdout)
def main():
"""See module docstring"""
def gamepad_callback(_tracker: Tracker) -> bool:
"""Callback for gamepad control.
Allows manual control of the slew rate via a gamepad when the 'B' button is held down,
overriding tracking behavior. This callback is registered with the Tracker object which
calls it on each control cycle.
Defined inside main() so this has easy access to objects that are within that scope.
Args:
_tracker: A reference to an object of type Tracker. Not used internally.
Returns:
True when the 'B' button is depressed which disables the normal control path. False
otherwise, which leaves the normal control path enabled.
"""
if game_pad.state.get('BTN_EAST', 0) == 1:
gamepad_x, gamepad_y = game_pad.get_proportional()
slew_rates = (mount.max_slew_rate * gamepad_x,
mount.max_slew_rate * gamepad_y)
for idx, axis_name in enumerate(mount.AxisName):
mount.slew(axis_name, slew_rates[idx])
return True
else:
return False
parser = track.ArgParser()
targets.add_program_arguments(parser)
laser.add_program_arguments(parser)
mounts.add_program_arguments(parser, meridian_side_required=True)
ntp.add_program_arguments(parser)
telem.add_program_arguments(parser)
control.add_program_arguments(parser)
args = parser.parse_args()
# Set priority of this thread to realtime. Do this before constructing objects since priority
# is inherited and some critical threads are created by libraries we have no direct control
# over.
os.sched_setscheduler(0, os.SCHED_RR, os.sched_param(11))
# Check if system clock is synchronized to GPS
if args.check_time_sync:
try:
ntp.check_ntp_status()
except ntp.NTPCheckFailure as e:
print('NTP check failed: ' + str(e))
if not click.confirm('Continue anyway?', default=True):
print('Aborting')
sys.exit(2)
# Load a MountModel object
try:
mount_model = track.model.load_stored_model()
except track.model.StaleParametersException:
if click.confirm('Stored alignment parameters are stale. Use anyway?',
default=True):
mount_model = track.model.load_stored_model(max_age=None)
elif args.target_type == 'camera':
if click.confirm(
'Use a default set of alignment parameters instead?',
default=True):
guide_cam_orientation = click.prompt(
'Enter guide camera orientation in degrees, clockwise positive',
type=float)
mount_model = track.model.load_default_model(
guide_cam_orientation=Longitude(guide_cam_orientation *
u.deg))
if 'mount_model' not in locals():
print(
'Aborting: No model could be loaded. To refresh stored model run align program.'
)
sys.exit(1)
# Create object with base type TelescopeMount
mount = mounts.make_mount_from_args(args)
telem_logger = telem.make_telem_logger_from_args(args)
target = targets.make_target_from_args(
args,
mount,
mount_model,
MeridianSide[args.meridian_side.upper()],
telem_logger=telem_logger,
)
tracker = Tracker(
mount=mount,
mount_model=mount_model,
target=target,
telem_logger=telem_logger,
)
try:
laser_pointer = laser.make_laser_from_args(args)
except OSError:
print('Could not connect to laser pointer FTDI device.')
laser_pointer = None
telem_sources = {}
try:
# Create gamepad object and register callback
game_pad = Gamepad()
if laser_pointer is not None:
game_pad.register_callback('BTN_SOUTH', laser_pointer.set)
tracker.register_callback(gamepad_callback)
telem_sources['gamepad'] = game_pad
print('Gamepad found and registered.')
except RuntimeError:
print('No gamepads found.')
if telem_logger is not None:
telem_logger.register_sources(telem_sources)
telem_logger.start()
stopping_conditions = control.make_stop_conditions_from_args(args)
try:
tracker.run(stopping_conditions)
except KeyboardInterrupt:
print('Got CTRL-C, shutting down...')
finally:
# don't rely on destructors to safe mount!
print('Safing mount...', end='', flush=True)
if mount.safe():
print('Mount safed successfully!')
else:
print('Warning: Mount may be in an unsafe state!')
try:
game_pad.stop()
except UnboundLocalError:
pass
if telem_logger is not None:
telem_logger.stop()
#! /usr/bin/python3
import os
import signal
try:
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(99))
signal.alarm(20)
while True:
pass
except PermissionError:
print("You need root permission to run as realtime process")
def run(self):
# Priority settings for "InputTask" process
#print(f"Input task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}")
pid = os.getpid()
sched = os.SCHED_FIFO
param = os.sched_param(50)
os.sched_setscheduler(pid, sched, param)
#print(f"Input task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n")
sys.stdin = os.fdopen(self.standard_input)
print()
print(f"Robot is currently in {self._mode} mode")
#Make sure master board has recorded a position before we try to read it
self.masterboard_started_event.wait()
with self.shared_position.get_lock():
initial_position = self.shared_position[:]
while (self._running):
try:
input_string = input(">")
joint_variables, input_message = parser.interpret_input(
input_string, self._mode, initial_position)
except EOFError:
self.terminate()
continue
if input_message == "simple walk":
if self._mode != "Walk":
print(f"Currently in {self._mode}, change mode.")
continue
initial_position = self.walk(initial_position, joint_variables)
elif input_message == "simple trot":
initial_position = self.trot(initial_position, joint_variables,
self._mode)
elif input_message == "Creep":
if self._mode != "Idle":
print(f"Currently in {self._mode}, change mode.")
initial_position = self.creep(initial_position,
joint_variables)
elif input_message == "Send":
if self._mode == "Walk":
zero_config = kinematics.walk_zero_config
elbows = kinematics.walk_elbow
joint_polarities = kinematics.walk_joint_polarities
else:
#return None, "InputError"
zero_config = kinematics.idle_zero_config
elbows = kinematics.idle_elbow
joint_polarities = kinematics.idle_joint_polarities
importlib.reload(input_coordinates)
coordinates = np.array(input_coordinates.coordinates,
dtype=float)
velocity = np.array(input_coordinates.velocity, dtype=float)
acceleration = np.array(input_coordinates.acceleration,
dtype=float)
points = kinematics.simple_walk_controller(
coordinates, initial_position, zero_config,
joint_polarities, elbows)
if None in points:
continue
initial_position = self.que_trajectory(initial_position,
points, velocity,
acceleration)
elif input_message != "":
initial_position = self.handle_input(input_message,
initial_position)
elif input_message == "InputError":
continue
else:
initial_position = self.que_trajectory(initial_position,
joint_variables)
sys.stdin.close()
def apply_default_sched(cls, pid, prio=0):
# attention: http://permalink.gmane.org/gmane.linux.kernel/1779369
param = os.sched_param(prio)
os.sched_setscheduler(pid, os.SCHED_OTHER, param)
def set_sched_param(pid, param):
if param.sched_priority != -1:
os.sched_setscheduler(pid, os.SCHED_RR,
os.sched_param(param.sched_priority))
if len(param.sched_affinity_cpus) > 0:
os.sched_setaffinity(pid, param.sched_affinity_cpus)
def apply_default_sched(cls, pid, prio=0):
# attention: http://permalink.gmane.org/gmane.linux.kernel/1779369
param = os.sched_param(prio)
os.sched_setscheduler(pid, os.SCHED_OTHER, param)
from pendulum import *
import os
param = os.sched_param(os.sched_get_priority_max(os.SCHED_FIFO))
os.sched_setscheduler(0, os.SCHED_FIFO, param)
bus = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=1000000)
motor = Actuator(0x141, bus)
pendulum = Pendulum(0.126, 0.15)
N = 4000
proc_time = np.zeros(N)
X_ar = np.zeros((4, N))
state = motor.get_state()
for i in range(N):
X_des = ident_traj(np.array([state.time]))
X = state.X
u = pendulum.control(X_des, X)
proc_time[i] = state.time
X_ar[0, i] = state.time
X_ar[1:3, i] = state.X.reshape((1, 2))
X_ar[3, i] = state.current
state = motor.send_current(u)
motor.send_current(0)
print(state.time)
np.savetxt("data_dyn.csv", np.transpose(X_ar), delimiter=',')
torque_limit = np.array([3, 3, 3, 3, 2, 2, 2],
dtype=np.float64) # Absolute torque limit
alpha = 0.01 # Exp. interpolation constant
J_reg = 1e-3 # Jacobian regularization constant
# Null-space theta configuration
target_th_null = np.zeros(7, dtype=np.float64)
target_th_null[3] = -1.55
target_th_null[5] = 1.9
# Max deviation between current and desired Cart pos (to avoid integration to inf)
maxCartDev = 0.2 # in m
max_sync_jitter = 0.2
use_inv_dyn = False
robot_name = "franka" # for use in signals
try:
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(35))
except OSError as err:
print("Error: Failed to set proc. to real-time scheduler\n{0}".format(err))
## Register Signals
b = pab.broker()
b.register_signal(robot_name + "_des_tau", pab.MsgType.des_tau)
# b.request_signal("task_policy", pab.MsgType.task_policy, True)
b.request_signal(robot_name + "_state", pab.MsgType.franka_state, True)
b_lidar = pab.broker()
print("Before Lidar")
b_lidar.request_signal("franka_lidar", pab.MsgType.franka_lidar, -1)
print("Setup completed. Starting...")
# set maximum distances
p0 = np.array([0.3, 0.3, 0.3])
sigma_ddq_null = 0.5 * np.identity(7)
def set_scheduler(self, pid, sched, prio):
os.sched_setscheduler(pid, sched, os.sched_param(prio))
def set_realtime_priority(level: int) -> None:
if not PC:
os.sched_setscheduler(
0, os.SCHED_FIFO,
os.sched_param(level)) # type: ignore[attr-defined]
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
ic_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
#self.trajectory_queue_init()
q, q_prime = self.master_board.get_state()
with self.shared_position.get_lock():
self.shared_position[:] = q
self.masterboard_started_event.set()
trajectory = None
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
self.prev_t = 0
self.prev_att_err = 0
self.acc_error = np.zeros(2)
self.kp = 200 #200
self.kd = 3.5 #2
self.ki = 6000 #3000
# Communication with MasterBoard
while (self._running and not timeout):
balance_mode = self.balance_mode
#Shared position. This allows InputTask to read the current position from a shared array
current_position = self.master_board.get_state()[0]
with self.shared_position.get_lock():
self.shared_position[:] = current_position
if balance_mode:
if not self.message_queue.empty():
input_message = self.message_queue.get(block=False)
self.handle_input(input_message, q)
attitude = self.master_board.get_attitude()
if self.prev_t == 0:
self.prev_t = current_time
q, q_prime = self.balance(attitude, current_position,
current_time)
timeout = self.master_board.set_reference(q, q_prime)
elif trajectory == None:
if not self.message_queue.empty():
input_message = self.message_queue.get(block=False)
self.handle_input(input_message, q)
# No trajectory, try to get new from queue.
elif not self.trajectory_queue.empty():
trajectory = self.trajectory_queue.get(
block=False) # Check queue
initial_trajectory_time = time.perf_counter()
else:
if self._terminating:
self._running = False
else:
# Follow current trajectory
time_before_initial_condition = time.perf_counter()
current_trajectory_time = time.perf_counter(
) - initial_trajectory_time
q, q_prime = trajectory.step(current_trajectory_time)
ic_time_array[cpt] = time.perf_counter(
) - time_before_initial_condition
timeout = self.master_board.set_reference(q, q_prime)
'''
for i in range(len(q)):
if i < 2:
self.plotter.add_data(f"Joint {i}, Position Set Point", q[i])
self.plotter.add_data(f"Joint {i}, Velocity Set Point", q_prime[i])
#pos_reading, vel_reading = self.master_board.get_state()
#self.plotter.add_data(f"Joint {i}, Position Reading", pos_reading[i])
#self.plotter.add_data(f"Joint {i}, Velocity Reading", vel_reading[i])
#self.plotter.add_data(f"Joint {i}, Position Error", pos_reading[i] - q[i])
#self.plotter.add_data(f"Joint {i}, Velocity Error", vel_reading[i] - q_prime[i])
'''
if trajectory.is_done():
trajectory = None
# Update current reference to each joint.
if self.debug_plot:
timeout = self.master_board.track_reference(self.plotter)
self.plotter.add_time(current_time)
else:
timeout = self.master_board.track_reference(None)
#self.plot.add_time(current_time)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset process priority
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
plt.plot(ic_time_array[0:cpt - 1])
plt.show()
if self.debug_plot:
#for i in range(config.N_SLAVES_CONTROLLED * 2):
# self.plotter.set_num_cols(3)
# self.plotter.create_axis([f"Joint {2*i}, Position Reference", f"Joint {2*i}, Position Reading"])
# self.plotter.create_axis([f"Joint {2*i}, Velocity Reference"])
# self.plotter.create_axis([f"Joint {2*i}, Velocity Reading"])
# self.plotter.create_axis([f"Joint {2*i+1}, Position Reference", f"Joint {2*i+1}, Position Reading"])
# self.plotter.create_axis([f"Joint {2*i+1}, Velocity Reference"])
# self.plotter.create_axis([f"Joint {2*i+1}, Velocity Reading"])
#self.plotter.create_axis([
# f"Joint {2*i}, Velocity Set Point",
# f"Joint {2*i+1}, Velocity Set Point"
#])
self.plotter.create_axis([
f"Joint {0}, Position Reference",
f"Joint {0}, Position Reading"
])
self.plotter.create_axis([
f"Joint {2}, Position Reference",
f"Joint {2}, Position Reading"
])
self.plotter.create_axis([
f"Joint {4}, Position Reference",
f"Joint {4}, Position Reading"
])
self.plotter.create_axis([
f"Joint {6}, Position Reference",
f"Joint {6}, Position Reading"
])
self.plotter.create_axis([
f"Joint {1}, Position Reference",
f"Joint {1}, Position Reading"
])
self.plotter.create_axis([
f"Joint {3}, Position Reference",
f"Joint {3}, Position Reading"
])
self.plotter.create_axis([
f"Joint {5}, Position Reference",
f"Joint {5}, Position Reading"
])
self.plotter.create_axis([
f"Joint {7}, Position Reference",
f"Joint {7}, Position Reading"
])
self.plotter.set_num_cols(4)
self.plotter.plot(grid=True)
def run(self):
# Priority settings for "MasterBoardTask"
print(
f"Master board task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid() # Identify process ID
sched = os.SCHED_FIFO # FIFO real time scheduler
param = os.sched_param(90) # Process priority
os.sched_setscheduler(pid, sched, param) # Update priority settings
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
# Local variables
dt = config.dt
timeout = False
current_time = 0.0
cpt = 0
# Troubleshooting arrays (Normally not used)
plot_duration = 1000
plot_array_size = int(plot_duration / dt)
time_per_timestep_array = np.zeros(plot_array_size + 1)
cycle_time_array = np.zeros(plot_array_size + 1)
ic_time_array = np.zeros(plot_array_size + 1)
# Trajectory and Queue initiation
#self.trajectory_queue_init()
q, q_prime = self.master_board.get_state()
trajectory = None
# Timer initiation
cycle_time = 0
program_start = time.perf_counter()
timestep_start = time.perf_counter()
timestep_end = time.perf_counter()
cycle_start = time.perf_counter()
# Communication with MasterBoard
while (self._running and not timeout):
if trajectory == None:
# No trajectory, try to get new from queue.
if not self.trajectory_queue.empty():
trajectory = self.trajectory_queue.get(
block=False) # Check queue
initial_trajectory_time = time.perf_counter()
else:
# Follow current trajectory
time_before_initial_condition = time.perf_counter()
current_trajectory_time = time.perf_counter(
) - initial_trajectory_time
q, q_prime = trajectory.step(current_trajectory_time)
ic_time_array[cpt] = time.perf_counter(
) - time_before_initial_condition
timeout = self.master_board.set_reference(q, q_prime)
if trajectory.is_done():
trajectory = None
# Update current reference to each joint.
timeout = self.master_board.track_reference(None)
for i in range(config.N_SLAVES_CONTROLLED * 2):
self.plotter.add_data(f"Joint {i}, Position Set Point", q)
self.plotter.add_time(current_time)
attitude = self.master_board.get_attitude()
if trajectory == None:
self.initial_position = self.balance(attitude,
self.initial_position)
# Cycle time is time spent calculating trajectory
cycle_time = time.perf_counter() - cycle_start
cycle_time_array[cpt] = cycle_time
if cycle_time >= dt:
cycle_time = dt
# Sleep MasterBoard for 1ms.
time.sleep(dt - cycle_time) # ideally: dt - cycle_time = 1ms
cycle_start = time.perf_counter() # Start time for next cycle
timestep_end = cycle_start
time_per_timestep_array[cpt] = timestep_end - timestep_start
timestep_start = cycle_start
current_time = current_time + dt
if cpt < plot_array_size:
cpt = cpt + 1
self.master_board.terminate()
#Reset process priority
sched = os.SCHED_OTHER
param = os.sched_param(os.sched_get_priority_min(sched))
os.sched_setscheduler(pid, sched, param)
print(
f"Master board task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}"
)
plt.plot(time_per_timestep_array[0:cpt - 1])
plt.show()
plt.plot(cycle_time_array[0:cpt - 1])
plt.show()
plt.plot(ic_time_array[0:cpt - 1])
plt.show()
for i in range(config.N_SLAVES_CONTROLLED):
self.plotter.set_num_cols(2)
self.plotter.create_axis([f"Joint {2*i}, Position Set Point"])
self.plotter.plot(grid=True)
def __init__(self, gModel, gModelName, aName, sysArgv):
'''
Constructor
'''
self.logger = logging.getLogger(__name__)
self.inst_ = self
self.appName = gModel["name"]
self.modelName = gModelName
self.name = aName
self.pid = os.getpid()
self.uuid = None
self.setupIfaces()
self.disco = None
self.deplc = None
# Assumption : pid is a 4 byte int
self.actorID = ipaddress.IPv4Address(
self.globalHost).packed + self.pid.to_bytes(4, 'big')
self.suffix = ""
if aName not in gModel["actors"]:
raise BuildError('Actor "%s" unknown' % aName)
self.model = gModel["actors"][
aName] # Fetch the relevant content from the model
self.INT_RE = re.compile(r"^[-]?\d+$")
self.parseParams(sysArgv)
# Use czmq's context
czmq_ctx = Zsys.init()
self.context = zmq.Context.shadow(czmq_ctx.value)
Zsys.handler_reset() # Reset previous signal handler
# Context for app sockets
self.appContext = zmq.Context()
if Config.SECURITY:
(self.public_key,
self.private_key) = zmq.auth.load_certificate(const.appCertFile)
_public = zmq.curve_public(self.private_key)
if (self.public_key != _public):
self.logger.error("bad security key(s)")
raise BuildError("invalid security key(s)")
hosts = ['127.0.0.1']
try:
with open(const.appDescFile, 'r') as f:
content = yaml.load(f, Loader=yaml.Loader)
hosts += content.hosts
except:
self.logger.error("Error loading app descriptor:%s",
str(sys.exc_info()[1]))
self.auth = ThreadAuthenticator(self.appContext)
self.auth.start()
self.auth.allow(*hosts)
self.auth.configure_curve(domain='*',
location=zmq.auth.CURVE_ALLOW_ANY)
else:
(self.public_key, self.private_key) = (None, None)
self.auth = None
self.appContext = self.context
try:
if os.path.isfile(const.logConfFile) and os.access(
const.logConfFile, os.R_OK):
spdlog_setup.from_file(const.logConfFile)
except Exception as e:
self.logger.error("error while configuring componentLogger: %s" %
repr(e))
messages = gModel[
"messages"] # Global message types (global on the network)
self.messageNames = []
for messageSpec in messages:
self.messageNames.append(messageSpec["name"])
locals_ = self.model[
"locals"] # Local message types (local to the host)
self.localNames = []
for messageSpec in locals_:
self.localNames.append(messageSpec["type"])
self.localNams = set(self.localNames)
internals = self.model[
"internals"] # Internal message types (internal to the actor process)
self.internalNames = []
for messageSpec in internals:
self.internalNames.append(messageSpec["type"])
self.internalNames = set(self.internalNames)
groups = gModel["groups"]
self.groupTypes = {}
for group in groups:
self.groupTypes[group["name"]] = {
"kind": group["kind"],
"message": group["message"],
"timed": group["timed"]
}
self.rt_actor = self.model.get(
"real-time") # If real time actor, set scheduler (if specified)
if self.rt_actor:
sched = self.model.get("scheduler")
if (sched):
_policy = sched.get("policy")
policy = {
"rr": os.SCHED_RR,
"pri": os.SCHED_FIFO
}.get(_policy, None)
priority = sched.get("priority", None)
if policy and priority:
try:
param = os.sched_param(priority)
os.sched_setscheduler(0, policy, param)
except Exception as e:
self.logger.error(
"Can't set up real-time scheduling '%r %r':\n %r"
% (_policy, priority, e))
try:
prctl.cap_effective.limit() # Drop all capabilities
prctl.cap_permitted.limit()
prctl.cap_inheritable.limit()
except Exception as e:
self.logger.error("Error while dropping capabilities")
self.components = {}
instSpecs = self.model["instances"]
compSpecs = gModel["components"]
ioSpecs = gModel["devices"]
for instName in instSpecs: # Create the component instances: the 'parts'
instSpec = instSpecs[instName]
instType = instSpec['type']
if instType in compSpecs:
typeSpec = compSpecs[instType]
ioComp = False
elif instType in ioSpecs:
typeSpec = ioSpecs[instType]
ioComp = True
else:
raise BuildError(
'Component type "%s" for instance "%s" is undefined' %
(instType, instName))
instFormals = typeSpec['formals']
instActuals = instSpec['actuals']
instArgs = self.buildInstArgs(instName, instFormals, instActuals)
# Check whether the component is C++ component
ccComponentFile = 'lib' + instType.lower() + '.so'
ccComp = os.path.isfile(ccComponentFile)
try:
if not ioComp:
if ccComp:
modObj = importlib.import_module('lib' +
instType.lower())
self.components[instName] = modObj.create_component_py(
self, self.model, typeSpec, instName, instType,
instArgs, self.appName, self.name, groups)
else:
self.components[instName] = Part(
self, typeSpec, instName, instType, instArgs)
else:
self.components[instName] = Peripheral(
self, typeSpec, instName, instType, instArgs)
except Exception as e:
traceback.print_exc()
self.logger.error("Error while constructing part '%s.%s': %s" %
(instType, instName, str(e)))
# gc.enable()
# gc.collect(0)
# gc.collect(1)
# gc.collect(2)
# gc.disable()
if os.name != "nt":
print("Making highest priority, os.SCHED_RR")
try:
pid = os.getpid()
niceValue = os.nice(-20)
sys.setswitchinterval(0.5)
print("sys.getswitchinterval", sys.getswitchinterval())
os.sched_setaffinity(pid, [(os.cpu_count() or 1) - 1])
os.sched_setscheduler(pid, os.SCHED_RR, os.sched_param(1))
print("sched_getscheduler", os.sched_getscheduler(pid))
print("sched_getparam", os.sched_getparam(pid))
print("sched_getaffinity", os.sched_getaffinity(pid))
print("sched_getprioritymax", os.sched_get_priority_max(0))
print("sched_getprioritymin", os.sched_get_priority_min(0))
print("sched_rr_getinterval", os.sched_rr_get_interval(pid))
print("nice", os.nice(0))
except PermissionError:
print("run as root to make top OS priority for more accurate results.")
else:
print("lol windows good luck")
for i in range(5):
print("pass", i + 1)
frame = rotate_image(frame, roll_vector)
cv2.imshow('Camera 0', frame)
if cv2.waitKey(1) == 27:
cap.release()
cv2.destroyAllWindows();
break
if __name__ == "__main__":
# Make process realtime
niceness = os.nice(0)
os.nice(-20-niceness)
print("Nice level of {} selected".format(os.nice(0)))
max_pri = os.sched_get_priority_max(os.SCHED_RR)
sched_param = os.sched_param(max_pri)
os.sched_setscheduler(0, os.SCHED_RR, sched_param)
# Parser for command-line arguments
parser = argp.ArgumentParser(description="Take a video and rotate it along the roll Euler vector.")
parser.add_argument('--seconds', metavar='s', type=int,
default=5, help="How many seconds to record")
parser.add_argument('--live', metavar='l', type=str2bool, nargs='?', const=True,
default=False, help="Should the recording be displayed as a window?")
args = parser.parse_args()
start = timer()
if args.live:
display_recording()
else:
save_recording(args.seconds)
end = timer()
def speed(self):
speed = 0
toll = 0
a = 0
total = 0
t1 = 0
t2 = 0
b = 0
n = os.fork()
if n > 0:
os.wait()
start_time = time.time() * 1000.0
pid3 = os.getpid()
pri = 99
sched = os.sched_param(pri)
sc3 = os.sched_setscheduler(pid3, os.SCHED_RR, sched)
if (speed > 0):
print("The car with ID" + str(self.id) +
" is overspeeding with speed" + str(speed))
f = open("speed.txt", "w+")
f.write("Car ID: " + str(self.id) + " Speed: " + str(speed) +
"\n")
f.close()
end_time = time.time() * 1000.0
tt = end_time - start_time
print("The total execution tine of speed defaulter task is\n" +
str(tt))
total = t1 + tt + t2
else:
z = os.fork()
if z > 0:
start_time = time.time() * 1000.0
pid1 = os.getpid()
pri = 99
sched = os.sched_param(pri)
s = os.sched_setscheduler(pid1, os.SCHED_RR, sched)
speed = 1000 / (self.time2 - self.time1)
print("The speed of the car with id " + str(self.id1) +
" is " + str(speed) + "\n")
print("Process id of Speed is " + str(pid1) + "\n")
sc = os.sched_getscheduler(pid1)
if sc == 1:
print("The scheduling policy of " + str(pid1) + " FIFO " +
"\n")
elif sc == 2:
print("The scheduling policy of " + str(pid1) +
" Round Robin " + "\n")
else:
print("The scheduling policy of " + str(pid1) + " SJF " +
"\n")
end_time = time.time() * 1000.0
tt = end_time - start_time
print("The total execution time of speed task is" + str(tt) +
"\n")
t1 = t1 + tt
exit()
else:
start_time = time.time() * 1000.0
pid2 = os.getpid()
pri1 = 99
sched = os.sched_param(pri1)
s1 = os.sched_setscheduler(pid2, os.SCHED_RR, sched)
toll = toll + 2.25
print("The toll of the car with id " + str(self.id1) + " is " +
str(toll) + "\n")
print("Process id of toll is " + str(pid2) + "\n")
sc1 = os.sched_getscheduler(pid2)
if sc1 == 1:
print("The scheduling policy of " + str(pid2) + " FIFO " +
"\n")
elif sc1 == 2:
print("The scheduling policy of " + str(pid2) +
" Round Robin " + "\n")
else:
print("The scheduling policy of " + str(pid1) + " SJF " +
"\n")
end_time = time.time() * 1000.0
tt = end_time - start_time
t2 = t2 + tt
print("The execution time of Toll task is" + str(tt) + "\n")
exit()
return total
return None
def get_cpu_tids(pid):
for tid in get_tids(pid):
name = get_thread_name(pid, tid)
if name is None:
continue
if name.startswith(b"CPU ") and name.endswith(b"/KVM\n"):
yield tid
def same_pid(old_pid, old_start_time):
return old_pid is not None and start_time(old_pid) == old_start_time
RT_SP = os.sched_param(90)
OT_SP = os.sched_param(0)
chrt_rt = lambda pid: os.sched_setscheduler(pid, os.SCHED_RR, RT_SP)
chrt_ot = lambda pid: os.sched_setscheduler(pid, os.SCHED_OTHER, OT_SP)
def set_rt(tids, rt):
chrt = chrt_rt if rt else chrt_ot
for tid in tids:
chrt(tid)
if __name__ == "__main__":
main()
