/** Set mode for a feature.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @param mode DC1394 mode desired
* @return true if mode set successfully
*/
bool Features::setMode(dc1394feature_info_t *finfo, dc1394feature_mode_t mode)
{
dc1394feature_t feature = finfo->id;
if (hasMode(finfo, mode))
{
// first, make sure the feature is powered on
setPower(finfo, DC1394_ON);
ROS_DEBUG_STREAM("setting feature " << featureName(feature)
<< " mode to " << modeName(mode));
if (DC1394_SUCCESS !=
dc1394_feature_set_mode(camera_, feature, mode))
{
ROS_WARN_STREAM("failed to set feature " << featureName(feature)
<< " mode to " << modeName(mode));
return false;
}
}
else
{
// device does not support this mode for this feature
ROS_DEBUG_STREAM("no " << modeName(mode)
<< " mode for feature " << featureName(feature));
return false;
}
return true;
}
/** Get current state of a feature from the camera.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @return current state ID
*/
Features::state_t Features::getState(dc1394feature_info_t *finfo)
{
dc1394feature_t feature = finfo->id;
dc1394error_t rc;
if (!finfo->available)
{
// not available: nothing more to do
return camera1394::Camera1394_None;
}
if (finfo->on_off_capable)
{
// get On/Off state
dc1394switch_t pwr;
rc = dc1394_feature_get_power(camera_, feature, &pwr);
if (rc != DC1394_SUCCESS)
{
ROS_WARN_STREAM("failed to get feature " << featureName(feature)
<< " Power setting ");
}
else if (pwr == DC1394_OFF)
{
// Off overrides mode settings
return camera1394::Camera1394_Off;
}
}
// not off, so get mode
dc1394feature_mode_t mode;
rc = dc1394_feature_get_mode(camera_, feature, &mode);
if (rc != DC1394_SUCCESS)
{
ROS_WARN_STREAM("failed to get current mode of feature "
<< featureName(feature));
// treat unavailable mode as Off
return camera1394::Camera1394_Off;
}
switch (mode)
{
case DC1394_FEATURE_MODE_MANUAL:
return camera1394::Camera1394_Manual;
case DC1394_FEATURE_MODE_AUTO:
return camera1394::Camera1394_Auto;
case DC1394_FEATURE_MODE_ONE_PUSH_AUTO:
return camera1394::Camera1394_OnePush;
default:
return camera1394::Camera1394_Off;
}
}
/** Set a feature Off.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
*/
void Features::setOff(dc1394feature_info_t *finfo)
{
dc1394feature_t feature = finfo->id;
if (finfo->on_off_capable)
{
ROS_DEBUG_STREAM("setting feature " << featureName(feature) << " Off");
if (DC1394_SUCCESS !=
dc1394_feature_set_power(camera_, feature, DC1394_OFF))
{
ROS_WARN_STREAM("failed to set feature " << featureName(feature)
<< " Off ");
}
}
else
{
// This device does not support turning this feature off.
// Inform the user, but pretend it worked.
ROS_DEBUG_STREAM("no Off mode for feature " << featureName(feature));
}
}
/** Set power for a feature On or Off.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @param on_off either DC1394_ON or DC1394_OFF
*/
void Features::setPower(dc1394feature_info_t *finfo, dc1394switch_t on_off)
{
dc1394feature_t feature = finfo->id;
if (finfo->on_off_capable)
{
ROS_DEBUG_STREAM("Setting power for feature " << featureName(feature)
<< " to " << on_off);
if (DC1394_SUCCESS !=
dc1394_feature_set_power(camera_, feature, on_off))
{
ROS_WARN_STREAM("failed to set feature " << featureName(feature)
<< " power to " << on_off);
}
}
else
{
// This device does not support turning this feature on or off.
// That's OK.
ROS_DEBUG_STREAM("no power control for feature " << featureName(feature));
}
}
// ######################################################################
void RawVisualCortex::start1()
{
GVX_TRACE(__PRETTY_FUNCTION__);
ComplexChannel::start1();
// initialize out input or output MGZ handlers if necessary:
if (itsSaveOutTo.getVal().empty() == false)
itsOutputMgzOut = rutz::make_shared(new MgzEncoder(itsSaveOutTo.getVal(), 9));
if (itsLoadOutFrom.getVal().empty() == false)
itsOutputMgzIn = rutz::make_shared(new MgzDecoder(itsLoadOutFrom.getVal()));
//write out some info to command line
for (uint i = 0; i < this->numChans(); ++i)
{
LINFO("Top-level channel (%u/%u): level=%s feature=%s submaps=%u name=%s",
i+1, this->numChans(),
featureHierarchyName(featureHierarchyLevel(this->subChan(i)->visualFeature())),
featureName(this->subChan(i)->visualFeature()),
this->subChan(i)->numSubmaps(),
this->subChan(i)->descriptiveName().c_str());
}
}
/** Get feature values.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @param value [out] pointer where parameter value stored
* @param value2 [out] optional pointer for second parameter value
* for white balance. Otherwise NULL.
*/
void Features::getValues(dc1394feature_info_t *finfo,
double *value, double *value2)
{
dc1394feature_t feature = finfo->id;
dc1394error_t rc;
if (!finfo->readout_capable)
{
ROS_INFO_STREAM("feature " << featureName(feature)
<< " value not available from device");
return;
}
if (feature == DC1394_FEATURE_WHITE_BALANCE)
{
// handle White Balance separately, it has two components
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
// @todo get absolute White Balance values
rc = DC1394_FUNCTION_NOT_SUPPORTED;
}
else
{
// get integer White Balance values
uint32_t bu_val;
uint32_t rv_val;
rc = dc1394_feature_whitebalance_get_value(camera_, &bu_val, &rv_val);
if (DC1394_SUCCESS == rc)
{
// convert to double
*value = bu_val;
*value2 = rv_val;
}
}
if (DC1394_SUCCESS == rc)
{
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " Blue/U: " << *value
<< " Red/V: " << *value2);
}
else
{
ROS_WARN_STREAM("failed to get values for feature "
<< featureName(feature));
}
}
else
{
// other features only have one component
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
float fval;
rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
if (DC1394_SUCCESS == rc)
{
*value = fval; // convert to double
}
}
else // no float representation
{
uint32_t ival;
rc = dc1394_feature_get_value(camera_, feature, &ival);
if (DC1394_SUCCESS == rc)
{
*value = ival; // convert to double
}
}
if (DC1394_SUCCESS == rc)
{
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " has value " << *value);
}
else
{
ROS_WARN_STREAM("failed to get value of feature "
<< featureName(feature));
}
}
}
/** Configure a feature for the currently open device.
*
* @pre feature_set_ initialized
*
* @param feature desired feature number
* @param control [in,out] pointer to control parameter (may change)
* @param value [in,out] pointer to requested parameter value (may
* change depending on device restrictions)
* @param value2 [in,out] optional pointer to second parameter value
* for white balance (may change depending on device
* restrictions). No second value if NULL pointer.
*
* The parameter values are represented as double despite the fact
* that most features on most cameras only allow unsigned 12-bit
* values. The exception is the rare feature that supports
* "absolute" values in 32-bit IEEE float format. Double can
* represent all possible option values accurately.
*/
void Features::configure(dc1394feature_t feature, int *control,
double *value, double *value2)
{
// device-relevant information for this feature
dc1394feature_info_t *finfo =
&feature_set_.feature[feature - DC1394_FEATURE_MIN];
if (!finfo->available) // feature not available?
{
*control = camera1394::Camera1394_None;
return;
}
switch (*control)
{
case camera1394::Camera1394_Off:
setPower(finfo, DC1394_OFF);
break;
case camera1394::Camera1394_Query:
getValues(finfo, value, value2);
break;
case camera1394::Camera1394_Auto:
if (!setMode(finfo, DC1394_FEATURE_MODE_AUTO))
{
setPower(finfo, DC1394_OFF);
}
break;
case camera1394::Camera1394_Manual:
if (!setMode(finfo, DC1394_FEATURE_MODE_MANUAL))
{
setPower(finfo, DC1394_OFF);
break;
}
// TODO: break this into some internal methods
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
float fmin, fmax;
if (DC1394_SUCCESS ==
dc1394_feature_get_absolute_boundaries(camera_, feature,
&fmin, &fmax))
{
// clamp *value between minimum and maximum
if (*value < fmin)
*value = (double) fmin;
else if (*value > fmax)
*value = (double) fmax;
}
else
{
ROS_WARN_STREAM("failed to get feature "
<< featureName(feature) << " boundaries ");
}
// @todo handle absolute White Balance values
float fval = *value;
if (DC1394_SUCCESS !=
dc1394_feature_set_absolute_value(camera_, feature, fval))
{
ROS_WARN_STREAM("failed to set feature "
<< featureName(feature) << " to " << fval);
}
}
else // no float representation
{
// round requested value to nearest integer
*value = rint(*value);
// clamp *value between minimum and maximum
if (*value < finfo->min)
*value = (double) finfo->min;
else if (*value > finfo->max)
*value = (double) finfo->max;
dc1394error_t rc;
uint32_t ival = (uint32_t) *value;
// handle White Balance, which has two components
if (feature == DC1394_FEATURE_WHITE_BALANCE)
{
*value2 = rint(*value2);
// clamp *value2 between same minimum and maximum
if (*value2 < finfo->min)
*value2 = (double) finfo->min;
else if (*value2 > finfo->max)
*value2 = (double) finfo->max;
uint32_t ival2 = (uint32_t) *value2;
rc = dc1394_feature_whitebalance_set_value(camera_, ival, ival2);
}
else
{
// other features only have one component
rc = dc1394_feature_set_value(camera_, feature, ival);
}
if (rc != DC1394_SUCCESS)
{
ROS_WARN_STREAM("failed to set feature "
<< featureName(feature) << " to " << ival);
}
}
break;
case camera1394::Camera1394_OnePush:
// Try to set OnePush mode
setMode(finfo, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
// Now turn the control off, so camera does not continue adjusting
setPower(finfo, DC1394_OFF);
break;
case camera1394::Camera1394_None:
// Invalid user input, because this feature actually does exist.
ROS_INFO_STREAM("feature " << featureName(feature)
<< " exists, cannot set to None");
break;
default:
ROS_WARN_STREAM("unknown state (" << *control
<<") for feature " << featureName(feature));
}
// return actual state reported by the device
*control = getState(finfo);
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " now in state " << *control);
}
