Tilting_Laser() : ROS_Slave(), next_shutter(0.0), scancount(0) { register_source(cloud = new FlowPointCloudFloat32("cloud")); register_source(shutter = new FlowEmpty("shutter")); register_sink(scans = new FlowLaserScan("scans"), ROS_CALLBACK(Tilting_Laser, scans_callback)); register_sink(encoder = new FlowActuator("encoder"), ROS_CALLBACK(Tilting_Laser, encoder_callback)); register_source(cmd = new FlowActuator("cmd")); register_with_master(); printf("package path is [%s]\n", get_my_package_path().c_str()); if (!get_double_param(".period", &period)) period = 5.0; if (!get_double_param(".min_ang", &min_ang)) min_ang = -1.3; if (!get_double_param(".max_ang", &max_ang)) max_ang = 0.6; unsigned long long time = Quaternion3D::Qgettime(); TR.setWithEulers(3, 2, 0, 0, 0, 0, 0, 0, time); TR.setWithEulers(2, 1, 0, 0, 0, 0, 0, 0, time); gettimeofday(&starttime,NULL); }
/* * Helper function for finding the normalisation constant. Extracts parameters from the * parameter file and then calls the sub-function to do the actual calculation. */ double find_normaliser(paramlist* params, void* f1, double_arr* events, char* type) { if (!has_required_params(params, normaliser_params, sizeof(normaliser_params)/sizeof(char*))) { printf("Some parameters required to find a normalisation constant are missing."\ " Ensure you have them in your parameter file.\n"); exit(1); } double combine_start = get_double_param(params, "delta_est_combine_start"); // Length of the interval when combining functions double combine_interval = get_double_param(params, "delta_est_combine_interval"); double combine_end = combine_start + combine_interval; // Start value for finding the normalisation constant double normaliser_start = get_double_param(params, "normaliser_est_initial"); // Maximum value to allow the normalisation function to look at double normaliser_end = get_double_param(params, "normaliser_est_max"); // Increase the normalisation constant value by this each iteration double normaliser_step = get_double_param(params, "normaliser_est_step"); // Number of subintervals to use when checking normalisation int normaliser_subintervals = get_int_param(params, "normaliser_est_subintervals"); // Find a normalisation constant. This is needed to make the estimated function // correctly line up with the original. For baseline estimates, this is usually 1. return _find_normaliser(f1, events, combine_start, combine_end, normaliser_start, normaliser_end, normaliser_step, normaliser_subintervals, type); }
static int get_acis_parms (Param_File_Type *p) { int i; if (-1 == _marx_acis_get_generic_parms (p)) return -1; if (-1 == pf_get_parameters (p, Parm_Table)) return -1; for (i = 0; i < _MARX_NUM_ACIS_I_CHIPS; i++) { _Marx_Acis_Chip_Type *ccd; ccd = &Acis_CCDS[i]; ccd->ccd_id = i; #if !MARX_HAS_ACIS_GAIN_MAP && !MARX_HAS_ACIS_FEF if (-1 == get_double_param (p, "ACIS_CCD%d_Gain", i, &ccd->ccd_gain)) return -1; if (ccd->ccd_gain <= 0.0) ccd->ccd_gain = 4; ccd->ccd_gain /= 1000.0; /* Convert to KeV */ if (-1 == get_double_param (p, "ACIS_CCD%d_Offset", i, &ccd->ccd_offset)) return -1; ccd->ccd_offset /= 1000.0; /* Convert to KeV */ if (-1 == get_double_param (p, "ACIS-I%d-FanoFactor", i, &ccd->fano_factor)) return -1; if (-1 == get_double_param (p, "ACIS-I%d-ReadNoise", i, &ccd->read_noise)) return -1; if (-1 == get_double_param (p, "ACIS-I%d-EnergyGain", i, &ccd->energy_gain)) return -1; #endif if (-1 == get_file_param (p, "ACIS-I%d-QEFile", i, &ccd->qe_file)) return -1; if (-1 == get_file_param (p, "ACIS-I%d-FilterFile", i, &ccd->filter_file)) return -1; } return 0; }
RangeSender() : ROS_Slave(), freq(1) { register_source(my_scan = new FlowRangeScan("scan")); my_scan->set_scan_size(10); printf("TALKER params:\n"); print_param_names(); printf("EO talker params\n"); bool b = get_double_param(".freq", &freq); printf("b = %d freq = %f\n", b, freq); }
/* * Helper function for the area delay calculation. Extracts parameters from * the parameter list and then calls the function which does the computation. * Also does hierarchical search if the parameter is set to yes, calling the * estimation function again to make a finer pass over the data. */ double estimate_delay_area(paramlist* params, char* outfile, void* f1, void* f2, char* hierarchical, char* type, int output_switch) { if (!has_required_params(params, area_params, sizeof(area_params)/sizeof(char*))){ printf("Some parameters required to perform an area estimate of the"\ " time delay are missing. Ensure you have them in your parameter file.\n"); exit(1); } int hc = strcmp(hierarchical, "yes") == 0; double max_delay = get_double_param(params, "delta_est_max_delta"); double step = get_double_param(params, "delta_est_step_coarse"); double resolution = get_double_param(params, "delta_est_area_resolution"); double comp_start = get_double_param(params, "delta_est_area_start"); double comp_end = comp_start + get_double_param(params, "delta_est_area_interval"); #ifdef VERBOSE printf("Estimating delay using area method.\n Max delay: %lf, Step %lf,"\ " Resolution %lf, Interval [%lf %lf]\n", max_delay, step, resolution, comp_start, comp_end); #endif double step_fine = 0; double fine_range = 0; if (hc){ step_fine = get_double_param(params, "delta_est_step_fine"); fine_range = get_double_param(params, "delta_est_fine_range"); } double result = _estimate_delay_area(outfile, f1, f2, comp_start, comp_end, -max_delay, max_delay, max_delay, resolution, step, type, output_switch); if (hc){ #ifdef VERBOSE printf("Initial result is %lf. Improving estimate using finer step of %lf" \ " in range [%lf, %lf].\n", result, step_fine, result - fine_range, result + fine_range); #endif double hres = _estimate_delay_area(outfile, f1, f2, comp_start, comp_end, result - fine_range, result + fine_range, max_delay, resolution, step_fine, type, output_switch); result = hres; } printf("Time delay estimated to be: %lf\n", result); return result; }
/* * Helper function for baseline estimator. Extracts parameters from the parameter list and * passes them to the estimator. */ est_arr* estimate_baseline(paramlist* params, char *event_file, char *output_file) { int subint = get_int_param(params, "base_iwls_subintervals"); int iterations = get_int_param(params, "base_iwls_iterations"); int breakpoints = get_int_param(params, "base_max_breakpoints"); double max_extension = get_double_param(params, "base_max_extension"); double start = get_double_param(params, "est_start_time"); double end = get_double_param(params, "est_interval_time") + start; double min_interval_proportion = get_double_param(params, "base_min_interval_proportion"); double pmf_threshold = get_double_param(params, "base_pmf_threshold"); double pmf_sum_threshold = get_double_param(params, "base_pmf_sum_threshold"); return _estimate_baseline(event_file, output_file, start, end, iterations, subint, breakpoints, max_extension, min_interval_proportion, pmf_threshold, pmf_sum_threshold); }
Servo() : ROS_Slave(), mot(NULL), ipdcmot_host("192.168.1.123"), pos_send_freq(1.0) { register_sink(setpos_blocking = new FlowFloat64("setpos_blocking"), ROS_CALLBACK(Servo, setpos_blocking_cb)); register_source(setpos_result = new FlowInt32("setpos_result")); register_sink(getpos_blocking = new FlowEmpty("getpos_blocking"), ROS_CALLBACK(Servo, getpos_blocking_cb)); register_source(getpos_result = new FlowFloat64("getpos_result")); register_sink(patrol = new FlowPatrol("patrol"), ROS_CALLBACK(Servo, patrol_cb)); register_source(pos_f = new FlowFloat64("pos")); register_source(status_f = new FlowString("status")); register_with_master(); if (!get_string_param(".host", ipdcmot_host)) printf("ipdcmot_host parameter not specified... defaulting to [%s]\n", ipdcmot_host.c_str()); printf("ipdcmot host set to [%s]\n", ipdcmot_host.c_str()); get_double_param(".pos_send_freq", &pos_send_freq); printf("position send frequency set to %f\n", pos_send_freq); mot = new IPDCMOT(ipdcmot_host, 75.0); status_f->data = "ready"; status_f->publish(); }
/* * Helper function for pmf delay estimator. Extracts parameters from the paramfile provided and * passes them on to another function to do the actual computation. Also performs hierarchical * estimates of the delay if the hierarchical parameter is set to yes. This will perform two passes * over the data and attempt to improve an initial coarse estimate with a finer search in a smaller * range. */ double estimate_delay_pmf(paramlist* params, char* outfile, double_arr* base_events, double_arr* f2_events, void* f1, void* f2, double normaliser, char* hierarchical, char* type, int output_switch) { if (!has_required_params(params, pmf_params, sizeof(pmf_params)/sizeof(char*))) { printf("Some parameters required to perform a PMF estimate" \ " of delay are missing. Ensure you have them in your parameter" \ " file.\n"); exit(1); } int hc = strcmp(hierarchical, "yes") == 0; // Resolution when combining functions double combine_step = get_double_param(params, "delta_est_pmf_resolution"); // Start of the interval used when combining the functions double combine_start = get_double_param(params, "delta_est_combine_start"); // Length of the interval when combining functions double combine_interval = get_double_param(params, "delta_est_combine_interval"); // Number of bins into which we split the event data int num_bins = get_int_param(params, "delta_est_num_bins"); double max_delay = get_double_param(params, "delta_est_max_delta"); double delta_step_coarse = get_double_param(params, "delta_est_step_coarse"); double delta_step_fine = 0; double fine_range = 0; if (hc){ if (!has_required_params(params, hierarchical_params, sizeof(hierarchical_params)/sizeof(char*))){ printf("Some parameters required to perform a hierarchical estimate"\ " of delay are missing. Ensure you have them in your parameter"\ " file.\n"); exit(1); } delta_step_fine = get_double_param(params, "delta_est_step_fine"); fine_range = get_double_param(params, "delta_est_fine_range"); } if (max_delay >= combine_interval){ printf("The maximum delay (%lf) cannot exceed the length of the"\ " interval (%lf).\n", max_delay, combine_interval); exit(1); } #ifdef VERBOSE printf("Estimating delay with pmf method.\n Combine step %lf, Combine interval"\ " [%lf %lf], Bins %d, Max delay %lf, Step %lf, normaliser %lf\n", combine_step, combine_start, combine_interval + combine_start, num_bins, max_delay, delta_step_coarse, normaliser); #endif double result = _estimate_delay_pmf(outfile, base_events, f2_events, f1, f2, combine_start, combine_interval, combine_step, num_bins, -max_delay, max_delay, max_delay, delta_step_coarse, normaliser, type, output_switch); if (hc){ #ifdef VERBOSE printf("Initial result is %lf. Improving estimate using finer step of %lf"\ " in range [%lf, %lf].\n", result, delta_step_fine, result - fine_range, result + fine_range); #endif char* hout = malloc(strlen(outfile) + strlen("_hier") + 5); sprintf(hout, "%s_hier", outfile); double hres = _estimate_delay_pmf(hout, base_events, f2_events, f1, f2, combine_start, combine_interval, combine_step, num_bins, result - fine_range, result + fine_range, max_delay, delta_step_fine, normaliser, type, output_switch); printf("Estimate revised to %lf from %lf.\n", hres, result); result = hres; free(hout); } return result; }