Example #1
0
core::ConfigNode VisionSystem::getConfig(core::ConfigNode config,
                                         std::string name)
{
    if (config.exists(name))
        return config[name];
    else
        return core::ConfigNode::fromString("{}");
}
Example #2
0
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)));
    }

}
Example #4
0
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");
}