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