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;
}
