void V4RCamNode::processQueue()
{
if(!queueLoadConfiguration_.empty()) {
load_controls(queueLoadConfiguration_);
if(hasInfoMsg()) ROS_INFO_STREAM(pullInfoMsg());
if(hasErrorMsg()) ROS_ERROR_STREAM(pullErrorMsg());
writeV4lParams();
queueLoadConfiguration_ = std::string();
} else if(!queueSaveConfiguration_.empty()) {
save_controls(queueSaveConfiguration_);
if(hasInfoMsg()) ROS_INFO_STREAM(pullInfoMsg());
if(hasErrorMsg()) ROS_ERROR_STREAM(pullErrorMsg());
queueLoadConfiguration_ = std::string();
}
}
int
main (int argc, char **argv)
{
CHandle handle = 0;
CResult res = C_SUCCESS;
// Parse the command line
if(cmdline_parser(argc, argv, &args_info) != 0)
exit(1);
// Display help if no arguments were specified
if(argc == 1) {
cmdline_parser_print_help();
exit(0);
}
res = c_init();
if(res) goto done;
// Open the device
if (!args_info.list_given && (!args_info.import_given || args_info.device_given)) {
handle = c_open_device(args_info.device_arg);
if(!handle) {
print_error("Unable to open device", -1);
res = C_INVALID_DEVICE;
goto done;
}
}
// List devices
if(args_info.list_given) {
res = list_devices();
goto done;
}
// Import dynamic controls from XML file
else if(args_info.import_given) {
res = add_control_mappings(handle, args_info.import_arg);
goto done;
}
// Import dynamic controls from XML files at default location
if(args_info.addctrl_given) {
// list all xml files at default data/vid dir
int nf=0;
char vid[5];
char pid[5];
short pid_set = 0;
if(fnmatch("[[:xdigit:]][[:xdigit:]][[:xdigit:]][[:xdigit:]]:[[:xdigit:]][[:xdigit:]][[:xdigit:]][[:xdigit:]]", args_info.addctrl_arg, 0))
{
if(fnmatch("[[:xdigit:]][[:xdigit:]][[:xdigit:]][[:xdigit:]]", args_info.addctrl_arg, 0))
{
printf("%s invalid: set at least a valid vid value, :pid is optional\n", args_info.addctrl_arg);
goto done;
}
else
{
/*extract vid and reset pid*/
int c = 0;
for (c = 0; c < 4; c++)
{
vid[c] = args_info.addctrl_arg[c];
pid[c] = 0;
}
vid[4] = '\0';
pid[4] = '\0';
}
}
else
{
/*extract vid and pid*/
int c = 0;
for (c = 0; c < 4; c++)
{
vid[c] = args_info.addctrl_arg[c];
pid[c] = args_info.addctrl_arg[c+5];
}
vid[4] = '\0';
pid[4] = '\0';
pid_set = 1; /*flag pid.xml check*/
//printf("vid:%s pid:%s\n", vid, pid);
}
/* get xml file list from DATA_DIR/vid/ */
char **xml_files = get_filename (DATA_DIR, vid);
/*check for pid.xml*/
char fname[9];
strcpy(fname, pid);
strcat(fname,".xml");
if(pid_set)
{
pid_set = 0; /*reset*/
nf=0;
while (xml_files[nf] != NULL)
{
if ( strcasecmp(fname, xml_files[nf]) == 0)
pid_set = 1; /*file exists so flag it*/
nf++;
}
}
/*parse xml files*/
nf = 0;
while (xml_files[nf] != NULL)
{
/* if pid was set and pid.xml exists parse it*/
if(pid_set)
{
if ((strcasecmp(fname, xml_files[nf]) == 0))
{
printf ( "Parsing: %s \n", xml_files[nf]);
res = add_control_mappings(handle, xml_files[nf]);
}
}
else /* parse all xml files inside vid dir */
{
printf ( "Parsing: %s \n", xml_files[nf]);
res = add_control_mappings(handle, xml_files[nf]);
}
free(xml_files[nf]);
xml_files[nf]=NULL;
nf++;
}
free(xml_files);
goto done;
}
// List frame formats
if(args_info.formats_given) {
printf("Listing available frame formats for device %s:\n", args_info.device_arg);
res = list_frame_formats(handle);
}
// List controls
else if(args_info.clist_given) {
printf("Listing available controls for device %s:\n", args_info.device_arg);
res = list_controls(handle);
}
// Retrieve control value
else if(args_info.get_given) {
CControlValue value;
// Resolve the control Id
CControlId controlId = get_control_id(handle, args_info.get_arg);
if(!controlId) {
res = 1;
print_handle_error(handle, "Unknown control specified", -1);
goto done;
}
// Retrieve the control value
res = c_get_control(handle, controlId, &value);
if(res) {
print_handle_error(handle, "Unable to retrieve control value", res);
goto done;
}
printf("%d\n", value.value);
}
// Retrieve raw control value
else if(args_info.get_raw_given) {
//scan input
uint16_t unit_id;
unsigned char selector;
sscanf(args_info.get_raw_arg, "%hu:%hhu", &unit_id, &selector);
CControlValue value;
value.type = CC_TYPE_RAW;
// the entity is only used for the generating a control name
//TODO: pass the guid through cmdline (optional)
unsigned char entity[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
res = c_read_xu_control(handle, entity, unit_id, selector, &value);
if(res) {
print_handle_error(handle, "Unable to retrieve control value", res);
goto done;
}
//print the raw value
uint8_t * val = value.raw.data;
int i=0;
printf("query current value of: (LE)0x");
for(i=0; i<value.raw.size; i++) {
printf("%.2x", val[i]);
}
printf(" (BE)0x");
for(i=value.raw.size-1; i >=0; i--) {
printf("%.2x", val[i]);
}
printf("\n");
//free the raw value alocation
if(value.raw.data) free(value.raw.data);
}
else if(args_info.set_given) {
CControlValue value;
// Parse the control value
if(args_info.inputs_num < 1) {
res = 3;
print_error("No control value specified", -1);
goto done;
}
if(parse_control_value(args_info.inputs[0], &value)) {
res = 2;
print_error("Invalid control value specified", -1);
goto done;
}
// Resolve the control Id
CControlId controlId = get_control_id(handle, args_info.set_arg);
if(!controlId) {
res = 1;
print_handle_error(handle, "Unknown control specified", -1);
goto done;
}
// Set the new control value
res = c_set_control(handle, controlId, &value);
if(res) {
print_handle_error(handle, "Unable to set new control value", res);
goto done;
}
}
// Set the raw control value
else if(args_info.set_raw_given) {
CControlValue value;
// Parse the control value
if(args_info.inputs_num < 1) {
res = 3;
print_error("No raw control value specified", -1);
goto done;
}
uint16_t unit_id;
unsigned char selector;
sscanf(args_info.set_raw_arg, "%hu:%hhu", &unit_id, &selector);
parse_raw_control_value (args_info.inputs[0], &value);
// the entity is only used for the generating a control name
//TODO: pass the guid through cmdline (optional)
unsigned char entity[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
res = c_write_xu_control(handle, entity, unit_id, selector, &value);
if(res) {
print_handle_error(handle, "Unable to set the control value", res);
goto done;
}
//print the raw value le and be format
uint8_t * val = value.raw.data;
int i=0;
printf("set value of : (LE)0x");
for(i=0; i<value.raw.size; i++) {
printf("%.2x", val[i]);
}
printf(" (BE)0x");
for(i=value.raw.size-1; i >=0; i--) {
printf("%.2x", val[i]);
}
printf("\n");
//free the raw value alocation
if(value.raw.data) free(value.raw.data);
}
else if(args_info.save_ctrl_given) {
res = save_controls( handle, args_info.save_ctrl_arg);
}
else if(args_info.load_ctrl_given) {
res = load_controls( handle, args_info.load_ctrl_arg);
}
// Clean up
done:
if(handle) c_close_device(handle);
c_cleanup();
cmdline_parser_free(&args_info);
return res;
}
static int eventThread(void *data)
{
struct pt_data *gdata = (struct pt_data *) data;
struct v4l2_control control;
SDL_Surface *pscreen = *gdata->ptscreen;
struct vdIn *videoIn = gdata->ptvideoIn;
SDL_Event *sdlevent = gdata->ptsdlevent;
SDL_Rect *drect = gdata->drect;
SDL_mutex *affmutex = gdata->affmutex;
unsigned char frmrate;
int x, y;
int mouseon = 0;
int value = 0;
int len = 0;
short incpantilt = INCPANTILT;
int boucle = 0;
action_gui curr_action = A_VIDEO;
while (videoIn->signalquit)
{
SDL_LockMutex(affmutex);
frmrate = gdata->frmrate;
while (SDL_PollEvent(sdlevent))
{ //scan the event queue
switch (sdlevent->type) {
case SDL_KEYUP:
case SDL_MOUSEBUTTONUP:
mouseon = 0;
incpantilt = INCPANTILT;
boucle = 0;
break;
case SDL_MOUSEBUTTONDOWN:
mouseon = 1;
case SDL_MOUSEMOTION:
SDL_GetMouseState(&x, &y);
curr_action = GUI_whichbutton(x, y, pscreen, videoIn);
break;
case SDL_VIDEORESIZE:
pscreen =
SDL_SetVideoMode(sdlevent->resize.w,
sdlevent->resize.h, 0,
SDL_VIDEO_Flags);
drect->w = sdlevent->resize.w;
drect->h = sdlevent->resize.h;
break;
case SDL_KEYDOWN:
curr_action = GUI_keytoaction(sdlevent->key.keysym.sym);
if (curr_action != A_VIDEO)
mouseon = 1;
break;
case SDL_QUIT:
printf("\nStop asked\n");
videoIn->signalquit = 0;
break;
}
} //end if poll
SDL_UnlockMutex(affmutex);
/* traiter les actions */
value = 0;
if (mouseon){
boucle++;
switch (curr_action) {
case A_BRIGHTNESS_UP:
if ((value =
v4l2UpControl(videoIn, V4L2_CID_BRIGHTNESS)) < 0)
printf("Set Brightness up error\n");
break;
case A_CONTRAST_UP:
if ((value =
v4l2UpControl(videoIn, V4L2_CID_CONTRAST)) < 0)
printf("Set Contrast up error \n");
break;
case A_SATURATION_UP:
if ((value =
v4l2UpControl(videoIn, V4L2_CID_SATURATION)) < 0)
printf("Set Saturation up error\n");
break;
case A_GAIN_UP:
if ((value = v4l2UpControl(videoIn, V4L2_CID_GAIN)) < 0)
printf("Set Gain up error\n");
break;
case A_SHARPNESS_UP:
if ((value =
v4l2UpControl(videoIn, V4L2_CID_SHARPNESS)) < 0)
printf("Set Sharpness up error\n");
break;
case A_GAMMA_UP:
if ((value = v4l2UpControl(videoIn, V4L2_CID_GAMMA)) < 0)
printf("Set Gamma up error\n");
break;
case A_PAN_UP:
if ((value =v4L2UpDownPan(videoIn, -incpantilt)) < 0)
printf("Set Pan up error\n");
break;
case A_TILT_UP:
if ((value =v4L2UpDownTilt(videoIn, -incpantilt)) < 0)
printf("Set Tilt up error\n");
break;
case A_PAN_RESET:
if (v4l2ResetPanTilt(videoIn,1) < 0)
printf("reset pantilt error\n");
break;
case A_SCREENSHOT:
SDL_Delay(200);
videoIn->getPict = 1;
value = 1;
break;
case A_RESET:
if (v4l2ResetControl(videoIn, V4L2_CID_BRIGHTNESS) < 0)
printf("reset Brightness error\n");
if (v4l2ResetControl(videoIn, V4L2_CID_SATURATION) < 0)
printf("reset Saturation error\n");
if (v4l2ResetControl(videoIn, V4L2_CID_CONTRAST) < 0)
printf("reset Contrast error\n");
if (v4l2ResetControl(videoIn, V4L2_CID_HUE) < 0)
printf("reset Hue error\n");
if (v4l2ResetControl(videoIn, V4L2_CID_SHARPNESS) < 0)
printf("reset Sharpness error\n");
if (v4l2ResetControl(videoIn, V4L2_CID_GAMMA) < 0)
printf("reset Gamma error\n");
if (v4l2ResetControl(videoIn, V4L2_CID_GAIN) < 0)
printf("reset Gain error\n");
if (v4l2ResetPanTilt(videoIn,3) < 0)
printf("reset pantilt error\n");
break;
case A_BRIGHTNESS_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_BRIGHTNESS)) < 0)
printf("Set Brightness down error\n");
break;
case A_CONTRAST_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_CONTRAST)) < 0)
printf("Set Contrast down error\n");
break;
case A_SATURATION_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_SATURATION)) < 0)
printf("Set Saturation down error\n");
break;
case A_GAIN_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_GAIN)) < 0)
printf("Set Gain down error\n");
break;
case A_SHARPNESS_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_SHARPNESS)) < 0)
printf("Set Sharpness down error\n");
break;
case A_GAMMA_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_GAMMA)) < 0)
printf("Set Gamma down error\n");
break;
case A_PAN_DOWN:
if ((value =v4L2UpDownPan(videoIn, incpantilt)) < 0)
printf("Set Pan down error\n");
break;
case A_TILT_DOWN:
if ((value =v4L2UpDownTilt(videoIn,incpantilt)) < 0)
printf("Set Tilt down error\n");
break;
case A_TILT_RESET:
if (v4l2ResetPanTilt(videoIn,2) < 0)
printf("reset pantilt error\n");
break;
case A_RECORD_TOGGLE:
SDL_Delay(200);
videoIn->toggleAvi = !videoIn->toggleAvi;
value = videoIn->toggleAvi;
if ( value == 1 ) {
printf("avi recording started\n");
videoIn->recordstart=SDL_GetTicks();
}
else {
int dur=SDL_GetTicks()-videoIn->recordstart;
printf("\navi recording stopped (%ds)\n",dur/1000);
videoIn->recordtime+=dur;
}
break;
case A_SWITCH_LIGHTFREQFILT:
if ((value =v4l2GetControl(videoIn,V4L2_CID_POWER_LINE_FREQUENCY)) < 0)
printf("Get value of light frequency filter error\n");
if(value < 2) // round switch 50->60->NoFliker->.
value++; // \_______________;
else
value=0;
if(value == 0)
printf("Current light frequency filter: 50Hz\n");
else if(value == 1)
printf("Current light frequency filter: 60Hz\n");
else if(value == 2)
printf("Current light frequency filter: NoFliker\n");
if ((value =v4l2SetLightFrequencyFilter(videoIn,value)) < 0)
printf("Switch light frequency filter error\n");
break;
case A_QUIT:
videoIn->signalquit = 0;
break;
case A_VIDEO:
break;
case A_CAPTURE_FRAME:
value = 1;
videoIn->rawFrameCapture = 1;
break;
case A_CAPTURE_FRAMESTREAM:
value = 1;
if (!videoIn->rawFrameCapture) {
videoIn->rawFrameCapture = 2;
videoIn->rfsBytesWritten = 0;
videoIn->rfsFramesWritten = 0;
printf("Starting raw frame stream capturing ...\n");
} else if(videoIn->framesWritten >= 5) {
videoIn->rawFrameCapture = 0;
printf("Stopped raw frame stream capturing. %u bytes written for %u frames.\n",
videoIn->rfsBytesWritten, videoIn->rfsFramesWritten);
}
break;
case A_CAPTURE_STREAM:
value = 1;
if (videoIn->captureFile == NULL) {
videoIn->captureFile = fopen("stream.raw", "wb");
if(videoIn->captureFile == NULL) {
perror("Unable to open file for raw stream capturing");
} else {
printf("Starting raw stream capturing to stream.raw ...\n");
}
videoIn->bytesWritten = 0;
videoIn->framesWritten = 0;
} else if(videoIn->framesWritten >= 5) {
fclose(videoIn->captureFile);
printf("Stopped raw stream capturing to stream.raw. %u bytes written for %u frames.\n",
videoIn->bytesWritten, videoIn->framesWritten);
videoIn->captureFile = NULL;
}
break;
case A_EXPOSURE_UP:
if ((value = v4l2UpControl(videoIn, V4L2_CID_EXPOSURE_ABSOLUTE)) < 0)
printf("Set Absolute Exposure up error\n");
break;
case A_EXPOSURE_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_EXPOSURE_ABSOLUTE)) < 0)
printf("Set Absolute Exposure down error\n");
break;
case A_EXPOSURE_ON:
control.id =V4L2_CID_EXPOSURE_AUTO;
control.value =1;
if ((value = ioctl(videoIn->fd, VIDIOC_S_CTRL, &control)) < 0)
printf("Set Auto Exposure on error\n");
else
printf("Auto Exposure set to %d \n", control.value);
break;
case A_EXPOSURE_OFF:
control.id =V4L2_CID_EXPOSURE_AUTO;
control.value =8;
if ((value = ioctl(videoIn->fd, VIDIOC_S_CTRL, &control)) < 0)
printf("Set Auto Exposure off error\n");
else
printf("Auto Exposure set to %d \n", control.value);
break;
case A_BALANCE_UP:
if ((value = v4l2UpControl(videoIn, V4L2_CID_WHITE_BALANCE_TEMPERATURE)) < 0)
printf("Set Balance Temperature up error\n");
break;
case A_BALANCE_DOWN:
if ((value = v4l2DownControl(videoIn, V4L2_CID_WHITE_BALANCE_TEMPERATURE)) < 0)
printf("Set Balance Temperature down error\n");
break;
case A_BALANCE_ON:
control.id =V4L2_CID_WHITE_BALANCE_TEMPERATURE_AUTO;
control.value =1;
if ((value = ioctl(videoIn->fd, VIDIOC_S_CTRL, &control)) < 0)
printf("Set Auto Balance on error\n");
else
printf("Auto Balance set to %d \n", control.value);
break;
case A_BALANCE_OFF:
control.id =V4L2_CID_WHITE_BALANCE_TEMPERATURE_AUTO;
control.value =0;
if ((value = ioctl(videoIn->fd, VIDIOC_S_CTRL, &control)) < 0)
printf("Set Auto Balance off error\n");
else
printf("Auto Balance set to %d \n", control.value);
break;
case A_SAVE:
printf("Save controls \n");
save_controls(videoIn->fd);
break;
case A_LOAD:
printf("load controls \n");
load_controls(videoIn->fd);
break;
default:
break;
}
if(!(boucle%10)) // smooth pan tilt method
if(incpantilt < (10*INCPANTILT))
incpantilt += (INCPANTILT/4);
if(value){
len = strlen(title_act[curr_action].title)+8;
snprintf(videoIn->status, len,"%s %06d",title_act[curr_action].title,value);
}
} else { // mouseon
len = strlen(title_act[curr_action].title)+9;
snprintf(videoIn->status, len,"%s, %02d Fps",title_act[curr_action].title, frmrate);
}
SDL_Delay(50);
//printf("fp/s %d \n",frmrate);
} //end main loop
/* Close the stream capture file */
if (videoIn->captureFile) {
fclose(videoIn->captureFile);
printf("Stopped raw stream capturing to stream.raw. %u bytes written for %u frames.\n",
videoIn->bytesWritten, videoIn->framesWritten);
}
/* Display stats for raw frame stream capturing */
if (videoIn->rawFrameCapture == 2) {
printf("Stopped raw frame stream capturing. %u bytes written for %u frames.\n",
videoIn->rfsBytesWritten, videoIn->rfsFramesWritten);
}
}
