<commit_before>#include "Controller.h"
#include "RCSRemoteResourceObject.h"

namespace OIC { namespace Service
{

    constexpr unsigned int CONTROLLER_POLLING_DISCOVERY_MS = 5000; // in milliseconds

/****************************************** Controller ************************************************/

    Controller::Controller() :
        m_discoveryCallback(std::bind(&Controller::foundResourceCallback, this, std::placeholders::_1)),
        m_discoveryCallbackBLE(std::bind(&Controller::foundResourceCallbackBLE, this, std::placeholders::_1)),
        m_resourceList(),
        m_RDStarted(false),
        m_resourceObjectCacheCallback(std::bind(&Controller::resourceObjectCacheCallback, this, std::placeholders::_1, std::placeholders::_2,
                                           std::placeholders::_3)),
        m_resourceObjectStateCallback(std::bind(&Controller::resourceObjectStateCallback, this, std::placeholders::_1,
                                           std::placeholders::_2, std::placeholders::_3))
	{
        this->configurePlatform();

        // Set up the scene and the collection
        SceneList::getInstance()->setName("Office");
        m_sceneCollection = SceneList::getInstance()->addNewSceneCollection();
        m_sceneCollection->setName("Meeting Room");

        m_sceneStart = m_sceneCollection->addNewScene("Start Conference");
        m_sceneStop = m_sceneCollection->addNewScene("Stop Conference");
        m_sceneState = SceneState::STOP_SCENE;
    }

    /**
     * @brief getInstance
     * @return
     */
    Controller* Controller::getInstance()
    {
        static Controller* instance(new Controller);
        return instance;
    }

    Controller::~Controller()
	{
        m_resourceList.clear();

        this->stop();
	}

    /**
      * @brief Start the Controller
      *
      * @return The result of the startup. OC_STACK_OK on success
      */
    OCStackResult Controller::start()
    {
        // Start the discoveryManager
        const std::vector<std::string> types{OIC_DEVICE_LIGHT, OIC_DEVICE_BUTTON, OIC_DEVICE_SENSOR, OIC_DEVICE_FAN, "oic.r.prov"
                                            , RASPBERRY_PI_DEVICE_CONTROLLER};
        m_discoveryTask = Controller::discoverResource(m_discoveryCallback, types);

        // Start the DiscoveryManager for BLE devices
        //m_discoveryManagerBLE.setTimer(TIMER_BLE_DISCOVERY);
        //m_discoveryManagerBLE.discoverResource("", types, m_discoveryCallbackBLE, "", CT_ADAPTER_GATT_BTLE);

        // Start the discovery manager
        return(this->startRD());
    }
    /**
      * @brief Stop the Controller
      *
      * @param OC_STACK_OK on success
      */
    OCStackResult Controller::stop()
    {
        OCStackResult result = this->stopRD();

        if(!m_discoveryTask->isCanceled())
        {
            m_discoveryTask->cancel();
        }

        for (auto iterator = m_resourceList.begin(); iterator != m_resourceList.end();)
        {
            try {
                if(iterator->second->getRemoteResourceObject() != nullptr)
                {
                    if(iterator->second->getRemoteResourceObject()->isCaching())
                    {
                        iterator->second->getRemoteResourceObject()->stopCaching();
                    }
                }
                else
                {
                    std::cerr << "Unable to stop caching for device: " << iterator->first << std::endl;
                }
            }
            catch (RCSException e)
            {
                std::cerr << "Failed to stop caching" << std::endl;
            }

            iterator++;
        }

        return result;
    }

    /**
     * @brief configurePlatform Configures the platform
     */
    void Controller::configurePlatform()
    {
        // Create PlatformConfig object
        PlatformConfig cfg {
            OC::ServiceType::InProc,
            OC::ModeType::Both,
            "0.0.0.0", // By setting to "0.0.0.0", it binds to all available interfaces
            0,         // Uses randomly available port
            OC::QualityOfService::HighQos
        };

        OCPlatform::Configure(cfg);
    }

    /**
      * @brief Prints the data of an resource object
      *
      * @param resurce  Pointer holding the resource data
      *
      * @return OC_NO_RESOURCE if the resource doesn't exist.
      */
    OCStackResult Controller::printResourceData(RCSRemoteResourceObject::Ptr resource)
    {
        std::cout << "===================================================" << std::endl;
        std::cout << "\t Uri of the resources: " << resource->getUri() << std::endl;
        std::cout << "\t Host address of the resources: " << resource->getAddress() << std::endl;
        std::cout << "\t Types are: " << std::endl;

        for (auto type : resource->getTypes())
        {
            std::cout << "\t\t type " << type << std::endl;
        }

        std::cout << "\t Interfaces are: " << std::endl;
        for (auto interface : resource->getInterfaces())
        {
            std::cout << "\t\t interface " << interface << std::endl;
        }
    }

    /**
     * @brief getControllerResourceObjCallback  Called by the ResourceObject to invoke a change
     *                                          in the specific resource
     * @return
     */
    ResourceObject::ResourceObjectCacheCallback Controller::getControllerResourceCacheObjCallback()
    {
        return this->m_resourceObjectCacheCallback;
    }

    ResourceObject::ResourceObjectStateCallback Controller::getControllerResourceStateObjCallback()
    {
        return this->m_resourceObjectStateCallback;
    }

     /**
       * @brief Function callback for found resources
       *
       * @param resource     The discovered resource.
       */
     void Controller::foundResourceCallback(RCSRemoteResourceObject::Ptr resource)
     {
        std::lock_guard<std::mutex> lock(m_resourceMutex);
        std::cout << __func__ << std::endl;

        if(this->isResourceLegit(resource))
        {
            // Make new ResourceObject
            ResourceObject::Ptr resourceObject = ResourceObject::Ptr(new ResourceObject(resource));

            if(m_resourceList.insert({resource->getUri() + resource->getAddress(), resourceObject}).second)
            {
                this->printResourceData(resource);
                this->addResourceToScene(resource);

                std::cout << "\tAdded device: " << resource->getUri() + resource->getAddress() << std::endl;
                std::cout << "\tDevice successfully added to the list" << std::endl;
            }
        }
     }

     /**
      * @brief foundResourceCallbackBLE Callback function when discoverying BLE devices
      *
      * @param resource The discovered resource
      */
     void Controller::foundResourceCallbackBLE(OCResource::Ptr resource)
     {
         std::cout << "===================================================" << std::endl;
         std::cout << "Found BLE device! " << std::endl;
         std::cout << "===================================================" << std::endl;

        Controller::foundResourceCallback(RCSRemoteResourceObject::fromOCResource(resource));
     }

    /**
      * Start the Resource Host. Initiates resource  discovery
      * and stores the discovered resources.
      *
      * @return Result of the startup
      */
    OCStackResult Controller::startRD()
    {
        std::cout << "Inside startRD" << std::endl;
        if(!m_RDStarted)
        {
            std::cout << "Starting OCResource Directory" << std::endl;
            if (OCRDStart() != OC_STACK_OK)
            {
                std::cerr << "Failed to start RD Server" << std::endl;
                return OC_STACK_ERROR;
            }

            std::cout << "RD Server started successfully" << std::endl;
            m_RDStarted = true;
        }

        return OC_STACK_OK;
    }

    /**
      * Stop the Resource Host. Clears all memory used by
      * the resource host.
      *
      * @return Result of the shutdown
      */
    OCStackResult Controller::stopRD()
    {
        if(m_RDStarted)
        {
            if(OCRDStop() != OC_STACK_OK)
            {
                std::cout << "Failed to stop the RD Server" << std::endl;
                return OC_STACK_ERROR;
            }
        }

        return OC_STACK_OK;
    }

    /**
     * @brief printAttributes Prints the attributes of a resource
     *
     * @param attr          Attributes to be printed
     */
    void Controller::printAttributes(const RCSResourceAttributes& attr)
    {
        if(attr.empty())
        {
            std::cout << "\tAttributes empty" << std::endl;
        }
        else
        {
            std::cout << "\t Attributes: " << std::endl;

            for (const auto& attribute : attr)
            {
                std::cout << "\t\t Key: " << attribute.key() << std::endl;
                std::cout << "\t\t Value: " << attribute.value().toString() << std::endl;
            }
        }
    }

    /**
      *  @brief Disovery of resources
      *
      *  @param address 	mutlicast or unicast address using RCSAddress class
      *  @param cb 			Callback to which discovered resources are notified
      *  @param uri 		Uri to discover. If null, do not include uri in discovery
      *  @param type        Resource type used as discovery filter
      *
      *  @return Pointer to the discovery task.
      */
    RCSDiscoveryManager::DiscoveryTask::Ptr Controller::discoverResource(RCSDiscoveryManager::ResourceDiscoveredCallback cb,
        RCSAddress address, const std::string& uri, const std::string& type)

    {
        RCSDiscoveryManager::DiscoveryTask::Ptr discoveryTask;

        try
        {
            if (type.empty() && uri.empty())
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResource(address, cb);
            }
            else if (type.empty() && !(uri.empty()))
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResource(address, uri, cb);
            }
            else if (!(type.empty()) && uri.empty())
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByType(address, type, cb);
            }
            else
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByType(address, uri, type, cb);
            }
        }
        catch(const RCSPlatformException& e)
        {
             std::cout << e.what() << std::endl;
        }
        catch(const RCSException& e)
        {
            std::cout << e.what() << std::endl;
        }

        return discoveryTask;
    }

    /**
      *  @brief Disovery of resources
      *
      *  @param address 	mutlicast or unicast address using RCSAddress class
      *  @param cb 			Callback to which discovered resources are notified
      *  @param uri 		Uri to discover. If null, do not include uri in discovery
      *  @param types       Resources types used as discovery filter
      *
      *  @return Pointer to the discovery task.
      */
    RCSDiscoveryManager::DiscoveryTask::Ptr Controller::discoverResource(RCSDiscoveryManager::ResourceDiscoveredCallback cb,
       const std::vector<std::string> &types, RCSAddress address, const std::string& uri)
    {
        RCSDiscoveryManager::DiscoveryTask::Ptr discoveryTask;

        try
            {
            if(uri.empty())
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByTypes(address, types, cb);
            }
            else
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByTypes(address, uri, types, cb);
            }
        }
        catch(const RCSPlatformException& e)
        {
             std::cout << e.what() << std::endl;
        }
        catch(const RCSException& e)
        {
            std::cout << e.what() << std::endl;
        }

        return discoveryTask;
    }

    /**
      * @brief Looks up the list of known resources type
      *
      * @param resource     Pointer to the resource object
      *
      * @return True if the type is found, false otherwise.
      */
    bool Controller::isResourceLegit(RCSRemoteResourceObject::Ptr resource)
    {
        // Filter platform and device resources
        std::string uri = resource->getUri();
        std::vector<std::string> types = resource->getTypes();

        if (uri == "/oic/p" || uri == "/oic/d")
        {
            return false;
        }
        else if(uri.size() > HOSTING_TAG_SIZE)
        {
            if (uri.compare(
                    uri.size()-HOSTING_TAG_SIZE, HOSTING_TAG_SIZE, HOSTING_TAG) == 0)
            {
                std::cout << "Device: " << uri << " is not a legit device. Device is hosting" << std::endl;
                return false;
            }
            return true;
        }
        else if (std::find_if(types.begin(), types.end(), [](const std::string &type) {return type == OIC_TYPE_RESOURCE_HOST;}) != types.end())
        {
            std::cout << "Resource type is Hosting. Not adding an additional monitoring state" << std::endl;
            return false;
        }
        else
        {
            return true;
        }
    }

    /**
     * @brief addResourceToScene Adds a resource to the two scenes
     *
     * @param resource THe resource to be added
     */
    void Controller::addResourceToScene(RCSRemoteResourceObject::Ptr resource)
    {
        // Search through the resource types
        for(const auto& type : resource->getTypes())
        {
            if(type.compare(OIC_DEVICE_LIGHT) == 0)
            {
                m_sceneStart->addNewSceneAction(resource, "power", true);
                m_sceneStop->addNewSceneAction(resource, "power", false);
            }
        }
    }

    /**
     * @brief executeSceneCallback Cb invoked when a scene is executed
     *
     * @param eCode Result of the scene execution.
     */
    void Controller::executeSceneCallback(int eCode)
    {
        std::cout << __func__ << std::endl;

        std::cout << "Result of eCode: " << eCode << std::endl;
    }

    /**
     * @brief resourceObjectCallback Callback invoked when a new request for a resource is invoked.
     * @param resource      The resource that has been changed
     * @param state         The type of change that occured
     */
    void Controller::resourceObjectCacheCallback(const RCSResourceAttributes &attrs, const ResourceObjectState &state, const ResourceDeviceType &deviceType)
    {
        // If the device is a button, search for the current state
        if(deviceType == ResourceDeviceType::OIC_BUTTON)
        {
            for(auto const &attr : attrs)
            {
                // Simple test scenario turning on/off the LED.
                const std::string key = attr.key();
                const RCSResourceAttributes::Value value = attr.value();
                if(key == "state" && value.toString() == "true")
                {
                    if(m_sceneState == SceneState::START_SCENE)
                    {
                        std::cout << "\nSetting Scene State: STOP_SCENE\n";
                        m_sceneState = SceneState::STOP_SCENE;
                        m_sceneStop->execute(std::bind(&Controller::executeSceneCallback, this, std::placeholders::_1));
                    }
                    else
                    {
                        std::cout << "\nSetting Scene State: START_SCENE\n";
                        m_sceneState = SceneState::START_SCENE;
                        m_sceneStart->execute(std::bind(&Controller::executeSceneCallback, this, std::placeholders::_1));
                    }
                }
            }
        }
    }

    /**
     * @brief State called when the resource's state changes
     *
     * @param state New state of the resource
     * @param resourceKey Key of the resource to find it in the map
     */
    void Controller::resourceObjectStateCallback(const ResourceState &state, const std::string &uri, const std::string &address)
    {
        std::cout << __func__ << std::endl;

        switch(state)
        {
        case ResourceState::ALIVE:
                std::cout << "Resource with uri " << uri << " is ALIVE again" << std::endl;
            break;
        case ResourceState::LOST_SIGNAL:
        case ResourceState::DESTROYED:
            {
                std::cout << "Lost Signal " << std::endl;

                // Find the resource and remove it from the list
                ResourceKey resourceKey = uri + address;

                // Find the object
                std::unordered_map<ResourceKey, ResourceObject::Ptr>::const_iterator object = m_resourceList.find (input);
                if( object == m_resourceList.end())
                {
                    std::cout << "Object not found!" << std::endl;
                }
                else
                {
                    // Destroy the object
                    std::cout << "Resetting resource object " << std::endl;
                    ResourceObject::Ptr resource = got->second;
                    resource.reset();
                }

                if(m_resourceList.erase(resourceKey) > 0)
                {
                    std::cout << "Removed resource: " << uri << std::endl;
                }
            }
            break;

        default:
                std::cout << "Unsupported resource state" << std::endl;
            break;
        }
    }

} }
<commit_msg>Few changes to freja board<commit_after>#include "Controller.h"
#include "RCSRemoteResourceObject.h"

namespace OIC { namespace Service
{

    constexpr unsigned int CONTROLLER_POLLING_DISCOVERY_MS = 5000; // in milliseconds

/****************************************** Controller ************************************************/

    Controller::Controller() :
        m_discoveryCallback(std::bind(&Controller::foundResourceCallback, this, std::placeholders::_1)),
        m_discoveryCallbackBLE(std::bind(&Controller::foundResourceCallbackBLE, this, std::placeholders::_1)),
        m_resourceList(),
        m_RDStarted(false),
        m_resourceObjectCacheCallback(std::bind(&Controller::resourceObjectCacheCallback, this, std::placeholders::_1, std::placeholders::_2,
                                           std::placeholders::_3)),
        m_resourceObjectStateCallback(std::bind(&Controller::resourceObjectStateCallback, this, std::placeholders::_1,
                                           std::placeholders::_2, std::placeholders::_3))
	{
        //this->configurePlatform();

        // Set up the scene and the collection
        SceneList::getInstance()->setName("Office");
        m_sceneCollection = SceneList::getInstance()->addNewSceneCollection();
        m_sceneCollection->setName("Meeting Room");

        m_sceneStart = m_sceneCollection->addNewScene("Start Conference");
        m_sceneStop = m_sceneCollection->addNewScene("Stop Conference");
        m_sceneState = SceneState::STOP_SCENE;
    }

    /**
     * @brief getInstance
     * @return
     */
    Controller* Controller::getInstance()
    {
        static Controller* instance(new Controller);
        return instance;
    }

    Controller::~Controller()
	{
        m_resourceList.clear();

        this->stop();
	}

    /**
      * @brief Start the Controller
      *
      * @return The result of the startup. OC_STACK_OK on success
      */
    OCStackResult Controller::start()
    {
        // Start the discoveryManager
        const std::vector<std::string> types{OIC_DEVICE_LIGHT, OIC_DEVICE_BUTTON, OIC_DEVICE_SENSOR, OIC_DEVICE_FAN, "oic.r.prov"
                                            , RASPBERRY_PI_DEVICE_CONTROLLER};
        m_discoveryTask = Controller::discoverResource(m_discoveryCallback, types);

        // Start the DiscoveryManager for BLE devices
        //m_discoveryManagerBLE.setTimer(TIMER_BLE_DISCOVERY);
        //m_discoveryManagerBLE.discoverResource("", types, m_discoveryCallbackBLE, "", CT_ADAPTER_GATT_BTLE);

        // Start the discovery manager
        return(startRD());
    }
    /**
      * @brief Stop the Controller
      *
      * @param OC_STACK_OK on success
      */
    OCStackResult Controller::stop()
    {
        OCStackResult result = this->stopRD();

        if(!m_discoveryTask->isCanceled())
        {
            m_discoveryTask->cancel();
        }

        for (auto iterator = m_resourceList.begin(); iterator != m_resourceList.end();)
        {
            try {
                if(iterator->second->getRemoteResourceObject() != nullptr)
                {
                    if(iterator->second->getRemoteResourceObject()->isCaching())
                    {
                        iterator->second->getRemoteResourceObject()->stopCaching();
                    }
                }
                else
                {
                    std::cerr << "Unable to stop caching for device: " << iterator->first << std::endl;
                }
            }
            catch (RCSException e)
            {
                std::cerr << "Failed to stop caching" << std::endl;
            }

            iterator++;
        }

        return result;
    }

    /**
     * @brief configurePlatform Configures the platform
     */
    void Controller::configurePlatform()
    {
        // Create PlatformConfig object
        /*PlatformConfig cfg {
            OC::ServiceType::InProc,
            OC::ModeType::Both,
            "0.0.0.0", // By setting to "0.0.0.0", it binds to all available interfaces
            0,         // Uses randomly available port
            OC::QualityOfService::HighQos
        };

        OCPlatform::Configure(cfg);*/
        OCStackResult result = OCInit(NULL, 0, OC_CLIENT_SERVER);
    }

    /**
      * @brief Prints the data of an resource object
      *
      * @param resurce  Pointer holding the resource data
      *
      * @return OC_NO_RESOURCE if the resource doesn't exist.
      */
    OCStackResult Controller::printResourceData(RCSRemoteResourceObject::Ptr resource)
    {
        std::cout << "===================================================" << std::endl;
        std::cout << "\t Uri of the resources: " << resource->getUri() << std::endl;
        std::cout << "\t Host address of the resources: " << resource->getAddress() << std::endl;
        std::cout << "\t Types are: " << std::endl;

        for (auto type : resource->getTypes())
        {
            std::cout << "\t\t type " << type << std::endl;
        }

        std::cout << "\t Interfaces are: " << std::endl;
        for (auto interface : resource->getInterfaces())
        {
            std::cout << "\t\t interface " << interface << std::endl;
        }
    }

    /**
     * @brief getControllerResourceObjCallback  Called by the ResourceObject to invoke a change
     *                                          in the specific resource
     * @return
     */
    ResourceObject::ResourceObjectCacheCallback Controller::getControllerResourceCacheObjCallback()
    {
        return this->m_resourceObjectCacheCallback;
    }

    ResourceObject::ResourceObjectStateCallback Controller::getControllerResourceStateObjCallback()
    {
        return this->m_resourceObjectStateCallback;
    }

     /**
       * @brief Function callback for found resources
       *
       * @param resource     The discovered resource.
       */
     void Controller::foundResourceCallback(RCSRemoteResourceObject::Ptr resource)
     {
        std::lock_guard<std::mutex> lock(m_resourceMutex);
        std::cout << __func__ << std::endl;

        if(this->isResourceLegit(resource))
        {
            // Make new ResourceObject
            ResourceObject::Ptr resourceObject = ResourceObject::Ptr(new ResourceObject(resource));

            if(m_resourceList.insert({resource->getUri() + resource->getAddress(), resourceObject}).second)
            {
                this->printResourceData(resource);
                this->addResourceToScene(resource);

                std::cout << "\tAdded device: " << resource->getUri() + resource->getAddress() << std::endl;
                std::cout << "\tDevice successfully added to the list" << std::endl;
            }
        }
     }

     /**
      * @brief foundResourceCallbackBLE Callback function when discoverying BLE devices
      *
      * @param resource The discovered resource
      */
     void Controller::foundResourceCallbackBLE(OCResource::Ptr resource)
     {
         std::cout << "===================================================" << std::endl;
         std::cout << "Found BLE device! " << std::endl;
         std::cout << "===================================================" << std::endl;

        Controller::foundResourceCallback(RCSRemoteResourceObject::fromOCResource(resource));
     }

    /**
      * Start the Resource Host. Initiates resource  discovery
      * and stores the discovered resources.
      *
      * @return Result of the startup
      */
    OCStackResult Controller::startRD()
    {
        OCStackResult result = OCInit(NULL, 0, OC_CLIENT_SERVER);

        if(result != OC_STACK_OK)
        {
            std::cout << "Failed to setup platform!" << std::endl;
        }
        std::cout << "Inside startRD" << std::endl;
        if(!m_RDStarted)
        {
            std::cout << "Starting OCResource Directory" << std::endl;
            if (OCRDStart() != OC_STACK_OK)
            {
                std::cerr << "Failed to start RD Server" << std::endl;
                return OC_STACK_ERROR;
            }

            std::cout << "RD Server started successfully" << std::endl;
            m_RDStarted = true;
        }

        return OC_STACK_OK;
    }

    /**
      * Stop the Resource Host. Clears all memory used by
      * the resource host.
      *
      * @return Result of the shutdown
      */
    OCStackResult Controller::stopRD()
    {
        if(m_RDStarted)
        {
            if(OCRDStop() != OC_STACK_OK)
            {
                std::cout << "Failed to stop the RD Server" << std::endl;
                return OC_STACK_ERROR;
            }
        }

        return OC_STACK_OK;
    }

    /**
     * @brief printAttributes Prints the attributes of a resource
     *
     * @param attr          Attributes to be printed
     */
    void Controller::printAttributes(const RCSResourceAttributes& attr)
    {
        if(attr.empty())
        {
            std::cout << "\tAttributes empty" << std::endl;
        }
        else
        {
            std::cout << "\t Attributes: " << std::endl;

            for (const auto& attribute : attr)
            {
                std::cout << "\t\t Key: " << attribute.key() << std::endl;
                std::cout << "\t\t Value: " << attribute.value().toString() << std::endl;
            }
        }
    }

    /**
      *  @brief Disovery of resources
      *
      *  @param address 	mutlicast or unicast address using RCSAddress class
      *  @param cb 			Callback to which discovered resources are notified
      *  @param uri 		Uri to discover. If null, do not include uri in discovery
      *  @param type        Resource type used as discovery filter
      *
      *  @return Pointer to the discovery task.
      */
    RCSDiscoveryManager::DiscoveryTask::Ptr Controller::discoverResource(RCSDiscoveryManager::ResourceDiscoveredCallback cb,
        RCSAddress address, const std::string& uri, const std::string& type)

    {
        RCSDiscoveryManager::DiscoveryTask::Ptr discoveryTask;

        try
        {
            if (type.empty() && uri.empty())
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResource(address, cb);
            }
            else if (type.empty() && !(uri.empty()))
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResource(address, uri, cb);
            }
            else if (!(type.empty()) && uri.empty())
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByType(address, type, cb);
            }
            else
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByType(address, uri, type, cb);
            }
        }
        catch(const RCSPlatformException& e)
        {
             std::cout << e.what() << std::endl;
        }
        catch(const RCSException& e)
        {
            std::cout << e.what() << std::endl;
        }

        return discoveryTask;
    }

    /**
      *  @brief Disovery of resources
      *
      *  @param address 	mutlicast or unicast address using RCSAddress class
      *  @param cb 			Callback to which discovered resources are notified
      *  @param uri 		Uri to discover. If null, do not include uri in discovery
      *  @param types       Resources types used as discovery filter
      *
      *  @return Pointer to the discovery task.
      */
    RCSDiscoveryManager::DiscoveryTask::Ptr Controller::discoverResource(RCSDiscoveryManager::ResourceDiscoveredCallback cb,
       const std::vector<std::string> &types, RCSAddress address, const std::string& uri)
    {
        RCSDiscoveryManager::DiscoveryTask::Ptr discoveryTask;

        try
            {
            if(uri.empty())
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByTypes(address, types, cb);
            }
            else
            {
                discoveryTask = RCSDiscoveryManager::getInstance()->discoverResourceByTypes(address, uri, types, cb);
            }
        }
        catch(const RCSPlatformException& e)
        {
             std::cout << e.what() << std::endl;
        }
        catch(const RCSException& e)
        {
            std::cout << e.what() << std::endl;
        }

        return discoveryTask;
    }

    /**
      * @brief Looks up the list of known resources type
      *
      * @param resource     Pointer to the resource object
      *
      * @return True if the type is found, false otherwise.
      */
    bool Controller::isResourceLegit(RCSRemoteResourceObject::Ptr resource)
    {
        // Filter platform and device resources
        std::string uri = resource->getUri();
        std::vector<std::string> types = resource->getTypes();

        if (uri == "/oic/p" || uri == "/oic/d")
        {
            return false;
        }
        else if(uri.size() > HOSTING_TAG_SIZE)
        {
            if (uri.compare(
                    uri.size()-HOSTING_TAG_SIZE, HOSTING_TAG_SIZE, HOSTING_TAG) == 0)
            {
                std::cout << "Device: " << uri << " is not a legit device. Device is hosting" << std::endl;
                return false;
            }
            return true;
        }
        else if (std::find_if(types.begin(), types.end(), [](const std::string &type) {return type == OIC_TYPE_RESOURCE_HOST;}) != types.end())
        {
            std::cout << "Resource type is Hosting. Not adding an additional monitoring state" << std::endl;
            return false;
        }
        else
        {
            return true;
        }
    }

    /**
     * @brief addResourceToScene Adds a resource to the two scenes
     *
     * @param resource THe resource to be added
     */
    void Controller::addResourceToScene(RCSRemoteResourceObject::Ptr resource)
    {
        // Search through the resource types
        for(const auto& type : resource->getTypes())
        {
            if(type.compare(OIC_DEVICE_LIGHT) == 0)
            {
                m_sceneStart->addNewSceneAction(resource, "power", true);
                m_sceneStop->addNewSceneAction(resource, "power", false);
            }
        }
    }

    /**
     * @brief executeSceneCallback Cb invoked when a scene is executed
     *
     * @param eCode Result of the scene execution.
     */
    void Controller::executeSceneCallback(int eCode)
    {
        std::cout << __func__ << std::endl;

        std::cout << "Result of eCode: " << eCode << std::endl;
    }

    /**
     * @brief resourceObjectCallback Callback invoked when a new request for a resource is invoked.
     * @param resource      The resource that has been changed
     * @param state         The type of change that occured
     */
    void Controller::resourceObjectCacheCallback(const RCSResourceAttributes &attrs, const ResourceObjectState &state, const ResourceDeviceType &deviceType)
    {
        // If the device is a button, search for the current state
        if(deviceType == ResourceDeviceType::OIC_BUTTON)
        {
            for(auto const &attr : attrs)
            {
                // Simple test scenario turning on/off the LED.
                const std::string key = attr.key();
                const RCSResourceAttributes::Value value = attr.value();
                if(key == "state" && value.toString() == "true")
                {
                    if(m_sceneState == SceneState::START_SCENE)
                    {
                        std::cout << "\nSetting Scene State: STOP_SCENE\n";
                        m_sceneState = SceneState::STOP_SCENE;
                        m_sceneStop->execute(std::bind(&Controller::executeSceneCallback, this, std::placeholders::_1));
                    }
                    else
                    {
                        std::cout << "\nSetting Scene State: START_SCENE\n";
                        m_sceneState = SceneState::START_SCENE;
                        m_sceneStart->execute(std::bind(&Controller::executeSceneCallback, this, std::placeholders::_1));
                    }
                }
            }
        }
    }

    /**
     * @brief State called when the resource's state changes
     *
     * @param state New state of the resource
     * @param resourceKey Key of the resource to find it in the map
     */
    void Controller::resourceObjectStateCallback(const ResourceState &state, const std::string &uri, const std::string &address)
    {
        std::cout << __func__ << std::endl;

        switch(state)
        {
        case ResourceState::ALIVE:
                std::cout << "Resource with uri " << uri << " is ALIVE again" << std::endl;
            break;
        case ResourceState::LOST_SIGNAL:
        case ResourceState::DESTROYED:
            {
                std::cout << "Lost Signal " << std::endl;

                // Find the resource and remove it from the list
                ResourceKey resourceKey = uri + address;

                // Find the object
                std::unordered_map<ResourceKey, ResourceObject::Ptr>::const_iterator object = m_resourceList.find (input);
                if( object == m_resourceList.end())
                {
                    std::cout << "Object not found!" << std::endl;
                }
                else
                {
                    // Destroy the object
                    std::cout << "Resetting resource object " << std::endl;
                    ResourceObject::Ptr resource = got->second;
                    resource.reset();
                }

                if(m_resourceList.erase(resourceKey) > 0)
                {
                    std::cout << "Removed resource: " << uri << std::endl;
                }
            }
            break;

        default:
                std::cout << "Unsupported resource state" << std::endl;
            break;
        }
    }

} }
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/memory/lib/phy/ddr_phy.H $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file ddr_phy.H
/// @brief Subroutines to manipulate the phy, or used during phy procedures
///
// *HWP HWP Owner: Brian Silver <bsilver@us.ibm.com>
// *HWP HWP Backup: Andre Marin <aamarin@us.ibm.com>
// *HWP Team: Memory
// *HWP Level: 2
// *HWP Consumed by: FSP:HB

#ifndef _MSS_DDR_PHY_H_
#define _MSS_DDR_PHY_H_

#include <fapi2.H>
#include <lib/mss_attribute_accessors.H>

namespace mss
{

///
/// @brief Perform initializations for the PHY
/// @param[in] i_target the MCBIST which has the PHYs to initialize
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode phy_scominit(const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target);

///
/// @brief change resetn to the given state
/// @param[in] i_target the mcbist
/// @param[in] i_state the desired state
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode change_resetn( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target, states i_state );

///
/// @brief perform the zctl enable process
/// @param[in] i_target the mcbist for the reset recover
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode enable_zctl( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

/// @brief Change mclk low
/// @param[in] i_target mcbist target
/// @param[in] i_state mss::HIGH or mss::LOW - desired state.
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode change_force_mclk_low (const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target,
        const mss::states i_state);
///
/// @brief Unset the PLL and check to see that the PLL's have started
/// @param[in] i_target the mcbist target
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode deassert_pll_reset( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Change the continuous update mode of the PR CNTL registers
/// @note Will take the SYSCLK control out of reset, too
/// @param[in] i_target the mcbist target
/// @param[in] i_state, mss::ON if you want to be in continuous mode, mss::OFF to turn it off
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode setup_phase_rotator_control_registers( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target,
        const states i_state );

///
/// @brief Deassetr the sys clk reset
/// @param[in] i_target the mcbist target
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode deassert_sysclk_reset( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Reset the training delay configureation
/// @param[in] i_target the mcbist target
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode reset_delay_values( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Check if the bang bang lock has succeeded
/// @param[in] i_target a MCBIST target
/// @return FAPI2_RC_SUCCESs iff ok
///
fapi2::ReturnCode check_bang_bang_lock( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Configure the DP16 sysclk
/// @param[in] i_target a MCBIST target
/// @return FAPI2_RC_SUCCESs iff ok
///
fapi2::ReturnCode setup_dp16_sysclk( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief check and process initial cal errors
/// @tparam T the type of the target in question
/// @param[in] i_target the port in question
/// @return fapi2::ReturnCode, FAPI2_RC_SUCCESS iff no error
///
template< fapi2::TargetType T >
fapi2::ReturnCode process_initial_cal_errors( const fapi2::Target<T>& i_target );

///
/// @brief Setup all the cal config register
/// @tparam T, the target type of the MCA/MBA
/// @param[in] i_target the target associated with this cal setup
/// @param[in] i_rank_pairs the vector of currently configured rank pairs
/// @param[in] i_cal_steps_enabled fapi2::buffer<uint16_t> representing the cal steps to enable
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode setup_cal_config( const fapi2::Target<T>& i_target,
                                    const std::vector<uint64_t> i_rank_pairs,
                                    const fapi2::buffer<uint16_t> i_cal_steps_enabled);

///
/// @brief Setup all the cal config register
/// @param[in] i_target the target associated with this cal setup
/// @param[in] i_rank one currently configured rank pairs
/// @param[in] i_cal_steps_enabled fapi2::buffer<uint16_t> representing the cal steps to enable
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
fapi2::ReturnCode setup_cal_config( const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target,
                                    const uint64_t i_rank,
                                    const fapi2::buffer<uint16_t> i_cal_steps_enabled);

///
/// @brief Setup odt_wr/rd_config
/// @tparam T the target type of the MCA/MBA
/// @param[in] i_target the target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode reset_odt_config( const fapi2::Target<T>& i_target );

///
/// @brief Setup odt_wr/rd_config
/// @param[in] i_target the MCA target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode reset_odt_config( const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target );

///
/// @brief Perform the DLL calibration
/// @tparam T the target type of the MCBIST
/// @param[in] i_target the target associated with this DLL cal
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode dll_calibration( const fapi2::Target<T>& i_target );

///
/// @brief Perform the DLL calibration
/// @param[in] i_target the target associated with this DLL cal
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode dll_calibration( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Flush the output drivers
/// @tparam T the target type of the MCBIST
/// @param[in] i_target the target associated with the phy reset sequence
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode flush_output_drivers( const fapi2::Target<T>& i_target );

///
/// @brief Flush the output drivers
/// @param[in] i_target the target associated with the phy reset sequence
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode flush_output_drivers( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Setup seq_rd_wr_data
/// @tparam T the target type of the MCA/MBA
/// @param[in] i_target the target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode reset_seq_rd_wr_data( const fapi2::Target<T>& i_target );

///
/// @brief Setup seq_rd_wr_data
/// @param[in] i_target the MCA target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode reset_seq_rd_wr_data( const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target );

///
/// @brief Return the DIMM target for the primary rank in the specificed rank pair
/// @tparam T fapi2::TargetType of the port target
/// @param[in] i_target the target
/// @param[in] i_rp the rank pair
/// @param[out] fapi2::Target<fapi2::TARGET_TYPE_DIMM>
/// @return FAPI2_RC_SUCCESS iff ok
///
template<fapi2::TargetType T>
fapi2::ReturnCode rank_pair_primary_to_dimm(const fapi2::Target<T>& i_target, const uint64_t i_rp,
        fapi2::Target<fapi2::TARGET_TYPE_DIMM>& o_dimm);

///
/// @brief Sets up the IO impedances (ADR DRV's and DP DRV's/RCV's)
/// @tparam T the fapi2::TargetType
/// @param[in] i_target the target (MCA/MCBIST or MBA?)
/// @return FAPI2_RC_SUCCESS if and only if ok
///
template< fapi2::TargetType T>
fapi2::ReturnCode reset_io_impedances(const fapi2::Target<T>& i_target);

}
#endif
<commit_msg>Map from Centaur canonical rank numbering to Nimbus<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/memory/lib/phy/ddr_phy.H $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file ddr_phy.H
/// @brief Subroutines to manipulate the phy, or used during phy procedures
///
// *HWP HWP Owner: Brian Silver <bsilver@us.ibm.com>
// *HWP HWP Backup: Andre Marin <aamarin@us.ibm.com>
// *HWP Team: Memory
// *HWP Level: 2
// *HWP Consumed by: FSP:HB

#ifndef _MSS_DDR_PHY_H_
#define _MSS_DDR_PHY_H_

#include <fapi2.H>
#include <lib/mss_attribute_accessors.H>

namespace mss
{

///
/// @brief Perform initializations for the PHY
/// @param[in] i_target the MCBIST which has the PHYs to initialize
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode phy_scominit(const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target);

///
/// @brief change resetn to the given state
/// @param[in] i_target the mcbist
/// @param[in] i_state the desired state
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode change_resetn( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target, states i_state );

///
/// @brief perform the zctl enable process
/// @param[in] i_target the mcbist for the reset recover
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode enable_zctl( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

/// @brief Change mclk low
/// @param[in] i_target mcbist target
/// @param[in] i_state mss::HIGH or mss::LOW - desired state.
/// @return FAPI2_RC_SUCCESS iff ok
///
fapi2::ReturnCode change_force_mclk_low (const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target,
        const mss::states i_state);
///
/// @brief Unset the PLL and check to see that the PLL's have started
/// @param[in] i_target the mcbist target
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode deassert_pll_reset( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Change the continuous update mode of the PR CNTL registers
/// @note Will take the SYSCLK control out of reset, too
/// @param[in] i_target the mcbist target
/// @param[in] i_state, mss::ON if you want to be in continuous mode, mss::OFF to turn it off
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode setup_phase_rotator_control_registers( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target,
        const states i_state );

///
/// @brief Deassetr the sys clk reset
/// @param[in] i_target the mcbist target
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode deassert_sysclk_reset( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Reset the training delay configureation
/// @param[in] i_target the mcbist target
/// @return FAPI2_RC_SUCCES iff ok
///
fapi2::ReturnCode reset_delay_values( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Check if the bang bang lock has succeeded
/// @param[in] i_target a MCBIST target
/// @return FAPI2_RC_SUCCESs iff ok
///
fapi2::ReturnCode check_bang_bang_lock( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Configure the DP16 sysclk
/// @param[in] i_target a MCBIST target
/// @return FAPI2_RC_SUCCESs iff ok
///
fapi2::ReturnCode setup_dp16_sysclk( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief check and process initial cal errors
/// @tparam T the type of the target in question
/// @param[in] i_target the port in question
/// @return fapi2::ReturnCode, FAPI2_RC_SUCCESS iff no error
///
template< fapi2::TargetType T >
fapi2::ReturnCode process_initial_cal_errors( const fapi2::Target<T>& i_target );

///
/// @brief Setup all the cal config register
/// @tparam T, the target type of the MCA/MBA
/// @param[in] i_target the target associated with this cal setup
/// @param[in] i_rank_pairs the vector of currently configured rank pairs
/// @param[in] i_cal_steps_enabled fapi2::buffer<uint16_t> representing the cal steps to enable
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode setup_cal_config( const fapi2::Target<T>& i_target,
                                    const std::vector<uint64_t> i_rank_pairs,
                                    const fapi2::buffer<uint16_t> i_cal_steps_enabled);

///
/// @brief Setup all the cal config register
/// @param[in] i_target the target associated with this cal setup
/// @param[in] i_rank one currently configured rank pairs
/// @param[in] i_cal_steps_enabled fapi2::buffer<uint16_t> representing the cal steps to enable
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
fapi2::ReturnCode setup_cal_config( const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target,
                                    const uint64_t i_rank,
                                    const fapi2::buffer<uint16_t> i_cal_steps_enabled);

// TODO RTC: 157753 tparams P and R can be pulled from an MCA trait once we have it
///
/// @brief Setup odt_wr/rd_config
/// @tparam T the target type of the MCA/MBA
/// @tparam P the maximum DIMM per T
/// @tparam R the maximum rank per DIMM on T
/// @param[in] i_target the target
/// @param[in] i_dimm_count the number of DIMM presently on the target
/// @param[in] i_odt_rd the RD ODT values from VPD
/// @param[in] i_odt_wr the WR ODT values from VPD
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T, uint64_t P, uint64_t R >
fapi2::ReturnCode reset_odt_config_helper( const fapi2::Target<T>& i_target,
        const uint64_t i_dimm_count,
        const uint8_t i_odt_rd[P][R],
        const uint8_t i_odt_wr[P][R] );

///
/// @brief Setup odt_wr/rd_config
/// @param[in] i_target the MCA target
/// @param[in] i_dimm_count the number of DIMM presently on the target
/// @param[in] i_odt_rd the RD ODT values from VPD
/// @param[in] i_odt_wr the WR ODT values from VPD
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode reset_odt_config_helper<fapi2::TARGET_TYPE_MCA, MAX_DIMM_PER_PORT, MAX_RANK_PER_DIMM>(
    const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target,
    const uint64_t i_dimm_count,
    const uint8_t i_odt_rd[MAX_DIMM_PER_PORT][MAX_RANK_PER_DIMM],
    const uint8_t i_odt_wr[MAX_DIMM_PER_PORT][MAX_RANK_PER_DIMM]);

///
/// @brief Setup odt_wr/rd_config, reads attributes
/// @tparam T the target type of the MCA/MBA
/// @param[in] i_target the target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode reset_odt_config( const fapi2::Target<T>& i_target );

///
/// @brief Setup odt_wr/rd_config, reads attributes
/// @param[in] i_target the MCA target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode reset_odt_config( const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target );

///
/// @brief Perform the DLL calibration
/// @tparam T the target type of the MCBIST
/// @param[in] i_target the target associated with this DLL cal
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode dll_calibration( const fapi2::Target<T>& i_target );

///
/// @brief Perform the DLL calibration
/// @param[in] i_target the target associated with this DLL cal
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode dll_calibration( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Flush the output drivers
/// @tparam T the target type of the MCBIST
/// @param[in] i_target the target associated with the phy reset sequence
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode flush_output_drivers( const fapi2::Target<T>& i_target );

///
/// @brief Flush the output drivers
/// @param[in] i_target the target associated with the phy reset sequence
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode flush_output_drivers( const fapi2::Target<fapi2::TARGET_TYPE_MCBIST>& i_target );

///
/// @brief Setup seq_rd_wr_data
/// @tparam T the target type of the MCA/MBA
/// @param[in] i_target the target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template< fapi2::TargetType T >
fapi2::ReturnCode reset_seq_rd_wr_data( const fapi2::Target<T>& i_target );

///
/// @brief Setup seq_rd_wr_data
/// @param[in] i_target the MCA target associated with this cal setup
/// @return FAPI2_RC_SUCCESS iff setup was successful
///
template<>
fapi2::ReturnCode reset_seq_rd_wr_data( const fapi2::Target<fapi2::TARGET_TYPE_MCA>& i_target );

///
/// @brief Return the DIMM target for the primary rank in the specificed rank pair
/// @tparam T fapi2::TargetType of the port target
/// @param[in] i_target the target
/// @param[in] i_rp the rank pair
/// @param[out] fapi2::Target<fapi2::TARGET_TYPE_DIMM>
/// @return FAPI2_RC_SUCCESS iff ok
///
template<fapi2::TargetType T>
fapi2::ReturnCode rank_pair_primary_to_dimm(const fapi2::Target<T>& i_target, const uint64_t i_rp,
        fapi2::Target<fapi2::TARGET_TYPE_DIMM>& o_dimm);

///
/// @brief Sets up the IO impedances (ADR DRV's and DP DRV's/RCV's)
/// @tparam T the fapi2::TargetType
/// @param[in] i_target the target (MCA/MCBIST or MBA?)
/// @return FAPI2_RC_SUCCESS if and only if ok
///
template< fapi2::TargetType T>
fapi2::ReturnCode reset_io_impedances(const fapi2::Target<T>& i_target);

}
#endif
<|endoftext|>


<commit_before>/**************************************************************************
 * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

/*
$Log$
Revision 1.4  2000/07/13 16:19:44  fca
Mainly coding conventions + some small bug fixes

Revision 1.3  2000/07/03 11:54:57  morsch
AliMUONSegmentation and AliMUONHitMap have been replaced by AliSegmentation and AliHitMap in STEER
The methods GetPadIxy and GetPadXxy of AliMUONSegmentation have changed name to GetPadI and GetPadC.

Revision 1.2  2000/06/15 07:58:48  morsch
Code from MUON-dev joined

Revision 1.1.2.2  2000/06/12 07:58:06  morsch
include TMath.h

Revision 1.1.2.1  2000/06/09 22:01:09  morsch
Code from AliMUONHitMap.h
Most coding rule violations corrected.

*/

#include "AliMUONHitMapA1.h"
#include "AliSegmentation.h"
#include "AliMUONResponse.h"
#include "AliMUONDigit.h"

#include <TObjArray.h>
#include <TMath.h>

ClassImp(AliMUONHitMapA1)

AliMUONHitMapA1::AliMUONHitMapA1(AliSegmentation *seg, TObjArray *dig)
{
// Constructor
    fSegmentation = seg;
    fNpx  = fSegmentation->Npx();
    fNpy  = fSegmentation->Npy();
    fMaxIndex=2*(fNpx+1)*2*(fNpy+1)+2*fNpy;

    fHitMap = new Int_t[fMaxIndex];
    fDigits =  dig;
    fNdigits = fDigits->GetEntriesFast();
    Clear();
}

AliMUONHitMapA1::AliMUONHitMapA1(const AliMUONHitMapA1 & hitMap)
{
// Dummy copy constructor
    ;
}

AliMUONHitMapA1::~AliMUONHitMapA1()
{
// Destructor
//    if (fDigits) delete   fDigits;
    if (fHitMap) delete[] fHitMap;
}

void AliMUONHitMapA1::Clear(const char *)
{
// Clear hitmap
    memset(fHitMap,0,sizeof(int)*fMaxIndex);
}

Int_t AliMUONHitMapA1::CheckedIndex(Int_t ix, Int_t iy) const
{
// Return checked indices ix, iy
    Int_t index=2*fNpy*(ix+fNpx)+(iy+fNpy);
    if (index > fMaxIndex) {
	printf("\n \n \n Try to read/write outside array !!!! \n \n %d %d %d %d %d %d",ix,iy, fMaxIndex, index, fNpx, fNpy);
	return  fMaxIndex-1;
    } else {
	return index;
    }
}

void  AliMUONHitMapA1::FillHits()
{
// Fill hits from digits list
    Int_t ndigits = fDigits->GetEntriesFast();
    //printf("\n Filling hits into HitMap\n");
    //printf("FindRawClusters -- ndigits %d \n",ndigits);
    if (!ndigits) return;
    AliMUONDigit *dig;
    for (Int_t ndig=0; ndig<fNdigits; ndig++) {
	dig = (AliMUONDigit*)fDigits->UncheckedAt(ndig);
	SetHit(dig->fPadX,dig->fPadY,ndig);
    }
}

void  AliMUONHitMapA1::SetHit(Int_t ix, Int_t iy, Int_t idigit)
{
// Assign digit to hit cell ix,iy
//    fHitMap[kMaxNpady*(ix+fNpx)+(iy+fNpy)]=idigit+1;
    fHitMap[CheckedIndex(ix, iy)]=idigit+1;
}

void AliMUONHitMapA1::DeleteHit(Int_t ix, Int_t iy)
{
// Delete hit at cell ix,iy
//    fHitMap[kMaxNpady*(ix+fNpx)+(iy+fNpy)]=0;
    fHitMap[CheckedIndex(ix, iy)]=0;
}

void AliMUONHitMapA1::FlagHit(Int_t ix, Int_t iy)
{
// Flag hit as used
    fHitMap[CheckedIndex(ix, iy)]=
	-TMath::Abs(fHitMap[CheckedIndex(ix, iy)]);
}

Int_t AliMUONHitMapA1::GetHitIndex(Int_t ix, Int_t iy) const
{
// Get absolute value of contents of hit cell ix,iy
    return TMath::Abs(fHitMap[CheckedIndex(ix, iy)])-1;
}

TObject* AliMUONHitMapA1::GetHit(Int_t ix, Int_t iy) const
{
    // Get pointer to object at hit cell ix, iy
    // Force crash if index does not exist ! (Manu)
    Int_t index=GetHitIndex(ix,iy);
    return (index <0) ? 0 : fDigits->UncheckedAt(GetHitIndex(ix,iy));
}

FlagType AliMUONHitMapA1::TestHit(Int_t ix, Int_t iy)
{
// Check if hit cell is empty, used or unused
//
    Int_t inf=fHitMap[CheckedIndex(ix, iy)];
    if (inf < 0) {
	return kUsed;
    } else if (inf == 0) {
	return kEmpty;
    } else {
	return kUnused;
    }
}

AliMUONHitMapA1 & AliMUONHitMapA1::operator = (const AliMUONHitMapA1 & rhs)
{
// Dummy assignment operator
    return *this;
}

<commit_msg>Correction on upper limits for x- and y- coordinates to make code work with slat chambers.<commit_after>/**************************************************************************
 * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

/*
$Log$
Revision 1.5  2000/10/02 21:28:09  fca
Removal of useless dependecies via forward declarations

Revision 1.4  2000/07/13 16:19:44  fca
Mainly coding conventions + some small bug fixes

Revision 1.3  2000/07/03 11:54:57  morsch
AliMUONSegmentation and AliMUONHitMap have been replaced by AliSegmentation and AliHitMap in STEER
The methods GetPadIxy and GetPadXxy of AliMUONSegmentation have changed name to GetPadI and GetPadC.

Revision 1.2  2000/06/15 07:58:48  morsch
Code from MUON-dev joined

Revision 1.1.2.2  2000/06/12 07:58:06  morsch
include TMath.h

Revision 1.1.2.1  2000/06/09 22:01:09  morsch
Code from AliMUONHitMap.h
Most coding rule violations corrected.

*/

#include "AliMUONHitMapA1.h"
#include "AliSegmentation.h"
#include "AliMUONResponse.h"
#include "AliMUONDigit.h"

#include <TObjArray.h>
#include <TMath.h>

ClassImp(AliMUONHitMapA1)

AliMUONHitMapA1::AliMUONHitMapA1(AliSegmentation *seg, TObjArray *dig)
{
// Constructor
    fSegmentation = seg;
    fNpx  = fSegmentation->Npx()+1;
    fNpy  = fSegmentation->Npy()+1;
    fMaxIndex=2*(fNpx+1)*2*(fNpy+1)+2*fNpy;

    fHitMap = new Int_t[fMaxIndex];
    fDigits =  dig;
    fNdigits = fDigits->GetEntriesFast();
    Clear();
}

AliMUONHitMapA1::AliMUONHitMapA1(const AliMUONHitMapA1 & hitMap)
{
// Dummy copy constructor
    ;
}

AliMUONHitMapA1::~AliMUONHitMapA1()
{
// Destructor
//    if (fDigits) delete   fDigits;
    if (fHitMap) delete[] fHitMap;
}

void AliMUONHitMapA1::Clear(const char *)
{
// Clear hitmap
    memset(fHitMap,0,sizeof(int)*fMaxIndex);
}

Int_t AliMUONHitMapA1::CheckedIndex(Int_t ix, Int_t iy) const
{
// Return checked indices ix, iy
    Int_t index=2*fNpy*(ix+fNpx)+(iy+fNpy);
    if (index > fMaxIndex) {
	printf("\n \n \n Try to read/write outside array !!!! \n \n %d %d %d %d %d %d",
	       ix,iy, fMaxIndex, index, fNpx, fNpy);
	return  fMaxIndex-1;
    } else {
	return index;
    }
}

void  AliMUONHitMapA1::FillHits()
{
// Fill hits from digits list
    Int_t ndigits = fDigits->GetEntriesFast();
    //printf("\n Filling hits into HitMap\n");
    //printf("FindRawClusters -- ndigits %d \n",ndigits);
    if (!ndigits) return;
    AliMUONDigit *dig;
    for (Int_t ndig=0; ndig<fNdigits; ndig++) {
	dig = (AliMUONDigit*)fDigits->UncheckedAt(ndig);
	SetHit(dig->fPadX,dig->fPadY,ndig);
    }
}

void  AliMUONHitMapA1::SetHit(Int_t ix, Int_t iy, Int_t idigit)
{
// Assign digit to hit cell ix,iy
//    fHitMap[kMaxNpady*(ix+fNpx)+(iy+fNpy)]=idigit+1;
    fHitMap[CheckedIndex(ix, iy)]=idigit+1;
}

void AliMUONHitMapA1::DeleteHit(Int_t ix, Int_t iy)
{
// Delete hit at cell ix,iy
//    fHitMap[kMaxNpady*(ix+fNpx)+(iy+fNpy)]=0;
    fHitMap[CheckedIndex(ix, iy)]=0;
}

void AliMUONHitMapA1::FlagHit(Int_t ix, Int_t iy)
{
// Flag hit as used
    fHitMap[CheckedIndex(ix, iy)]=
	-TMath::Abs(fHitMap[CheckedIndex(ix, iy)]);
}

Int_t AliMUONHitMapA1::GetHitIndex(Int_t ix, Int_t iy) const
{
// Get absolute value of contents of hit cell ix,iy
    return TMath::Abs(fHitMap[CheckedIndex(ix, iy)])-1;
}

TObject* AliMUONHitMapA1::GetHit(Int_t ix, Int_t iy) const
{
    // Get pointer to object at hit cell ix, iy
    // Force crash if index does not exist ! (Manu)
    Int_t index=GetHitIndex(ix,iy);
    return (index <0) ? 0 : fDigits->UncheckedAt(GetHitIndex(ix,iy));
}

FlagType AliMUONHitMapA1::TestHit(Int_t ix, Int_t iy)
{
// Check if hit cell is empty, used or unused
//
    Int_t inf=fHitMap[CheckedIndex(ix, iy)];
    if (inf < 0) {
	return kUsed;
    } else if (inf == 0) {
	return kEmpty;
    } else {
	return kUnused;
    }
}

AliMUONHitMapA1 & AliMUONHitMapA1::operator = (const AliMUONHitMapA1 & rhs)
{
// Dummy assignment operator
    return *this;
}

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "camera_display.h"
#include "rviz/visualization_manager.h"
#include "rviz/render_panel.h"
#include "rviz/properties/property.h"
#include "rviz/properties/property_manager.h"
#include "rviz/common.h"
#include "rviz/window_manager_interface.h"

#include <tf/transform_listener.h>

#include <boost/bind.hpp>

#include <ogre_tools/axes.h>

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreRectangle2D.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreTextureManager.h>
#include <OGRE/OgreViewport.h>
#include <OGRE/OgreRenderWindow.h>
#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreRoot.h>
#include <OGRE/OgreRenderSystem.h>

#include <wx/frame.h>

namespace rviz
{

CameraDisplay::RenderListener::RenderListener(CameraDisplay* display)
: display_(display)
{

}

void CameraDisplay::RenderListener::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
{
  Ogre::Pass* pass = display_->material_->getTechnique(0)->getPass(0);

  if (pass->getNumTextureUnitStates() > 0)
  {
    Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState(0);
    tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, display_->alpha_ );
  }
  else
  {
    display_->material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, display_->alpha_));
    display_->material_->setDiffuse(Ogre::ColourValue(0.0f, 1.0f, 1.0f, display_->alpha_));
  }

  display_->updateCamera();
}

void CameraDisplay::RenderListener::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
{
  Ogre::Pass* pass = display_->material_->getTechnique(0)->getPass(0);

  if (pass->getNumTextureUnitStates() > 0)
  {
    Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState(0);
    tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0f );
  }
  else
  {
    display_->material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, 0.0f));
    display_->material_->setDiffuse(Ogre::ColourValue(0.0f, 1.0f, 1.0f, 0.0f));
  }
}

CameraDisplay::CameraDisplay( const std::string& name, VisualizationManager* manager )
: Display( name, manager )
, caminfo_tf_filter_(*manager->getTFClient(), "", 2, update_nh_)
, new_caminfo_(false)
, texture_(update_nh_)
, frame_(0)
, render_listener_(this)
{
  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();

  {
    static int count = 0;
    std::stringstream ss;
    ss << "CameraDisplayObject" << count++;

    screen_rect_ = new Ogre::Rectangle2D(true);
    screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
    screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);

    ss << "Material";
    material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
    //material_->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME);
    material_->getTechnique(0)->getPass(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
    material_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);

    material_->setReceiveShadows(false);
    material_->setDepthCheckEnabled(false);

#if 1
    material_->getTechnique(0)->setLightingEnabled(false);
    Ogre::TextureUnitState* tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState();
    tu->setTextureName(texture_.getTexture()->getName());
    tu->setTextureFiltering( Ogre::TFO_NONE );
    tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
#else
    material_->getTechnique(0)->setLightingEnabled(true);
    material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f));
#endif

    material_->setCullingMode(Ogre::CULL_NONE);
    Ogre::AxisAlignedBox aabInf;
    aabInf.setInfinite();
    screen_rect_->setBoundingBox(aabInf);
    screen_rect_->setMaterial(material_->getName());
    scene_node_->attachObject(screen_rect_);

  }

  setAlpha( 0.5f );

  wxWindow* parent = 0;

  WindowManagerInterface* wm = vis_manager_->getWindowManager();
  if (wm)
  {
    parent = wm->getParentWindow();
  }
  else
  {
    frame_ = new wxFrame(0, wxID_ANY, wxString::FromAscii(name.c_str()), wxDefaultPosition, wxDefaultSize, wxMINIMIZE_BOX | wxMAXIMIZE_BOX | wxRESIZE_BORDER | wxCAPTION | wxCLIP_CHILDREN);
    parent = frame_;
  }

  render_panel_ = new RenderPanel(parent, false);
  render_panel_->SetSize(wxSize(640, 480));
  if (wm)
  {
    wm->addPane(name, render_panel_);
  }

  render_panel_->createRenderWindow();

  render_panel_->setAutoRender(false);
  render_panel_->getRenderWindow()->addListener(&render_listener_);
  render_panel_->getViewport()->setOverlaysEnabled(false);
  render_panel_->getViewport()->setClearEveryFrame(true);
  render_panel_->getRenderWindow()->setActive(false);

  {
    std::stringstream ss;
    static uint32_t count = 0;
    ss << "CameraDisplayCamera" << count++;
    camera_ = scene_manager_->createCamera( ss.str() );
    render_panel_->setCamera(camera_);

    camera_->setPosition(Ogre::Vector3(-5, 5, 5));
    camera_->lookAt(Ogre::Vector3(0, 0, 0));
    camera_->setNearClipDistance( 0.1f );
  }

  caminfo_tf_filter_.connectInput(caminfo_sub_);
  caminfo_tf_filter_.registerCallback(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
}

CameraDisplay::~CameraDisplay()
{
  unsubscribe();
  caminfo_tf_filter_.clear();

  if (frame_)
  {
    frame_->Destroy();
  }
  else
  {
    WindowManagerInterface* wm = vis_manager_->getWindowManager();
    wm->removePane(render_panel_);
    render_panel_->Destroy();
  }

  scene_manager_->destroyCamera(camera_);
  delete screen_rect_;

  scene_node_->getParentSceneNode()->removeAndDestroyChild(scene_node_->getName());
}

void CameraDisplay::onEnable()
{
  subscribe();

  if (frame_)
  {
    frame_->Show(true);
  }
  else
  {
    WindowManagerInterface* wm = vis_manager_->getWindowManager();
    wm->showPane(render_panel_);
  }

  render_panel_->getRenderWindow()->setActive(true);
}

void CameraDisplay::onDisable()
{
  render_panel_->getRenderWindow()->setActive(false);

  if (frame_)
  {
    frame_->Show(false);
  }
  else
  {
    WindowManagerInterface* wm = vis_manager_->getWindowManager();
    wm->closePane(render_panel_);
  }

  unsubscribe();

  clear();
}

void CameraDisplay::subscribe()
{
  if ( !isEnabled() )
  {
    return;
  }

  texture_.setTopic(topic_);

  // parse out the namespace from the topic so we can subscribe to the caminfo
  std::string caminfo_topic = "cam_info";
  size_t pos = topic_.rfind('/');
  if (pos != std::string::npos)
  {
    std::string ns = topic_;
    ns.erase(pos);

    caminfo_topic = ns + "/" + caminfo_topic;
  }

  caminfo_sub_.subscribe(update_nh_, caminfo_topic, 1);
}

void CameraDisplay::unsubscribe()
{
  texture_.setTopic("");
  caminfo_sub_.unsubscribe();
}

void CameraDisplay::setAlpha( float alpha )
{
  alpha_ = alpha;

  propertyChanged(alpha_property_);

  causeRender();
}

void CameraDisplay::setTopic( const std::string& topic )
{
  unsubscribe();

  topic_ = topic;
  clear();

  subscribe();

  propertyChanged(topic_property_);
}

void CameraDisplay::clear()
{
  texture_.clear();

  new_caminfo_ = false;
  current_caminfo_.reset();
}

void CameraDisplay::update(float wall_dt, float ros_dt)
{
  texture_.update();
}

void CameraDisplay::updateCamera()
{
  sensor_msgs::CameraInfo::ConstPtr info;
  {
    boost::mutex::scoped_lock lock(caminfo_mutex_);

    info = current_caminfo_;
  }

  if (!info)
  {
    return;
  }

  tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, 0.0f ) ),
                              ros::Time(), info->header.frame_id );
  try
  {
    vis_manager_->getTFClient()->transformPose(fixed_frame_, pose, pose);
  }
  catch (tf::TransformException& e)
  {

  }

  Ogre::Vector3 position = Ogre::Vector3( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
  robotToOgre(position);

  btQuaternion quat;
  pose.getBasis().getRotation( quat );
  Ogre::Quaternion orientation(quat.w(), quat.x(), quat.y(), quat.z());
  robotToOgre(orientation);

  // convert vision (Z-forward) frame to ogre frame (Z-out)
  orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);

  float width = info->width;
  float height = info->height;

  // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
  if (info->width == 0)
  {
    ROS_DEBUG("Malformed CameraInfo on camera [%s], width = 0", getName().c_str());

    width = texture_.getWidth();
  }

  if (info->height == 0)
  {
    ROS_DEBUG("Malformed CameraInfo on camera [%s], height = 0", getName().c_str());

    height = texture_.getHeight();
  }

  if (height == 0.0 || width == 0.0)
  {
    ROS_ERROR("Could not determine width/height of image, due to malformed CameraInfo");
    return;
  }

  double fx = info->P[0];
  double fy = info->P[5];
  double fovy = 2*atan(height / (2 * fy));
  double aspect_ratio = width / height;
  camera_->setFOVy(Ogre::Radian(fovy));
  camera_->setAspectRatio(aspect_ratio);

  // Add the camera's translation relative to the left camera (from P[3]);
  // Tx = -1*(P[3] / P[0])
  double tx = -1 * (info->P[3] / fx);
  Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
  position = position + (right * tx);

  camera_->setPosition(position);
  camera_->setOrientation(orientation);

  double cx = info->P[2];
  double cy = info->P[6];
  double normalized_cx = cx / width;
  double normalized_cy = cy / height;
  double dx = 2*(0.5 - normalized_cx);
  double dy = 2*(normalized_cy - 0.5);
  screen_rect_->setCorners(-1.0f + dx, 1.0f + dy, 1.0f + dx, -1.0f + dy);

#if 0
  static ogre_tools::Axes* debug_axes = new ogre_tools::Axes(scene_manager_, 0, 0.2, 0.02);
  debug_axes->setPosition(position);
  debug_axes->setOrientation(orientation);
#endif
}

void CameraDisplay::caminfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
{
  boost::mutex::scoped_lock lock(caminfo_mutex_);
  current_caminfo_ = msg;
  new_caminfo_ = true;
}

void CameraDisplay::createProperties()
{
  topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Image Topic", property_prefix_, boost::bind( &CameraDisplay::getTopic, this ),
                                                                         boost::bind( &CameraDisplay::setTopic, this, _1 ), parent_category_, this );
  ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
  topic_prop->setMessageType(sensor_msgs::Image::__s_getDataType());

  alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &CameraDisplay::getAlpha, this ),
                                                                      boost::bind( &CameraDisplay::setAlpha, this, _1 ), parent_category_, this );

}

void CameraDisplay::fixedFrameChanged()
{
  caminfo_tf_filter_.setTargetFrame(fixed_frame_);
}

void CameraDisplay::targetFrameChanged()
{

}

void CameraDisplay::reset()
{
  clear();
}

} // namespace rviz
<commit_msg>cam_info -> camera_info<commit_after>/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "camera_display.h"
#include "rviz/visualization_manager.h"
#include "rviz/render_panel.h"
#include "rviz/properties/property.h"
#include "rviz/properties/property_manager.h"
#include "rviz/common.h"
#include "rviz/window_manager_interface.h"

#include <tf/transform_listener.h>

#include <boost/bind.hpp>

#include <ogre_tools/axes.h>

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreRectangle2D.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreTextureManager.h>
#include <OGRE/OgreViewport.h>
#include <OGRE/OgreRenderWindow.h>
#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreRoot.h>
#include <OGRE/OgreRenderSystem.h>

#include <wx/frame.h>

namespace rviz
{

CameraDisplay::RenderListener::RenderListener(CameraDisplay* display)
: display_(display)
{

}

void CameraDisplay::RenderListener::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
{
  Ogre::Pass* pass = display_->material_->getTechnique(0)->getPass(0);

  if (pass->getNumTextureUnitStates() > 0)
  {
    Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState(0);
    tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, display_->alpha_ );
  }
  else
  {
    display_->material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, display_->alpha_));
    display_->material_->setDiffuse(Ogre::ColourValue(0.0f, 1.0f, 1.0f, display_->alpha_));
  }

  display_->updateCamera();
}

void CameraDisplay::RenderListener::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
{
  Ogre::Pass* pass = display_->material_->getTechnique(0)->getPass(0);

  if (pass->getNumTextureUnitStates() > 0)
  {
    Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState(0);
    tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0f );
  }
  else
  {
    display_->material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, 0.0f));
    display_->material_->setDiffuse(Ogre::ColourValue(0.0f, 1.0f, 1.0f, 0.0f));
  }
}

CameraDisplay::CameraDisplay( const std::string& name, VisualizationManager* manager )
: Display( name, manager )
, caminfo_tf_filter_(*manager->getTFClient(), "", 2, update_nh_)
, new_caminfo_(false)
, texture_(update_nh_)
, frame_(0)
, render_listener_(this)
{
  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();

  {
    static int count = 0;
    std::stringstream ss;
    ss << "CameraDisplayObject" << count++;

    screen_rect_ = new Ogre::Rectangle2D(true);
    screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
    screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);

    ss << "Material";
    material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
    //material_->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME);
    material_->getTechnique(0)->getPass(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
    material_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);

    material_->setReceiveShadows(false);
    material_->setDepthCheckEnabled(false);

#if 1
    material_->getTechnique(0)->setLightingEnabled(false);
    Ogre::TextureUnitState* tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState();
    tu->setTextureName(texture_.getTexture()->getName());
    tu->setTextureFiltering( Ogre::TFO_NONE );
    tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
#else
    material_->getTechnique(0)->setLightingEnabled(true);
    material_->setAmbient(Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f));
#endif

    material_->setCullingMode(Ogre::CULL_NONE);
    Ogre::AxisAlignedBox aabInf;
    aabInf.setInfinite();
    screen_rect_->setBoundingBox(aabInf);
    screen_rect_->setMaterial(material_->getName());
    scene_node_->attachObject(screen_rect_);

  }

  setAlpha( 0.5f );

  wxWindow* parent = 0;

  WindowManagerInterface* wm = vis_manager_->getWindowManager();
  if (wm)
  {
    parent = wm->getParentWindow();
  }
  else
  {
    frame_ = new wxFrame(0, wxID_ANY, wxString::FromAscii(name.c_str()), wxDefaultPosition, wxDefaultSize, wxMINIMIZE_BOX | wxMAXIMIZE_BOX | wxRESIZE_BORDER | wxCAPTION | wxCLIP_CHILDREN);
    parent = frame_;
  }

  render_panel_ = new RenderPanel(parent, false);
  render_panel_->SetSize(wxSize(640, 480));
  if (wm)
  {
    wm->addPane(name, render_panel_);
  }

  render_panel_->createRenderWindow();

  render_panel_->setAutoRender(false);
  render_panel_->getRenderWindow()->addListener(&render_listener_);
  render_panel_->getViewport()->setOverlaysEnabled(false);
  render_panel_->getViewport()->setClearEveryFrame(true);
  render_panel_->getRenderWindow()->setActive(false);

  {
    std::stringstream ss;
    static uint32_t count = 0;
    ss << "CameraDisplayCamera" << count++;
    camera_ = scene_manager_->createCamera( ss.str() );
    render_panel_->setCamera(camera_);

    camera_->setPosition(Ogre::Vector3(-5, 5, 5));
    camera_->lookAt(Ogre::Vector3(0, 0, 0));
    camera_->setNearClipDistance( 0.1f );
  }

  caminfo_tf_filter_.connectInput(caminfo_sub_);
  caminfo_tf_filter_.registerCallback(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
}

CameraDisplay::~CameraDisplay()
{
  unsubscribe();
  caminfo_tf_filter_.clear();

  if (frame_)
  {
    frame_->Destroy();
  }
  else
  {
    WindowManagerInterface* wm = vis_manager_->getWindowManager();
    wm->removePane(render_panel_);
    render_panel_->Destroy();
  }

  scene_manager_->destroyCamera(camera_);
  delete screen_rect_;

  scene_node_->getParentSceneNode()->removeAndDestroyChild(scene_node_->getName());
}

void CameraDisplay::onEnable()
{
  subscribe();

  if (frame_)
  {
    frame_->Show(true);
  }
  else
  {
    WindowManagerInterface* wm = vis_manager_->getWindowManager();
    wm->showPane(render_panel_);
  }

  render_panel_->getRenderWindow()->setActive(true);
}

void CameraDisplay::onDisable()
{
  render_panel_->getRenderWindow()->setActive(false);

  if (frame_)
  {
    frame_->Show(false);
  }
  else
  {
    WindowManagerInterface* wm = vis_manager_->getWindowManager();
    wm->closePane(render_panel_);
  }

  unsubscribe();

  clear();
}

void CameraDisplay::subscribe()
{
  if ( !isEnabled() )
  {
    return;
  }

  texture_.setTopic(topic_);

  // parse out the namespace from the topic so we can subscribe to the caminfo
  std::string caminfo_topic = "camera_info";
  size_t pos = topic_.rfind('/');
  if (pos != std::string::npos)
  {
    std::string ns = topic_;
    ns.erase(pos);

    caminfo_topic = ns + "/" + caminfo_topic;
  }

  caminfo_sub_.subscribe(update_nh_, caminfo_topic, 1);
}

void CameraDisplay::unsubscribe()
{
  texture_.setTopic("");
  caminfo_sub_.unsubscribe();
}

void CameraDisplay::setAlpha( float alpha )
{
  alpha_ = alpha;

  propertyChanged(alpha_property_);

  causeRender();
}

void CameraDisplay::setTopic( const std::string& topic )
{
  unsubscribe();

  topic_ = topic;
  clear();

  subscribe();

  propertyChanged(topic_property_);
}

void CameraDisplay::clear()
{
  texture_.clear();

  new_caminfo_ = false;
  current_caminfo_.reset();
}

void CameraDisplay::update(float wall_dt, float ros_dt)
{
  texture_.update();
}

void CameraDisplay::updateCamera()
{
  sensor_msgs::CameraInfo::ConstPtr info;
  {
    boost::mutex::scoped_lock lock(caminfo_mutex_);

    info = current_caminfo_;
  }

  if (!info)
  {
    return;
  }

  tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, 0.0f ) ),
                              ros::Time(), info->header.frame_id );
  try
  {
    vis_manager_->getTFClient()->transformPose(fixed_frame_, pose, pose);
  }
  catch (tf::TransformException& e)
  {

  }

  Ogre::Vector3 position = Ogre::Vector3( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
  robotToOgre(position);

  btQuaternion quat;
  pose.getBasis().getRotation( quat );
  Ogre::Quaternion orientation(quat.w(), quat.x(), quat.y(), quat.z());
  robotToOgre(orientation);

  // convert vision (Z-forward) frame to ogre frame (Z-out)
  orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);

  float width = info->width;
  float height = info->height;

  // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
  if (info->width == 0)
  {
    ROS_DEBUG("Malformed CameraInfo on camera [%s], width = 0", getName().c_str());

    width = texture_.getWidth();
  }

  if (info->height == 0)
  {
    ROS_DEBUG("Malformed CameraInfo on camera [%s], height = 0", getName().c_str());

    height = texture_.getHeight();
  }

  if (height == 0.0 || width == 0.0)
  {
    ROS_ERROR("Could not determine width/height of image, due to malformed CameraInfo");
    return;
  }

  double fx = info->P[0];
  double fy = info->P[5];
  double fovy = 2*atan(height / (2 * fy));
  double aspect_ratio = width / height;
  camera_->setFOVy(Ogre::Radian(fovy));
  camera_->setAspectRatio(aspect_ratio);

  // Add the camera's translation relative to the left camera (from P[3]);
  // Tx = -1*(P[3] / P[0])
  double tx = -1 * (info->P[3] / fx);
  Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
  position = position + (right * tx);

  camera_->setPosition(position);
  camera_->setOrientation(orientation);

  double cx = info->P[2];
  double cy = info->P[6];
  double normalized_cx = cx / width;
  double normalized_cy = cy / height;
  double dx = 2*(0.5 - normalized_cx);
  double dy = 2*(normalized_cy - 0.5);
  screen_rect_->setCorners(-1.0f + dx, 1.0f + dy, 1.0f + dx, -1.0f + dy);

#if 0
  static ogre_tools::Axes* debug_axes = new ogre_tools::Axes(scene_manager_, 0, 0.2, 0.02);
  debug_axes->setPosition(position);
  debug_axes->setOrientation(orientation);
#endif
}

void CameraDisplay::caminfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
{
  boost::mutex::scoped_lock lock(caminfo_mutex_);
  current_caminfo_ = msg;
  new_caminfo_ = true;
}

void CameraDisplay::createProperties()
{
  topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Image Topic", property_prefix_, boost::bind( &CameraDisplay::getTopic, this ),
                                                                         boost::bind( &CameraDisplay::setTopic, this, _1 ), parent_category_, this );
  ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
  topic_prop->setMessageType(sensor_msgs::Image::__s_getDataType());

  alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &CameraDisplay::getAlpha, this ),
                                                                      boost::bind( &CameraDisplay::setAlpha, this, _1 ), parent_category_, this );

}

void CameraDisplay::fixedFrameChanged()
{
  caminfo_tf_filter_.setTargetFrame(fixed_frame_);
}

void CameraDisplay::targetFrameChanged()
{

}

void CameraDisplay::reset()
{
  clear();
}

} // namespace rviz
<|endoftext|>