<commit_before>/*!
 * @file InverseKinematicsData.cpp
 * @author Francesco Romano
 * @copyright 2016 iCub Facility - Istituto Italiano di Tecnologia
 *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 * @date 2016
 *
 */

#include "InverseKinematicsData.h"
#include "InverseKinematicsNLP.h"
#include "TransformConstraint.h"
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/ClassicalAcc.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/Model/Model.h>

#include <cassert>
#include <private/InverseKinematicsData.h>

namespace internal {
namespace kinematics {

    InverseKinematicsData::InverseKinematicsData(const InverseKinematicsData&) {}
    InverseKinematicsData& InverseKinematicsData::operator=(const InverseKinematicsData&) { return *this; }

    InverseKinematicsData::InverseKinematicsData()
    : m_dofs(0)
    , m_rotationParametrization(iDynTree::InverseKinematicsRotationParametrizationQuaternion)
    , m_targetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintFull)
    , m_areBaseInitialConditionsSet(false)
    , m_areJointsInitialConditionsSet(false)
    , m_solver(NULL)
    // The default values for the ipopt related parameters are exactly the one of IPOPT,
    // see https://www.coin-or.org/Ipopt/documentation/node41.html
    //     https://www.coin-or.org/Ipopt/documentation/node42.html
    , m_maxIter(3000)
    , m_maxCpuTime(1e6)
    , m_tol(1e-8)
    , m_constrTol(1e-4)
    , m_verbosityLevel(0)
    {
        //These variables are touched only once.
        m_state.worldGravity.zero();

        m_state.baseTwist.zero();
    }

    bool InverseKinematicsData::setModel(const iDynTree::Model& model)
    {
        bool result = m_dynamics.loadRobotModel(model);
        if (!result || !m_dynamics.isValid()) {
            std::cerr << "[ERROR] Error loading robot model" << std::endl;
            return false;
        }

        // I don't know if KinDyn can perform some operations on the model
        // For safety I get the model loaded instead of using the input one
        const iDynTree::Model& loadedModel = m_dynamics.model();
        m_dofs = loadedModel.getNrOfDOFs();

        //prepare jiont limits
        m_jointLimits.clear();
        m_jointLimits.resize(m_dofs);
        //TODO to be changed to +_ infinity
        //default: no limits
        m_jointLimits.assign(m_dofs, std::pair<double, double>(-2e+19, 2e+19));

        //for each joint, ask the limits
        for (iDynTree::JointIndex jointIdx = 0; jointIdx < loadedModel.getNrOfJoints(); ++jointIdx) {
            iDynTree::IJointConstPtr joint = loadedModel.getJoint(jointIdx);
            //if the joint does not have limits skip it
            if (!joint->hasPosLimits())
                continue;
            //for each DoF modelled by the joint get the limits
            for (unsigned dof = 0; dof < joint->getNrOfDOFs(); ++dof) {
                if (!joint->getPosLimits(dof,
                                         m_jointLimits[joint->getDOFsOffset() + dof].first,
                                         m_jointLimits[joint->getDOFsOffset() + dof].second))
                    continue;
            }
        }

        //We set a new model, clear the variables
        clearProblem();

        updateRobotConfiguration();

        return true;
    }

    void InverseKinematicsData::clearProblem()
    {
        //resize vectors
        m_jointInitialConditions.resize(m_dofs);
        m_jointInitialConditions.zero();
        m_jointsResults.resize(m_dofs);
        m_jointsResults.zero();
        m_preferredJointsConfiguration.resize(m_dofs);
        m_preferredJointsConfiguration.zero();
        m_preferredJointsWeight = 1e-6;

        m_state.jointsConfiguration.resize(m_dofs);
        m_state.jointsConfiguration.zero();

        m_state.jointsVelocity.resize(m_dofs);
        m_state.jointsVelocity.zero();

        m_state.basePose.setPosition(iDynTree::Position(0, 0, 0));
        m_state.basePose.setRotation(iDynTree::Rotation::Identity());

        m_constraints.clear();
        m_targets.clear();
        m_comConstraint.setActive(false);

        m_areBaseInitialConditionsSet = false;
        m_areJointsInitialConditionsSet = false;
    }

    bool InverseKinematicsData::addFrameConstraint(const kinematics::TransformConstraint& frameTransformConstraint)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransformConstraint.getFrameName());
        if (frameIndex < 0)
            return false;

        //add the constraint to the set
        std::pair<TransformMap::iterator, bool> result = m_constraints.insert(TransformMap::value_type(frameIndex, frameTransformConstraint));
        return result.second;
    }

    bool InverseKinematicsData::addTarget(const kinematics::TransformConstraint& frameTransform)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransform.getFrameName());
        if (frameIndex < 0)
            return false;

        std::pair<TransformMap::iterator, bool> result = m_targets.insert(TransformMap::value_type(frameIndex, frameTransform));
        return result.second;
    }

    TransformMap::iterator InverseKinematicsData::getTargetRefIfItExists(const std::string targetFrameName)
    {
        // The error for this check is already printed in getFrameIndex
        int frameIndex = m_dynamics.getFrameIndex(targetFrameName);
        if (frameIndex == iDynTree::FRAME_INVALID_INDEX)
            return m_targets.end();

        // Find the target (if this fails, it will return m_targets.end()
        return m_targets.find(frameIndex);
    }

    void InverseKinematicsData::updatePositionTarget(TransformMap::iterator target, iDynTree::Position newPos, double newPosWeight)
    {
        assert(target != m_targets.end());
        target->second.setPosition(newPos);
        target->second.setPositionWeight(newPosWeight);
    }

    void InverseKinematicsData::updateRotationTarget(TransformMap::iterator target, iDynTree::Rotation newRot, double newRotWeight)
    {
        assert(target != m_targets.end());
        target->second.setRotation(newRot);
        target->second.setRotationWeight(newRotWeight);
    }

    iDynTree::KinDynComputations& InverseKinematicsData::dynamics() { return m_dynamics; }

    bool InverseKinematicsData::setInitialCondition(const iDynTree::Transform* baseTransform,
                                                    const iDynTree::VectorDynSize* initialJointCondition)
    {
        if (baseTransform) {
            m_baseInitialCondition = *baseTransform;
            m_areBaseInitialConditionsSet = true;
        }
        if (initialJointCondition) {
            m_jointInitialConditions = *initialJointCondition;
            m_areJointsInitialConditionsSet = true;
        }

        return true;
    }

    bool InverseKinematicsData::setRobotConfiguration(const iDynTree::Transform& baseConfiguration,
                                                      const iDynTree::VectorDynSize& jointConfiguration)
    {
        assert(m_state.jointsConfiguration.size() == jointConfiguration.size());
        m_state.jointsConfiguration = jointConfiguration;
        m_state.basePose = baseConfiguration;
        updateRobotConfiguration();
        return true;
    }

    bool InverseKinematicsData::setJointConfiguration(const std::string& jointName, const double jointConfiguration)
    {
        iDynTree::JointIndex jointIndex = m_dynamics.model().getJointIndex(jointName);
        if (jointIndex == iDynTree::JOINT_INVALID_INDEX) return false;
        m_state.jointsConfiguration(jointIndex) = jointConfiguration;
        updateRobotConfiguration();
        return true;
    }

    bool InverseKinematicsData::setDesiredJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, const double weight)
    {
        assert(m_preferredJointsConfiguration.size() == desiredJointConfiguration.size());
        m_preferredJointsConfiguration = desiredJointConfiguration;
        if( weight >= 0.0 ) {
            m_preferredJointsWeight = weight;
        }
        return true;
    }

    void InverseKinematicsData::setRotationParametrization(enum iDynTree::InverseKinematicsRotationParametrization parametrization)
    {
        m_rotationParametrization = parametrization;
    }

    enum iDynTree::InverseKinematicsRotationParametrization InverseKinematicsData::rotationParametrization() { return m_rotationParametrization; }

    void InverseKinematicsData::updateRobotConfiguration()
    {
        m_dynamics.setRobotState(m_state.basePose,
                                 m_state.jointsConfiguration,
                                 m_state.baseTwist,
                                 m_state.jointsVelocity,
                                 m_state.worldGravity);
    }

    void InverseKinematicsData::prepareForOptimization()
    {
        //Do all stuff needed before starting an optimization problem
        //1) prepare initial condition if not explicitly set
        if (!m_areBaseInitialConditionsSet) {
            m_baseInitialCondition = m_state.basePose;
        }

        if (!m_areJointsInitialConditionsSet) {
            m_jointInitialConditions = m_state.jointsConfiguration;
        }

        //2) Check joint limits.. Is this necessary?
        for (size_t i = 0; i < m_jointInitialConditions.size(); ++i) {
            //check joint to be inside limit
            double &jointValue = m_jointInitialConditions(i);
            if (jointValue < m_jointLimits[i].first || jointValue > m_jointLimits[i].second) {
                std::cerr << "[WARNING] InverseKinematics: joint with DOFIndex " << i << " initial condition is outside the limits " << m_jointLimits[i].first << " " << m_jointLimits[i].second << std::endl;
                //set the initial value to be at the middle of the limits
                jointValue = (m_jointLimits[i].second + m_jointLimits[i].first) / 2.0;
            }
        }

    }

    void InverseKinematicsData::setTargetResolutionMode(enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
    {
        m_targetResolutionMode = mode;
    }

    enum iDynTree::InverseKinematicsTreatTargetAsConstraint InverseKinematicsData::targetResolutionMode()
    {
        return m_targetResolutionMode;
    }

    bool InverseKinematicsData::solveProblem()
    {
        Ipopt::ApplicationReturnStatus solverStatus;

        if (Ipopt::IsNull(m_solver)) {
            m_solver = IpoptApplicationFactory();

            //TODO: set options
            //For example, one needed option is the linear solver type
            //Best thing is to wrap the IPOPT options with new structure so as to abstract them
            m_solver->Options()->SetStringValue("hessian_approximation", "limited-memory");
            m_solver->Options()->SetIntegerValue("print_level",m_verbosityLevel);
            m_solver->Options()->SetIntegerValue("max_iter", m_maxIter);
            m_solver->Options()->SetNumericValue("max_cpu_time", m_maxCpuTime);
            m_solver->Options()->SetNumericValue("tol",m_tol);
            m_solver->Options()->SetNumericValue("constr_viol_tol",m_constrTol);
            m_solver->Options()->SetIntegerValue("acceptable_iter",0);
#ifndef NDEBUG
            m_solver->Options()->SetStringValue("derivative_test", "first-order");
#endif
            if (!m_solverName.empty()) {
                m_solver->Options()->SetStringValue("linear_solver", m_solverName);
            }

            solverStatus = m_solver->Initialize();
            if (solverStatus != Ipopt::Solve_Succeeded) {
                return false;
            }
        }

        prepareForOptimization();

        //instantiate the IpOpt problem
        internal::kinematics::InverseKinematicsNLP *iKin = new internal::kinematics::InverseKinematicsNLP(*this);
        //Do something (if necessary)
        Ipopt::SmartPtr<Ipopt::TNLP> problem(iKin);

        // Ask Ipopt to solve the problem
        solverStatus = m_solver->OptimizeTNLP(problem);

        if (solverStatus == Ipopt::Solve_Succeeded || solverStatus == Ipopt::Solved_To_Acceptable_Level ) {
            return true;
        } else {
            return false;
        }
    }

    void InverseKinematicsData::getSolution(iDynTree::Transform & baseTransformSolution,
                                            iDynTree::VectorDynSize & shapeSolution)
    {
        baseTransformSolution = m_baseResults;
        shapeSolution         = m_jointsResults;
    }

}
}
<commit_msg>[inverse-kinematics] Fix handling of initial position outside of joints limits<commit_after>/*!
 * @file InverseKinematicsData.cpp
 * @author Francesco Romano
 * @copyright 2016 iCub Facility - Istituto Italiano di Tecnologia
 *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 * @date 2016
 *
 */

#include "InverseKinematicsData.h"
#include "InverseKinematicsNLP.h"
#include "TransformConstraint.h"
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/ClassicalAcc.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/Model/Model.h>

#include <cassert>
#include <private/InverseKinematicsData.h>

namespace internal {
namespace kinematics {

    InverseKinematicsData::InverseKinematicsData(const InverseKinematicsData&) {}
    InverseKinematicsData& InverseKinematicsData::operator=(const InverseKinematicsData&) { return *this; }

    InverseKinematicsData::InverseKinematicsData()
    : m_dofs(0)
    , m_rotationParametrization(iDynTree::InverseKinematicsRotationParametrizationQuaternion)
    , m_targetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintFull)
    , m_areBaseInitialConditionsSet(false)
    , m_areJointsInitialConditionsSet(false)
    , m_solver(NULL)
    // The default values for the ipopt related parameters are exactly the one of IPOPT,
    // see https://www.coin-or.org/Ipopt/documentation/node41.html
    //     https://www.coin-or.org/Ipopt/documentation/node42.html
    , m_maxIter(3000)
    , m_maxCpuTime(1e6)
    , m_tol(1e-8)
    , m_constrTol(1e-4)
    , m_verbosityLevel(0)
    {
        //These variables are touched only once.
        m_state.worldGravity.zero();

        m_state.baseTwist.zero();
    }

    bool InverseKinematicsData::setModel(const iDynTree::Model& model)
    {
        bool result = m_dynamics.loadRobotModel(model);
        if (!result || !m_dynamics.isValid()) {
            std::cerr << "[ERROR] Error loading robot model" << std::endl;
            return false;
        }

        // I don't know if KinDyn can perform some operations on the model
        // For safety I get the model loaded instead of using the input one
        const iDynTree::Model& loadedModel = m_dynamics.model();
        m_dofs = loadedModel.getNrOfDOFs();

        //prepare jiont limits
        m_jointLimits.clear();
        m_jointLimits.resize(m_dofs);
        //TODO to be changed to +_ infinity
        //default: no limits
        m_jointLimits.assign(m_dofs, std::pair<double, double>(-2e+19, 2e+19));

        //for each joint, ask the limits
        for (iDynTree::JointIndex jointIdx = 0; jointIdx < loadedModel.getNrOfJoints(); ++jointIdx) {
            iDynTree::IJointConstPtr joint = loadedModel.getJoint(jointIdx);
            //if the joint does not have limits skip it
            if (!joint->hasPosLimits())
                continue;
            //for each DoF modelled by the joint get the limits
            for (unsigned dof = 0; dof < joint->getNrOfDOFs(); ++dof) {
                if (!joint->getPosLimits(dof,
                                         m_jointLimits[joint->getDOFsOffset() + dof].first,
                                         m_jointLimits[joint->getDOFsOffset() + dof].second))
                    continue;
            }
        }

        //We set a new model, clear the variables
        clearProblem();

        updateRobotConfiguration();

        return true;
    }

    void InverseKinematicsData::clearProblem()
    {
        //resize vectors
        m_jointInitialConditions.resize(m_dofs);
        m_jointInitialConditions.zero();
        m_jointsResults.resize(m_dofs);
        m_jointsResults.zero();
        m_preferredJointsConfiguration.resize(m_dofs);
        m_preferredJointsConfiguration.zero();
        m_preferredJointsWeight = 1e-6;

        m_state.jointsConfiguration.resize(m_dofs);
        m_state.jointsConfiguration.zero();

        m_state.jointsVelocity.resize(m_dofs);
        m_state.jointsVelocity.zero();

        m_state.basePose.setPosition(iDynTree::Position(0, 0, 0));
        m_state.basePose.setRotation(iDynTree::Rotation::Identity());

        m_constraints.clear();
        m_targets.clear();
        m_comConstraint.setActive(false);

        m_areBaseInitialConditionsSet = false;
        m_areJointsInitialConditionsSet = false;
    }

    bool InverseKinematicsData::addFrameConstraint(const kinematics::TransformConstraint& frameTransformConstraint)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransformConstraint.getFrameName());
        if (frameIndex < 0)
            return false;

        //add the constraint to the set
        std::pair<TransformMap::iterator, bool> result = m_constraints.insert(TransformMap::value_type(frameIndex, frameTransformConstraint));
        return result.second;
    }

    bool InverseKinematicsData::addTarget(const kinematics::TransformConstraint& frameTransform)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransform.getFrameName());
        if (frameIndex < 0)
            return false;

        std::pair<TransformMap::iterator, bool> result = m_targets.insert(TransformMap::value_type(frameIndex, frameTransform));
        return result.second;
    }

    TransformMap::iterator InverseKinematicsData::getTargetRefIfItExists(const std::string targetFrameName)
    {
        // The error for this check is already printed in getFrameIndex
        int frameIndex = m_dynamics.getFrameIndex(targetFrameName);
        if (frameIndex == iDynTree::FRAME_INVALID_INDEX)
            return m_targets.end();

        // Find the target (if this fails, it will return m_targets.end()
        return m_targets.find(frameIndex);
    }

    void InverseKinematicsData::updatePositionTarget(TransformMap::iterator target, iDynTree::Position newPos, double newPosWeight)
    {
        assert(target != m_targets.end());
        target->second.setPosition(newPos);
        target->second.setPositionWeight(newPosWeight);
    }

    void InverseKinematicsData::updateRotationTarget(TransformMap::iterator target, iDynTree::Rotation newRot, double newRotWeight)
    {
        assert(target != m_targets.end());
        target->second.setRotation(newRot);
        target->second.setRotationWeight(newRotWeight);
    }

    iDynTree::KinDynComputations& InverseKinematicsData::dynamics() { return m_dynamics; }

    bool InverseKinematicsData::setInitialCondition(const iDynTree::Transform* baseTransform,
                                                    const iDynTree::VectorDynSize* initialJointCondition)
    {
        if (baseTransform) {
            m_baseInitialCondition = *baseTransform;
            m_areBaseInitialConditionsSet = true;
        }
        if (initialJointCondition) {
            m_jointInitialConditions = *initialJointCondition;
            m_areJointsInitialConditionsSet = true;
        }

        return true;
    }

    bool InverseKinematicsData::setRobotConfiguration(const iDynTree::Transform& baseConfiguration,
                                                      const iDynTree::VectorDynSize& jointConfiguration)
    {
        assert(m_state.jointsConfiguration.size() == jointConfiguration.size());
        m_state.jointsConfiguration = jointConfiguration;
        m_state.basePose = baseConfiguration;
        updateRobotConfiguration();
        return true;
    }

    bool InverseKinematicsData::setJointConfiguration(const std::string& jointName, const double jointConfiguration)
    {
        iDynTree::JointIndex jointIndex = m_dynamics.model().getJointIndex(jointName);
        if (jointIndex == iDynTree::JOINT_INVALID_INDEX) return false;
        m_state.jointsConfiguration(jointIndex) = jointConfiguration;
        updateRobotConfiguration();
        return true;
    }

    bool InverseKinematicsData::setDesiredJointConfiguration(const iDynTree::VectorDynSize& desiredJointConfiguration, const double weight)
    {
        assert(m_preferredJointsConfiguration.size() == desiredJointConfiguration.size());
        m_preferredJointsConfiguration = desiredJointConfiguration;
        if( weight >= 0.0 ) {
            m_preferredJointsWeight = weight;
        }
        return true;
    }

    void InverseKinematicsData::setRotationParametrization(enum iDynTree::InverseKinematicsRotationParametrization parametrization)
    {
        m_rotationParametrization = parametrization;
    }

    enum iDynTree::InverseKinematicsRotationParametrization InverseKinematicsData::rotationParametrization() { return m_rotationParametrization; }

    void InverseKinematicsData::updateRobotConfiguration()
    {
        m_dynamics.setRobotState(m_state.basePose,
                                 m_state.jointsConfiguration,
                                 m_state.baseTwist,
                                 m_state.jointsVelocity,
                                 m_state.worldGravity);
    }

    void InverseKinematicsData::prepareForOptimization()
    {
        //Do all stuff needed before starting an optimization problem
        //1) prepare initial condition if not explicitly set
        if (!m_areBaseInitialConditionsSet) {
            m_baseInitialCondition = m_state.basePose;
        }

        if (!m_areJointsInitialConditionsSet) {
            m_jointInitialConditions = m_state.jointsConfiguration;
        }

        //2) Check joint limits.. Is this necessary?
        for (size_t i = 0; i < m_jointInitialConditions.size(); ++i) {
            //check joint to be inside limit
            double &jointValue = m_jointInitialConditions(i);
            if (jointValue < m_jointLimits[i].first || jointValue > m_jointLimits[i].second) {
                std::cerr << "[WARNING] InverseKinematics: joint with DOFIndex " << i << " initial condition is outside the limits " << m_jointLimits[i].first << " " << m_jointLimits[i].second << std::endl;
                //set the initial value to at the limit
                if (jointValue < m_jointLimits[i].first) {
                    jointValue = m_jointLimits[i].first;
                }

                if (jointValue > m_jointLimits[i].second) {
                    jointValue = m_jointLimits[i].second;
                }
            }
        }

    }

    void InverseKinematicsData::setTargetResolutionMode(enum iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
    {
        m_targetResolutionMode = mode;
    }

    enum iDynTree::InverseKinematicsTreatTargetAsConstraint InverseKinematicsData::targetResolutionMode()
    {
        return m_targetResolutionMode;
    }

    bool InverseKinematicsData::solveProblem()
    {
        Ipopt::ApplicationReturnStatus solverStatus;

        if (Ipopt::IsNull(m_solver)) {
            m_solver = IpoptApplicationFactory();

            //TODO: set options
            //For example, one needed option is the linear solver type
            //Best thing is to wrap the IPOPT options with new structure so as to abstract them
            m_solver->Options()->SetStringValue("hessian_approximation", "limited-memory");
            m_solver->Options()->SetIntegerValue("print_level",m_verbosityLevel);
            m_solver->Options()->SetIntegerValue("max_iter", m_maxIter);
            m_solver->Options()->SetNumericValue("max_cpu_time", m_maxCpuTime);
            m_solver->Options()->SetNumericValue("tol",m_tol);
            m_solver->Options()->SetNumericValue("constr_viol_tol",m_constrTol);
            m_solver->Options()->SetIntegerValue("acceptable_iter",0);
#ifndef NDEBUG
            m_solver->Options()->SetStringValue("derivative_test", "first-order");
#endif
            if (!m_solverName.empty()) {
                m_solver->Options()->SetStringValue("linear_solver", m_solverName);
            }

            solverStatus = m_solver->Initialize();
            if (solverStatus != Ipopt::Solve_Succeeded) {
                return false;
            }
        }

        prepareForOptimization();

        //instantiate the IpOpt problem
        internal::kinematics::InverseKinematicsNLP *iKin = new internal::kinematics::InverseKinematicsNLP(*this);
        //Do something (if necessary)
        Ipopt::SmartPtr<Ipopt::TNLP> problem(iKin);

        // Ask Ipopt to solve the problem
        solverStatus = m_solver->OptimizeTNLP(problem);

        if (solverStatus == Ipopt::Solve_Succeeded || solverStatus == Ipopt::Solved_To_Acceptable_Level ) {
            return true;
        } else {
            return false;
        }
    }

    void InverseKinematicsData::getSolution(iDynTree::Transform & baseTransformSolution,
                                            iDynTree::VectorDynSize & shapeSolution)
    {
        baseTransformSolution = m_baseResults;
        shapeSolution         = m_jointsResults;
    }

}
}
<|endoftext|>


<commit_before>/*
 *  Agent.cpp
 *  openc2e
 *
 *  Created by Alyssa Milburn on Tue May 25 2004.
 *  Copyright (c) 2004 Alyssa Milburn. All rights reserved.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2 of the License, or (at your option) any later version.
 *
 *  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.
 *
 */

#include "Agent.h"
#include "World.h"
#include "physics.h"
#include <iostream>
#include <sstream>
#include "caosVM.h"

Agent::Agent(unsigned char f, unsigned char g, unsigned short s, unsigned int p) :
  visible(true), family(f), genus(g), species(s), zorder(p), vm(0), timerrate(0) {
	velx.setFloat(0.0f);
	vely.setFloat(0.0f);
	accg = 0.3f;
	range = 500;
	sufferphysics = false;
	x = 0.0f; y = 0.0f;
	clac[0] = 1; // activate 1
	clik = -1;

	// AgentRef stuff
	self.ref = this;
	self.next = self.prev = &self;

	dying = false;
	unid = -1;

	zorder_iter = world.agents.insert(this);
}

Agent::~Agent() {
	world.agents.erase(zorder_iter);
	if (vm)
		world.freeVM(vm);
	zotrefs();
	if (unid != -1)
		world.freeUNID(unid);
}

void Agent::moveTo(float _x, float _y) {
	x = _x; y = _y;
}

void Agent::fireScript(unsigned short event) {
	if (dying) return;

	script &s = world.scriptorium.getScript(family, genus, species, event);
	if (s.lines.empty()) return;
	if (!vm) vm = world.getVM(this);
	if (vm->fireScript(s, (event == 9)))
		vm->setTarg(this);

	// This slows us down too much :)
#if 0
	std::cout << "Agent::fireScript fired " << (unsigned int)family << " " << (unsigned int)genus << " " << species << " ";
	const std::string n = world.catalogue.getAgentName(family, genus, species);
	if (n.size())
		std::cout << "(" << n << ") ";
	std::cout << (unsigned int)event << std::endl;
#endif
}

void Agent::tick() {
	if (dying) return;

	if (sufferphysics && accg) {
		float newvely = vely.floatValue + accg;
		float destx = x + velx.floatValue;
		float desty = y + newvely;
		//std::cout << x << ", " << y << ": " << destx << ", " << desty << "! " << accg << "\n";
		Room *r1 = world.map.roomAt((unsigned int)x, (unsigned int)y);
		if (!r1) {
			std::cout << "not doing physics on agent, outside room system!\n";
		}
		Room *r2 = world.map.roomAt((unsigned int)destx + getWidth(), (unsigned int)desty + getHeight());
		Room *r3 = world.map.roomAt((unsigned int)destx, (unsigned int)desty);

		if (r1 && (r1 == r2) && (r2 == r3)) {
			vely.setFloat(newvely);
			moveTo(destx, desty);
		} else if (r1) {
			physicsHandler p;

			//std::cout << x + getWidth() / 2 << " " << y + getHeight() << " " << destx + (getWidth() / 2) << " " << desty + getHeight() << " " << r1->x_left << " " << r1->y_left_floor << " " << r1->x_right << " " << r1->y_right_floor << "\n";

			bool c = p.collidePoints(x + (getWidth() / 2), y + getHeight(), destx + (getWidth() / 2), desty + getHeight(),
					r1->x_left, r1->y_left_floor, r1->x_right, r1->y_right_floor);

			bool hitfloor = false;
			if (c) {
				destx = p.getCollisionX() - (getWidth() / 2);
				desty = p.getCollisionY() - getHeight();
				if (desty != y) hitfloor = true;
			}

			r1 = world.map.roomAt(destx, desty);
			r2 = world.map.roomAt(destx + getWidth() - 1, desty + getHeight() - 1);

			if (r1 && r2) {
				// quick hack to see if we can travel to the next room
				bool hacked = false;
				if (r1 != r2) {
					if (r1->doors[r2] && r1->doors[r2]->perm == 100) {
						moveTo(destx, desty + 2);
						vely.setFloat(newvely);
						hacked = true;
					}
				}
				if (!hacked) {
					// okay, we can't travel to the next room.
					moveTo(destx, desty);
					// this is supposed to only fire if we hit the floor, but it's sort of iffy
					vely.setFloat(newvely);
					if (hitfloor) {
						fireScript(6); // script Collision
						if (vm) vm->setVariables(velx, vely);
					}
					// then set vely to 0 so we don't do stupid things
					vely.setFloat(0);
				}
			} else {
				// we're not in the room system..
				vely.setFloat(0);
			}
		}
	} else {
		if (vely.hasDecimal())
			y = y + vely.getFloat();
		if (velx.hasDecimal())
			x = x + velx.getFloat();
	}

	if (timerrate) {
		tickssincelasttimer++;
		if (tickssincelasttimer == timerrate) {
			fireScript(9);
			tickssincelasttimer = 0;
		}
	}

	if (vm)
		vm->tick();
}

void Agent::kill() {
	assert(!dying);
	dying = true; // what a world, what a world...
	if (vm)
		vm->stop();
	zotrefs();
	world.killqueue.push_back(this);
}

void Agent::zotrefs() {
	while (self.next != &self)
		self.next->clear();
}

void Agent::setZOrder(unsigned int z) {
	zorder = z;
	world.agents.erase(zorder_iter);
	zorder_iter = world.agents.insert(this);
}

int Agent::getUNID() {
      if (unid != -1)
              return unid;
      return unid = world.getUNID(this);
}

std::string Agent::identify() const {
	std::ostringstream o;
	o << (int)family << " " << (int)genus << " " << species << " ";
	const std::string n = world.catalogue.getAgentName(family, genus, species);
	if (n.size())
		o << "(" + n + ")";
	if (unid != -1)
		o << " unid " << unid;
	else
		o << " (no unid assigned)";
	return o.str();
}

bool agentzorder::operator ()(const Agent *s1, const Agent *s2) const {
	return s1->zorder < s2->zorder;
}

<commit_msg>replace physics with a hack which works a little better<commit_after>/*
 *  Agent.cpp
 *  openc2e
 *
 *  Created by Alyssa Milburn on Tue May 25 2004.
 *  Copyright (c) 2004 Alyssa Milburn. All rights reserved.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2 of the License, or (at your option) any later version.
 *
 *  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.
 *
 */

#include "Agent.h"
#include "World.h"
#include "physics.h"
#include <iostream>
#include <sstream>
#include "caosVM.h"

Agent::Agent(unsigned char f, unsigned char g, unsigned short s, unsigned int p) :
  visible(true), family(f), genus(g), species(s), zorder(p), vm(0), timerrate(0) {
	velx.setFloat(0.0f);
	vely.setFloat(0.0f);
	accg = 0.3f;
	range = 500;
	sufferphysics = false;
	x = 0.0f; y = 0.0f;
	clac[0] = 1; // activate 1
	clik = -1;

	// AgentRef stuff
	self.ref = this;
	self.next = self.prev = &self;

	dying = false;
	unid = -1;

	zorder_iter = world.agents.insert(this);
}

Agent::~Agent() {
	world.agents.erase(zorder_iter);
	if (vm)
		world.freeVM(vm);
	zotrefs();
	if (unid != -1)
		world.freeUNID(unid);
}

void Agent::moveTo(float _x, float _y) {
	x = _x; y = _y;
}

void Agent::fireScript(unsigned short event) {
	if (dying) return;

	script &s = world.scriptorium.getScript(family, genus, species, event);
	if (s.lines.empty()) return;
	if (!vm) vm = world.getVM(this);
	if (vm->fireScript(s, (event == 9)))
		vm->setTarg(this);

	// This slows us down too much :)
#if 0
	std::cout << "Agent::fireScript fired " << (unsigned int)family << " " << (unsigned int)genus << " " << species << " ";
	const std::string n = world.catalogue.getAgentName(family, genus, species);
	if (n.size())
		std::cout << "(" << n << ") ";
	std::cout << (unsigned int)event << std::endl;
#endif
}

void Agent::tick() {
	if (dying) return;

	if (sufferphysics && accg) {
		float newvely = vely.floatValue + accg;
		float destx = x + velx.floatValue;
		float desty = y + newvely;
		//std::cout << x << ", " << y << ": " << destx << ", " << desty << "! " << accg << "\n";
		Room *r1 = world.map.roomAt((unsigned int)x, (unsigned int)y);
		if (!r1) {
			std::cout << "not doing physics on agent " << identify() << ", outside room system!\n";
		}
		Room *r2 = world.map.roomAt((unsigned int)destx + getWidth(), (unsigned int)desty + getHeight());
		Room *r3 = world.map.roomAt((unsigned int)destx, (unsigned int)desty);

		//if (r1 && (r1 == r2) && (r2 == r3)) {
			// If the object is already in a room, and the destination point is also in the room,
			// for now we don't bother checking. We *should*, because (crazily) it could take a
			// path out of the room and back in again, once we're doing more than dumb velocity.
		//	vely.setFloat(newvely);
		//	moveTo(destx, desty);
		/*} else */ if (r1) { // if we *are* actually in a room
			// Otherwise, check the motion pixel-by-pixel. Oh boy.

			bool moved = true;

			int ix = x, iy = y, idestx = destx, idesty = desty;

			if ((ix == idestx) && (iy != idesty)) { // vertical line
				int direction = (iy < idesty ? 1 : -1);

				while (iy != idesty) {
					iy += direction;

					Room *room1 = world.map.roomAt(ix, iy);
					Room *room2 = world.map.roomAt(ix + getWidth(), iy + getHeight());
					if ((!room1) || (!room2)) {
						iy -= direction;
						break;
					}
				}
			} else if ((iy == idesty) && (ix != idestx)) { // horizontal line
				int direction = (ix < idestx ? 1 : -1);

				while (ix != idestx) {
					ix += direction;

					Room *room1 = world.map.roomAt(ix, iy);
					Room *room2 = world.map.roomAt(ix + getWidth(), iy + getHeight());
					if ((!room1) || (!room2)) {
						ix -= direction;
						break;
					}
				}
			} else if ((iy != idesty) && (ix != idestx)) { // sloped line
				// TODO
				moved = false;
			} else moved = false; // else we ain't going anywhere

			if (moved) { // if we did actually try and go somewhere
				// TODO: this totally destroys floatness!
				x = ix;
				y = iy;
			}

			vely.setFloat(newvely);
		}
	} else {
		if (vely.hasDecimal())
			y = y + vely.getFloat();
		if (velx.hasDecimal())
			x = x + velx.getFloat();
	}

	if (timerrate) {
		tickssincelasttimer++;
		if (tickssincelasttimer == timerrate) {
			fireScript(9);
			tickssincelasttimer = 0;
		}
	}

	if (vm)
		vm->tick();
}

void Agent::kill() {
	assert(!dying);
	dying = true; // what a world, what a world...
	if (vm)
		vm->stop();
	zotrefs();
	world.killqueue.push_back(this);
}

void Agent::zotrefs() {
	while (self.next != &self)
		self.next->clear();
}

void Agent::setZOrder(unsigned int z) {
	zorder = z;
	world.agents.erase(zorder_iter);
	zorder_iter = world.agents.insert(this);
}

int Agent::getUNID() {
      if (unid != -1)
              return unid;
      return unid = world.getUNID(this);
}

std::string Agent::identify() const {
	std::ostringstream o;
	o << (int)family << " " << (int)genus << " " << species << " ";
	const std::string n = world.catalogue.getAgentName(family, genus, species);
	if (n.size())
		o << "(" + n + ")";
	if (unid != -1)
		o << " unid " << unid;
	else
		o << " (no unid assigned)";
	return o.str();
}

bool agentzorder::operator ()(const Agent *s1, const Agent *s2) const {
	return s1->zorder < s2->zorder;
}

<|endoftext|>


<commit_before>#define CATCH_CONFIG_MAIN  // This tells Catch to provide a main() - only do this in one cpp file
#include <catch.hpp>
#include "GmshReader.hpp"
#include "GmshReaderException.hpp"

TEST_CASE( "Check for exceptions", "[exceptions]" )
{
    SECTION( "Throw a GmshReaderException for invalid mesh files" )
    {
        using namespace gmsh;
        REQUIRE_THROWS_AS(gmsh::Reader reader("invalid_file_name",
                                              Reader::NodalOrdering::Global,
                                              Reader::IndexingBase::One),
                          GmshReaderException);
    }
}
<commit_msg>Fixed compile error with gcc-7<commit_after>#define CATCH_CONFIG_MAIN // This tells Catch to provide a main() - only do this in one
                          // cpp file
#include "GmshReader.hpp"
#include "GmshReaderException.hpp"
#include <catch.hpp>

using namespace gmsh;

TEST_CASE("Check for exceptions", "[exceptions]")
{
    SECTION("Throw a GmshReaderException for invalid mesh files")
    {
        REQUIRE_THROWS_AS(gmsh::Reader("invalid_file_name",
                                       Reader::NodalOrdering::Global,
                                       Reader::IndexingBase::One),
                          GmshReaderException);
    }
}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>

#include "modules/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const common::VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory, bool* is_replan) {
  *is_replan = true;
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t matched_index = prev_trajectory->QueryNearestPoint(veh_rel_time);

  if (matched_index == prev_trajectory_size) {
    AWARN << "The previous trajectory is not long enough, something is wrong";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "the previous trajectory doesn't cover current time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto matched_point = prev_trajectory->TrajectoryPointAt(matched_index);
  const double position_diff =
      std::hypot(matched_point.path_point().x() - vehicle_state.x(),
                 matched_point.path_point().y() - vehicle_state.y());

  if (position_diff > FLAGS_replan_distance_threshold) {
    AWARN << "the distance between matched point and actual position is too "
             "large";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time = veh_rel_time + planning_cycle_time;
  std::size_t forward_index =
      prev_trajectory->QueryNearestPoint(forward_rel_time);

  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() + matched_index,
      prev_trajectory->trajectory_points().begin() + forward_index + 1);

  const double zero_time = veh_rel_time;
  const double zero_s =
      prev_trajectory->TrajectoryPointAt(forward_index).path_point().s();

  for (auto& tp : stitching_trajectory) {
    tp.set_relative_time(tp.relative_time() - zero_time);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  *is_replan = false;
  return stitching_trajectory;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: Added position difference to record perfect and real planning error.<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>

#include "modules/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const common::VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory, bool* is_replan) {
  *is_replan = true;
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t matched_index = prev_trajectory->QueryNearestPoint(veh_rel_time);

  if (matched_index == prev_trajectory_size) {
    AWARN << "The previous trajectory is not long enough, something is wrong";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "the previous trajectory doesn't cover current time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto matched_point = prev_trajectory->TrajectoryPointAt(matched_index);
  const double position_diff =
      std::hypot(matched_point.path_point().x() - vehicle_state.x(),
                 matched_point.path_point().y() - vehicle_state.y());

  ADEBUG << "Control diff: " << position_diff;

  if (position_diff > FLAGS_replan_distance_threshold) {
    AWARN << "the distance between matched point and actual position is too "
             "large, position difference = "
          << position_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time = veh_rel_time + planning_cycle_time;
  std::size_t forward_index =
      prev_trajectory->QueryNearestPoint(forward_rel_time);

  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() + matched_index,
      prev_trajectory->trajectory_points().begin() + forward_index + 1);

  const double zero_time = veh_rel_time;
  const double zero_s =
      prev_trajectory->TrajectoryPointAt(forward_index).path_point().s();

  for (auto& tp : stitching_trajectory) {
    tp.set_relative_time(tp.relative_time() - zero_time);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  *is_replan = false;
  return stitching_trajectory;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>
#include <list>
#include <utility>

#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// only used in navigation mode
void TrajectoryStitcher::TransformLastPublishedTrajectory(const double x_diff,
    const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {

  if (!prev_trajectory) {
    return;
  }

  // R^-1
  auto cos_theta = std::cos(theta_diff);
  auto sin_theta = std::sin(-theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  auto trajectory_points = prev_trajectory->trajectory_points();
  std::for_each(trajectory_points.begin(), trajectory_points.end(),
      [&cos_theta, &sin_theta, &tx, &ty, &theta_diff]
       (common::TrajectoryPoint& p) {
        auto x = p.path_point().x();
        auto y = p.path_point().y();
        auto theta = p.path_point().theta();

        auto x_new = cos_theta * x - sin_theta * y + tx;
        auto y_new = sin_theta * x + cos_theta * y + ty;
        auto theta_new = common::math::WrapAngle(theta + theta_diff);

        p.mutable_path_point()->set_x(x_new);
        p.mutable_path_point()->set_y(y_new);
        p.mutable_path_point()->set_theta(theta_new);
      });
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory, bool* is_replan) {
  *is_replan = true;
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t matched_index = prev_trajectory->QueryNearestPoint(veh_rel_time);

  if (matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto matched_point = prev_trajectory->Evaluate(veh_rel_time);

  if (!matched_point.has_path_point()) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(), *prev_trajectory);

  auto lon_diff = std::fabs(frenet_sd.first);
  auto lat_diff = std::fabs(frenet_sd.second);

  ADEBUG << "Control lateral diff: " << lat_diff
         << ", longitudinal diff: " << lon_diff;

  if (lat_diff > FLAGS_replan_lateral_distance_threshold ||
      lon_diff > FLAGS_replan_longitudinal_distance_threshold) {
    AERROR << "the distance between matched point and actual position is too "
              "large. Replan is triggered. lat_diff = "
           << lat_diff << ", lon_diff = " << lon_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time =
      prev_trajectory->TrajectoryPointAt(matched_index).relative_time() +
      planning_cycle_time;

  std::size_t forward_index =
      prev_trajectory->QueryNearestPoint(forward_rel_time);

  ADEBUG << "matched_index: " << matched_index;
  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() +
          std::max(0, static_cast<int>(matched_index - 1)),
      prev_trajectory->trajectory_points().begin() + forward_index + 1);

  const double zero_s = matched_point.path_point().s();

  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      return ComputeReinitStitchingTrajectory(vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  *is_replan = false;
  return stitching_trajectory;
}

std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y,
    const PublishableTrajectory& prev_trajectory) {
  auto index = prev_trajectory.QueryNearestPoint({x, y});
  auto p = prev_trajectory.TrajectoryPointAt(index);

  common::math::Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  common::math::Vec2d n(std::cos(p.path_point().theta()),
      std::sin(p.path_point().theta()));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: minor bug fix for trajectory stitcher<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/common/trajectory/trajectory_stitcher.h"

#include <algorithm>
#include <list>
#include <utility>

#include "modules/common/configs/config_gflags.h"
#include "modules/common/log.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::common::util::DistanceXY;

std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(
    const VehicleState& vehicle_state) {
  TrajectoryPoint init_point;
  init_point.mutable_path_point()->set_x(vehicle_state.x());
  init_point.mutable_path_point()->set_y(vehicle_state.y());
  init_point.mutable_path_point()->set_z(vehicle_state.z());
  init_point.mutable_path_point()->set_theta(vehicle_state.heading());
  init_point.mutable_path_point()->set_kappa(vehicle_state.kappa());
  init_point.set_v(vehicle_state.linear_velocity());
  init_point.set_a(vehicle_state.linear_acceleration());
  init_point.set_relative_time(0.0);

  return std::vector<TrajectoryPoint>(1, init_point);
}

// only used in navigation mode
void TrajectoryStitcher::TransformLastPublishedTrajectory(const double x_diff,
    const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {

  if (!prev_trajectory) {
    return;
  }

  // R^-1
  auto cos_theta = std::cos(theta_diff);
  auto sin_theta = std::sin(-theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  auto trajectory_points = prev_trajectory->trajectory_points();
  std::for_each(trajectory_points.begin(), trajectory_points.end(),
      [&cos_theta, &sin_theta, &tx, &ty, &theta_diff]
       (common::TrajectoryPoint& p) {
        auto x = p.path_point().x();
        auto y = p.path_point().y();
        auto theta = p.path_point().theta();

        auto x_new = cos_theta * x - sin_theta * y + tx;
        auto y_new = sin_theta * x + cos_theta * y + ty;
        auto theta_new = common::math::WrapAngle(theta + theta_diff);

        p.mutable_path_point()->set_x(x_new);
        p.mutable_path_point()->set_y(y_new);
        p.mutable_path_point()->set_theta(theta_new);
      });
}

// Planning from current vehicle state:
// if 1. the auto-driving mode is off or
//    2. we don't have the trajectory from last planning cycle or
//    3. the position deviation from actual and target is too high
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const VehicleState& vehicle_state, const double current_timestamp,
    const double planning_cycle_time,
    const PublishableTrajectory* prev_trajectory, bool* is_replan) {
  *is_replan = true;
  if (!FLAGS_enable_trajectory_stitcher) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (!prev_trajectory) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  std::size_t prev_trajectory_size = prev_trajectory->NumOfPoints();

  if (prev_trajectory_size == 0) {
    ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()
           << "] size is zero! Previous planning not exist or failed. Use "
              "origin car status instead.";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  std::size_t matched_index = prev_trajectory->QueryNearestPoint(veh_rel_time);

  if (matched_index == 0 &&
      veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
    AWARN << "current time smaller than the previous trajectory's first time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }
  if (matched_index + 1 >= prev_trajectory_size) {
    AWARN << "current time beyond the previous trajectory's last time";
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto matched_point = prev_trajectory->Evaluate(veh_rel_time);

  if (!matched_point.has_path_point()) {
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  auto frenet_sd = ComputePositionProjection(
      vehicle_state.x(), vehicle_state.y(), *prev_trajectory);

  auto lon_diff = std::fabs(matched_point.path_point().s() - frenet_sd.first);
  auto lat_diff = std::fabs(frenet_sd.second);

  ADEBUG << "Control lateral diff: " << lat_diff
         << ", longitudinal diff: " << lon_diff;

  if (lat_diff > FLAGS_replan_lateral_distance_threshold ||
      lon_diff > FLAGS_replan_longitudinal_distance_threshold) {
    AERROR << "the distance between matched point and actual position is too "
              "large. Replan is triggered. lat_diff = "
           << lat_diff << ", lon_diff = " << lon_diff;
    return ComputeReinitStitchingTrajectory(vehicle_state);
  }

  double forward_rel_time =
      prev_trajectory->TrajectoryPointAt(matched_index).relative_time() +
      planning_cycle_time;

  std::size_t forward_index =
      prev_trajectory->QueryNearestPoint(forward_rel_time);

  ADEBUG << "matched_index: " << matched_index;
  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->trajectory_points().begin() +
          std::max(0, static_cast<int>(matched_index - 1)),
      prev_trajectory->trajectory_points().begin() + forward_index + 1);

  const double zero_s = matched_point.path_point().s();

  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      return ComputeReinitStitchingTrajectory(vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  *is_replan = false;
  return stitching_trajectory;
}

std::pair<double, double> TrajectoryStitcher::ComputePositionProjection(
    const double x, const double y,
    const PublishableTrajectory& prev_trajectory) {
  auto index = prev_trajectory.QueryNearestPoint({x, y});
  auto p = prev_trajectory.TrajectoryPointAt(index);

  common::math::Vec2d v(x - p.path_point().x(), y - p.path_point().y());
  common::math::Vec2d n(std::cos(p.path_point().theta()),
      std::sin(p.path_point().theta()));

  std::pair<double, double> frenet_sd;
  frenet_sd.first = v.InnerProd(n) + p.path_point().s();
  frenet_sd.second = v.CrossProd(n);
  return frenet_sd;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>#include "btest.h"
#include <iostream>

// TEST
template <typename Type>
TEST<Type>::TEST(Type check, Type expect)
	: checked(check), expected(expect) {
}

template <typename Type>
bool TEST<Type>::CHK_EQUAL() {
	if (checked == expected) { return true; }
	else { return false; }
}

// TEST_ENVIRON
template <typename Type>
TEST_ENVIRON<Type>::TEST_ENVIRON() {

	PRINT_PASS = true;
	PRINT_CHECK = true;
	ADD_TESTS = true;

}

template <typename Type>
bool TEST_ENVIRON<Type>::EQUALITY(Type test, Type expect) {

	TEST<Type> newtest(test, expect);

	if (PRINT_CHECK) { std::cout << "Expecting: " << expect << std::endl; 	std::cout << "Checking: " << test << std::endl;
	}

	if (newtest.CHK_EQUAL()) {
		if (PRINT_PASS) std::cout << "[PASS]" << std::endl;
		if (ADD_TESTS) tests.push_back(newtest);
		return true;

	}
	else {

		if (PRINT_PASS) std::cout << "[FAIL]" << std::endl;
		if (ADD_TESTS) tests.push_back(newtest);
		return false;

	}
}

<commit_msg>fuck i deleted BTest.cpp again oh nogit add BTest.cppgit add BTest.cppgit add BTest.cpp!<commit_after><|endoftext|>


<commit_before>#include <stdexcept>

#include "helpers.h"
#include "device-interface.h"
#include "input-driver-test.h"

#include <xorg/gtest/xorg-gtest.h>

#include <X11/extensions/XInput.h>
#include <X11/extensions/XInput2.h>

/**
 * A test fixture for testing the XInput 2.x extension.
 *
 * @tparam The XInput 2.x minor version
 */
class XInput2Test : public InputDriverTest,
                    public DeviceInterface,
                    public ::testing::WithParamInterface<int> {
protected:
    virtual void SetUp() {
        SetDevice("tablets/N-Trig-MultiTouch.desc");
        InputDriverTest::SetUp();
    }

    virtual void SetUpConfigAndLog(const std::string &param) {
        config.AddDefaultScreenWithDriver();
        config.SetAutoAddDevices(true);
        config.WriteConfig("/tmp/xi2-tests.conf");
        server.SetOption("-logfile", "/tmp/Xorg-xi2-tests.log");
        server.SetOption("-config", config.GetPath());
    }

    virtual int RegisterXI2(int major, int minor)
    {
        return InputDriverTest::RegisterXI2(2, GetParam());
    }
};

TEST_P(XInput2Test, XITouchscreenPointerEmulation)
{
    SCOPED_TRACE("\n"
                 "When an initial touch is made, any movement of the pointer\n"
                 "should be raised as if button 1 is being held. After the\n"
                 "touch is released, further movement should have button 1\n"
                 "released.\n");

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "N-Trig MultiTouch"));

    XIEventMask mask;
    mask.deviceid = XIAllMasterDevices;
    mask.mask_len = XIMaskLen(XI_HierarchyChanged);
    mask.mask = reinterpret_cast<unsigned char*>(calloc(mask.mask_len, 1));
    XISetMask(mask.mask, XI_ButtonPress);
    XISetMask(mask.mask, XI_ButtonRelease);
    XISetMask(mask.mask, XI_Motion);

    XISelectEvents(Display(), DefaultRootWindow(Display()), &mask, 1);

    free(mask.mask);

    XFlush(Display());

    std::auto_ptr<xorg::testing::evemu::Device> mouse_device;
    try {
      mouse_device = std::auto_ptr<xorg::testing::evemu::Device>(
          new xorg::testing::evemu::Device(
              RECORDINGS_DIR "mice/PIXART-USB-OPTICAL-MOUSE.desc")
          );
    } catch (std::runtime_error &error) {
      std::cerr << "Failed to create evemu device, skipping test.\n";
      return;
    }

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "PIXART USB OPTICAL MOUSE"));

    XEvent event;
    XGenericEventCookie *xcookie;
    XIDeviceEvent *device_event;

    /* Move the mouse, check that the button is not pressed. */
    mouse_device->PlayOne(EV_REL, ABS_X, -1, 1);
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_Motion));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));
    xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    device_event = reinterpret_cast<XIDeviceEvent*>(xcookie->data);
    ASSERT_FALSE(XIMaskIsSet(device_event->buttons.mask, 1));
    XFreeEventData(Display(), xcookie);

    /* Touch the screen, wait for press event */
    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_begin.events");
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_ButtonPress));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    /* Move the mouse again, button 1 should now be pressed. */
    mouse_device->PlayOne(EV_REL, ABS_X, -1, 1);
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_Motion));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));
    xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    device_event = reinterpret_cast<XIDeviceEvent*>(xcookie->data);
    ASSERT_TRUE(XIMaskIsSet(device_event->buttons.mask, 1));
    XFreeEventData(Display(), xcookie);

    /* Release the screen, wait for release event */
    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_end.events");
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_ButtonRelease));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    /* Move the mouse again, button 1 should now be released. */
    mouse_device->PlayOne(EV_REL, ABS_X, -1, 1);
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_Motion));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));
    xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    device_event = reinterpret_cast<XIDeviceEvent*>(xcookie->data);
    ASSERT_FALSE(XIMaskIsSet(device_event->buttons.mask, 1));
    XFreeEventData(Display(), xcookie);
}

TEST_P(XInput2Test, XIQueryPointerTouchscreen)
{
    SCOPED_TRACE("\n"
                 "XIQueryPointer for XInput 2.1 and earlier should report the\n"
                 "first button pressed if a touch is physically active. For \n"
                 "XInput 2.2 and later clients, the first button should not be\n"
                 "reported.");
    XIEventMask mask;
    mask.deviceid = XIAllDevices;
    mask.mask_len = XIMaskLen(XI_HierarchyChanged);
    mask.mask = reinterpret_cast<unsigned char*>(calloc(mask.mask_len, 1));
    XISetMask(mask.mask, XI_HierarchyChanged);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    mask.deviceid = XIAllMasterDevices;
    XIClearMask(mask.mask, XI_HierarchyChanged);
    XISetMask(mask.mask, XI_ButtonPress);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    free(mask.mask);

    XFlush(Display());

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "N-Trig MultiTouch"));

    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_begin.events");

    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_ButtonPress));

    XEvent event;
    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    XGenericEventCookie *xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    XIDeviceEvent *device_event =
        reinterpret_cast<XIDeviceEvent*>(xcookie->data);

    Window root;
    Window child;
    double root_x;
    double root_y;
    double win_x;
    double win_y;
    XIButtonState buttons;
    XIModifierState modifiers;
    XIGroupState group;
    ASSERT_TRUE(XIQueryPointer(Display(), device_event->deviceid,
                               DefaultRootWindow(Display()), &root, &child,
                               &root_x, &root_y, &win_x, &win_y, &buttons,
                               &modifiers, &group));

    /* Test if button 1 is pressed */
    ASSERT_GE(buttons.mask_len, XIMaskLen(2));
    if (GetParam() < 2)
        EXPECT_TRUE(XIMaskIsSet(buttons.mask, 1));
    else
        EXPECT_FALSE(XIMaskIsSet(buttons.mask, 1));

    XFreeEventData(Display(), xcookie);
}

#ifdef HAVE_XI22
TEST_P(XInput2Test, DisableDeviceEndTouches)
{
    SCOPED_TRACE("When a device is disabled, any physically active touches\n"
                 "should end.");
    /* This is an XInput 2.2 and later test only */
    if (GetParam() < 2)
        return;

    XIEventMask mask;
    mask.deviceid = XIAllDevices;
    mask.mask_len = XIMaskLen(XI_TouchEnd);
    mask.mask = reinterpret_cast<unsigned char*>(calloc(mask.mask_len, 1));
    XISetMask(mask.mask, XI_HierarchyChanged);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    mask.deviceid = XIAllMasterDevices;
    XIClearMask(mask.mask, XI_HierarchyChanged);
    XISetMask(mask.mask, XI_TouchBegin);
    XISetMask(mask.mask, XI_TouchUpdate);
    XISetMask(mask.mask, XI_TouchEnd);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    free(mask.mask);

    XFlush(Display());

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "N-Trig MultiTouch"));

    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_begin.events");

    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_TouchBegin));

    XEvent event;
    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    XGenericEventCookie *xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    XIDeviceEvent *device_event =
        reinterpret_cast<XIDeviceEvent*>(xcookie->data);

    XDevice *xdevice = XOpenDevice(Display(), device_event->sourceid);
    XFreeEventData(Display(), xcookie);
    ASSERT_TRUE(xdevice != NULL);

    XDeviceEnableControl enable_control;
    enable_control.enable = false;
    XDeviceControl *control =
        reinterpret_cast<XDeviceControl*>(&enable_control);
    ASSERT_TRUE(XChangeDeviceControl(Display(), xdevice, DEVICE_ENABLE,
                                     control));
    XCloseDevice(Display(), xdevice);
    XFlush(Display());

    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_TouchEnd));
}

/**
 * Test class for testing
 * @tparam The device ID
 */
class XInput2TouchSelectionTest : public XInput2Test
{
protected:
    virtual int RegisterXI2(int major, int minor)
    {
        return InputDriverTest::RegisterXI2(2, 2);
    }
};

static bool error_handler_triggered;

/* ASSERT_*() macros can only be used in void functions, hence the
   indirection here */
static void _error_handler(XErrorEvent *ev) {
    ASSERT_EQ((int)ev->error_code, BadAccess);
    error_handler_triggered = true;
}
static int error_handler(Display *dpy, XErrorEvent *ev) {
    _error_handler(ev);
    return 0;
}
static void _fail_error_handler(XErrorEvent *ev) {
    FAIL() << "X protocol error: errorcode: " << (int)ev->error_code <<
    " on request " << (int)ev->request_code << " minor " << (int)ev->minor_code;
}
static int fail_error_handler(Display *dpy, XErrorEvent *ev) {
    _fail_error_handler(ev);
    return 0;
}

TEST_P(XInput2TouchSelectionTest, TouchSelectionConflicts)
{
    SCOPED_TRACE("If client A has a selection on a device,\n"
                 "client B selecting on the same device returns BadAccess.\n"
                 "If client A has a selection on XIAll(Master)Devices, \n"
                 "selecting on the same or a specific device returns"
                 "BadAccess\n");

    unsigned char m[XIMaskLen(XI_TouchEnd)] = {0};
    XIEventMask mask;
    mask.mask = m;
    mask.mask_len = sizeof(m);

    XISetMask(mask.mask, XI_TouchBegin);
    XISetMask(mask.mask, XI_TouchUpdate);
    XISetMask(mask.mask, XI_TouchEnd);

    int clientA_deviceid = GetParam();

    /* client A */
    mask.deviceid = clientA_deviceid;
    XSetErrorHandler(fail_error_handler);
    ::Display *dpy2 = XOpenDisplay(server.GetDisplayString().c_str());
    ASSERT_TRUE(dpy2);

    XISelectEvents(dpy2, DefaultRootWindow(dpy2), &mask, 1);
    XSync(dpy2, False);

    XSetErrorHandler(error_handler);

    /* covers XIAllDevices, XIAllMasterDevices and VCP */
    for (int clientB_deviceid = 0; clientB_deviceid < 3; clientB_deviceid++) {
        error_handler_triggered = false;
        mask.deviceid = clientB_deviceid;
        XISelectEvents(Display(), DefaultRootWindow(Display()), &mask, 1);
        XSync(Display(), False);
        ASSERT_EQ(error_handler_triggered,
                  (clientA_deviceid <= clientB_deviceid))
                  << "failed for " << clientA_deviceid << "/" << clientB_deviceid;
    }
}
INSTANTIATE_TEST_CASE_P(, XInput2TouchSelectionTest, ::testing::Range(0, 3));

#endif

INSTANTIATE_TEST_CASE_P(, XInput2Test, ::testing::Range(0, 3));

int main(int argc, char** argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}
<commit_msg>server/xi2: XChangeDeviceControl returns a Status, not a boolean<commit_after>#include <stdexcept>

#include "helpers.h"
#include "device-interface.h"
#include "input-driver-test.h"

#include <xorg/gtest/xorg-gtest.h>

#include <X11/extensions/XInput.h>
#include <X11/extensions/XInput2.h>

/**
 * A test fixture for testing the XInput 2.x extension.
 *
 * @tparam The XInput 2.x minor version
 */
class XInput2Test : public InputDriverTest,
                    public DeviceInterface,
                    public ::testing::WithParamInterface<int> {
protected:
    virtual void SetUp() {
        SetDevice("tablets/N-Trig-MultiTouch.desc");
        InputDriverTest::SetUp();
    }

    virtual void SetUpConfigAndLog(const std::string &param) {
        config.AddDefaultScreenWithDriver();
        config.SetAutoAddDevices(true);
        config.WriteConfig("/tmp/xi2-tests.conf");
        server.SetOption("-logfile", "/tmp/Xorg-xi2-tests.log");
        server.SetOption("-config", config.GetPath());
    }

    virtual int RegisterXI2(int major, int minor)
    {
        return InputDriverTest::RegisterXI2(2, GetParam());
    }
};

TEST_P(XInput2Test, XITouchscreenPointerEmulation)
{
    SCOPED_TRACE("\n"
                 "When an initial touch is made, any movement of the pointer\n"
                 "should be raised as if button 1 is being held. After the\n"
                 "touch is released, further movement should have button 1\n"
                 "released.\n");

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "N-Trig MultiTouch"));

    XIEventMask mask;
    mask.deviceid = XIAllMasterDevices;
    mask.mask_len = XIMaskLen(XI_HierarchyChanged);
    mask.mask = reinterpret_cast<unsigned char*>(calloc(mask.mask_len, 1));
    XISetMask(mask.mask, XI_ButtonPress);
    XISetMask(mask.mask, XI_ButtonRelease);
    XISetMask(mask.mask, XI_Motion);

    XISelectEvents(Display(), DefaultRootWindow(Display()), &mask, 1);

    free(mask.mask);

    XFlush(Display());

    std::auto_ptr<xorg::testing::evemu::Device> mouse_device;
    try {
      mouse_device = std::auto_ptr<xorg::testing::evemu::Device>(
          new xorg::testing::evemu::Device(
              RECORDINGS_DIR "mice/PIXART-USB-OPTICAL-MOUSE.desc")
          );
    } catch (std::runtime_error &error) {
      std::cerr << "Failed to create evemu device, skipping test.\n";
      return;
    }

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "PIXART USB OPTICAL MOUSE"));

    XEvent event;
    XGenericEventCookie *xcookie;
    XIDeviceEvent *device_event;

    /* Move the mouse, check that the button is not pressed. */
    mouse_device->PlayOne(EV_REL, ABS_X, -1, 1);
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_Motion));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));
    xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    device_event = reinterpret_cast<XIDeviceEvent*>(xcookie->data);
    ASSERT_FALSE(XIMaskIsSet(device_event->buttons.mask, 1));
    XFreeEventData(Display(), xcookie);

    /* Touch the screen, wait for press event */
    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_begin.events");
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_ButtonPress));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    /* Move the mouse again, button 1 should now be pressed. */
    mouse_device->PlayOne(EV_REL, ABS_X, -1, 1);
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_Motion));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));
    xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    device_event = reinterpret_cast<XIDeviceEvent*>(xcookie->data);
    ASSERT_TRUE(XIMaskIsSet(device_event->buttons.mask, 1));
    XFreeEventData(Display(), xcookie);

    /* Release the screen, wait for release event */
    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_end.events");
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_ButtonRelease));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    /* Move the mouse again, button 1 should now be released. */
    mouse_device->PlayOne(EV_REL, ABS_X, -1, 1);
    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_Motion));

    ASSERT_EQ(Success, XNextEvent(Display(), &event));
    xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    device_event = reinterpret_cast<XIDeviceEvent*>(xcookie->data);
    ASSERT_FALSE(XIMaskIsSet(device_event->buttons.mask, 1));
    XFreeEventData(Display(), xcookie);
}

TEST_P(XInput2Test, XIQueryPointerTouchscreen)
{
    SCOPED_TRACE("\n"
                 "XIQueryPointer for XInput 2.1 and earlier should report the\n"
                 "first button pressed if a touch is physically active. For \n"
                 "XInput 2.2 and later clients, the first button should not be\n"
                 "reported.");
    XIEventMask mask;
    mask.deviceid = XIAllDevices;
    mask.mask_len = XIMaskLen(XI_HierarchyChanged);
    mask.mask = reinterpret_cast<unsigned char*>(calloc(mask.mask_len, 1));
    XISetMask(mask.mask, XI_HierarchyChanged);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    mask.deviceid = XIAllMasterDevices;
    XIClearMask(mask.mask, XI_HierarchyChanged);
    XISetMask(mask.mask, XI_ButtonPress);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    free(mask.mask);

    XFlush(Display());

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "N-Trig MultiTouch"));

    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_begin.events");

    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_ButtonPress));

    XEvent event;
    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    XGenericEventCookie *xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    XIDeviceEvent *device_event =
        reinterpret_cast<XIDeviceEvent*>(xcookie->data);

    Window root;
    Window child;
    double root_x;
    double root_y;
    double win_x;
    double win_y;
    XIButtonState buttons;
    XIModifierState modifiers;
    XIGroupState group;
    ASSERT_TRUE(XIQueryPointer(Display(), device_event->deviceid,
                               DefaultRootWindow(Display()), &root, &child,
                               &root_x, &root_y, &win_x, &win_y, &buttons,
                               &modifiers, &group));

    /* Test if button 1 is pressed */
    ASSERT_GE(buttons.mask_len, XIMaskLen(2));
    if (GetParam() < 2)
        EXPECT_TRUE(XIMaskIsSet(buttons.mask, 1));
    else
        EXPECT_FALSE(XIMaskIsSet(buttons.mask, 1));

    XFreeEventData(Display(), xcookie);
}

#ifdef HAVE_XI22
TEST_P(XInput2Test, DisableDeviceEndTouches)
{
    SCOPED_TRACE("When a device is disabled, any physically active touches\n"
                 "should end.");
    /* This is an XInput 2.2 and later test only */
    if (GetParam() < 2)
        return;

    XIEventMask mask;
    mask.deviceid = XIAllDevices;
    mask.mask_len = XIMaskLen(XI_TouchEnd);
    mask.mask = reinterpret_cast<unsigned char*>(calloc(mask.mask_len, 1));
    XISetMask(mask.mask, XI_HierarchyChanged);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    mask.deviceid = XIAllMasterDevices;
    XIClearMask(mask.mask, XI_HierarchyChanged);
    XISetMask(mask.mask, XI_TouchBegin);
    XISetMask(mask.mask, XI_TouchUpdate);
    XISetMask(mask.mask, XI_TouchEnd);

    ASSERT_EQ(Success,
              XISelectEvents(Display(), DefaultRootWindow(Display()), &mask,
                             1));

    free(mask.mask);

    XFlush(Display());

    ASSERT_TRUE(xorg::testing::XServer::WaitForDevice(Display(), "N-Trig MultiTouch"));

    dev->Play(RECORDINGS_DIR "tablets/N-Trig-MultiTouch.touch_1_begin.events");

    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_TouchBegin));

    XEvent event;
    ASSERT_EQ(Success, XNextEvent(Display(), &event));

    XGenericEventCookie *xcookie = &event.xcookie;
    ASSERT_TRUE(XGetEventData(Display(), xcookie));

    XIDeviceEvent *device_event =
        reinterpret_cast<XIDeviceEvent*>(xcookie->data);

    XDevice *xdevice = XOpenDevice(Display(), device_event->sourceid);
    XFreeEventData(Display(), xcookie);
    ASSERT_TRUE(xdevice != NULL);

    XDeviceEnableControl enable_control;
    enable_control.enable = false;
    XDeviceControl *control =
        reinterpret_cast<XDeviceControl*>(&enable_control);
    ASSERT_EQ(XChangeDeviceControl(Display(), xdevice, DEVICE_ENABLE, control),
              Success);
    XCloseDevice(Display(), xdevice);
    XFlush(Display());

    ASSERT_TRUE(xorg::testing::XServer::WaitForEventOfType(Display(),
                                                           GenericEvent,
                                                           xi2_opcode,
                                                           XI_TouchEnd));
}

/**
 * Test class for testing
 * @tparam The device ID
 */
class XInput2TouchSelectionTest : public XInput2Test
{
protected:
    virtual int RegisterXI2(int major, int minor)
    {
        return InputDriverTest::RegisterXI2(2, 2);
    }
};

static bool error_handler_triggered;

/* ASSERT_*() macros can only be used in void functions, hence the
   indirection here */
static void _error_handler(XErrorEvent *ev) {
    ASSERT_EQ((int)ev->error_code, BadAccess);
    error_handler_triggered = true;
}
static int error_handler(Display *dpy, XErrorEvent *ev) {
    _error_handler(ev);
    return 0;
}
static void _fail_error_handler(XErrorEvent *ev) {
    FAIL() << "X protocol error: errorcode: " << (int)ev->error_code <<
    " on request " << (int)ev->request_code << " minor " << (int)ev->minor_code;
}
static int fail_error_handler(Display *dpy, XErrorEvent *ev) {
    _fail_error_handler(ev);
    return 0;
}

TEST_P(XInput2TouchSelectionTest, TouchSelectionConflicts)
{
    SCOPED_TRACE("If client A has a selection on a device,\n"
                 "client B selecting on the same device returns BadAccess.\n"
                 "If client A has a selection on XIAll(Master)Devices, \n"
                 "selecting on the same or a specific device returns"
                 "BadAccess\n");

    unsigned char m[XIMaskLen(XI_TouchEnd)] = {0};
    XIEventMask mask;
    mask.mask = m;
    mask.mask_len = sizeof(m);

    XISetMask(mask.mask, XI_TouchBegin);
    XISetMask(mask.mask, XI_TouchUpdate);
    XISetMask(mask.mask, XI_TouchEnd);

    int clientA_deviceid = GetParam();

    /* client A */
    mask.deviceid = clientA_deviceid;
    XSetErrorHandler(fail_error_handler);
    ::Display *dpy2 = XOpenDisplay(server.GetDisplayString().c_str());
    ASSERT_TRUE(dpy2);

    XISelectEvents(dpy2, DefaultRootWindow(dpy2), &mask, 1);
    XSync(dpy2, False);

    XSetErrorHandler(error_handler);

    /* covers XIAllDevices, XIAllMasterDevices and VCP */
    for (int clientB_deviceid = 0; clientB_deviceid < 3; clientB_deviceid++) {
        error_handler_triggered = false;
        mask.deviceid = clientB_deviceid;
        XISelectEvents(Display(), DefaultRootWindow(Display()), &mask, 1);
        XSync(Display(), False);
        ASSERT_EQ(error_handler_triggered,
                  (clientA_deviceid <= clientB_deviceid))
                  << "failed for " << clientA_deviceid << "/" << clientB_deviceid;
    }
}
INSTANTIATE_TEST_CASE_P(, XInput2TouchSelectionTest, ::testing::Range(0, 3));

#endif

INSTANTIATE_TEST_CASE_P(, XInput2Test, ::testing::Range(0, 3));

int main(int argc, char** argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}
<|endoftext|>