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)