def _load(self, node: Node):
load_complete = True
parameters = self.__dict__
for name, value in parameters.items():
if name in ['robot_name', 'fleet_name']:
continue
try:
node.declare_parameter(name)
except ParameterAlreadyDeclaredException:
pass
Utils.logger.info(f'Loading {name} of type {type(value)}')
if type(value) is str:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().string_value
elif type(value) is bool:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().bool_value
elif type(value) is float:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().double_value
if not self.__dict__[name] and type(value) is not bool:
load_complete = False
if not load_complete:
Utils.logger.info('Parameters not yet fully loaded. Reloading..')
time.sleep(2)
self._load(node)
def __init__(self, node:Node):
node.declare_parameter("robot_type", "wolfgang")
anim_package = node.get_parameter('robot_type').get_parameter_value().string_value + "_animations"
path = get_package_share_directory(anim_package)
self.basepath = abspath(path + "/animations")
self.cache = {}
self.files = [] # Animations cached for find_all_animations
self.names = [] # Plain animation names, without filename-extension
self.animpath = self._get_animpath()
def generate(args=None):
rclpy.init(args=args)
# Node name should be declared
node = Node('generate')
# Even if you pass parameters via command line by using yaml file,
# should declare parameter name
node.declare_parameter(name='config_path', value='')
config_path = node.get_parameter('config_path').value
try:
factory(config_path)
except Exception as e:
# In python3 Exception has no attribute 'message'
print(e)
exit(-1)
exit(0)
def view_taylor(args=None):
rclpy.init(args=args)
node = Node('view_taylor')
node.declare_parameter(name='config_path', value='')
config_path = node.get_parameter('config_path').value
f = open(config_path, 'r')
config_dict = yaml.load(f, Loader=yaml.FullLoader)
model = models.create(config_dict['model'])
contex = taylor_expansion_context.create(config_dict['context'], model)
contex.run_expansion()
try:
input('press enter to terminate')
except:
pass
exit(0)
def main(args=None):
rclpy.init(args=args)
nh = Node('webserver')
nh.declare_parameter('port')
os.chdir(
os.path.dirname(__file__) +
'/../../../../share/pimouse_control2/contents')
param = nh.get_parameter_or('port')
if param.type_ == Parameter.Type.NOT_SET:
port = 8000
else:
port = param.value
handler = http.server.SimpleHTTPRequestHandler
with socketserver.TCPServer(('', port), handler) as httpd:
print('serving at port', port)
httpd.serve_forever()
nh.destroy_node()
rclpy.shutdown()
def __init__(self, node: Node):
self.node = node
self.list_update_frames = node.declare_parameter(
'list_update_frames', 10,
ParameterDescriptor(
type=ParameterType.PARAMETER_INTEGER,
description=
'Update list every this frames. So update rate of the list is 20 (asciimatics frame rate) / list_update_frames.',
integer_range=[
IntegerRange(from_value=1, to_value=20, step=1)
])).value
self.selected_action = None
self.selected_node = None
self.selected_topic = None
def main():
rclpy.init(args=None)
node = Node('system_monitor')
pub = node.create_publisher(WorkloadMsg, 'system_workload', 1)
diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1)
hostname = socket.gethostname()
diag_array = DiagnosticArray()
diag_cpu = DiagnosticStatus()
# start all names with "SYSTEM" for diagnostic analysesr
diag_cpu.name = "SYSTEMCPU"
diag_cpu.hardware_id = "CPU"
diag_mem = DiagnosticStatus()
diag_mem.name = "SYSTEMMemory"
diag_cpu.hardware_id = "Memory"
node.declare_parameter('update_frequency', 10.0)
node.declare_parameter('do_memory', True)
node.declare_parameter('do_cpu', True)
node.declare_parameter('cpu_load_percentage', 80.0)
node.declare_parameter('memoroy_load_percentage', 80.0)
node.declare_parameter('network_rate_received_errors', 10.0)
node.declare_parameter('network_rate_send_errors', 10.0)
rate = node.get_parameter('update_frequency').get_parameter_value().double_value
do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value
do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value
cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value
memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value
network_rate_received_errors = node.get_parameter(
'network_rate_received_errors').get_parameter_value().double_value
network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value
while rclpy.ok():
last_send_time = time.time()
running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (
-1, [], 0)
memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1)
interfaces = network_interfaces.collect_all(node.get_clock())
msg = WorkloadMsg(
hostname=hostname,
cpus=cpu_usages,
running_processes=running_processes,
cpu_usage_overall=overall_usage_percentage,
memory_available=memory_available,
memory_used=memory_used,
memory_total=memory_total,
filesystems=[],
network_interfaces=interfaces
)
pub.publish(msg)
diag_array.status = []
# publish diagnostic message to show this in the rqt diagnostic viewer
diag_cpu.message = str(overall_usage_percentage) + "%"
if overall_usage_percentage >= cpu_load_percentage:
diag_cpu.level = DiagnosticStatus.WARN
else:
diag_cpu.level = DiagnosticStatus.OK
diag_array.status.append(diag_cpu)
memory_usage = round((memory_used / memory_total) * 100, 2)
diag_mem.message = str(memory_usage) + "%"
if memory_usage >= memoroy_load_percentage:
diag_mem.level = DiagnosticStatus.WARN
else:
diag_mem.level = DiagnosticStatus.OK
diag_array.status.append(diag_mem)
for interface in interfaces:
diag_net = DiagnosticStatus()
diag_net.name = "SYSTEM" + interface.name
diag_net.hardware_id = interface.name
if interface.rate_received_packets_errors >= network_rate_received_errors \
or interface.rate_sent_packets_errors >= network_rate_send_errors:
diag_net.level = DiagnosticStatus.WARN
else:
diag_net.level = DiagnosticStatus.OK
diag_array.status.append(diag_net)
diag_array.header.stamp = node.get_clock().now().to_msg()
diagnostic_pub.publish(diag_array)
# sleep to have correct rate. we dont use ROS time since we are interested in system things
dt = time.time() - last_send_time
time.sleep(max(0, (1 / rate) - dt))
def optimize(args=None):
rclpy.init(args=args)
node = Node('optimize')
node.declare_parameter(name='opt_config_path', value='')
node.declare_parameter(name='data_config_path', value='')
# データ
data_config_path = node.get_parameter('data_config_path').value
data = generate(data_config_path, as_node=True)
opt_config_path = node.get_parameter('opt_config_path').value
with open(opt_config_path, 'r') as f:
opt_config_dict = yaml.load(f, Loader=yaml.FullLoader)
# モデル
model = models.create(opt_config_dict['model'])
# パラメータ更新器
updater = updaters.create(opt_config_dict['updater'])
# 最適化処理クラス
optimizer = optimizers.create(model, data.obs, updater,
opt_config_dict['optimizer'])
# 描画クラス(optional)
plotter = None
if 'plotter' in opt_config_dict and opt_config_dict['plotter']:
plotter = plotters.create(opt_config_dict['plotter'], model, data.obs)
# 1周期毎に停止(optional)
once = False
if 'once' in opt_config_dict and opt_config_dict['once']:
once = opt_config_dict['once']
print('initial param : {}'.format(model.get_param()))
print('initial error of sum squares: {}'.format(optimizer.ess()))
print('Optimize...')
previous_num = 0
num_iteration = 0
while True:
if plotter:
print('Plotting...')
plotter.plot()
if once:
try:
print('press enter to next iteration')
input()
except:
pass
num_iteration = optimizer.optimize(once=once)
if previous_num == num_iteration:
break
previous_num = num_iteration
print('num of iteration : {}'.format(num_iteration))
print('current param : {}'.format(model.get_param()))
print('current error of sum squares: {}'.format(optimizer.ess()))
try:
input('\npress enter to terminate')
except:
pass
exit(0)
def _load(self, node: Node, retries=5):
if retries == 0:
raise ValueError('Failed to load parameters.')
time.sleep(0.1) # Throttle so we don't run out of stack space
load_complete = True
params = self.__dict__
# fixed parameters are those defined in init function
fixed_params = list(inspect.signature(self.__init__).parameters.keys())
for name, param_type in params.items():
# If provided in __init__, do not load
if name in fixed_params:
continue
# Declare parameter if it is new
try:
node.declare_parameter(name)
except ParameterAlreadyDeclaredException:
pass
Utils.logger.info(
f'Loading ROS2 parameter {name} of type {param_type}')
# Load the appropriate value
if param_type is str:
param = node.get_parameter(
name).get_parameter_value().string_value
if param:
self.__dict__[name] = param
elif param_type is bool:
param = node.get_parameter(
name).get_parameter_value().bool_value
self.__dict__[name] = param
elif param_type is float:
param = node.get_parameter(
name).get_parameter_value().double_value
if param:
self.__dict__[name] = param
elif param_type is int:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().integer_value
if param:
self.__dict__[name] = param
if type(self.__dict__[name]) is not type:
Utils.logger.info(f'{name} has value {self.__dict__[name]}')
else:
Utils.logger.info(f'{name} was not loaded.')
param = None
if any(map(lambda x: type(x) is type, self.__dict__.values())):
load_complete = False
if not load_complete:
Utils.logger.info('Parameters not yet fully loaded. Reloading..')
time.sleep(1) # Throttle so we don't run out of stack space
self._load(node, retries=(retries - 1))
else:
Utils.logger.info(f'{self.__class__.__name__} parameters: ')
Utils.logger.info(f'{self}')
def __init__(self, node: Node):
node.get_logger().info('Initializing parameters')
# Declare parameters of the ROS2 node and their default values:
# The topic prefix to use (can be empty if not required)
node.declare_parameter(name='ros_topic_prefix', value='bno055/')
# The type of the sensor connection. Either "uart" or "i2c":
node.declare_parameter(name='connection_type',
value=UART.CONNECTIONTYPE_UART)
# UART port
node.declare_parameter('uart_port', value='/dev/ttyUSB0')
# UART Baud Rate
node.declare_parameter('uart_baudrate', value=115200)
# UART Timeout in seconds
node.declare_parameter('uart_timeout', value=0.1)
# tf frame id
node.declare_parameter('frame_id', value='bno055')
# Node timer frequency in Hz, defining how often sensor data is requested
node.declare_parameter('data_query_frequency', value=10)
# Node timer frequency in Hz, defining how often calibration status data is requested
node.declare_parameter('calib_status_frequency', value=0.1)
# sensor operation mode
node.declare_parameter('operation_mode', value=0x0C)
# placement_axis_remap defines the position and orientation of the sensor mount
node.declare_parameter('placement_axis_remap', value='P1')
# +/- 2000 units (at max 2G) (1 unit = 1 mg = 1 LSB = 0.01 m/s2)
node.declare_parameter('acc_offset', value=[0xFFEC, 0x00A5, 0xFFE8])
# +/- 6400 units (1 unit = 1/16 uT)
node.declare_parameter('mag_offset', value=[0xFFB4, 0xFE9E, 0x027D])
# +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps)
node.declare_parameter('gyr_offset', value=[0x0002, 0xFFFF, 0xFFFF])
# get the parameters - requires CLI arguments '--ros-args --params-file <parameter file>'
node.get_logger().info('Parameters set to:')
try:
self.ros_topic_prefix = node.get_parameter('ros_topic_prefix')
node.get_logger().info('\tros_topic_prefix:\t"%s"' %
self.ros_topic_prefix.value)
self.connection_type = node.get_parameter('connection_type')
node.get_logger().info('\tconnection_type:\t"%s"' %
self.connection_type.value)
self.uart_port = node.get_parameter('uart_port')
node.get_logger().info('\tuart_port:\t\t"%s"' %
self.uart_port.value)
self.uart_baudrate = node.get_parameter('uart_baudrate')
node.get_logger().info('\tuart_baudrate:\t\t"%s"' %
self.uart_baudrate.value)
self.uart_timeout = node.get_parameter('uart_timeout')
node.get_logger().info('\tuart_timeout:\t\t"%s"' %
self.uart_timeout.value)
self.frame_id = node.get_parameter('frame_id')
node.get_logger().info('\tframe_id:\t\t"%s"' % self.frame_id.value)
self.data_query_frequency = node.get_parameter(
'data_query_frequency')
node.get_logger().info('\tdata_query_frequency:\t"%s"' %
self.data_query_frequency.value)
self.calib_status_frequency = node.get_parameter(
'calib_status_frequency')
node.get_logger().info('\tcalib_status_frequency:\t"%s"' %
self.calib_status_frequency.value)
self.operation_mode = node.get_parameter('operation_mode')
node.get_logger().info('\toperation_mode:\t\t"%s"' %
self.operation_mode.value)
self.placement_axis_remap = node.get_parameter(
'placement_axis_remap')
node.get_logger().info('\tplacement_axis_remap:\t"%s"' %
self.placement_axis_remap.value)
self.acc_offset = node.get_parameter('acc_offset')
node.get_logger().info('\tacc_offset:\t\t"%s"' %
self.acc_offset.value)
self.mag_offset = node.get_parameter('mag_offset')
node.get_logger().info('\tmag_offset:\t\t"%s"' %
self.mag_offset.value)
self.gyr_offset = node.get_parameter('gyr_offset')
node.get_logger().info('\tgyr_offset:\t\t"%s"' %
self.gyr_offset.value)
except Exception as e: # noqa: B902
node.get_logger().warn(
'Could not get parameters...setting variables to default')
node.get_logger().warn('Error: "%s"' % e)
def __init__(self, node: Node):
node.get_logger().info('Initializing parameters')
# Declare parameters of the ROS2 node and their default values:
# The topic prefix to use (can be empty if not required)
node.declare_parameter(name='ros_topic_prefix', value='bno055/')
# The type of the sensor connection. Either "uart" or "i2c":
node.declare_parameter(name='connection_type',
value=UART.CONNECTIONTYPE_UART)
# UART port
node.declare_parameter('uart_port', value='/dev/ttyUSB0')
# UART Baud Rate
node.declare_parameter('uart_baudrate', value=115200)
# UART Timeout in seconds
node.declare_parameter('uart_timeout', value=0.1)
# tf frame id
node.declare_parameter('frame_id', value='bno055')
# Node timer frequency in Hz, defining how often sensor data is requested
node.declare_parameter('data_query_frequency', value=10)
# Node timer frequency in Hz, defining how often calibration status data is requested
node.declare_parameter('calib_status_frequency', value=0.1)
# sensor operation mode
node.declare_parameter('operation_mode', value=0x0C)
# placement_axis_remap defines the position and orientation of the sensor mount
node.declare_parameter('placement_axis_remap', value='P1')
# scaling factor for acceleration
node.declare_parameter('acc_factor', value=100.0)
# scaling factor for magnetometer
node.declare_parameter('mag_factor', value=16000000.0)
# scaling factor for gyroscope
node.declare_parameter('gyr_factor', value=900.0)
# determines whether to use default offsets or not
node.declare_parameter('set_offsets', value=False)
# +/- 2000 units (at max 2G) (1 unit = 1 mg = 1 LSB = 0.01 m/s2)
node.declare_parameter('offset_acc',
value=registers.DEFAULT_OFFSET_ACC)
# +/- 6400 units (1 unit = 1/16 uT)
node.declare_parameter('offset_mag',
value=registers.DEFAULT_OFFSET_MAG)
# +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps)
node.declare_parameter('offset_gyr',
value=registers.DEFAULT_OFFSET_GYR)
# +/-1000 units
node.declare_parameter('radius_acc',
value=registers.DEFAULT_RADIUS_ACC)
# +/-960 units
node.declare_parameter('radius_mag',
value=registers.DEFAULT_RADIUS_MAG)
# Sensor standard deviation squared (^2) defaults [x, y, z]
node.declare_parameter('variance_acc',
value=registers.DEFAULT_VARIANCE_ACC)
node.declare_parameter('variance_angular_vel',
value=registers.DEFAULT_VARIANCE_ANGULAR_VEL)
node.declare_parameter('variance_orientation',
value=registers.DEFAULT_VARIANCE_ORIENTATION)
node.declare_parameter('variance_mag',
value=registers.DEFAULT_VARIANCE_MAG)
# get the parameters - requires CLI arguments '--ros-args --params-file <parameter file>'
node.get_logger().info('Parameters set to:')
try:
self.ros_topic_prefix = node.get_parameter('ros_topic_prefix')
node.get_logger().info('\tros_topic_prefix:\t"%s"' %
self.ros_topic_prefix.value)
self.connection_type = node.get_parameter('connection_type')
node.get_logger().info('\tconnection_type:\t"%s"' %
self.connection_type.value)
self.uart_port = node.get_parameter('uart_port')
node.get_logger().info('\tuart_port:\t\t"%s"' %
self.uart_port.value)
self.uart_baudrate = node.get_parameter('uart_baudrate')
node.get_logger().info('\tuart_baudrate:\t\t"%s"' %
self.uart_baudrate.value)
self.uart_timeout = node.get_parameter('uart_timeout')
node.get_logger().info('\tuart_timeout:\t\t"%s"' %
self.uart_timeout.value)
self.frame_id = node.get_parameter('frame_id')
node.get_logger().info('\tframe_id:\t\t"%s"' % self.frame_id.value)
self.data_query_frequency = node.get_parameter(
'data_query_frequency')
node.get_logger().info('\tdata_query_frequency:\t"%s"' %
self.data_query_frequency.value)
self.calib_status_frequency = node.get_parameter(
'calib_status_frequency')
node.get_logger().info('\tcalib_status_frequency:\t"%s"' %
self.calib_status_frequency.value)
self.operation_mode = node.get_parameter('operation_mode')
node.get_logger().info('\toperation_mode:\t\t"%s"' %
self.operation_mode.value)
self.placement_axis_remap = node.get_parameter(
'placement_axis_remap')
node.get_logger().info('\tplacement_axis_remap:\t"%s"' %
self.placement_axis_remap.value)
self.acc_factor = node.get_parameter('acc_factor')
node.get_logger().info('\tacc_factor:\t\t"%s"' %
self.acc_factor.value)
self.mag_factor = node.get_parameter('mag_factor')
node.get_logger().info('\tmag_factor:\t\t"%s"' %
self.mag_factor.value)
self.gyr_factor = node.get_parameter('gyr_factor')
node.get_logger().info('\tgyr_factor:\t\t"%s"' %
self.gyr_factor.value)
self.set_offsets = node.get_parameter('set_offsets')
node.get_logger().info('\tset_offsets:\t\t"%s"' %
self.set_offsets.value)
self.offset_acc = node.get_parameter('offset_acc')
node.get_logger().info('\toffset_acc:\t\t"%s"' %
self.offset_acc.value)
self.radius_acc = node.get_parameter('radius_acc')
node.get_logger().info('\tradius_acc:\t\t"%s"' %
self.radius_acc.value)
self.offset_mag = node.get_parameter('offset_mag')
node.get_logger().info('\toffset_mag:\t\t"%s"' %
self.offset_mag.value)
self.radius_mag = node.get_parameter('radius_mag')
node.get_logger().info('\tradius_mag:\t\t"%s"' %
self.radius_mag.value)
self.offset_gyr = node.get_parameter('offset_gyr')
node.get_logger().info('\toffset_gyr:\t\t"%s"' %
self.offset_gyr.value)
self.variance_acc = node.get_parameter('variance_acc')
node.get_logger().info('\tvariance_acc:\t\t"%s"' %
self.variance_acc.value)
self.variance_angular_vel = node.get_parameter(
'variance_angular_vel')
node.get_logger().info('\tvariance_angular_vel:\t"%s"' %
self.variance_angular_vel.value)
self.variance_orientation = node.get_parameter(
'variance_orientation')
node.get_logger().info('\tvariance_orientation:\t"%s"' %
self.variance_orientation.value)
self.variance_mag = node.get_parameter('variance_mag')
node.get_logger().info('\tvariance_mag:\t\t"%s"' %
self.variance_mag.value)
except Exception as e: # noqa: B902
node.get_logger().warn(
'Could not get parameters...setting variables to default')
node.get_logger().warn('Error: "%s"' % e)
