<commit_before>#include <cstdint>
#include <cstring>
#include "antenna/driver.h"
#include "antenna_state.h"
#include "antenna_task.hpp"
#include "gsl/gsl_util"
#include "logger/logger.h"
#include "mission/base.hpp"
#include "mission/obc.hpp"
#include "system.h"
#include "time/TimePoint.h"

using namespace std::chrono_literals;

namespace mission
{
    namespace antenna
    {
        /**
         * @addtogroup mission_atenna
         * @{
         */
        /**
         * @brief Type definition of the specific deployment step handler.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to deployment process state.
         * @param[in] driver Reference to current antenna driver instance
         */
        typedef void DeploymentProcedure(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief This deployment step performs regular deployment operation.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void RegularDeploymentStep(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief This deployment step resets current hardware channel.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void ResetDriverStep(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief This deployment step finalizes antenna deployment process.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void FinishDeploymentStep(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief Deployment step retry limit.
         *
         * This value controls how many times each deployment step will be repeated
         * in case of errors before process can advance further.
         */
        static constexpr std::uint8_t StepRetryLimit = 3;

        /**
         * @brief Hardware operation retry limit.
         *
         * This value controls how many times each hardware operation will be issued
         * in case of errors before declaring it as a failure.
         */
        static constexpr std::uint8_t RetryLimit = 3;

        /**
         * @brief Class that describes single deployment step.
         *
         * The entire process is driven by the array of steps, each described by the object of this type.
         */
        struct AntennaDeploymentStep final
        {
            /**
             * @brief Pointer to the procedure the performs current step.
             */
            DeploymentProcedure* procedure;

            /**
             * @brief Hardware channel that should be used to perform this step.
             */
            AntennaChannel channel;

            /**
             * @brief Identifier of the antenna that should be used/affected by this step.
             */
            AntennaId antennaId;

            /**
             * @brief Antenna deployment process timeout in seconds.
             */
            uint8_t deploymentTimeout;

            /**
             * @brief Flag indicating whether hardware deployment switches should be overriden.
             */
            bool overrideSwitches;
        };

        /**
         * @brief Default timeout value that should be enough in case there are no problems.
         */
        constexpr std::uint8_t DefaultTimeout = 9;

        /**
         * @brief Extended timeout that should take care of the most of the problems.
         */
        constexpr std::uint8_t MediumTimeout = 19;

        /**
         * @brief Long timeouts for heating problems.
         */
        constexpr std::uint8_t LongTimeout = 39;

        /**
         * @brief Last resort timeout in case everything else fails.
         */
        constexpr std::uint8_t EmergencyTimeout = 59;

        /**
         * @brief Array of antenna deployment steps.
         *
         * The entire process is composed of steps that are run in sequence from the beginning. Steps themselves are
         * grouped into series that together form logical variant of the antenna deployment process.
         *
         * Following is the list of the deployment stages that are defined in this array:
         * - Automatic deployment (primary hardware channel)
         * - Manual deployment with overridden deployment switches (primary hardware channel)
         * - Deployment finalization (primary hardware channel)
         *
         * - Automatic deployment (backup hardware channel)
         * - Manual deployment with overridden deployment switches (backup hardware channel)
         * - Deployment finalization (backup hardware channel)
         */
        static const AntennaDeploymentStep deploymentSteps[] = {
            {ResetDriverStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA_AUTO_ID, 0, false},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA_AUTO_ID, DefaultTimeout, false},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA1_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA2_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA3_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA4_ID, LongTimeout, true},
            {FinishDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA_AUTO_ID, 0, false},

            {ResetDriverStep, ANTENNA_BACKUP_CHANNEL, ANTENNA_AUTO_ID, 0, false},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA_AUTO_ID, DefaultTimeout, false},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA1_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA2_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA3_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA4_ID, LongTimeout, true},
            {FinishDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA_AUTO_ID, 0, false},
        };

        /**
         * @brief Number of steps in the antenna deployment process.
         */
        static constexpr uint8_t DeploymentStepLimit = count_of(deploymentSteps);

        AntennaMissionState::AntennaMissionState(AntennaDriver& antennaDriver)
            : _overrideState(false), //
              _inProgress(false),    //
              _stepNumber(0),        //
              _retryCount(0),        //
              _driver(antennaDriver)
        {
        }

        void AntennaMissionState::Retry(std::uint8_t limit)
        {
            if ((this->_retryCount + 1) == limit)
            {
                NextStep();
            }
            else
            {
                ++this->_retryCount;
            }
        }

        bool AntennaMissionState::IsFinished() const
        {
            return this->_stepNumber >= DeploymentStepLimit;
        }

        void AntennaMissionState::Update(const AntennaDeploymentStatus& status)
        {
            this->_inProgress = status.IsDeploymentActive[0] | //
                status.IsDeploymentActive[1] |                 //
                status.IsDeploymentActive[2] |                 //
                status.IsDeploymentActive[3];
        }

        std::uint8_t AntennaMissionState::StepCount()
        {
            return DeploymentStepLimit;
        }

        /**
         * @brief This procedure is supposed to stop any deployment process that may currently be active on the
         * passed hardware channel.
         *
         * @param[in] driver Reference to current antenna driver instance.
         * @param[in] channel Hardware channel that should be used for current operation.
         * @param[in] retryCount Number of step retry attempts in case of errors.
         * @return True if operation has been successfully completed, false otherwise.
         */
        static bool EndDeployment(AntennaDriver& driver, AntennaChannel channel, std::uint8_t retryCount)
        {
            while (retryCount-- > 0)
            {
                const OSResult status = driver.FinishDeployment(&driver, channel);
                if (OS_RESULT_SUCCEEDED(status))
                {
                    return true;
                }
            }

            return false;
        }

        /**
         * @brief This procedure is supposed to stop any deployment process that may currently be active on the
         * hardware channel used in the previous step.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void StopDeployment(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            if (stateDescriptor.StepNumber() == 0)
            {
                return;
            }

            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber() - 1];
            EndDeployment(driver, step.channel, RetryLimit);
        }

        /**
         * @brief This procedure is supposed to start deployment process described by the current step.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void BeginDeployment(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];
            std::uint8_t counter = RetryLimit;
            while (counter-- > 0)
            {
                const OSResult result = driver.DeployAntenna(&driver,
                    step.channel,
                    step.antennaId,
                    std::chrono::seconds(step.deploymentTimeout),
                    step.overrideSwitches //
                    );

                if (OS_RESULT_SUCCEEDED(result))
                {
                    stateDescriptor.NextStep();
                    return;
                }
            }

            stateDescriptor.Retry(StepRetryLimit);
        }

        void RegularDeploymentStep(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];

            LOGF(LOG_LEVEL_INFO,
                "[ant] [Step%d] Regular deployment (channel %d, antenna %d)",
                stateDescriptor.StepNumber(),
                step.channel,
                step.antennaId);

            StopDeployment(state, stateDescriptor, driver);
            BeginDeployment(state, stateDescriptor, driver);
        }

        void ResetDriverStep(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];

            LOGF(LOG_LEVEL_INFO, "[ant] [Step%d] Reseting driver %d", stateDescriptor.StepNumber(), step.channel);

            std::uint8_t counter = RetryLimit;
            while (counter-- > 0)
            {
                const OSResult result = driver.Reset(&driver, step.channel);
                if (OS_RESULT_SUCCEEDED(result))
                {
                    stateDescriptor.NextStep();
                    return;
                }
            }

            stateDescriptor.Retry(StepRetryLimit);
        }

        void FinishDeploymentStep(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];

            LOGF(LOG_LEVEL_INFO, "[ant] [Step%d] Finish deployment %d", stateDescriptor.StepNumber(), step.channel);

            if (EndDeployment(driver, step.channel, RetryLimit))
            {
                stateDescriptor.NextStep();
            }
            else
            {
                stateDescriptor.Retry(StepRetryLimit);
            }
        }

        /**
         * @brief This procedure is deployment action entry point.
         *
         * This procedure runs the antenna deployment process.
         * @param[in] state Pointer to global satellite state.
         * @param[in] param Pointer to the deployment condition private context. This pointer should point
         * at the object of AntennaMissionState type.
         */
        static void AntennaDeploymentAction(SystemState& state, void* param)
        {
            auto stateDescriptor = static_cast<AntennaMissionState*>(param);

            LOGF(LOG_LEVEL_DEBUG, "[ant] Executing antenna deployment step %d", stateDescriptor->StepNumber());

            if (state.PersistentState.Get<state::AntennaConfiguration>().IsDeploymentDisabled())
            {
                LOG(LOG_LEVEL_DEBUG, "[ant] Antenna deployment disabled");
                stateDescriptor->Finish();
            }
            else
            {
                const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor->StepNumber()];
                DeploymentProcedure* procedure = step.procedure;
                procedure(state, *stateDescriptor, stateDescriptor->Driver());
            }
        }

        /**
         * @brief Procedure that verifies whether the antenna deployment process should be executed.
         * @param[in] state Pointer to global satellite state.
         * @param[in] param Pointer to the deployment condition private context. This pointer should point
         * at the object of AntennaMissionState type.
         *
         * @return True if the deployment action should be performed, false otherwise.
         */
        static bool AntennaDeploymentCondition(const SystemState& state, void* param)
        {
            AntennaMissionState* stateDescriptor = (AntennaMissionState*)param;
            if (!IsInitialSilentPeriodFinished(state.Time))
            {
                return false;
            }

            if (stateDescriptor->IsDeploymentInProgress())
            {
                return false;
            }

            return !stateDescriptor->IsFinished();
        }

        /**
         * @brief This procedure is antenna deployment update descriptor entry point.
         *
         * This procedure updates the global satellite state as well as deployemnt process private state.
         * @param[in] state Pointer to global satellite state.
         * @param[in] param Pointer to the deployment condition private context. This pointer should point
         * at the object of AntennaMissionState type.
         * @return Operation status.
         */
        static UpdateResult AntennaDeploymentUpdate(SystemState& state, void* param)
        {
            UNREFERENCED_PARAMETER(state);
            AntennaMissionState* stateDescriptor = (AntennaMissionState*)param;
            state.AntennaState.SetDeployment(stateDescriptor->IsFinished());
            if (stateDescriptor->IsFinished())
            {
                return UpdateResult::Ok;
            }

            AntennaDeploymentStatus deploymentStatus;
            AntennaDriver& driver = stateDescriptor->Driver();
            const OSResult result = driver.GetDeploymentStatus(&driver,
                deploymentSteps[stateDescriptor->StepNumber()].channel,
                &deploymentStatus //
                );

            if (OS_RESULT_FAILED(result))
            {
                return UpdateResult::Failure;
            }

            stateDescriptor->Update(deploymentStatus);
            return UpdateResult::Ok;
        }

        AntennaTask::AntennaTask(AntennaDriver& driver) : state(driver)
        {
        }

        ActionDescriptor<SystemState> AntennaTask::BuildAction()
        {
            ActionDescriptor<SystemState> descriptor;
            descriptor.name = "Deploy Antenna Action";
            descriptor.param = &this->state;
            descriptor.condition = AntennaDeploymentCondition;
            descriptor.actionProc = AntennaDeploymentAction;
            return descriptor;
        }

        UpdateDescriptor<SystemState> AntennaTask::BuildUpdate()
        {
            UpdateDescriptor<SystemState> descriptor;
            descriptor.name = "Deploy Antenna Update";
            descriptor.param = &this->state;
            descriptor.updateProc = AntennaDeploymentUpdate;
            return descriptor;
        }

        StopAntennaDeploymentTask::StopAntennaDeploymentTask(std::uint8_t /*mark*/)
        {
        }
        mission::ActionDescriptor<SystemState> StopAntennaDeploymentTask::BuildAction()
        {
            mission::ActionDescriptor<SystemState> action;
            action.name = "Stop antenna deployment";
            action.param = this;
            action.condition = Condition;
            action.actionProc = Action;
            return action;
        }

        void StopAntennaDeploymentTask::DisableDeployment()
        {
            this->_shouldDisable = true;
        }

        bool StopAntennaDeploymentTask::Condition(const SystemState& state, void* param)
        {
            auto This = reinterpret_cast<StopAntennaDeploymentTask*>(param);
            auto alreadyDisabled = state.PersistentState.Get<state::AntennaConfiguration>().IsDeploymentDisabled();

            return This->_shouldDisable && !alreadyDisabled && state.AntennaState.IsDeployed();
        }

        void StopAntennaDeploymentTask::Action(SystemState& state, void*)
        {
            LOG(LOG_LEVEL_INFO, "[ant] Disabling antenna deployment");

            state.PersistentState.Set(state::AntennaConfiguration(true));
        }

        /** @}*/
    }
}
<commit_msg>Do not abort mission loop on antenna NACK<commit_after>#include <cstdint>
#include <cstring>
#include "antenna/driver.h"
#include "antenna_state.h"
#include "antenna_task.hpp"
#include "gsl/gsl_util"
#include "logger/logger.h"
#include "mission/base.hpp"
#include "mission/obc.hpp"
#include "system.h"
#include "time/TimePoint.h"

using namespace std::chrono_literals;

namespace mission
{
    namespace antenna
    {
        /**
         * @addtogroup mission_atenna
         * @{
         */
        /**
         * @brief Type definition of the specific deployment step handler.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to deployment process state.
         * @param[in] driver Reference to current antenna driver instance
         */
        typedef void DeploymentProcedure(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief This deployment step performs regular deployment operation.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void RegularDeploymentStep(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief This deployment step resets current hardware channel.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void ResetDriverStep(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief This deployment step finalizes antenna deployment process.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void FinishDeploymentStep(const SystemState& state, //
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            );

        /**
         * @brief Deployment step retry limit.
         *
         * This value controls how many times each deployment step will be repeated
         * in case of errors before process can advance further.
         */
        static constexpr std::uint8_t StepRetryLimit = 3;

        /**
         * @brief Hardware operation retry limit.
         *
         * This value controls how many times each hardware operation will be issued
         * in case of errors before declaring it as a failure.
         */
        static constexpr std::uint8_t RetryLimit = 3;

        /**
         * @brief Class that describes single deployment step.
         *
         * The entire process is driven by the array of steps, each described by the object of this type.
         */
        struct AntennaDeploymentStep final
        {
            /**
             * @brief Pointer to the procedure the performs current step.
             */
            DeploymentProcedure* procedure;

            /**
             * @brief Hardware channel that should be used to perform this step.
             */
            AntennaChannel channel;

            /**
             * @brief Identifier of the antenna that should be used/affected by this step.
             */
            AntennaId antennaId;

            /**
             * @brief Antenna deployment process timeout in seconds.
             */
            uint8_t deploymentTimeout;

            /**
             * @brief Flag indicating whether hardware deployment switches should be overriden.
             */
            bool overrideSwitches;
        };

        /**
         * @brief Default timeout value that should be enough in case there are no problems.
         */
        constexpr std::uint8_t DefaultTimeout = 9;

        /**
         * @brief Extended timeout that should take care of the most of the problems.
         */
        constexpr std::uint8_t MediumTimeout = 19;

        /**
         * @brief Long timeouts for heating problems.
         */
        constexpr std::uint8_t LongTimeout = 39;

        /**
         * @brief Last resort timeout in case everything else fails.
         */
        constexpr std::uint8_t EmergencyTimeout = 59;

        /**
         * @brief Array of antenna deployment steps.
         *
         * The entire process is composed of steps that are run in sequence from the beginning. Steps themselves are
         * grouped into series that together form logical variant of the antenna deployment process.
         *
         * Following is the list of the deployment stages that are defined in this array:
         * - Automatic deployment (primary hardware channel)
         * - Manual deployment with overridden deployment switches (primary hardware channel)
         * - Deployment finalization (primary hardware channel)
         *
         * - Automatic deployment (backup hardware channel)
         * - Manual deployment with overridden deployment switches (backup hardware channel)
         * - Deployment finalization (backup hardware channel)
         */
        static const AntennaDeploymentStep deploymentSteps[] = {
            {ResetDriverStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA_AUTO_ID, 0, false},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA_AUTO_ID, DefaultTimeout, false},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA1_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA2_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA3_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA4_ID, LongTimeout, true},
            {FinishDeploymentStep, ANTENNA_PRIMARY_CHANNEL, ANTENNA_AUTO_ID, 0, false},

            {ResetDriverStep, ANTENNA_BACKUP_CHANNEL, ANTENNA_AUTO_ID, 0, false},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA_AUTO_ID, DefaultTimeout, false},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA1_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA2_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA3_ID, LongTimeout, true},
            {RegularDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA4_ID, LongTimeout, true},
            {FinishDeploymentStep, ANTENNA_BACKUP_CHANNEL, ANTENNA_AUTO_ID, 0, false},
        };

        /**
         * @brief Number of steps in the antenna deployment process.
         */
        static constexpr uint8_t DeploymentStepLimit = count_of(deploymentSteps);

        AntennaMissionState::AntennaMissionState(AntennaDriver& antennaDriver)
            : _overrideState(false), //
              _inProgress(false),    //
              _stepNumber(0),        //
              _retryCount(0),        //
              _driver(antennaDriver)
        {
        }

        void AntennaMissionState::Retry(std::uint8_t limit)
        {
            if ((this->_retryCount + 1) == limit)
            {
                NextStep();
            }
            else
            {
                ++this->_retryCount;
            }
        }

        bool AntennaMissionState::IsFinished() const
        {
            return this->_stepNumber >= DeploymentStepLimit;
        }

        void AntennaMissionState::Update(const AntennaDeploymentStatus& status)
        {
            this->_inProgress = status.IsDeploymentActive[0] | //
                status.IsDeploymentActive[1] |                 //
                status.IsDeploymentActive[2] |                 //
                status.IsDeploymentActive[3];
        }

        std::uint8_t AntennaMissionState::StepCount()
        {
            return DeploymentStepLimit;
        }

        /**
         * @brief This procedure is supposed to stop any deployment process that may currently be active on the
         * passed hardware channel.
         *
         * @param[in] driver Reference to current antenna driver instance.
         * @param[in] channel Hardware channel that should be used for current operation.
         * @param[in] retryCount Number of step retry attempts in case of errors.
         * @return True if operation has been successfully completed, false otherwise.
         */
        static bool EndDeployment(AntennaDriver& driver, AntennaChannel channel, std::uint8_t retryCount)
        {
            while (retryCount-- > 0)
            {
                const OSResult status = driver.FinishDeployment(&driver, channel);
                if (OS_RESULT_SUCCEEDED(status))
                {
                    return true;
                }
            }

            return false;
        }

        /**
         * @brief This procedure is supposed to stop any deployment process that may currently be active on the
         * hardware channel used in the previous step.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void StopDeployment(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            if (stateDescriptor.StepNumber() == 0)
            {
                return;
            }

            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber() - 1];
            EndDeployment(driver, step.channel, RetryLimit);
        }

        /**
         * @brief This procedure is supposed to start deployment process described by the current step.
         *
         * @param[in] state Reference to global satellite state.
         * @param[in] stateDescriptor Reference to the deployment process state object.
         * @param[in] driver Reference to current antenna driver instance.
         */
        static void BeginDeployment(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];
            std::uint8_t counter = RetryLimit;
            while (counter-- > 0)
            {
                const OSResult result = driver.DeployAntenna(&driver,
                    step.channel,
                    step.antennaId,
                    std::chrono::seconds(step.deploymentTimeout),
                    step.overrideSwitches //
                    );

                if (OS_RESULT_SUCCEEDED(result))
                {
                    stateDescriptor.NextStep();
                    return;
                }
            }

            stateDescriptor.Retry(StepRetryLimit);
        }

        void RegularDeploymentStep(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];

            LOGF(LOG_LEVEL_INFO,
                "[ant] [Step%d] Regular deployment (channel %d, antenna %d)",
                stateDescriptor.StepNumber(),
                step.channel,
                step.antennaId);

            StopDeployment(state, stateDescriptor, driver);
            BeginDeployment(state, stateDescriptor, driver);
        }

        void ResetDriverStep(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];

            LOGF(LOG_LEVEL_INFO, "[ant] [Step%d] Reseting driver %d", stateDescriptor.StepNumber(), step.channel);

            std::uint8_t counter = RetryLimit;
            while (counter-- > 0)
            {
                const OSResult result = driver.Reset(&driver, step.channel);
                if (OS_RESULT_SUCCEEDED(result))
                {
                    stateDescriptor.NextStep();
                    return;
                }
            }

            stateDescriptor.Retry(StepRetryLimit);
        }

        void FinishDeploymentStep(const SystemState& state,
            AntennaMissionState& stateDescriptor,
            AntennaDriver& driver //
            )
        {
            UNREFERENCED_PARAMETER(state);
            const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor.StepNumber()];

            LOGF(LOG_LEVEL_INFO, "[ant] [Step%d] Finish deployment %d", stateDescriptor.StepNumber(), step.channel);

            if (EndDeployment(driver, step.channel, RetryLimit))
            {
                stateDescriptor.NextStep();
            }
            else
            {
                stateDescriptor.Retry(StepRetryLimit);
            }
        }

        /**
         * @brief This procedure is deployment action entry point.
         *
         * This procedure runs the antenna deployment process.
         * @param[in] state Pointer to global satellite state.
         * @param[in] param Pointer to the deployment condition private context. This pointer should point
         * at the object of AntennaMissionState type.
         */
        static void AntennaDeploymentAction(SystemState& state, void* param)
        {
            auto stateDescriptor = static_cast<AntennaMissionState*>(param);

            LOGF(LOG_LEVEL_DEBUG, "[ant] Executing antenna deployment step %d", stateDescriptor->StepNumber());

            if (state.PersistentState.Get<state::AntennaConfiguration>().IsDeploymentDisabled())
            {
                LOG(LOG_LEVEL_DEBUG, "[ant] Antenna deployment disabled");
                stateDescriptor->Finish();
            }
            else
            {
                const AntennaDeploymentStep& step = deploymentSteps[stateDescriptor->StepNumber()];
                DeploymentProcedure* procedure = step.procedure;
                procedure(state, *stateDescriptor, stateDescriptor->Driver());
            }
        }

        /**
         * @brief Procedure that verifies whether the antenna deployment process should be executed.
         * @param[in] state Pointer to global satellite state.
         * @param[in] param Pointer to the deployment condition private context. This pointer should point
         * at the object of AntennaMissionState type.
         *
         * @return True if the deployment action should be performed, false otherwise.
         */
        static bool AntennaDeploymentCondition(const SystemState& state, void* param)
        {
            AntennaMissionState* stateDescriptor = (AntennaMissionState*)param;
            if (!IsInitialSilentPeriodFinished(state.Time))
            {
                return false;
            }

            if (stateDescriptor->IsDeploymentInProgress())
            {
                return false;
            }

            return !stateDescriptor->IsFinished();
        }

        /**
         * @brief This procedure is antenna deployment update descriptor entry point.
         *
         * This procedure updates the global satellite state as well as deployemnt process private state.
         * @param[in] state Pointer to global satellite state.
         * @param[in] param Pointer to the deployment condition private context. This pointer should point
         * at the object of AntennaMissionState type.
         * @return Operation status.
         */
        static UpdateResult AntennaDeploymentUpdate(SystemState& state, void* param)
        {
            UNREFERENCED_PARAMETER(state);
            AntennaMissionState* stateDescriptor = (AntennaMissionState*)param;
            state.AntennaState.SetDeployment(stateDescriptor->IsFinished());
            if (stateDescriptor->IsFinished())
            {
                return UpdateResult::Ok;
            }

            AntennaDeploymentStatus deploymentStatus;
            AntennaDriver& driver = stateDescriptor->Driver();
            const OSResult result = driver.GetDeploymentStatus(&driver,
                deploymentSteps[stateDescriptor->StepNumber()].channel,
                &deploymentStatus //
                );

            if (OS_RESULT_FAILED(result))
            {
                return UpdateResult::Warning;
            }

            stateDescriptor->Update(deploymentStatus);
            return UpdateResult::Ok;
        }

        AntennaTask::AntennaTask(AntennaDriver& driver) : state(driver)
        {
        }

        ActionDescriptor<SystemState> AntennaTask::BuildAction()
        {
            ActionDescriptor<SystemState> descriptor;
            descriptor.name = "Deploy Antenna Action";
            descriptor.param = &this->state;
            descriptor.condition = AntennaDeploymentCondition;
            descriptor.actionProc = AntennaDeploymentAction;
            return descriptor;
        }

        UpdateDescriptor<SystemState> AntennaTask::BuildUpdate()
        {
            UpdateDescriptor<SystemState> descriptor;
            descriptor.name = "Deploy Antenna Update";
            descriptor.param = &this->state;
            descriptor.updateProc = AntennaDeploymentUpdate;
            return descriptor;
        }

        StopAntennaDeploymentTask::StopAntennaDeploymentTask(std::uint8_t /*mark*/)
        {
        }
        mission::ActionDescriptor<SystemState> StopAntennaDeploymentTask::BuildAction()
        {
            mission::ActionDescriptor<SystemState> action;
            action.name = "Stop antenna deployment";
            action.param = this;
            action.condition = Condition;
            action.actionProc = Action;
            return action;
        }

        void StopAntennaDeploymentTask::DisableDeployment()
        {
            this->_shouldDisable = true;
        }

        bool StopAntennaDeploymentTask::Condition(const SystemState& state, void* param)
        {
            auto This = reinterpret_cast<StopAntennaDeploymentTask*>(param);
            auto alreadyDisabled = state.PersistentState.Get<state::AntennaConfiguration>().IsDeploymentDisabled();

            return This->_shouldDisable && !alreadyDisabled && state.AntennaState.IsDeployed();
        }

        void StopAntennaDeploymentTask::Action(SystemState& state, void*)
        {
            LOG(LOG_LEVEL_INFO, "[ant] Disabling antenna deployment");

            state.PersistentState.Set(state::AntennaConfiguration(true));
        }

        /** @}*/
    }
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: convdicxml.hxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: obo $ $Date: 2004-04-27 16:06:48 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _LINGUISTIC_CONVDICXML_HXX_
#define _LINGUISTIC_CONVDICXML_HXX_

#ifndef _COM_SUN_STAR_LINGUISTIC2_XCONVERSIONDICTIONARY_HPP_
#include <com/sun/star/linguistic2/XConversionDictionary.hpp>
#endif
#ifndef _COM_SUN_STAR_UTIL_XFLUSHABLE_HPP_
#include <com/sun/star/util/XFlushable.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XSERVICEINFO_HPP_
#include <com/sun/star/lang/XServiceInfo.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_XDOCUMENTHANDLER_HPP_
#include <com/sun/star/xml/sax/XDocumentHandler.hpp>
#endif
#ifndef _XMLOFF_XMLEXP_HXX
#include <xmloff/xmlexp.hxx>
#endif
#ifndef _XMLOFF_XMLIMP_HXX
#include <xmloff/xmlimp.hxx>
#endif

#ifndef _CPPUHELPER_IMPLBASE3_HXX_
#include <cppuhelper/implbase3.hxx>
#endif
#ifndef _CPPUHELPER_INTERFACECONTAINER_H_
#include <cppuhelper/interfacecontainer.h>
#endif

#ifndef _STRING_HXX
#include <tools/string.hxx>
#endif
#ifndef _RTL_USTRING_HXX_
#include <rtl/ustring.hxx>
#endif

#ifndef _LINGUISTIC_MISC_HXX_
#include "misc.hxx"
#endif
#ifndef _LINGUISTIC_DEFS_HXX_
#include "defs.hxx"
#endif

class ConvDic;

///////////////////////////////////////////////////////////////////////////

class ConvDicXMLExport : public SvXMLExport
{
    ConvDic     &rDic;
    sal_Bool    bSuccess;

protected:
    //void ExportNodes(const SmNode *pIn, int nLevel);

public:
    ConvDicXMLExport( ConvDic &rConvDic,
        const rtl::OUString &rFileName,
        com::sun::star::uno::Reference< com::sun::star::xml::sax::XDocumentHandler > &rHandler) :
        SvXMLExport ( rFileName, rHandler ),
        rDic        ( rConvDic ),
        bSuccess    ( sal_False )
    {
    }
    virtual ~ConvDicXMLExport()
    {
    }

    // XServiceInfo (override parent method)
    ::rtl::OUString SAL_CALL getImplementationName() throw( ::com::sun::star::uno::RuntimeException );

    // SvXMLExport
    void _ExportAutoStyles()    {}
    void _ExportMasterStyles()  {}
    void _ExportContent();
    sal_uInt32 exportDoc( enum ::xmloff::token::XMLTokenEnum eClass );

    sal_Bool    Export( SfxMedium &rMedium );
};

class ConvDicXMLImport : public SvXMLImport
{
    ConvDic        *pDic;       // conversion dictionary to be used
                                // if != NULL: whole file will be read and
                                //   all entries will be added to the dictionary
                                // if == NULL: no entries will be added
                                //   but the language and conversion type will
                                //   still be determined!

    INT16           nLanguage;          // language of the dictionary
    sal_Int16       nConversionType;    // conversion type the dictionary is used for
    sal_Bool        bSuccess;

public:

    //!!  see comment for pDic member
    ConvDicXMLImport( ConvDic *pConvDic, const rtl::OUString &rFileName ) :
        SvXMLImport ( IMPORT_ALL ),
        pDic        ( pConvDic )
    {
        nLanguage       = LANGUAGE_NONE;
        nConversionType = -1;
        bSuccess        = sal_False;
    }

    virtual ~ConvDicXMLImport() throw ()
    {
    }

    // XServiceInfo (override parent method)
    ::rtl::OUString SAL_CALL getImplementationName() throw( ::com::sun::star::uno::RuntimeException );

    virtual void SAL_CALL startDocument(void) throw( ::com::sun::star::xml::sax::SAXException, ::com::sun::star::uno::RuntimeException );
    virtual void SAL_CALL endDocument(void) throw( ::com::sun::star::xml::sax::SAXException, ::com::sun::star::uno::RuntimeException );

    virtual SvXMLImportContext * CreateContext(
        sal_uInt16 nPrefix, const rtl::OUString &rLocalName,
        const com::sun::star::uno::Reference < com::sun::star::xml::sax::XAttributeList > &rxAttrList );

    ConvDic *   GetDic()                    { return pDic; }
    INT16       GetLanguage() const         { return nLanguage; }
    sal_Int16   GetConversionType() const   { return nConversionType; }
    sal_Bool    GetSuccess() const          { return bSuccess; }

    void        SetLanguage( INT16 nLang )              { nLanguage = nLang; }
    void        SetConversionType( sal_Int16 nType )    { nConversionType = nType; }
};

///////////////////////////////////////////////////////////////////////////

#endif

<commit_msg>#110680#, #i10000#: Constructor of SvXMLExport / SvXMLImport now needs a getProcessServiceFactory() as first parameter.<commit_after>/*************************************************************************
 *
 *  $RCSfile: convdicxml.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2004-05-04 10:45:47 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef _LINGUISTIC_CONVDICXML_HXX_
#define _LINGUISTIC_CONVDICXML_HXX_

#ifndef _COM_SUN_STAR_LINGUISTIC2_XCONVERSIONDICTIONARY_HPP_
#include <com/sun/star/linguistic2/XConversionDictionary.hpp>
#endif
#ifndef _COM_SUN_STAR_UTIL_XFLUSHABLE_HPP_
#include <com/sun/star/util/XFlushable.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_XSERVICEINFO_HPP_
#include <com/sun/star/lang/XServiceInfo.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_XDOCUMENTHANDLER_HPP_
#include <com/sun/star/xml/sax/XDocumentHandler.hpp>
#endif
#ifndef _XMLOFF_XMLEXP_HXX
#include <xmloff/xmlexp.hxx>
#endif
#ifndef _XMLOFF_XMLIMP_HXX
#include <xmloff/xmlimp.hxx>
#endif

#ifndef _CPPUHELPER_IMPLBASE3_HXX_
#include <cppuhelper/implbase3.hxx>
#endif
#ifndef _CPPUHELPER_INTERFACECONTAINER_H_
#include <cppuhelper/interfacecontainer.h>
#endif

#ifndef _STRING_HXX
#include <tools/string.hxx>
#endif
#ifndef _RTL_USTRING_HXX_
#include <rtl/ustring.hxx>
#endif

#ifndef _LINGUISTIC_MISC_HXX_
#include "misc.hxx"
#endif
#ifndef _LINGUISTIC_DEFS_HXX_
#include "defs.hxx"
#endif

class ConvDic;

///////////////////////////////////////////////////////////////////////////

class ConvDicXMLExport : public SvXMLExport
{
    ConvDic     &rDic;
    sal_Bool    bSuccess;

protected:
    //void ExportNodes(const SmNode *pIn, int nLevel);

public:
    ConvDicXMLExport( ConvDic &rConvDic,
        const rtl::OUString &rFileName,
        com::sun::star::uno::Reference< com::sun::star::xml::sax::XDocumentHandler > &rHandler) :
        SvXMLExport ( utl::getProcessServiceFactory(), rFileName, rHandler ),
        rDic        ( rConvDic ),
        bSuccess    ( sal_False )
    {
    }
    virtual ~ConvDicXMLExport()
    {
    }

    // XServiceInfo (override parent method)
    ::rtl::OUString SAL_CALL getImplementationName() throw( ::com::sun::star::uno::RuntimeException );

    // SvXMLExport
    void _ExportAutoStyles()    {}
    void _ExportMasterStyles()  {}
    void _ExportContent();
    sal_uInt32 exportDoc( enum ::xmloff::token::XMLTokenEnum eClass );

    sal_Bool    Export( SfxMedium &rMedium );
};

class ConvDicXMLImport : public SvXMLImport
{
    ConvDic        *pDic;       // conversion dictionary to be used
                                // if != NULL: whole file will be read and
                                //   all entries will be added to the dictionary
                                // if == NULL: no entries will be added
                                //   but the language and conversion type will
                                //   still be determined!

    INT16           nLanguage;          // language of the dictionary
    sal_Int16       nConversionType;    // conversion type the dictionary is used for
    sal_Bool        bSuccess;

public:

    //!!  see comment for pDic member
    ConvDicXMLImport( ConvDic *pConvDic, const rtl::OUString &rFileName ) :
        SvXMLImport ( utl::getProcessServiceFactory(), IMPORT_ALL ),
        pDic        ( pConvDic )
    {
        nLanguage       = LANGUAGE_NONE;
        nConversionType = -1;
        bSuccess        = sal_False;
    }

    virtual ~ConvDicXMLImport() throw ()
    {
    }

    // XServiceInfo (override parent method)
    ::rtl::OUString SAL_CALL getImplementationName() throw( ::com::sun::star::uno::RuntimeException );

    virtual void SAL_CALL startDocument(void) throw( ::com::sun::star::xml::sax::SAXException, ::com::sun::star::uno::RuntimeException );
    virtual void SAL_CALL endDocument(void) throw( ::com::sun::star::xml::sax::SAXException, ::com::sun::star::uno::RuntimeException );

    virtual SvXMLImportContext * CreateContext(
        sal_uInt16 nPrefix, const rtl::OUString &rLocalName,
        const com::sun::star::uno::Reference < com::sun::star::xml::sax::XAttributeList > &rxAttrList );

    ConvDic *   GetDic()                    { return pDic; }
    INT16       GetLanguage() const         { return nLanguage; }
    sal_Int16   GetConversionType() const   { return nConversionType; }
    sal_Bool    GetSuccess() const          { return bSuccess; }

    void        SetLanguage( INT16 nLang )              { nLanguage = nLang; }
    void        SetConversionType( sal_Int16 nType )    { nConversionType = nType; }
};

///////////////////////////////////////////////////////////////////////////

#endif

<|endoftext|>


<commit_before>// Scintilla source code edit control
/** @file Selection.cxx
 ** Classes maintaining the selection.
 **/
// Copyright 2009 by Neil Hodgson <neilh@scintilla.org>
// The License.txt file describes the conditions under which this software may be distributed.

#include <stdlib.h>

#include <vector>

#include "Platform.h"

#include "Scintilla.h"

#include "Selection.h"

#ifdef SCI_NAMESPACE
using namespace Scintilla;
#endif

void SelectionPosition::MoveForInsertDelete(bool insertion, int startChange, int length) {
	if (insertion) {
		if (position > startChange) {
			position += length;
		}
	} else {
		if (position > startChange) {
			int endDeletion = startChange + length;
			if (position > endDeletion) {
				position -= length;
			} else {
				position = startChange;
			}
		}
	}
}

bool SelectionPosition::operator <(const SelectionPosition &other) const {
	if (position == other.position)
		return virtualSpace < other.virtualSpace;
	else
		return position < other.position;
}

bool SelectionPosition::operator >(const SelectionPosition &other) const {
	if (position == other.position)
		return virtualSpace > other.virtualSpace;
	else
		return position > other.position;
}

bool SelectionPosition::operator <=(const SelectionPosition &other) const {
	if (position == other.position && virtualSpace == other.virtualSpace)
		return true;
	else
		return other > *this;
}

bool SelectionPosition::operator >=(const SelectionPosition &other) const {
	if (position == other.position && virtualSpace == other.virtualSpace)
		return true;
	else
		return *this > other;
}

int SelectionRange::Length() const {
	if (anchor > caret) {
		return anchor.Position() - caret.Position();
	} else {
		return caret.Position() - anchor.Position();
	}
}

bool SelectionRange::Contains(int pos) const {
	if (anchor > caret)
		return (pos >= caret.Position()) && (pos <= anchor.Position());
	else
		return (pos >= anchor.Position()) && (pos <= caret.Position());
}

bool SelectionRange::Contains(SelectionPosition sp) const {
	if (anchor > caret)
		return (sp >= caret) && (sp <= anchor);
	else
		return (sp >= anchor) && (sp <= caret);
}

bool SelectionRange::ContainsCharacter(int posCharacter) const {
	if (anchor > caret)
		return (posCharacter >= caret.Position()) && (posCharacter < anchor.Position());
	else
		return (posCharacter >= anchor.Position()) && (posCharacter < caret.Position());
}

SelectionSegment SelectionRange::Intersect(SelectionSegment check) const {
	SelectionSegment inOrder(caret, anchor);
	if ((inOrder.start <= check.end) || (inOrder.end >= check.start)) {
		SelectionSegment portion = check;
		if (portion.start < inOrder.start)
			portion.start = inOrder.start;
		if (portion.end > inOrder.end)
			portion.end = inOrder.end;
		if (portion.start > portion.end)
			return SelectionSegment();
		else
			return portion;
	} else {
		return SelectionSegment();
	}
}

bool SelectionRange::Trim(SelectionRange range) {
	SelectionPosition startRange = range.Start();
	SelectionPosition endRange = range.End();
	SelectionPosition start = Start();
	SelectionPosition end = End();
	PLATFORM_ASSERT(start <= end);
	PLATFORM_ASSERT(startRange <= endRange);
	if ((startRange <= end) && (endRange >= start)) {
		if ((start > startRange) && (end < endRange)) {
			// Completely covered by range -> empty at start
			end = start;
		} else if ((start < startRange) && (end > endRange)) {
			// Completely covers range -> empty at start
			end = start;
		} else if (start <= startRange) {
			// Trim end
			end = startRange;
		} else { //
			PLATFORM_ASSERT(end >= endRange);
			// Trim start
			start = endRange;
		}
		if (anchor > caret) {
			caret = start;
			anchor = end;
		} else {
			anchor = start;
			caret = end;
		}
		return Empty();
	} else {
		return false;
	}
}

// If range is all virtual collapse to start of virtual space
void SelectionRange::MinimizeVirtualSpace() {
	if (caret.Position() == anchor.Position()) {
		int virtualSpace = caret.VirtualSpace();
		if (virtualSpace > anchor.VirtualSpace())
			virtualSpace = anchor.VirtualSpace();
		caret.SetVirtualSpace(virtualSpace);
		anchor.SetVirtualSpace(virtualSpace);
	}
}

Selection::Selection() : mainRange(0), moveExtends(false), selType(selStream), tentativeMain(false) {
	AddSelection(SelectionPosition(0));
}

Selection::~Selection() {
}

bool Selection::IsRectangular() const {
	return (selType == selRectangle) || (selType == selThin);
}

int Selection::MainCaret() const {
	return ranges[mainRange].caret.Position();
}

int Selection::MainAnchor() const {
	return ranges[mainRange].anchor.Position();
}

SelectionRange &Selection::Rectangular() {
	return rangeRectangular;
}

size_t Selection::Count() const {
	return ranges.size();
}

size_t Selection::Main() const {
	return mainRange;
}

void Selection::SetMain(size_t r) {
	PLATFORM_ASSERT(r < ranges.size());
	mainRange = r;
}

SelectionRange &Selection::Range(size_t r) {
	return ranges[r];
}

SelectionRange &Selection::RangeMain() {
	return ranges[mainRange];
}

bool Selection::MoveExtends() const {
	return moveExtends;
}

void Selection::SetMoveExtends(bool moveExtends_) {
	moveExtends = moveExtends_;
}

bool Selection::Empty() const {
	for (size_t i=0; i<ranges.size(); i++) {
		if (!ranges[i].Empty())
			return false;
	}
	return true;
}

SelectionPosition Selection::Last() const {
	SelectionPosition lastPosition;
	for (size_t i=0; i<ranges.size(); i++) {
		if (lastPosition < ranges[i].caret)
			lastPosition = ranges[i].caret;
		if (lastPosition < ranges[i].anchor)
			lastPosition = ranges[i].anchor;
	}
	return lastPosition;
}

int Selection::Length() const {
	int len = 0;
	for (size_t i=0; i<ranges.size(); i++) {
		len += ranges[i].Length();
	}
	return len;
}

void Selection::MovePositions(bool insertion, int startChange, int length) {
	for (size_t i=0; i<ranges.size(); i++) {
		ranges[i].caret.MoveForInsertDelete(insertion, startChange, length);
		ranges[i].anchor.MoveForInsertDelete(insertion, startChange, length);
	}
}

void Selection::TrimSelection(SelectionRange range) {
	for (size_t i=0; i<ranges.size();) {
		if ((i != mainRange) && (ranges[i].Trim(range))) {
			// Trimmed to empty so remove
			for (size_t j=i;j<ranges.size()-1;j++) {
				ranges[j] = ranges[j+1];
				if (j == mainRange-1)
					mainRange--;
			}
			ranges.pop_back();
		} else {
			i++;
		}
	}
}

void Selection::SetSelection(SelectionRange range) {
	ranges.clear();
	ranges.push_back(range);
	mainRange = ranges.size() - 1;
}

void Selection::AddSelection(SelectionRange range) {
	TrimSelection(range);
	ranges.push_back(range);
	mainRange = ranges.size() - 1;
}

void Selection::TentativeSelection(SelectionRange range) {
	if (!tentativeMain) {
		rangesSaved = ranges;
	}
	ranges = rangesSaved;
	AddSelection(range);
	TrimSelection(ranges[mainRange]);
	tentativeMain = true;
}

void Selection::CommitTentative() {
	rangesSaved.clear();
	tentativeMain = false;
}

int Selection::CharacterInSelection(int posCharacter) const {
	for (size_t i=0; i<ranges.size(); i++) {
		if (ranges[i].ContainsCharacter(posCharacter))
			return i == mainRange ? 1 : 2;
	}
	return 0;
}

int Selection::InSelectionForEOL(int pos) const {
	for (size_t i=0; i<ranges.size(); i++) {
		if (!ranges[i].Empty() && (pos > ranges[i].Start().Position()) && (pos <= ranges[i].End().Position()))
			return i == mainRange ? 1 : 2;
	}
	return 0;
}

int Selection::VirtualSpaceFor(int pos) const {
	int virtualSpace = 0;
	for (size_t i=0; i<ranges.size(); i++) {
		if ((ranges[i].caret.Position() == pos) && (virtualSpace < ranges[i].caret.VirtualSpace()))
			virtualSpace = ranges[i].caret.VirtualSpace();
		if ((ranges[i].anchor.Position() == pos) && (virtualSpace < ranges[i].anchor.VirtualSpace()))
			virtualSpace = ranges[i].anchor.VirtualSpace();
	}
	return virtualSpace;
}

void Selection::Clear() {
	ranges.clear();
	ranges.push_back(SelectionRange());
	mainRange = ranges.size() - 1;
	selType = selStream;
	moveExtends = false;
	ranges[mainRange].Reset();
	rangeRectangular.Reset();
}

void Selection::RemoveDuplicates() {
	for (size_t i=0; i<ranges.size()-1; i++) {
		if (ranges[i].Empty()) {
			size_t j=i+1;
			while (j<ranges.size()) {
				if (ranges[i] == ranges[j]) {
					ranges.erase(ranges.begin() + j);
					if (mainRange >= j)
						mainRange--;
				} else {
					j++;
				}
			}
		}
	}
}

void Selection::RotateMain() {
	mainRange = (mainRange + 1) % ranges.size();
}

<commit_msg>Avoid warning.<commit_after>// Scintilla source code edit control
/** @file Selection.cxx
 ** Classes maintaining the selection.
 **/
// Copyright 2009 by Neil Hodgson <neilh@scintilla.org>
// The License.txt file describes the conditions under which this software may be distributed.

#include <stdlib.h>

#include <vector>

#include "Platform.h"

#include "Scintilla.h"

#include "Selection.h"

#ifdef SCI_NAMESPACE
using namespace Scintilla;
#endif

void SelectionPosition::MoveForInsertDelete(bool insertion, int startChange, int length) {
	if (insertion) {
		if (position > startChange) {
			position += length;
		}
	} else {
		if (position > startChange) {
			int endDeletion = startChange + length;
			if (position > endDeletion) {
				position -= length;
			} else {
				position = startChange;
			}
		}
	}
}

bool SelectionPosition::operator <(const SelectionPosition &other) const {
	if (position == other.position)
		return virtualSpace < other.virtualSpace;
	else
		return position < other.position;
}

bool SelectionPosition::operator >(const SelectionPosition &other) const {
	if (position == other.position)
		return virtualSpace > other.virtualSpace;
	else
		return position > other.position;
}

bool SelectionPosition::operator <=(const SelectionPosition &other) const {
	if (position == other.position && virtualSpace == other.virtualSpace)
		return true;
	else
		return other > *this;
}

bool SelectionPosition::operator >=(const SelectionPosition &other) const {
	if (position == other.position && virtualSpace == other.virtualSpace)
		return true;
	else
		return *this > other;
}

int SelectionRange::Length() const {
	if (anchor > caret) {
		return anchor.Position() - caret.Position();
	} else {
		return caret.Position() - anchor.Position();
	}
}

bool SelectionRange::Contains(int pos) const {
	if (anchor > caret)
		return (pos >= caret.Position()) && (pos <= anchor.Position());
	else
		return (pos >= anchor.Position()) && (pos <= caret.Position());
}

bool SelectionRange::Contains(SelectionPosition sp) const {
	if (anchor > caret)
		return (sp >= caret) && (sp <= anchor);
	else
		return (sp >= anchor) && (sp <= caret);
}

bool SelectionRange::ContainsCharacter(int posCharacter) const {
	if (anchor > caret)
		return (posCharacter >= caret.Position()) && (posCharacter < anchor.Position());
	else
		return (posCharacter >= anchor.Position()) && (posCharacter < caret.Position());
}

SelectionSegment SelectionRange::Intersect(SelectionSegment check) const {
	SelectionSegment inOrder(caret, anchor);
	if ((inOrder.start <= check.end) || (inOrder.end >= check.start)) {
		SelectionSegment portion = check;
		if (portion.start < inOrder.start)
			portion.start = inOrder.start;
		if (portion.end > inOrder.end)
			portion.end = inOrder.end;
		if (portion.start > portion.end)
			return SelectionSegment();
		else
			return portion;
	} else {
		return SelectionSegment();
	}
}

bool SelectionRange::Trim(SelectionRange range) {
	SelectionPosition startRange = range.Start();
	SelectionPosition endRange = range.End();
	SelectionPosition start = Start();
	SelectionPosition end = End();
	PLATFORM_ASSERT(start <= end);
	PLATFORM_ASSERT(startRange <= endRange);
	if ((startRange <= end) && (endRange >= start)) {
		if ((start > startRange) && (end < endRange)) {
			// Completely covered by range -> empty at start
			end = start;
		} else if ((start < startRange) && (end > endRange)) {
			// Completely covers range -> empty at start
			end = start;
		} else if (start <= startRange) {
			// Trim end
			end = startRange;
		} else { //
			PLATFORM_ASSERT(end >= endRange);
			// Trim start
			start = endRange;
		}
		if (anchor > caret) {
			caret = start;
			anchor = end;
		} else {
			anchor = start;
			caret = end;
		}
		return Empty();
	} else {
		return false;
	}
}

// If range is all virtual collapse to start of virtual space
void SelectionRange::MinimizeVirtualSpace() {
	if (caret.Position() == anchor.Position()) {
		int virtualSpace = caret.VirtualSpace();
		if (virtualSpace > anchor.VirtualSpace())
			virtualSpace = anchor.VirtualSpace();
		caret.SetVirtualSpace(virtualSpace);
		anchor.SetVirtualSpace(virtualSpace);
	}
}

Selection::Selection() : mainRange(0), moveExtends(false), tentativeMain(false), selType(selStream) {
	AddSelection(SelectionPosition(0));
}

Selection::~Selection() {
}

bool Selection::IsRectangular() const {
	return (selType == selRectangle) || (selType == selThin);
}

int Selection::MainCaret() const {
	return ranges[mainRange].caret.Position();
}

int Selection::MainAnchor() const {
	return ranges[mainRange].anchor.Position();
}

SelectionRange &Selection::Rectangular() {
	return rangeRectangular;
}

size_t Selection::Count() const {
	return ranges.size();
}

size_t Selection::Main() const {
	return mainRange;
}

void Selection::SetMain(size_t r) {
	PLATFORM_ASSERT(r < ranges.size());
	mainRange = r;
}

SelectionRange &Selection::Range(size_t r) {
	return ranges[r];
}

SelectionRange &Selection::RangeMain() {
	return ranges[mainRange];
}

bool Selection::MoveExtends() const {
	return moveExtends;
}

void Selection::SetMoveExtends(bool moveExtends_) {
	moveExtends = moveExtends_;
}

bool Selection::Empty() const {
	for (size_t i=0; i<ranges.size(); i++) {
		if (!ranges[i].Empty())
			return false;
	}
	return true;
}

SelectionPosition Selection::Last() const {
	SelectionPosition lastPosition;
	for (size_t i=0; i<ranges.size(); i++) {
		if (lastPosition < ranges[i].caret)
			lastPosition = ranges[i].caret;
		if (lastPosition < ranges[i].anchor)
			lastPosition = ranges[i].anchor;
	}
	return lastPosition;
}

int Selection::Length() const {
	int len = 0;
	for (size_t i=0; i<ranges.size(); i++) {
		len += ranges[i].Length();
	}
	return len;
}

void Selection::MovePositions(bool insertion, int startChange, int length) {
	for (size_t i=0; i<ranges.size(); i++) {
		ranges[i].caret.MoveForInsertDelete(insertion, startChange, length);
		ranges[i].anchor.MoveForInsertDelete(insertion, startChange, length);
	}
}

void Selection::TrimSelection(SelectionRange range) {
	for (size_t i=0; i<ranges.size();) {
		if ((i != mainRange) && (ranges[i].Trim(range))) {
			// Trimmed to empty so remove
			for (size_t j=i;j<ranges.size()-1;j++) {
				ranges[j] = ranges[j+1];
				if (j == mainRange-1)
					mainRange--;
			}
			ranges.pop_back();
		} else {
			i++;
		}
	}
}

void Selection::SetSelection(SelectionRange range) {
	ranges.clear();
	ranges.push_back(range);
	mainRange = ranges.size() - 1;
}

void Selection::AddSelection(SelectionRange range) {
	TrimSelection(range);
	ranges.push_back(range);
	mainRange = ranges.size() - 1;
}

void Selection::TentativeSelection(SelectionRange range) {
	if (!tentativeMain) {
		rangesSaved = ranges;
	}
	ranges = rangesSaved;
	AddSelection(range);
	TrimSelection(ranges[mainRange]);
	tentativeMain = true;
}

void Selection::CommitTentative() {
	rangesSaved.clear();
	tentativeMain = false;
}

int Selection::CharacterInSelection(int posCharacter) const {
	for (size_t i=0; i<ranges.size(); i++) {
		if (ranges[i].ContainsCharacter(posCharacter))
			return i == mainRange ? 1 : 2;
	}
	return 0;
}

int Selection::InSelectionForEOL(int pos) const {
	for (size_t i=0; i<ranges.size(); i++) {
		if (!ranges[i].Empty() && (pos > ranges[i].Start().Position()) && (pos <= ranges[i].End().Position()))
			return i == mainRange ? 1 : 2;
	}
	return 0;
}

int Selection::VirtualSpaceFor(int pos) const {
	int virtualSpace = 0;
	for (size_t i=0; i<ranges.size(); i++) {
		if ((ranges[i].caret.Position() == pos) && (virtualSpace < ranges[i].caret.VirtualSpace()))
			virtualSpace = ranges[i].caret.VirtualSpace();
		if ((ranges[i].anchor.Position() == pos) && (virtualSpace < ranges[i].anchor.VirtualSpace()))
			virtualSpace = ranges[i].anchor.VirtualSpace();
	}
	return virtualSpace;
}

void Selection::Clear() {
	ranges.clear();
	ranges.push_back(SelectionRange());
	mainRange = ranges.size() - 1;
	selType = selStream;
	moveExtends = false;
	ranges[mainRange].Reset();
	rangeRectangular.Reset();
}

void Selection::RemoveDuplicates() {
	for (size_t i=0; i<ranges.size()-1; i++) {
		if (ranges[i].Empty()) {
			size_t j=i+1;
			while (j<ranges.size()) {
				if (ranges[i] == ranges[j]) {
					ranges.erase(ranges.begin() + j);
					if (mainRange >= j)
						mainRange--;
				} else {
					j++;
				}
			}
		}
	}
}

void Selection::RotateMain() {
	mainRange = (mainRange + 1) % ranges.size();
}

<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);

    cv::Mat rvec, tvec;
    cv::Vec4f dist_param(0,0,0,0);
    cv::solvePnP(object_pts, image_pts, intrinsics, dist_param,
            rvec, tvec);
    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg )
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Mat tmp;
    cv::Point2d opticalCenter(0.5*subscribed_gray.rows,
                              0.5*subscribed_gray.cols);
    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = frame_;
        marker_transform.header.stamp = ros::Time::now();
        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        marker_transform.type = visualization_msgs::Marker::ARROW;
        //marker_transform.type = visualization_msgs::Marker::CUBE;
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

    	double tag_size = GetTagSize(detections[i].id);
        /*
        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size;
        marker_transform.scale.z = 0.01 * tag_size;
        */
        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size;
        marker_transform.scale.z = tag_size;

        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);
    }
    marker_publisher_.publish(marker_transforms);

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
		image_subscriber.shutdown();
		info_subscriber.shutdown();
		running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, 0);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("tf_frame", frame_, DEFAULT_TF_FRAME);

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}
<commit_msg>testing sizes<commit_after>#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);

    cv::Mat rvec, tvec;
    cv::Vec4f dist_param(0,0,0,0);
    cv::solvePnP(object_pts, image_pts, intrinsics, dist_param,
            rvec, tvec);
    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg )
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Mat tmp;
    cv::Point2d opticalCenter(0.5*subscribed_gray.rows,
                              0.5*subscribed_gray.cols);
    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = frame_;
        marker_transform.header.stamp = ros::Time::now();
        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        marker_transform.type = visualization_msgs::Marker::ARROW;
        //marker_transform.type = visualization_msgs::Marker::CUBE;
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

    	double tag_size = GetTagSize(detections[i].id);
        /*
        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size;
        marker_transform.scale.z = 0.01 * tag_size;
        */
        marker_transform.scale.x = tag_size;
        marker_transform.scale.y = tag_size*5;
        marker_transform.scale.z = tag_size;

        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);
    }
    marker_publisher_.publish(marker_transforms);

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
		image_subscriber.shutdown();
		info_subscriber.shutdown();
		running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, 0);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("tf_frame", frame_, DEFAULT_TF_FRAME);

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>#include "lib/hi_res_time.h"
#include "lib/input.h"
#include "lib/text_renderer.h"
#include "lib/window_manager.h"

#include <string.h>
#include <windows.h>

inline unsigned GetRand()
{
    static unsigned r = 0x12345671;
    return r = ((r * 214013 + 2531011) >> 16) & 0xffff;
}

double CalcMillionPixelsPerSec(BitmapRGBA *backBuffer)
{
    unsigned iterations = 1000 * 1000 * 10;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; i++)
    {
        unsigned r = GetRand();
        RGBAColour c;
        c.r = r & 0xff;
        c.g = r & 0xff;
        c.b = r & 0xff;
        c.a = 128;
        PutPixUnclipped(backBuffer, r & 255, (r >> 8) & 255, c);
    }
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numPixels = (double)iterations;
    return (numPixels / duration) / 1e6;
}

double CalcBillionRectFillPixelsPerSec(BitmapRGBA *backBuffer)
{
    unsigned iterations = 1000 * 300;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; i++)
        RectFill(backBuffer, 10, 10, 100, 100, g_colourWhite);
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numPixels = 100.0 * 100.0 * (double)iterations;
    return (numPixels / duration) / 1e9;
}

double CalcMillionLinePixelsPerSec(BitmapRGBA *backBuffer)
{
    unsigned iterations = 1000 * 600;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; ++i)
    {
        DrawLine(backBuffer, 300, 300, 320, 600, g_colourWhite);   // 320 long, nice slope
        DrawLine(backBuffer, 300, 300, 600, 320, g_colourWhite);
        DrawLine(backBuffer, 300, 300, 301, 315, g_colourWhite);   // 16 long, nice slope
        DrawLine(backBuffer, 300, 300, 315, 301, g_colourWhite);
        DrawLine(backBuffer, 300, 300, 450, 451, g_colourWhite);   // 151 long, annoying slope
        DrawLine(backBuffer, 300, 300, 451, 450, g_colourWhite);
    }
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numPixels = (320 + 16 + 151) * 2 * iterations;
    return (numPixels / duration) / 1e6;
}

double CalcMillionCharsPerSec(BitmapRGBA *backBuffer, TextRenderer *font)
{
    static char const *str = "Here's some interesting text []# !";
    unsigned iterations = 1000 * 100;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; i++)
        DrawTextSimple(font, g_colourWhite, backBuffer, 10, 10, str);
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numChars = iterations * (double)strlen(str);
    return (numChars / duration) / 1000000.0;
}

int WINAPI WinMain(HINSTANCE _hInstance, HINSTANCE, LPSTR cmdLine, int)
{
	CreateWin(100, 100, 1024, 768, true, "Benchmark");
    TextRenderer *font = CreateTextRenderer("Courier", 8);
    BitmapRGBA *backBmp = CreateBitmapRGBA(800, 600);

    ClearBitmap(g_window->backBuffer, g_colourBlack);
    int textY = 10;
    double score;

    // Put pixel
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcMillionPixelsPerSec(backBmp);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Million putpixels per sec = %.1f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 100, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    // Line draw
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcMillionLinePixelsPerSec(backBmp);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Million line pixels per sec = %.2f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 200, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    // Rect fill
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcBillionRectFillPixelsPerSec(backBmp);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Rectfill billion pixels per sec = %.2f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 300, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    // Text render
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcMillionCharsPerSec(backBmp, font);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Million chars per sec = %.2f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 400, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Press ESC to quit");

    while (!g_window->windowClosed && !g_inputManager.keyDowns[KEY_ESC])
    {
        AdvanceWin();
        Sleep(100);
    }
}
<commit_msg>Moved lines onto visible part of bitmap<commit_after>#include "lib/hi_res_time.h"
#include "lib/input.h"
#include "lib/text_renderer.h"
#include "lib/window_manager.h"

#include <string.h>
#include <windows.h>

inline unsigned GetRand()
{
    static unsigned r = 0x12345671;
    return r = ((r * 214013 + 2531011) >> 16) & 0xffff;
}

double CalcMillionPixelsPerSec(BitmapRGBA *backBuffer)
{
    unsigned iterations = 1000 * 1000 * 10;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; i++)
    {
        unsigned r = GetRand();
        RGBAColour c;
        c.r = r & 0xff;
        c.g = r & 0xff;
        c.b = r & 0xff;
        c.a = 128;
        PutPixUnclipped(backBuffer, r & 255, (r >> 8) & 255, c);
    }
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numPixels = (double)iterations;
    return (numPixels / duration) / 1e6;
}

double CalcBillionRectFillPixelsPerSec(BitmapRGBA *backBuffer)
{
    unsigned iterations = 1000 * 300;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; i++)
        RectFill(backBuffer, 10, 10, 100, 100, g_colourWhite);
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numPixels = 100.0 * 100.0 * (double)iterations;
    return (numPixels / duration) / 1e9;
}

double CalcMillionLinePixelsPerSec(BitmapRGBA *backBuffer)
{
    unsigned iterations = 1000 * 600;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; ++i)
    {
        DrawLine(backBuffer, 0, 0, 20,  300, g_colourWhite);   // 320 long, nice slope
        DrawLine(backBuffer, 0, 0, 300, 20, g_colourWhite);
        DrawLine(backBuffer, 0, 0, 1,   15, g_colourWhite);    // 16 long, nice slope
        DrawLine(backBuffer, 0, 0, 15,  1, g_colourWhite);
        DrawLine(backBuffer, 0, 0, 150, 151, g_colourWhite);   // 151 long, annoying slope
        DrawLine(backBuffer, 0, 0, 151, 150, g_colourWhite);
    }
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numPixels = (320 + 16 + 151) * 2 * iterations;
    return (numPixels / duration) / 1e6;
}

double CalcMillionCharsPerSec(BitmapRGBA *backBuffer, TextRenderer *font)
{
    static char const *str = "Here's some interesting text []# !";
    unsigned iterations = 1000 * 100;
    double startTime = GetHighResTime();
    for (unsigned i = 0; i < iterations; i++)
        DrawTextSimple(font, g_colourWhite, backBuffer, 10, 10, str);
    double endTime = GetHighResTime();
    double duration = endTime - startTime;
    double numChars = iterations * (double)strlen(str);
    return (numChars / duration) / 1000000.0;
}

int WINAPI WinMain(HINSTANCE _hInstance, HINSTANCE, LPSTR cmdLine, int)
{
	CreateWin(100, 100, 1024, 768, true, "Benchmark");
    TextRenderer *font = CreateTextRenderer("Courier", 8);
    BitmapRGBA *backBmp = CreateBitmapRGBA(800, 600);

    ClearBitmap(g_window->backBuffer, g_colourBlack);
    int textY = 10;
    double score;

    // Put pixel
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcMillionPixelsPerSec(backBmp);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Million putpixels per sec = %.1f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 100, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    // Line draw
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcMillionLinePixelsPerSec(backBmp);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Million line pixels per sec = %.2f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 200, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    // Rect fill
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcBillionRectFillPixelsPerSec(backBmp);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Rectfill billion pixels per sec = %.2f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 300, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    // Text render
    ClearBitmap(backBmp, g_colourBlack);
    score = CalcMillionCharsPerSec(backBmp, font);
    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Million chars per sec = %.2f", score);
    textY += font->charHeight;
    QuickBlit(g_window->backBuffer, 400, 400, backBmp);
    AdvanceWin();
    if (g_window->windowClosed || g_inputManager.keyDowns[KEY_ESC]) return 0;

    DrawTextLeft(font, g_colourWhite, g_window->backBuffer, 10, textY,
        "Press ESC to quit");

    while (!g_window->windowClosed && !g_inputManager.keyDowns[KEY_ESC])
    {
        AdvanceWin();
        Sleep(100);
    }
}
<|endoftext|>