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);
}
void load_sources()
{
if (register_source(&tcpdumptraceinfo) == -1 || register_source(&tcpdumpnicinfo) == -1) {
printf("Cannot register basic sources\n");
exit(-1);
}
}
ServoStepper() : ROS_Slave()
{
register_source(setpos_request = new FlowFloat64("setpos_request"));
register_source(getpos_request = new FlowEmpty("getpos_request"));
register_sink(setpos_result = new FlowInt32("setpos_result"), ROS_CALLBACK(ServoStepper, setpos_result_callback));
register_sink(getpos_result = new FlowFloat64("getpos_result"), ROS_CALLBACK(ServoStepper, getpos_result_callback));
setpos_pair = new ROS_FlowPair(setpos_request, setpos_result);
getpos_pair = new ROS_FlowPair(getpos_request, getpos_result);
// TODO: pull start_pos, end_pos, step from master parameter dictionary
start_pos = -10;
end_pos = 10;
step = 0.05;
}
Calibrator() : ROS_Slave()
{
register_sink(image_in = new FlowImage("image_in"), ROS_CALLBACK(Calibrator, image_received));
codec_in = new ImageCodec<FlowImage>(image_in);
register_source(image_out = new FlowImage("image_out"));
codec_out = new ImageCodec<FlowImage>(image_out);
register_sink(observe = new FlowPTZActuatorNoSub("observe"), ROS_CALLBACK(Calibrator, ptz_received));
register_source(control = new FlowPTZActuatorNoSub("control"));
register_sink(key = new FlowSDLKeyEvent("key"), ROS_CALLBACK(Calibrator, key_received));
register_with_master();
cvimage_in = cvCreateMatHeader(480, 704, CV_8UC3);
cvimage_out = cvCreateMatHeader(480, 704, CV_8UC3);
cvimage_bgr = cvCreateMat(480, 704, CV_8UC3);
cvimage_undistort = cvCreateMat(480, 704, CV_8UC3);
if ((intrinsic_matrix = (CvMat*)cvLoad("intrinsic.dat")) == 0) {
intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1 );
}
if ((distortion_coeffs = (CvMat*)cvLoad("distortion.dat")) == 0) {
distortion_coeffs = cvCreateMat( 4, 1, CV_32FC1 );
}
matToScreen(intrinsic_matrix, "intrinsic");
matToScreen(distortion_coeffs, "distortion");
calibrated = false;
undistort = false;
centering = false;
take_pic = false;
img_cnt = 0;
time_t rawtime;
struct tm* timeinfo;
time(&rawtime);
timeinfo = localtime(&rawtime);
sprintf(dir_name, "images/%.2d%.2d%.2d_%.2d%.2d%.2d", timeinfo->tm_mon + 1, timeinfo->tm_mday,timeinfo->tm_year - 100,timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
if (mkdir(dir_name, 0755)) {
std::cout << "Failed to make directory: " << dir_name;
}
last_corners = new CvPoint2D32f[12*12];
}
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);
}
AxisCamPolled() : ROS_Slave(), cam(NULL), frame_id(0)
{
register_source(image = new FlowImage("image"));
register_sink(shutter = new FlowEmpty("shutter"),
ROS_CALLBACK(AxisCamPolled, shutter_cb));
if (!get_string_param(".host", axis_host))
{
printf("axis_host parameter not specified; defaulting to 192.168.0.90\n");
axis_host = "192.168.0.90";
}
printf("axis host set to [%s]\n", axis_host.c_str());
get_int_param(".frame_id", &frame_id);
cam = new AxisCam(axis_host);
}
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();
}
int source_init_all(void)
{
extern sourceprot_t ffmpeg_source;
register_source(&ffmpeg_source);
return 0;
}
