core::ConfigNode VisionSystem::getConfig(core::ConfigNode config,
std::string name)
{
if (config.exists(name))
return config[name];
else
return core::ConfigNode::fromString("{}");
}
core::ConfigNode VisionSystem::findVisionSystemConfig(core::ConfigNode cfg,
std::string& nodeUsed)
{
core::ConfigNode config(core::ConfigNode::fromString("{}"));
// Attempt to find the section deeper in the file
if (cfg.exists("Subsystems"))
{
cfg = cfg["Subsystems"];
// Attempt to find a VisionSystem subsystem
core::NodeNameList nodeNames(cfg.subNodes());
BOOST_FOREACH(std::string nodeName, nodeNames)
{
core::ConfigNode subsysCfg(cfg[nodeName]);
if (("VisionSystem" == subsysCfg["type"].asString("NONE"))||
("SimVision" == subsysCfg["type"].asString("NONE")))
{
config = subsysCfg;
std::stringstream ss;
ss << "Subsystem:" << nodeName << ":" << nodeUsed;
nodeUsed = ss.str();
}
}
void ModularStateEstimator::init(core::ConfigNode config,
core::EventHubPtr eventHub)
{
//Removed 7/13/2013 to test Combined DVL Acclerometer Kalman filter
//modules.push_back(EstimationModulePtr(
// new BasicDVLEstimationModule(
// config["DVLEstimationModule"],
// eventHub,
// m_estimatedState)));
modules.push_back(EstimationModulePtr(
new DVLAccelerometerEstimator(
eventHub,
m_estimatedState)));
modules.push_back(EstimationModulePtr(
new BasicIMUEstimationModule(
config["IMUEstimationModule"],
eventHub,
m_estimatedState)));
modules.push_back(EstimationModulePtr(
new DepthKalmanModule(
config["DepthEstimationModule"],
eventHub,
m_estimatedState)));
if(config.exists("GreenBuoy"))
{
addObstacle(config["GreenBuoy"], Obstacle::GREEN_BUOY);
modules.push_back(
EstimationModulePtr(new SimpleBuoyEstimationModule(
config["GreenBuoyEstimationModule"],
eventHub,
m_estimatedState,
Obstacle::GREEN_BUOY,
vision::EventType::BUOY_FOUND)));
}
if(config.exists("RedBuoy"))
{
addObstacle(config["RedBuoy"], Obstacle::RED_BUOY);
modules.push_back(
EstimationModulePtr(new SimpleBuoyEstimationModule(
config["RedBuoyEstimationModule"],
eventHub,
m_estimatedState,
Obstacle::RED_BUOY,
vision::EventType::BUOY_FOUND)));
}
if(config.exists("YellowBuoy"))
{
addObstacle(config["YellowBuoy"], Obstacle::YELLOW_BUOY);
modules.push_back(
EstimationModulePtr(new SimpleBuoyEstimationModule(
config["YellowBuoyEstimationModule"],
eventHub,
m_estimatedState,
Obstacle::YELLOW_BUOY,
vision::EventType::BUOY_FOUND)));
}
}
void DC1394Camera::init(core::ConfigNode config, uint64_t guid)
{
LOGGER.infoStream() << guid << ": Initializing camera ";
m_guid = guid;
// Grab our camera
m_camera = dc1394_camera_new(s_libHandle, guid);
if (!m_camera) {
LOGGER.errorStream() <<
"Failed to initialize camera with guid: " << guid;
assert(m_camera && "Couldn't initialize camera");
}
// Determines settings and frame size
dc1394error_t err = DC1394_FAILURE;
dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_640x480_RGB8;
dc1394framerate_t frameRate = DC1394_FRAMERATE_15;
// Check for the whitebalance feature
dc1394feature_info_t whiteBalance;
whiteBalance.id = DC1394_FEATURE_WHITE_BALANCE;
err = dc1394_feature_get(m_camera, &whiteBalance);
assert(DC1394_SUCCESS == err && "Could not get white balance feature info");
bool hasWhiteBalance = (whiteBalance.available == DC1394_TRUE);
uint32_t uValue, vValue;
if (hasWhiteBalance)
{
err = dc1394_feature_whitebalance_get_value(m_camera, &uValue, &vValue);
LOGGER.infoStream() << m_guid
<< ": Current U: " << uValue
<< " V: " << vValue;
}
// Actually set the values if the user wants to
if (config.exists("uValue") && config.exists("vValue"))
{
bool uAuto =
boost::to_lower_copy(config["uValue"].asString("auto")) == "auto";
bool vAuto =
boost::to_lower_copy(config["vValue"].asString("auto")) == "auto";
bool autoVal = uAuto && vAuto;
if ((uAuto || vAuto) && !autoVal)
{
assert(false && "Both Whitebalance values must either be auto or manual");
}
if (autoVal)
{
setWhiteBalance(0, 0, true);
}
else
{
// Read in config values
uint32_t u_b_value = static_cast<uint32_t>(config["uValue"].asInt());
uint32_t v_r_value = static_cast<uint32_t>(config["vValue"].asInt());
// Set values
setWhiteBalance(u_b_value, v_r_value);
}
}
else if (config.exists("uValue") || config.exists("vValue"))
{
assert(false && "Must set both the U and V values for white balance");
}
err = dc1394_feature_whitebalance_get_value(m_camera, &uValue, &vValue);
LOGGER.infoStream() << m_guid
<< ": Set U: " << uValue
<< " V: " << vValue;
if (config.exists("brightness"))
{
// Read in and set values
if (boost::to_lower_copy(config["brightness"].asString("auto")) == "auto")
{
setBrightness(0, true);
}
else
{
uint32_t value = static_cast<uint32_t>(config["brightness"].asInt());
setBrightness(value);
}
}
if (config.exists("exposure"))
{
// Read in and set values
if (boost::to_lower_copy(config["exposure"].asString("auto")) == "auto")
{
setExposure(0, true);
}
else
{
uint32_t value = static_cast<uint32_t>(config["exposure"].asInt());
setExposure(value);
}
}
// Grab image size
err = dc1394_get_image_size_from_video_mode(m_camera, videoMode,
&m_width, &m_height);
assert(DC1394_SUCCESS == err && "Could not get image size");
float fRate;
err = dc1394_framerate_as_float(frameRate, &fRate);
assert(DC1394_SUCCESS == err && "Could not get framerate as float");
m_fps = fRate;
// Setup the capture
err = dc1394_video_set_iso_speed(m_camera, DC1394_ISO_SPEED_400);
assert(DC1394_SUCCESS == err && "Could not set iso speed");
err = dc1394_video_set_mode(m_camera, videoMode);
assert(DC1394_SUCCESS == err && "Could not set video mode");
err = dc1394_video_set_framerate(m_camera, frameRate);
assert(DC1394_SUCCESS == err && "Could not set framerate");
// Start data transfer
err = dc1394_video_set_transmission(m_camera, DC1394_ON);
assert(DC1394_SUCCESS == err && "Could not start camera iso transmission");
err = dc1394_capture_setup(m_camera, DMA_BUFFER_SIZE,
DC1394_CAPTURE_FLAGS_DEFAULT);
assert(DC1394_SUCCESS == err && "Could not setup camera make sure"
" that the video mode and framerate are supported by your camera");
}
