<commit_before>#include "pointers.h"
#include <boost/python.hpp>
#include <dart/dynamics/dynamics.h>
#include "util.h"
#include "types.h"

using ::boost::python::object;
using ::dart::dynamics::Joint;

static void Joint_setName_default(Joint *joint, std::string const &name)
{
    joint->setName(name);
}

static std::string FreeJoint_test(dart::dynamics::FreeJoint *self)
{
    return "banana";
}

static object Joint_getattribute(object self, object name)
{
    using ::boost::python::eval;
    using ::boost::python::import;

    object dartpy = import("dartpy");
    object Joint_class = dartpy.attr("Joint");
    object FreeJoint_class = dartpy.attr("FreeJoint");

    self.attr("__class__") = FreeJoint_class;

    return eval("object").attr("__getattribute__")(self, name);
}

static void python_Joint_base()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::BodyNode;
    using dart::dynamics::BodyNodePtr;
    using dart::dynamics::DegreeOfFreedom;
    using dart::dynamics::DegreeOfFreedomPtr;
    using dart::dynamics::JointPtr;
    using dart::dynamics::SkeletonPtr;

    typedef return_by_smart_ptr<BodyNodePtr> return_BodyNodePtr;
    typedef return_by_smart_ptr<DegreeOfFreedomPtr> return_DegreeOfFredomPtr;

    scope joint_class(
        class_<Joint, JointPtr, noncopyable>("Joint", no_init)
            .add_property("name",
                make_function(&Joint::getName,
                              return_value_policy<copy_const_reference>()),
                &Joint_setName_default)
            .add_property("actuator_type",
                &Joint::getActuatorType,
                &Joint::setActuatorType)
            .add_property("properties",
                make_function(&Joint::getJointProperties,
                              return_value_policy<copy_const_reference>()),
                &Joint::setProperties)
            .add_property("is_kinematic", &Joint::isKinematic)
            .add_property("is_dynamic", &Joint::isDynamic)
            .add_property("position_limited",
                &Joint::isPositionLimited, &Joint::setPositionLimited)
            .add_property("skeleton",
                static_cast<SkeletonPtr (Joint::*)()>(&Joint::getSkeleton))
            .add_property("child_body_node", make_function(
                static_cast<BodyNode *(Joint::*)()>(&Joint::getChildBodyNode),
                return_value_policy<return_BodyNodePtr>()))
            .add_property("parent_body_node", make_function(
                static_cast<BodyNode *(Joint::*)()>(&Joint::getParentBodyNode),
                return_value_policy<return_BodyNodePtr>()))
            .add_property("transform_from_parent_body_node",
                make_function(&Joint::getTransformFromParentBodyNode,
                              return_value_policy<copy_const_reference>()),
                &Joint::setTransformFromParentBodyNode)
            .add_property("transform_from_child_body_node",
                make_function(&Joint::getTransformFromChildBodyNode,
                              return_value_policy<copy_const_reference>()),
                &Joint::setTransformFromChildBodyNode)
            .add_property("index_in_skeleton", &Joint::getIndexInSkeleton)
            .add_property("index_in_tree", &Joint::getIndexInTree)
            .add_property("joint_index_in_skeleton", &Joint::getJointIndexInSkeleton)
            .add_property("joint_index_in_tree", &Joint::getJointIndexInTree)
            .add_property("tree_index", &Joint::getTreeIndex)
            .add_property("num_dofs", &Joint::getNumDofs)
            .add_property("commands", &Joint::getCommands, &Joint::setCommands)
            .add_property("commands", &Joint::getPositions, &Joint::setPositions)
            .def("__getattribute__", &Joint_getattribute)
            .def("set_name",
                make_function(&Joint::setName,
                              return_value_policy<copy_const_reference>()))
            .def("get_dof",
                static_cast<DegreeOfFreedom *(Joint::*)(size_t)>(&Joint::getDof),
                return_value_policy<return_DegreeOfFredomPtr>())
            //.def("set_dof_names", &Joint::setDofName)
            //.def("get_dof_name", &Joint::isDofNamePreserved)
            .def("preserve_dof_names", &Joint::preserveDofName)
            .def("is_dof_name_preserved", &Joint::isDofNamePreserved)
            .def("copy", static_cast<void (Joint::*)(Joint const *)>(&Joint::copy))
            .def("get_command", &Joint::getCommand)
            .def("set_command", &Joint::setCommand)
            .def("reset_commands", &Joint::resetCommands)
            .def("set_position", &Joint::setPosition)
            .def("get_position", &Joint::getPosition)
            .def("reset_positions", &Joint::resetPositions)
            .def("get_position_lower_limit", &Joint::getPositionLowerLimit)
            .def("set_position_lower_limit", &Joint::setPositionLowerLimit)
            .def("get_position_upper_limit", &Joint::getPositionUpperLimit)
            .def("set_position_upper_limit", &Joint::setPositionUpperLimit)
    );

    class_<Joint::Properties>("Properties")
        .def_readwrite("name", &Joint::Properties::mName)
        .def_readwrite("T_parent_body_to_joint",
            &Joint::Properties::mT_ParentBodyToJoint)
        .def_readwrite("T_child_body_to_joint",
            &Joint::Properties::mT_ChildBodyToJoint)
        .def_readwrite("is_position_limited",
            &Joint::Properties::mIsPositionLimited)
        .def_readwrite("actuator_type",
            &Joint::Properties::mActuatorType)
        ;
}

static void python_ZeroDofJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::ZeroDofJoint;

    scope joint_class(
        class_<ZeroDofJoint, bases<Joint>,
               noncopyable>("ZeroDofJoint", no_init)

    );

    class_<ZeroDofJoint::Properties>("Properties");
}

static void python_FreeJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::FreeJoint;

    scope joint_class(
        class_<FreeJoint, bases<Joint>,
               noncopyable>("FreeJoint", no_init)
            .def("test", &FreeJoint_test)
    );
}

static void python_SingleDofJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::SingleDofJoint;

    scope joint_class(
        class_<SingleDofJoint, noncopyable>("SingleDofJoint", no_init)
    );

    class_<SingleDofJoint::UniqueProperties>("UniqueProperties")
        .def_readwrite("position_lower_limit",
            &SingleDofJoint::UniqueProperties::mPositionLowerLimit)
        .def_readwrite("position_upper_limit",
            &SingleDofJoint::UniqueProperties::mPositionUpperLimit)
        .def_readwrite("velocity_lower_limit",
            &SingleDofJoint::UniqueProperties::mVelocityLowerLimit)
        .def_readwrite("velocity_upper_limit",
            &SingleDofJoint::UniqueProperties::mVelocityUpperLimit)
        .def_readwrite("acceleration_lower_limit",
            &SingleDofJoint::UniqueProperties::mAccelerationLowerLimit)
        .def_readwrite("acceleration_upper_limit",
            &SingleDofJoint::UniqueProperties::mAccelerationUpperLimit)
        .def_readwrite("force_lower_limit",
            &SingleDofJoint::UniqueProperties::mForceLowerLimit)
        .def_readwrite("force_upper_limit",
            &SingleDofJoint::UniqueProperties::mForceUpperLimit)
        .def_readwrite("spring_stiffness",
            &SingleDofJoint::UniqueProperties::mSpringStiffness)
        .def_readwrite("rest_position",
            &SingleDofJoint::UniqueProperties::mRestPosition)
        .def_readwrite("damping_coefficient",
            &SingleDofJoint::UniqueProperties::mDampingCoefficient)
        .def_readwrite("friction",
            &SingleDofJoint::UniqueProperties::mFriction)
        .def_readwrite("preserve_dof_name",
            &SingleDofJoint::UniqueProperties::mPreserveDofName)
        .def_readwrite("dof_name",
            &SingleDofJoint::UniqueProperties::mDofName)
        ;

    class_<SingleDofJoint::Properties, bases<
                Joint::Properties, SingleDofJoint::UniqueProperties>
            >("Properties")
        ;
}

static void python_RevoluteJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::RevoluteJoint;
    using dart::dynamics::SingleDofJoint;

    scope joint_class(
        class_<RevoluteJoint, noncopyable>("RevoluteJoint", no_init)
    );

    class_<RevoluteJoint::UniqueProperties>("UniqueProperties")
        .def_readwrite("axis", &RevoluteJoint::UniqueProperties::mAxis)
        ;

    class_<RevoluteJoint::Properties, bases<
            SingleDofJoint::Properties, RevoluteJoint::UniqueProperties>
          >("Properties")
        ;
}

static void python_WeldJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::WeldJoint;
    using dart::dynamics::ZeroDofJoint;

    scope joint_class(
        class_<WeldJoint, noncopyable>("WeldJoint", no_init)
    );

    class_<WeldJoint::Properties,
           bases<ZeroDofJoint::Properties> >("Properties")
        ;
}

void python_Joint()
{
    using namespace ::boost::python;

    using ::dart::python::JointType;

    typedef ::dart::dynamics::Joint::ActuatorType ActuatorType;

    enum_<ActuatorType>("ActuatorType")
        .value("FORCE", Joint::FORCE)
        .value("PASSIVE", Joint::PASSIVE)
        .value("SERVO", Joint::SERVO)
        .value("ACCELERATION", Joint::ACCELERATION)
        .value("VELOCITY", Joint::VELOCITY)
        .value("LOCKED", Joint::LOCKED)
        ;

    enum_<JointType::Enum>("JointType")
        .value("PRISMATIC", JointType::PRISMATIC)
        .value("REVOLUTE", JointType::REVOLUTE)
        .value("SCREW", JointType::SCREW)
        .value("WELD", JointType::WELD)
        .value("UNIVERSAL", JointType::UNIVERSAL)
        .value("BALL", JointType::BALL)
        .value("EULER", JointType::EULER)
        .value("PLANAR", JointType::PLANAR)
        .value("TRANSLATIONAL", JointType::TRANSLATIONAL)
        .value("FREE", JointType::FREE)
        ;

    python_Joint_base();
    python_ZeroDofJoint();
    python_SingleDofJoint();
    python_RevoluteJoint();
    python_FreeJoint();
    python_WeldJoint();
}
<commit_msg>Joint pointers are automatically resolved.<commit_after>#include <unordered_map>
#include "pointers.h"
#include <boost/python.hpp>
#include <dart/dynamics/dynamics.h>
#include "util.h"
#include "types.h"

using ::boost::python::object;
using ::dart::dynamics::Joint;

namespace dart {
namespace python {

class JointMap {
public:
    void addOverride(std::string const &jointType,
                     std::string const &className)
    {
        m_jointTypeOverrides[jointType] = className;
    }

    template <class JointT>
    std::string const &getPythonName() const
    {
        return getPythonName(JointT::getStaticType());
    }

    /// Maps from Joint::getType() to the Python class name.
    std::string const &getPythonName(std::string const &jointType) const
    {
        auto const it = m_jointTypeOverrides.find(jointType);

        if (it != std::end(m_jointTypeOverrides)) {
            return it->second;
        } else {
            return jointType;
        }
    }

    static JointMap &instance()
    {
        if (!m_instance) {
            m_instance.reset(new JointMap);
        }
        return *m_instance;
    }

private:
    static std::shared_ptr<JointMap> m_instance;

    std::unordered_map<std::string, std::string> m_jointTypeOverrides;

    JointMap() = default;
    JointMap(JointMap const &) = delete;
    JointMap &operator=(JointMap const &) = delete;
};

std::shared_ptr<JointMap> JointMap::m_instance;

static void Joint_setName_default(Joint *joint, std::string const &name)
{
    joint->setName(name);
}

static std::string FreeJoint_test(dart::dynamics::FreeJoint *self)
{
    return "banana";
}

static object Joint_getattribute(object self, object name)
{
    using ::boost::python::eval;
    using ::boost::python::import;

    return eval("object").attr("__getattribute__")(self, name);
}

static void python_Joint_base()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::BodyNode;
    using dart::dynamics::BodyNodePtr;
    using dart::dynamics::DegreeOfFreedom;
    using dart::dynamics::DegreeOfFreedomPtr;
    using dart::dynamics::JointPtr;
    using dart::dynamics::SkeletonPtr;
    using dart::python::JointMap;

    typedef return_by_smart_ptr<BodyNodePtr> return_BodyNodePtr;
    typedef return_by_smart_ptr<DegreeOfFreedomPtr> return_DegreeOfFredomPtr;

    scope joint_class(
        class_<Joint, JointPtr, noncopyable>(
                JointMap::instance().getPythonName<Joint>().c_str(),
                no_init)
            .add_property("name",
                make_function(&Joint::getName,
                              return_value_policy<copy_const_reference>()),
                &Joint_setName_default)
            .add_property("actuator_type",
                &Joint::getActuatorType,
                &Joint::setActuatorType)
            .add_property("properties",
                make_function(&Joint::getJointProperties,
                              return_value_policy<copy_const_reference>()),
                &Joint::setProperties)
            .add_property("is_kinematic", &Joint::isKinematic)
            .add_property("is_dynamic", &Joint::isDynamic)
            .add_property("position_limited",
                &Joint::isPositionLimited, &Joint::setPositionLimited)
            .add_property("skeleton",
                static_cast<SkeletonPtr (Joint::*)()>(&Joint::getSkeleton))
            .add_property("child_body_node", make_function(
                static_cast<BodyNode *(Joint::*)()>(&Joint::getChildBodyNode),
                return_value_policy<return_BodyNodePtr>()))
            .add_property("parent_body_node", make_function(
                static_cast<BodyNode *(Joint::*)()>(&Joint::getParentBodyNode),
                return_value_policy<return_BodyNodePtr>()))
            .add_property("transform_from_parent_body_node",
                make_function(&Joint::getTransformFromParentBodyNode,
                              return_value_policy<copy_const_reference>()),
                &Joint::setTransformFromParentBodyNode)
            .add_property("transform_from_child_body_node",
                make_function(&Joint::getTransformFromChildBodyNode,
                              return_value_policy<copy_const_reference>()),
                &Joint::setTransformFromChildBodyNode)
            .add_property("index_in_skeleton", &Joint::getIndexInSkeleton)
            .add_property("index_in_tree", &Joint::getIndexInTree)
            .add_property("joint_index_in_skeleton", &Joint::getJointIndexInSkeleton)
            .add_property("joint_index_in_tree", &Joint::getJointIndexInTree)
            .add_property("tree_index", &Joint::getTreeIndex)
            .add_property("num_dofs", &Joint::getNumDofs)
            .add_property("commands", &Joint::getCommands, &Joint::setCommands)
            .add_property("commands", &Joint::getPositions, &Joint::setPositions)
            .def("__getattribute__", &Joint_getattribute)
            .def("set_name",
                make_function(&Joint::setName,
                              return_value_policy<copy_const_reference>()))
            .def("get_dof",
                static_cast<DegreeOfFreedom *(Joint::*)(size_t)>(&Joint::getDof),
                return_value_policy<return_DegreeOfFredomPtr>())
            //.def("set_dof_names", &Joint::setDofName)
            //.def("get_dof_name", &Joint::isDofNamePreserved)
            .def("preserve_dof_names", &Joint::preserveDofName)
            .def("is_dof_name_preserved", &Joint::isDofNamePreserved)
            .def("copy", static_cast<void (Joint::*)(Joint const *)>(&Joint::copy))
            .def("get_command", &Joint::getCommand)
            .def("set_command", &Joint::setCommand)
            .def("reset_commands", &Joint::resetCommands)
            .def("set_position", &Joint::setPosition)
            .def("get_position", &Joint::getPosition)
            .def("reset_positions", &Joint::resetPositions)
            .def("get_position_lower_limit", &Joint::getPositionLowerLimit)
            .def("set_position_lower_limit", &Joint::setPositionLowerLimit)
            .def("get_position_upper_limit", &Joint::getPositionUpperLimit)
            .def("set_position_upper_limit", &Joint::setPositionUpperLimit)
    );

    class_<Joint::Properties>("Properties")
        .def_readwrite("name", &Joint::Properties::mName)
        .def_readwrite("T_parent_body_to_joint",
            &Joint::Properties::mT_ParentBodyToJoint)
        .def_readwrite("T_child_body_to_joint",
            &Joint::Properties::mT_ChildBodyToJoint)
        .def_readwrite("is_position_limited",
            &Joint::Properties::mIsPositionLimited)
        .def_readwrite("actuator_type",
            &Joint::Properties::mActuatorType)
        ;
}

static void python_ZeroDofJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::ZeroDofJoint;

    scope joint_class(
        class_<ZeroDofJoint, bases<Joint>, noncopyable>(
               JointMap::instance().getPythonName<ZeroDofJoint>().c_str(),
               no_init)

    );

    class_<ZeroDofJoint::Properties>("Properties");
}

static void python_FreeJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::FreeJoint;

    scope joint_class(
        class_<FreeJoint, bases<Joint>, noncopyable>(
               JointMap::instance().getPythonName<FreeJoint>().c_str(),
               no_init)
            .def("test", &FreeJoint_test)
    );
}

static void python_SingleDofJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::SingleDofJoint;

    scope joint_class(
        class_<SingleDofJoint, noncopyable>(
            JointMap::instance().getPythonName<SingleDofJoint>().c_str(),
            no_init)
    );

    class_<SingleDofJoint::UniqueProperties>("UniqueProperties")
        .def_readwrite("position_lower_limit",
            &SingleDofJoint::UniqueProperties::mPositionLowerLimit)
        .def_readwrite("position_upper_limit",
            &SingleDofJoint::UniqueProperties::mPositionUpperLimit)
        .def_readwrite("velocity_lower_limit",
            &SingleDofJoint::UniqueProperties::mVelocityLowerLimit)
        .def_readwrite("velocity_upper_limit",
            &SingleDofJoint::UniqueProperties::mVelocityUpperLimit)
        .def_readwrite("acceleration_lower_limit",
            &SingleDofJoint::UniqueProperties::mAccelerationLowerLimit)
        .def_readwrite("acceleration_upper_limit",
            &SingleDofJoint::UniqueProperties::mAccelerationUpperLimit)
        .def_readwrite("force_lower_limit",
            &SingleDofJoint::UniqueProperties::mForceLowerLimit)
        .def_readwrite("force_upper_limit",
            &SingleDofJoint::UniqueProperties::mForceUpperLimit)
        .def_readwrite("spring_stiffness",
            &SingleDofJoint::UniqueProperties::mSpringStiffness)
        .def_readwrite("rest_position",
            &SingleDofJoint::UniqueProperties::mRestPosition)
        .def_readwrite("damping_coefficient",
            &SingleDofJoint::UniqueProperties::mDampingCoefficient)
        .def_readwrite("friction",
            &SingleDofJoint::UniqueProperties::mFriction)
        .def_readwrite("preserve_dof_name",
            &SingleDofJoint::UniqueProperties::mPreserveDofName)
        .def_readwrite("dof_name",
            &SingleDofJoint::UniqueProperties::mDofName)
        ;

    class_<SingleDofJoint::Properties, bases<
                Joint::Properties, SingleDofJoint::UniqueProperties>
            >("Properties")
        ;
}

static void python_RevoluteJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::RevoluteJoint;
    using dart::dynamics::SingleDofJoint;

    scope joint_class(
        class_<RevoluteJoint, noncopyable>(
                JointMap::instance().getPythonName<RevoluteJoint>().c_str(),
                no_init)
    );

    class_<RevoluteJoint::UniqueProperties>("UniqueProperties")
        .def_readwrite("axis", &RevoluteJoint::UniqueProperties::mAxis)
        ;

    class_<RevoluteJoint::Properties, bases<
            SingleDofJoint::Properties, RevoluteJoint::UniqueProperties>
          >("Properties")
        ;
}

static void python_WeldJoint()
{
    using namespace boost::python;

    using boost::noncopyable;
    using dart::dynamics::WeldJoint;
    using dart::dynamics::ZeroDofJoint;

    scope joint_class(
        class_<WeldJoint, noncopyable>(
                JointMap::instance().getPythonName<WeldJoint>().c_str(),
                no_init)
    );

    class_<WeldJoint::Properties,
           bases<ZeroDofJoint::Properties> >("Properties")
        ;
}

} // namespace python
} // namespace dart

void python_Joint()
{
    using namespace ::boost::python;
    using namespace ::dart::python;

    using ::dart::python::JointType;

    typedef ::dart::dynamics::Joint::ActuatorType ActuatorType;

    enum_<ActuatorType>("ActuatorType")
        .value("FORCE", Joint::FORCE)
        .value("PASSIVE", Joint::PASSIVE)
        .value("SERVO", Joint::SERVO)
        .value("ACCELERATION", Joint::ACCELERATION)
        .value("VELOCITY", Joint::VELOCITY)
        .value("LOCKED", Joint::LOCKED)
        ;

    enum_<JointType::Enum>("JointType")
        .value("PRISMATIC", JointType::PRISMATIC)
        .value("REVOLUTE", JointType::REVOLUTE)
        .value("SCREW", JointType::SCREW)
        .value("WELD", JointType::WELD)
        .value("UNIVERSAL", JointType::UNIVERSAL)
        .value("BALL", JointType::BALL)
        .value("EULER", JointType::EULER)
        .value("PLANAR", JointType::PLANAR)
        .value("TRANSLATIONAL", JointType::TRANSLATIONAL)
        .value("FREE", JointType::FREE)
        ;

    python_Joint_base();
    python_ZeroDofJoint();
    python_SingleDofJoint();
    python_RevoluteJoint();
    python_FreeJoint();
    python_WeldJoint();
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: cmdlineargs.cxx,v $
 *
 *  $Revision: 1.7 $
 *
 *  last change: $Author: cd $ $Date: 2001-12-20 09:00: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 _DESKTOP_COMMANDLINEARGS_HXX_
#include <cmdlineargs.hxx>
#endif
#ifndef _SV_SVAPP_HXX
#include <vcl/svapp.hxx>
#endif
#ifndef _RTL_URI_HXX_
#include <rtl/uri.hxx>
#endif

namespace desktop
{

CommandLineArgs::CommandLineArgs()
{
    ResetParamValues();
}

// intialize class with command line parameters from process environment
CommandLineArgs::CommandLineArgs( const ::vos::OExtCommandLine& aExtCmdLine )
{
    ResetParamValues();
    ParseCommandLine_Impl( aExtCmdLine );
}

// intialize class with command line parameters from interprocess communication (officeipcthread)
CommandLineArgs::CommandLineArgs( const ::rtl::OUString& aCmdLineArgs )
{
    ResetParamValues();
    ParseCommandLine_String( aCmdLineArgs );
}

// ----------------------------------------------------------------------------

void CommandLineArgs::ParseCommandLine_Impl( const ::vos::OExtCommandLine& aExtCmdLine )
{
    sal_Bool    bPrintEvent     = sal_False;
    sal_Bool    bPrintToEvent   = sal_False;
    sal_Bool    bOpenEvent      = sal_True;
    sal_Int32   bPrinterName    = sal_False;

    ::vos::OExtCommandLine aCmdLine;
    sal_uInt32 nCount = aCmdLine.getCommandArgCount();
    for( sal_uInt32 i=0; i < nCount; i++ )
    {
        ::rtl::OUString aArg;
        aCmdLine.getCommandArg( i, aArg );
        String aArgStr = aArg;

        // unknown command line parameter ?
        if ( !InterpretCommandLineParameter( aArg ))
        {
            const xub_Unicode* pArg = aArgStr.GetBuffer();

            // only with '-', because Unix uses / as root-dir!!
            if ( *pArg == (sal_Unicode)'-' )
            {
                pArg++;

                // a switch
                if ( (*pArg == (sal_Unicode)'p') ||
                     (*pArg == (sal_Unicode)'P') )
                {
                    if ( aArgStr.EqualsIgnoreCaseAscii( "-pt" ))
                    {
                        bPrintEvent = sal_False;
                        bPrintToEvent = sal_True;

                        bPrinterName = sal_True;
                    }
                    else
                    {
                        bPrintEvent = sal_True;
                        bPrintToEvent = sal_False;
                    }

                    bOpenEvent = FALSE;    // no more open events
                }
            }
            else
            {
                if ( bPrinterName && bPrintToEvent )
                {
                    // first argument after "-pt" this must be the printer name
                    m_aPrinterName = aArgStr;
                    m_bPrinterName = sal_True;
                    bPrinterName = sal_False;
                }
                else
                {
                    // interpreted as file name
                    if ( bOpenEvent )
                    {
                        // append open event
                        if ( m_aOpenList.getLength() > 0 )
                            m_aOpenList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                        m_aOpenList += aArgStr;
                        m_bOpenList = sal_True;
                    }
                    else if ( bPrintEvent )
                    {
                        // append print event
                        if( m_aPrintList.getLength() )
                            m_aPrintList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                        m_aPrintList += aArgStr;
                        m_bPrintList = sal_True;
                    }
                    else
                    {
                        if ( m_aPrintToList.getLength() )
                            m_aPrintToList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                        m_aPrintToList += aArgStr;
                        m_bPrintToList = sal_True;
                    }
                }
            }
        }
    }
}

void CommandLineArgs::ParseCommandLine_String( const ::rtl::OUString& aCmdLineString )
{
    // parse command line arguments
    sal_Bool    bPrintEvent     = sal_False;
    sal_Bool    bOpenEvent      = sal_True;
    sal_Bool    bPrintToEvent   = sal_False;
    sal_Bool    bPrinterName    = sal_False;

    sal_Int32 nIndex = 0;
    do
    {
        ::rtl::OUString aArg    = aCmdLineString.getToken( 0, '|', nIndex );
        String          aArgStr = aArg;

        if ( aArg.getLength() > 0 )
        {
            if ( !InterpretCommandLineParameter( aArg ))
            {
                if ( aArgStr.GetChar(0) == '-' )
                {
                    // handle this argument as an option
                    if ( aArgStr.EqualsIgnoreCaseAscii( "-p" ))
                    {
                        bPrintEvent = sal_True;
                        bPrintToEvent = sal_False;
                    }
                    else if ( aArgStr.EqualsIgnoreCaseAscii( "-pt" ))
                    {
                        bPrintEvent = sal_False;
                        bPrintToEvent = sal_True;
                        bPrinterName = sal_True;
                    }

                    bOpenEvent  = sal_False;    // Ab hier keine OpenEvents mehr
                }
                else
                {
                    if ( bPrinterName && bPrintToEvent )
                    {
                        // first argument after "-pt" this must be the printer name
                        m_aPrinterName = aArgStr;
                        m_bPrinterName = sal_True;
                        bPrinterName = sal_False;
                    }
                    else
                    {
                        // handle this argument as a filename
                        if ( bOpenEvent )
                        {
                            // Open Event anhaengen
                            if ( m_aOpenList.getLength() )
                                m_aOpenList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aOpenList += aArgStr;
                            m_bOpenList = sal_True;
                        }
                        else if ( bPrintEvent )
                        {
                            // Print Event anhaengen
                            if( m_aPrintList.getLength() )
                                m_aPrintList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aPrintList += aArgStr;
                            m_bPrintList = sal_True;
                        }
                        else
                        {
                            if ( m_aPrintToList.getLength() )
                                m_aPrintToList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aPrintToList += aArgStr;
                            m_bPrintToList = sal_True;
                        }
                    }
                }
            }
        }
    }
    while ( nIndex >= 0 );
}

sal_Bool CommandLineArgs::InterpretCommandLineParameter( const ::rtl::OUString& aArg )
{
    String aArgStr( aArg );

    if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-minimized" )) == sal_True )
    {
        m_bMinimized = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-invisible" )) == sal_True )
    {
        m_bInvisible = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-norestore" )) == sal_True )
    {
        m_bNoRestore = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-bean" )) == sal_True )
    {
        m_bBean = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-plugin" )) == sal_True )
    {
        m_bPlugin = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-server" )) == sal_True )
    {
        m_bServer = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-headless" )) == sal_True )
    {
        m_bHeadless = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-quickstart" )) == sal_True )
    {
        m_bQuickstart = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-terminate_after_init" )) == sal_True )
    {
        m_bTerminateAfterInit = sal_True;
        return sal_True;
    }
    else if ( aArgStr.Copy(0, 8).EqualsIgnoreCaseAscii( "-accept=" ))
    {
        m_aAcceptString = aArgStr.Copy( 8 );
        m_bAcceptString = sal_True;
        return sal_True;
    }
    else if ( aArgStr.CompareIgnoreCaseToAscii( "-portal," ,
                                                RTL_CONSTASCII_LENGTH( "-portal," )) == COMPARE_EQUAL )
    {
        m_aPortalConnectString = aArgStr.Copy( RTL_CONSTASCII_LENGTH( "-portal," ));
        m_bPortalConnectString = sal_True;
        return sal_True;
    }
    else if ( aArgStr.Copy( 0, 7 ).EqualsIgnoreCaseAscii( "-userid" ))
    {
        ::rtl::OUString aUserId = aArgStr;

        sal_Int32 nPos = aUserId.indexOf( '[' );
        sal_Int32 nEndpos = aUserId.lastIndexOf( ']' );
        if( nPos != -1 && nEndpos != -1 )
        {
            m_aUserDir = ::rtl::Uri::decode(
                aUserId.copy( nPos + 1, nEndpos - nPos - 1 ),
                rtl_UriDecodeWithCharset,
                RTL_TEXTENCODING_UTF8 );
            m_bUserDir = sal_True;
        }
        return sal_True;
    }
    else if ( aArgStr.Copy( 0, 15).EqualsIgnoreCaseAscii( "-clientdisplay=" ))
    {
        m_aClientDisplay = aArgStr.Copy( 15 );
        m_bClientDisplay = sal_True;
        return sal_True;
    }
    else if ( aArgStr.Copy(0, 9).EqualsIgnoreCaseAscii( "-version=" ))
    {
        m_aVersionString = aArgStr.Copy( 9 );
        m_bVersionString = sal_True;
        return sal_True;
    }

    return sal_False;
}

void CommandLineArgs::ResetParamValues()
{
    m_bMinimized            = sal_False;
    m_bInvisible            = sal_False;
    m_bNoRestore            = sal_False;
    m_bBean                 = sal_False;
    m_bPlugin               = sal_False;
    m_bHeadless             = sal_False;
    m_bServer               = sal_False;
    m_bQuickstart           = sal_False;
    m_bOpenList             = sal_False;
    m_bPrintList            = sal_False;
    m_bPortalConnectString  = sal_False;
    m_bAcceptString         = sal_False;
    m_bUserDir              = sal_False;
    m_bClientDisplay        = sal_False;
    m_bTerminateAfterInit   = sal_False;
    m_bVersionString        = sal_False;
    m_bPrintToList          = sal_False;
    m_bPrinterName          = sal_False;
}

} // namespace desktop
<commit_msg>#97557# Skip unsupported cmdline args<commit_after>/*************************************************************************
 *
 *  $RCSfile: cmdlineargs.cxx,v $
 *
 *  $Revision: 1.8 $
 *
 *  last change: $Author: cd $ $Date: 2002-02-26 08:18:25 $
 *
 *  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 _DESKTOP_COMMANDLINEARGS_HXX_
#include <cmdlineargs.hxx>
#endif
#ifndef _SV_SVAPP_HXX
#include <vcl/svapp.hxx>
#endif
#ifndef _RTL_URI_HXX_
#include <rtl/uri.hxx>
#endif

namespace desktop
{

CommandLineArgs::CommandLineArgs()
{
    ResetParamValues();
}

// intialize class with command line parameters from process environment
CommandLineArgs::CommandLineArgs( const ::vos::OExtCommandLine& aExtCmdLine )
{
    ResetParamValues();
    ParseCommandLine_Impl( aExtCmdLine );
}

// intialize class with command line parameters from interprocess communication (officeipcthread)
CommandLineArgs::CommandLineArgs( const ::rtl::OUString& aCmdLineArgs )
{
    ResetParamValues();
    ParseCommandLine_String( aCmdLineArgs );
}

// ----------------------------------------------------------------------------

void CommandLineArgs::ParseCommandLine_Impl( const ::vos::OExtCommandLine& aExtCmdLine )
{
    sal_Bool    bPrintEvent     = sal_False;
    sal_Bool    bPrintToEvent   = sal_False;
    sal_Bool    bOpenEvent      = sal_True;
    sal_Bool    bForceOpenEvent = sal_True;
    sal_Bool    bForceNewEvent  = sal_True;
    sal_Int32   bPrinterName    = sal_False;

    ::vos::OExtCommandLine aCmdLine;

    sal_uInt32      nCount = aCmdLine.getCommandArgCount();
    ::rtl::OUString aDummy;
    String          aArguments;

    // Extract cmdline parameters and concat them to the cmdline string format
    for( sal_uInt32 i=0; i < nCount; i++ )
    {
        aCmdLine.getCommandArg( i, aDummy );
        aArguments += String( aDummy );
        aArguments += '|';
    }

    // Parse string as a cmdline string
    ParseCommandLine_String( ::rtl::OUString( aArguments ));
}

void CommandLineArgs::ParseCommandLine_String( const ::rtl::OUString& aCmdLineString )
{
    // parse command line arguments
    sal_Bool    bPrintEvent     = sal_False;
    sal_Bool    bOpenEvent      = sal_True;
    sal_Bool    bPrintToEvent   = sal_False;
    sal_Bool    bPrinterName    = sal_False;
    sal_Bool    bForceOpenEvent = sal_True;
    sal_Bool    bForceNewEvent  = sal_True;

    sal_Int32 nIndex = 0;
    do
    {
        ::rtl::OUString aArg    = aCmdLineString.getToken( 0, '|', nIndex );
        String          aArgStr = aArg;

        if ( aArg.getLength() > 0 )
        {
            if ( !InterpretCommandLineParameter( aArg ))
            {
                if ( aArgStr.GetChar(0) == '-' )
                {
                    // handle this argument as an option
                    if ( aArgStr.EqualsIgnoreCaseAscii( "-n" ))
                    {
                        // force new documents based on the following documents
                        bForceNewEvent  = sal_True;
                        bOpenEvent      = sal_False;
                        bForceOpenEvent = sal_False;
                    }
                    else if ( aArgStr.EqualsIgnoreCaseAscii( "-o" ))
                    {
                        // force open documents regards if they are templates or not
                        bForceOpenEvent = sal_True;
                        bOpenEvent      = sal_False;
                        bForceNewEvent  = sal_False;
                    }
                    else if ( aArgStr.EqualsIgnoreCaseAscii( "-pt" ))
                    {
                        // Print to special printer
                        bPrintEvent = sal_False;
                        bPrintToEvent = sal_True;
                        bOpenEvent = sal_False;
                        bPrinterName = sal_True;
                    }
                    else if ( aArgStr.EqualsIgnoreCaseAscii( "-p" ))
                    {
                        // Print to default printer
                        bPrintEvent = sal_True;
                        bPrintToEvent = sal_False;
                        bOpenEvent = sal_False;
                    }
                }
                else
                {
                    if ( bPrinterName && bPrintToEvent )
                    {
                        // first argument after "-pt" this must be the printer name
                        m_aPrinterName = aArgStr;
                        m_bPrinterName = sal_True;
                        bPrinterName = sal_False;
                    }
                    else
                    {
                        // handle this argument as a filename
                        if ( bOpenEvent )
                        {
                            // Open Event anhaengen
                            if ( m_aOpenList.getLength() )
                                m_aOpenList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aOpenList += aArgStr;
                            m_bOpenList = sal_True;
                        }
                        else if ( bPrintEvent )
                        {
                            // Print Event anhaengen
                            if( m_aPrintList.getLength() )
                                m_aPrintList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aPrintList += aArgStr;
                            m_bPrintList = sal_True;
                        }
                        else if ( bPrintToEvent )
                        {
                            if ( m_aPrintToList.getLength() )
                                m_aPrintToList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aPrintToList += aArgStr;
                            m_bPrintToList = sal_True;
                        }
                        else if ( bForceNewEvent )
                        {
                            if ( m_aForceNewList.getLength() )
                                m_aForceNewList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aForceNewList += aArgStr;
                            m_bForceNewList = sal_True;
                        }
                        else if ( bForceOpenEvent )
                        {
                            if ( m_aForceOpenList.getLength() )
                                m_aForceOpenList += ::rtl::OUString::valueOf( (sal_Unicode)APPEVENT_PARAM_DELIMITER );
                            m_aForceOpenList += aArgStr;
                            m_bForceOpenList = sal_True;
                        }
                    }
                }
            }
        }
    }
    while ( nIndex >= 0 );
}

sal_Bool CommandLineArgs::InterpretCommandLineParameter( const ::rtl::OUString& aArg )
{
    String aArgStr( aArg );

    if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-minimized" )) == sal_True )
    {
        m_bMinimized = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-invisible" )) == sal_True )
    {
        m_bInvisible = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-norestore" )) == sal_True )
    {
        m_bNoRestore = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-bean" )) == sal_True )
    {
        m_bBean = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-plugin" )) == sal_True )
    {
        m_bPlugin = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-server" )) == sal_True )
    {
        m_bServer = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-headless" )) == sal_True )
    {
        m_bHeadless = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-quickstart" )) == sal_True )
    {
        m_bQuickstart = sal_True;
        return sal_True;
    }
    else if ( aArg.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM( "-terminate_after_init" )) == sal_True )
    {
        m_bTerminateAfterInit = sal_True;
        return sal_True;
    }
    else if ( aArgStr.Copy(0, 8).EqualsIgnoreCaseAscii( "-accept=" ))
    {
        m_aAcceptString = aArgStr.Copy( 8 );
        m_bAcceptString = sal_True;
        return sal_True;
    }
    else if ( aArgStr.CompareIgnoreCaseToAscii( "-portal," ,
                                                RTL_CONSTASCII_LENGTH( "-portal," )) == COMPARE_EQUAL )
    {
        m_aPortalConnectString = aArgStr.Copy( RTL_CONSTASCII_LENGTH( "-portal," ));
        m_bPortalConnectString = sal_True;
        return sal_True;
    }
    else if ( aArgStr.Copy( 0, 7 ).EqualsIgnoreCaseAscii( "-userid" ))
    {
        ::rtl::OUString aUserId = aArgStr;

        sal_Int32 nPos = aUserId.indexOf( '[' );
        sal_Int32 nEndpos = aUserId.lastIndexOf( ']' );
        if( nPos != -1 && nEndpos != -1 )
        {
            m_aUserDir = ::rtl::Uri::decode(
                aUserId.copy( nPos + 1, nEndpos - nPos - 1 ),
                rtl_UriDecodeWithCharset,
                RTL_TEXTENCODING_UTF8 );
            m_bUserDir = sal_True;
        }
        return sal_True;
    }
    else if ( aArgStr.Copy( 0, 15).EqualsIgnoreCaseAscii( "-clientdisplay=" ))
    {
        m_aClientDisplay = aArgStr.Copy( 15 );
        m_bClientDisplay = sal_True;
        return sal_True;
    }
    else if ( aArgStr.Copy(0, 9).EqualsIgnoreCaseAscii( "-version=" ))
    {
        m_aVersionString = aArgStr.Copy( 9 );
        m_bVersionString = sal_True;
        return sal_True;
    }

    return sal_False;
}

void CommandLineArgs::ResetParamValues()
{
    m_bMinimized            = sal_False;
    m_bInvisible            = sal_False;
    m_bNoRestore            = sal_False;
    m_bBean                 = sal_False;
    m_bPlugin               = sal_False;
    m_bHeadless             = sal_False;
    m_bServer               = sal_False;
    m_bQuickstart           = sal_False;
    m_bOpenList             = sal_False;
    m_bPrintList            = sal_False;
    m_bPortalConnectString  = sal_False;
    m_bAcceptString         = sal_False;
    m_bUserDir              = sal_False;
    m_bClientDisplay        = sal_False;
    m_bTerminateAfterInit   = sal_False;
    m_bVersionString        = sal_False;
    m_bPrintToList          = sal_False;
    m_bPrinterName          = sal_False;
    m_bForceOpenList        = sal_False;
    m_bForceNewList         = sal_False;
}

} // namespace desktop
<|endoftext|>


<commit_before>#pragma once

#include "boost_defs.hpp"

#include <CoreFoundation/CoreFoundation.h>
#include <boost/optional.hpp>
#include <cmath>

namespace krbn {
class system_preferences final {
public:
  class values {
  public:
    values(void) : keyboard_fn_state_(system_preferences::get_keyboard_fn_state()),
                   swipe_scroll_direction_(system_preferences::get_swipe_scroll_direction()) {
    }

    bool get_keyboard_fn_state(void) const {
      return keyboard_fn_state_;
    }

    bool get_swipe_scroll_direction(void) const {
      return swipe_scroll_direction_;
    }

    bool operator==(const system_preferences::values& other) const {
      return keyboard_fn_state_ == other.keyboard_fn_state_ &&
             swipe_scroll_direction_ == other.swipe_scroll_direction_;
    }

    bool operator!=(const system_preferences::values& other) const { return !(*this == other); }

  private:
    bool keyboard_fn_state_;
    bool swipe_scroll_direction_;
  };

  static boost::optional<bool> get_bool_property(CFStringRef _Nonnull key, CFStringRef _Nonnull application_id) {
    boost::optional<bool> value = boost::none;
    if (auto v = CFPreferencesCopyAppValue(key, application_id)) {
      if (CFBooleanGetTypeID() == CFGetTypeID(v)) {
        value = CFBooleanGetValue(static_cast<CFBooleanRef>(v));
      }
      CFRelease(v);
    }
    return value;
  }

  static boost::optional<float> get_float_property(CFStringRef _Nonnull key, CFStringRef _Nonnull application_id) {
    boost::optional<float> value = boost::none;
    if (auto v = CFPreferencesCopyAppValue(key, application_id)) {
      if (CFNumberGetTypeID() == CFGetTypeID(v)) {
        float vv;
        if (CFNumberGetValue(static_cast<CFNumberRef>(v), kCFNumberFloatType, &vv)) {
          value = vv;
        }
      }
      CFRelease(v);
    }
    return value;
  }

  /// "Use all F1, F2, etc. keys as standard function keys."
  static bool get_keyboard_fn_state(void) {
    if (auto value = system_preferences::get_bool_property(CFSTR("com.apple.keyboard.fnState"), CFSTR("Apple Global Domain"))) {
      return *value;
    }
    // default value
    return false;
  }

  // Scroll direction (Natural == 1)
  static bool get_swipe_scroll_direction(void) {
    if (auto value = system_preferences::get_bool_property(CFSTR("com.apple.swipescrolldirection"), CFSTR("Apple Global Domain"))) {
      return *value;
    }
    // default value
    return true;
  }
};
} // namespace krbn
<commit_msg>add get_keyboard_type<commit_after>#pragma once

#include "boost_defs.hpp"

#include "cf_utility.hpp"
#include <CoreFoundation/CoreFoundation.h>
#include <boost/optional.hpp>
#include <cmath>

namespace krbn {
class system_preferences final {
public:
  class values {
  public:
    values(void) : keyboard_fn_state_(system_preferences::get_keyboard_fn_state()),
                   swipe_scroll_direction_(system_preferences::get_swipe_scroll_direction()) {
    }

    bool get_keyboard_fn_state(void) const {
      return keyboard_fn_state_;
    }

    bool get_swipe_scroll_direction(void) const {
      return swipe_scroll_direction_;
    }

    boost::optional<uint8_t> get_keyboard_type(void) const {
      return keyboard_type_;
    }

    bool operator==(const system_preferences::values& other) const {
      return keyboard_fn_state_ == other.keyboard_fn_state_ &&
             swipe_scroll_direction_ == other.swipe_scroll_direction_;
    }

    bool operator!=(const system_preferences::values& other) const { return !(*this == other); }

  private:
    bool keyboard_fn_state_;
    bool swipe_scroll_direction_;
    boost::optional<uint8_t> keyboard_type_;
  };

  static boost::optional<bool> get_bool_property(CFStringRef _Nonnull key, CFStringRef _Nonnull application_id) {
    boost::optional<bool> value = boost::none;
    if (auto v = CFPreferencesCopyAppValue(key, application_id)) {
      if (CFBooleanGetTypeID() == CFGetTypeID(v)) {
        value = CFBooleanGetValue(static_cast<CFBooleanRef>(v));
      }
      CFRelease(v);
    }
    return value;
  }

  static boost::optional<float> get_float_property(CFStringRef _Nonnull key, CFStringRef _Nonnull application_id) {
    boost::optional<float> value = boost::none;
    if (auto v = CFPreferencesCopyAppValue(key, application_id)) {
      if (CFNumberGetTypeID() == CFGetTypeID(v)) {
        float vv;
        if (CFNumberGetValue(static_cast<CFNumberRef>(v), kCFNumberFloatType, &vv)) {
          value = vv;
        }
      }
      CFRelease(v);
    }
    return value;
  }

  static CFDictionaryRef _Nullable copy_dictionary_property(CFStringRef _Nonnull key, CFStringRef _Nonnull application_id) {
    CFDictionaryRef value = nullptr;
    if (auto v = CFPreferencesCopyAppValue(key, application_id)) {
      if (CFDictionaryGetTypeID() == CFGetTypeID(v)) {
        CFRetain(v);
        value = static_cast<CFDictionaryRef>(v);
      }
      CFRelease(v);
    }
    return value;
  }

  /// "Use all F1, F2, etc. keys as standard function keys."
  static bool get_keyboard_fn_state(void) {
    if (auto value = system_preferences::get_bool_property(CFSTR("com.apple.keyboard.fnState"), CFSTR("Apple Global Domain"))) {
      return *value;
    }
    // default value
    return false;
  }

  // Scroll direction (Natural == 1)
  static bool get_swipe_scroll_direction(void) {
    if (auto value = system_preferences::get_bool_property(CFSTR("com.apple.swipescrolldirection"), CFSTR("Apple Global Domain"))) {
      return *value;
    }
    // default value
    return true;
  }

  static boost::optional<uint8_t> get_keyboard_type(uint8_t country_code) {
    boost::optional<uint8_t> result;

    if (auto value = copy_dictionary_property(CFSTR("keyboardtype"), CFSTR("com.apple.keyboardtype"))) {
      uint16_t vendor_id = 0x16c0;
      uint16_t product_id = 0x27db;
      if (auto key = CFStringCreateWithCString(kCFAllocatorDefault,
                                               fmt::format("{0}-{1}-{2}",
                                                           static_cast<uint16_t>(product_id),
                                                           static_cast<uint16_t>(vendor_id),
                                                           country_code)
                                                   .c_str(),
                                               kCFStringEncodingUTF8)) {
        if (auto keyboard_type = CFDictionaryGetValue(value, key)) {
          result = cf_utility::to_int64_t(keyboard_type);
        }

        CFRelease(key);
      }

      CFRelease(value);
    }

    return result;
  }
};
} // namespace krbn
<|endoftext|>


<commit_before>/*++

Module Name:

    CommandProcessor.cpp

Abstract:

    Code for running the top-level commands of SNAP

Authors:

    Bill Bolosky, November, 2014

Environment:

User mode service.

Revision History:

    Pulled from the main program and expanded to handle daemon mode

--*/

#include "stdafx.h"
#include "options.h"
#include "FASTA.h"
#include "GenomeIndex.h"
#include "SingleAligner.h"
#include "PairedAligner.h"
#include "exit.h"
#include "SeedSequencer.h"
#include "AlignerOptions.h"
#include "CommandProcessor.h"
#include "Error.h"
#include "Compat.h"

const char *SNAP_VERSION = "1.0dev.97";

static void usage()
{
	WriteErrorMessage(
		"Usage: snap-aligner <command> [<options>]\n"
		"Commands:\n"
		"   index    build a genome index\n"
		"   single   align single-end reads\n"
		"   paired   align paired-end reads\n"
		"   daemon   run in daemon mode--accept commands remotely\n"
		"Type a command without arguments to see its help.\n");
}

void ProcessNonDaemonCommands(int argc, const char **argv) {
	if (strcmp(argv[1], "index") == 0) {
		if (CommandPipe == NULL) {
			GenomeIndex::runIndexer(argc - 2, argv + 2);
		} else {
			//
			// The error cases in index build don't really free memory properly, so we just don't allow it in daemon mode.
			//
			WriteErrorMessage("The index command is not available in daemon mode.  Please run 'snap-aligner index' directly.\n");
		}
	} else if (strcmp(argv[1], "single") == 0 || strcmp(argv[1], "paired") == 0) {
		for (int i = 1; i < argc; /* i is increased below */) {
			unsigned nArgsConsumed;
			if (strcmp(argv[i], "single") == 0) {
				SingleAlignerContext single;
				single.runAlignment(argc - i, argv + i, SNAP_VERSION, &nArgsConsumed);
			} else if (strcmp(argv[i], "paired") == 0) {
				PairedAlignerContext paired;
				paired.runAlignment(argc - i, argv + i, SNAP_VERSION, &nArgsConsumed);
			} else {
				fprintf(stderr, "Invalid command: %s\n\n", argv[i]);
				usage();
				return;
			}
			_ASSERT(nArgsConsumed > 0);
			i += nArgsConsumed;
		}
	} else {
		WriteErrorMessage("Invalid command: %s\n\n", argv[1]);
		usage();
	}
}

static void daemonUsage()
{
	fprintf(stderr, "Usage: snap-aligner daemon [Named pipe name]\n");
	soft_exit_no_print(1);    // Don't use soft_exit, it's confusing people to get an "error" message after the usage
}

void RunDaemonMode(int argc, const char **argv)
{
	if (argc < 2 || argc > 3) {
		daemonUsage();
	}

	printf("SNAP in daemon mode, waiting for commands to execute\n");

	const char *pipeName = argc == 3 ? argv[2] : DEFAULT_NAMED_PIPE_NAME;
	CommandPipe = OpenNamedPipe(pipeName, true);

	if (NULL == CommandPipe) {
		WriteErrorMessage("Unable to open named pipe for command IO.\n");
		soft_exit(1);
	}

	const size_t commandBufferSize = 10000;	// Yes, this is fixed size, no it's not a buffer overflow.  The named pipe reader just quits if it's too long.
	char commandBuffer[commandBufferSize];

	//
	// Format of commands is argc (in ascii) followed by argc arguments, each in one line.
	//
	for (;;) {
		if (!ReadFromNamedPipe(CommandPipe, commandBuffer, commandBufferSize)) {
			CloseNamedPipe(CommandPipe);
			CommandPipe = NULL;
			WriteStatusMessage("Named pipe closed.  Exiting\n");
			soft_exit_no_print(0);
		}

		int argc = atoi(commandBuffer);
		if (0 == argc) {
			WriteErrorMessage("Expected argument count on named pipe, got '%s'; ignoring.\n", commandBuffer);
		} else {
			char **argv = new char*[argc];
			for (int i = 0; i < argc; i++) {
				argv[i] = new char[commandBufferSize];
				if (!ReadFromNamedPipe(CommandPipe, argv[i], commandBufferSize)) {
					CloseNamedPipe(CommandPipe);
					CommandPipe = NULL;
					WriteStatusMessage("Error reading argument #%d from named pipe.\n", i);
					soft_exit(1);
				}
			} // for each arg

			if (argc > 1 && strcmp(argv[1], "exit") == 0) {
				WriteStatusMessage("SNAP server exiting by request\n");
				WriteToNamedPipe(CommandPipe, CommandExecutedString);
				soft_exit_no_print(1);
			}

			printf("Executing command: ");
			for (int i = 1; i < argc; i++) {
				printf("%s ", argv[i]);
			}
			printf("\n");

			ProcessNonDaemonCommands(argc, (const char **) argv);

			printf("\n");

			for (int i = 0; i < argc; i++) {
				delete[] argv[i];
				argv[i] = NULL;
			}
			delete[] argv;
			argv = NULL;
		}
		WriteToNamedPipe(CommandPipe, CommandExecutedString);
	}
}

void ProcessTopLevelCommands(int argc, const char **argv)
{
	fprintf(stderr, "Welcome to SNAP version %s.\n\n", SNAP_VERSION);       // Can't use WriteStatusMessage, because we haven't parsed args yet to determine if -hdp is specified.  Just stick with stderr.

	InitializeSeedSequencers();

	if (argc < 2) {
		usage();
		soft_exit_no_print(1);
	}

	if (strcmp(argv[1], "daemon") == 0) {
		RunDaemonMode(argc, argv);
	} else {
		ProcessNonDaemonCommands(argc, argv);
	}
}

NamedPipe *CommandPipe = NULL;
const char *CommandExecutedString = "***SNAP Command completed execution***";
<commit_msg>1.0dev.98: new version number for previous commit, because Ravi and I both used .97 for different things<commit_after>/*++

Module Name:

    CommandProcessor.cpp

Abstract:

    Code for running the top-level commands of SNAP

Authors:

    Bill Bolosky, November, 2014

Environment:

User mode service.

Revision History:

    Pulled from the main program and expanded to handle daemon mode

--*/

#include "stdafx.h"
#include "options.h"
#include "FASTA.h"
#include "GenomeIndex.h"
#include "SingleAligner.h"
#include "PairedAligner.h"
#include "exit.h"
#include "SeedSequencer.h"
#include "AlignerOptions.h"
#include "CommandProcessor.h"
#include "Error.h"
#include "Compat.h"

const char *SNAP_VERSION = "1.0dev.98";

static void usage()
{
	WriteErrorMessage(
		"Usage: snap-aligner <command> [<options>]\n"
		"Commands:\n"
		"   index    build a genome index\n"
		"   single   align single-end reads\n"
		"   paired   align paired-end reads\n"
		"   daemon   run in daemon mode--accept commands remotely\n"
		"Type a command without arguments to see its help.\n");
}

void ProcessNonDaemonCommands(int argc, const char **argv) {
	if (strcmp(argv[1], "index") == 0) {
		if (CommandPipe == NULL) {
			GenomeIndex::runIndexer(argc - 2, argv + 2);
		} else {
			//
			// The error cases in index build don't really free memory properly, so we just don't allow it in daemon mode.
			//
			WriteErrorMessage("The index command is not available in daemon mode.  Please run 'snap-aligner index' directly.\n");
		}
	} else if (strcmp(argv[1], "single") == 0 || strcmp(argv[1], "paired") == 0) {
		for (int i = 1; i < argc; /* i is increased below */) {
			unsigned nArgsConsumed;
			if (strcmp(argv[i], "single") == 0) {
				SingleAlignerContext single;
				single.runAlignment(argc - i, argv + i, SNAP_VERSION, &nArgsConsumed);
			} else if (strcmp(argv[i], "paired") == 0) {
				PairedAlignerContext paired;
				paired.runAlignment(argc - i, argv + i, SNAP_VERSION, &nArgsConsumed);
			} else {
				fprintf(stderr, "Invalid command: %s\n\n", argv[i]);
				usage();
				return;
			}
			_ASSERT(nArgsConsumed > 0);
			i += nArgsConsumed;
		}
	} else {
		WriteErrorMessage("Invalid command: %s\n\n", argv[1]);
		usage();
	}
}

static void daemonUsage()
{
	fprintf(stderr, "Usage: snap-aligner daemon [Named pipe name]\n");
	soft_exit_no_print(1);    // Don't use soft_exit, it's confusing people to get an "error" message after the usage
}

void RunDaemonMode(int argc, const char **argv)
{
	if (argc < 2 || argc > 3) {
		daemonUsage();
	}

	printf("SNAP in daemon mode, waiting for commands to execute\n");

	const char *pipeName = argc == 3 ? argv[2] : DEFAULT_NAMED_PIPE_NAME;
	CommandPipe = OpenNamedPipe(pipeName, true);

	if (NULL == CommandPipe) {
		WriteErrorMessage("Unable to open named pipe for command IO.\n");
		soft_exit(1);
	}

	const size_t commandBufferSize = 10000;	// Yes, this is fixed size, no it's not a buffer overflow.  The named pipe reader just quits if it's too long.
	char commandBuffer[commandBufferSize];

	//
	// Format of commands is argc (in ascii) followed by argc arguments, each in one line.
	//
	for (;;) {
		if (!ReadFromNamedPipe(CommandPipe, commandBuffer, commandBufferSize)) {
			CloseNamedPipe(CommandPipe);
			CommandPipe = NULL;
			WriteStatusMessage("Named pipe closed.  Exiting\n");
			soft_exit_no_print(0);
		}

		int argc = atoi(commandBuffer);
		if (0 == argc) {
			WriteErrorMessage("Expected argument count on named pipe, got '%s'; ignoring.\n", commandBuffer);
		} else {
			char **argv = new char*[argc];
			for (int i = 0; i < argc; i++) {
				argv[i] = new char[commandBufferSize];
				if (!ReadFromNamedPipe(CommandPipe, argv[i], commandBufferSize)) {
					CloseNamedPipe(CommandPipe);
					CommandPipe = NULL;
					WriteStatusMessage("Error reading argument #%d from named pipe.\n", i);
					soft_exit(1);
				}
			} // for each arg

			if (argc > 1 && strcmp(argv[1], "exit") == 0) {
				WriteStatusMessage("SNAP server exiting by request\n");
				WriteToNamedPipe(CommandPipe, CommandExecutedString);
				soft_exit_no_print(1);
			}

			printf("Executing command: ");
			for (int i = 1; i < argc; i++) {
				printf("%s ", argv[i]);
			}
			printf("\n");

			ProcessNonDaemonCommands(argc, (const char **) argv);

			printf("\n");

			for (int i = 0; i < argc; i++) {
				delete[] argv[i];
				argv[i] = NULL;
			}
			delete[] argv;
			argv = NULL;
		}
		WriteToNamedPipe(CommandPipe, CommandExecutedString);
	}
}

void ProcessTopLevelCommands(int argc, const char **argv)
{
	fprintf(stderr, "Welcome to SNAP version %s.\n\n", SNAP_VERSION);       // Can't use WriteStatusMessage, because we haven't parsed args yet to determine if -hdp is specified.  Just stick with stderr.

	InitializeSeedSequencers();

	if (argc < 2) {
		usage();
		soft_exit_no_print(1);
	}

	if (strcmp(argv[1], "daemon") == 0) {
		RunDaemonMode(argc, argv);
	} else {
		ProcessNonDaemonCommands(argc, argv);
	}
}

NamedPipe *CommandPipe = NULL;
const char *CommandExecutedString = "***SNAP Command completed execution***";
<|endoftext|>


<commit_before>#include <Eigen/Geometry>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <robot_kf/WheelOdometry.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <robot_kf/robot_kf.h>

static double const big = 99999.0;

static boost::shared_ptr<tf::TransformListener> sub_tf;
static boost::shared_ptr<tf::TransformBroadcaster> pub_tf;
static ros::Subscriber sub_compass, sub_encoders, sub_gps;
static ros::Publisher pub_fused;

static robot_kf::KalmanFilter kf;
static geometry_msgs::Twist velocity;

static bool watch_compass, watch_encoders, watch_gps;
static std::string global_frame_id, odom_frame_id, base_frame_id;

static void publish(ros::Time stamp)
{
    Eigen::Vector3d const state = kf.getState();

    // Wrap the fused state estimate in a ROS message.
    geometry_msgs::PoseStamped fused_base;
    fused_base.header.stamp    = stamp;
    fused_base.header.frame_id = odom_frame_id;
    fused_base.pose.position.x = state[0];
    fused_base.pose.position.y = state[1];
    fused_base.pose.position.z = 0.0;
    fused_base.pose.orientation = tf::createQuaternionMsgFromYaw(state[2]);

    /*
     * We actually want to publish the /map to /base_footprint transform, but
     * this would cause /base_footprint to have two parents. Instead, we need
     * to publish the /map to /odom transform. See this chart:
     *
     * /map --[T1]--> /odom --[T2]--> /base_footprint
     * /map ----------[T3]----------> /base_footprint
     *
     * The output of the Kalman filter is T3, but we want T1. We find T1 by
     * computing T1 = T3 * inv(T2), where T2 is provided by the odometry
     * source.
     */
    tf::Vector3 const pos(state[0], state[1], 0);
    tf::Quaternion const ori = tf::createQuaternionFromYaw(state[2]);
    tf::Transform t3(ori, pos);

    tf::StampedTransform t2;
    try {
        sub_tf->lookupTransform(odom_frame_id, base_frame_id, ros::Time(0), t2);
    } catch (tf::TransformException const &e) {
        ROS_WARN("%s", e.what());
        return;
    }

    tf::Transform const t1 = t3 * t2.inverse();
    tf::StampedTransform transform(t1, stamp, global_frame_id, odom_frame_id);
    pub_tf->sendTransform(transform);

    // Publish the odometry message.
    nav_msgs::Odometry msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = global_frame_id;
    msg.child_frame_id = base_frame_id;
    msg.pose.pose =  fused_base.pose;
    // Propagate the covariance forward.
    msg.pose.covariance[0] = -1;
    msg.twist.twist = velocity;
    msg.twist.covariance[0] = -1;
    pub_fused.publish(msg);
}

static void updateCompass(sensor_msgs::Imu const &msg)
{
    if (msg.header.frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "Imu message must have frame_id '%s'",
                           base_frame_id.c_str());
        return;
    }

    double const yaw = tf::getYaw(msg.orientation);
    Eigen::Map<Eigen::Matrix3d const> cov_raw(&msg.orientation_covariance.front());

    kf.update_compass(yaw, cov_raw(2, 2));
    if (watch_compass) publish(msg.header.stamp);
}

static void updateEncoders(robot_kf::WheelOdometry const &msg)
{
    if (msg.header.frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "WheelOdometry message must have frame_id '%s'", base_frame_id.c_str());
        return;
    } else if (msg.separation <= 0) {
        ROS_ERROR_THROTTLE(10, "Wheel separation in WheelOdometry message must be positive.");
        return;
    }

    Eigen::Vector2d const z = (Eigen::Vector2d() <<
        msg.left.movement, msg.right.movement).finished();
    Eigen::Matrix2d const cov_z = (Eigen::Matrix2d() <<
        msg.left.variance, 0.0, 0.0, msg.right.variance).finished();

    // Compute velocity using the wheel encoders. Theoretically the GPS and
    // compass could also be used to estimate these velocities, but those
    // estimates are too noisy to justify the complexity of adding two more
    // state variables.
    double const movement_linear = (z[1] + z[0]) / 2;
    double const movement_angular = (z[1] - z[0]) / msg.separation;
    double const delta_time = msg.timestep.toSec();
    velocity.linear.x = movement_linear / delta_time;
    velocity.angular.z = movement_angular / delta_time;

    kf.update_encoders(z, cov_z, msg.separation);
    if (watch_encoders) publish(msg.header.stamp);
}

static void updateGps(nav_msgs::Odometry const &msg)
{
    try {
        ros::Time const stamp = msg.header.stamp;
        std::string const frame_id = msg.child_frame_id;

        // Transform the position into the base coordinate frame.
        geometry_msgs::PointStamped stamped_in, stamped_out;
        stamped_in.header.stamp = stamp;
        stamped_in.header.frame_id = frame_id;
        stamped_in.point = msg.pose.pose.position;
        sub_tf->transformPoint(base_frame_id, stamped_in, stamped_out);

        Eigen::Vector2d const z = (Eigen::Vector2d() <<
            stamped_out.point.x, stamped_out.point.y).finished();
        Eigen::Map<Eigen::Matrix<double, 6, 6> const> cov_raw(
            &msg.pose.covariance.front()
        );
        Eigen::Matrix3d const cov3_raw = cov_raw.topLeftCorner<3, 3>();

        // Rotate the covariance matrix according to the transformation.
        tf::StampedTransform transform;
        Eigen::Affine3d eigen_transform;
        sub_tf->lookupTransform(base_frame_id, frame_id, stamp, transform);
        tf::TransformTFToEigen(transform, eigen_transform);

        Eigen::Matrix3d const rotation = eigen_transform.rotation();
        Eigen::Matrix3d const cov3 = rotation.transpose() * cov3_raw * rotation;
        Eigen::Matrix2d const cov  = cov3.topLeftCorner<2, 2>();

        kf.update_gps(z, cov);

        if (watch_gps) publish(msg.header.stamp);
    } catch (tf::TransformException const &e) {
        ROS_WARN("%s", e.what());
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "robot_kf_node");

    // Initialize the skeleton twist message.
    velocity.linear.y = 0;
    velocity.linear.z = 0;
    velocity.angular.x = 0;
    velocity.angular.y = 0;

    ros::NodeHandle nh, nh_node("~");
    nh_node.param<bool>("watch_compass",  watch_compass,  true);
    nh_node.param<bool>("watch_encoders", watch_encoders, true);
    nh_node.param<bool>("watch_gps",      watch_gps,      true);
    nh_node.param<std::string>("global_frame_id", global_frame_id, "/map");
    nh_node.param<std::string>("odom_frame_id",   odom_frame_id,   "/odom");
    nh_node.param<std::string>("base_frame_id",   base_frame_id,   "/base_footprint");

    sub_tf = boost::make_shared<tf::TransformListener>();
    pub_tf = boost::make_shared<tf::TransformBroadcaster>();
    sub_compass  = nh.subscribe("compass", 1, &updateCompass);
    sub_encoders = nh.subscribe("wheel_odom", 1, &updateEncoders);
    sub_gps      = nh.subscribe("gps", 1, &updateGps);
    pub_fused    = nh.advertise<nav_msgs::Odometry>("odom_fused", 100);

    ros::spin();
    return 0;
}
<commit_msg>attempted to fix gittering<commit_after>#include <Eigen/Geometry>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <robot_kf/WheelOdometry.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <robot_kf/robot_kf.h>

static double const big = 99999.0;

static boost::shared_ptr<tf::TransformListener> sub_tf;
static boost::shared_ptr<tf::TransformBroadcaster> pub_tf;
static ros::Subscriber sub_compass, sub_encoders, sub_gps;
static ros::Publisher pub_fused;

static robot_kf::KalmanFilter kf;
static geometry_msgs::Twist velocity;

static bool watch_compass, watch_encoders, watch_gps;
static std::string global_frame_id, odom_frame_id, base_frame_id;

static void publish(ros::Time stamp)
{
    Eigen::Vector3d const state = kf.getState();

    // Wrap the fused state estimate in a ROS message.
    geometry_msgs::PoseStamped fused_base;
    fused_base.header.stamp    = stamp;
    fused_base.header.frame_id = odom_frame_id;
    fused_base.pose.position.x = state[0];
    fused_base.pose.position.y = state[1];
    fused_base.pose.position.z = 0.0;
    fused_base.pose.orientation = tf::createQuaternionMsgFromYaw(state[2]);

    /*
     * We actually want to publish the /map to /base_footprint transform, but
     * this would cause /base_footprint to have two parents. Instead, we need
     * to publish the /map to /odom transform. See this chart:
     *
     * /map --[T1]--> /odom --[T2]--> /base_footprint
     * /map ----------[T3]----------> /base_footprint
     *
     * The output of the Kalman filter is T3, but we want T1. We find T1 by
     * computing T1 = T3 * inv(T2), where T2 is provided by the odometry
     * source.
     */
    tf::Vector3 const pos(state[0], state[1], 0);
    tf::Quaternion const ori = tf::createQuaternionFromYaw(state[2]);
    tf::Transform t3(ori, pos);

    tf::StampedTransform t2;
    try {
        sub_tf->lookupTransform(odom_frame_id, base_frame_id, stamp, t2);
    } catch (tf::TransformException const &e) {
        ROS_WARN("%s", e.what());
        return;
    }

    tf::Transform const t1 = t3 * t2.inverse();
    tf::StampedTransform transform(t1, stamp, global_frame_id, odom_frame_id);
    pub_tf->sendTransform(transform);

    // Publish the odometry message.
    nav_msgs::Odometry msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = global_frame_id;
    msg.child_frame_id = base_frame_id;
    msg.pose.pose =  fused_base.pose;
    // Propagate the covariance forward.
    msg.pose.covariance[0] = -1;
    msg.twist.twist = velocity;
    msg.twist.covariance[0] = -1;
    pub_fused.publish(msg);
}

static void updateCompass(sensor_msgs::Imu const &msg)
{
    if (msg.header.frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "Imu message must have frame_id '%s'",
                           base_frame_id.c_str());
        return;
    }

    double const yaw = tf::getYaw(msg.orientation);
    Eigen::Map<Eigen::Matrix3d const> cov_raw(&msg.orientation_covariance.front());

    kf.update_compass(yaw, cov_raw(2, 2));
    if (watch_compass) publish(msg.header.stamp);
}

static void updateEncoders(robot_kf::WheelOdometry const &msg)
{
    if (msg.header.frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "WheelOdometry message must have frame_id '%s'", base_frame_id.c_str());
        return;
    } else if (msg.separation <= 0) {
        ROS_ERROR_THROTTLE(10, "Wheel separation in WheelOdometry message must be positive.");
        return;
    }

    Eigen::Vector2d const z = (Eigen::Vector2d() <<
        msg.left.movement, msg.right.movement).finished();
    Eigen::Matrix2d const cov_z = (Eigen::Matrix2d() <<
        msg.left.variance, 0.0, 0.0, msg.right.variance).finished();

    // Compute velocity using the wheel encoders. Theoretically the GPS and
    // compass could also be used to estimate these velocities, but those
    // estimates are too noisy to justify the complexity of adding two more
    // state variables.
    double const movement_linear = (z[1] + z[0]) / 2;
    double const movement_angular = (z[1] - z[0]) / msg.separation;
    double const delta_time = msg.timestep.toSec();
    velocity.linear.x = movement_linear / delta_time;
    velocity.angular.z = movement_angular / delta_time;

    kf.update_encoders(z, cov_z, msg.separation);
    if (watch_encoders) publish(msg.header.stamp);
}

static void updateGps(nav_msgs::Odometry const &msg)
{
    if (msg.header.frame_id != global_frame_id) {
        ROS_ERROR_THROTTLE(10, "GPS message must have frame_id '%s'",
                           global_frame_id.c_str());
        return;
    } else if (msg.child_frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "GPS message must have child_frame_id '%s'",
                           base_frame_id.c_str());
        return;
    }
    Eigen::Vector2d const z = (Eigen::Vector2d() <<
        msg.pose.pose.position.x, msg.pose.pose.position.y).finished();
    Eigen::Map<Eigen::Matrix<double, 6, 6> const> cov_raw(
        &msg.pose.covariance.front()
    );
    Eigen::Matrix2d const cov = cov_raw.topLeftCorner<2, 2>();

    kf.update_gps(z, cov);

    if (watch_gps) publish(msg.header.stamp);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "robot_kf_node");

    // Initialize the skeleton twist message.
    velocity.linear.y = 0;
    velocity.linear.z = 0;
    velocity.angular.x = 0;
    velocity.angular.y = 0;

    ros::NodeHandle nh, nh_node("~");
    nh_node.param<bool>("watch_compass",  watch_compass,  true);
    nh_node.param<bool>("watch_encoders", watch_encoders, false);
    nh_node.param<bool>("watch_gps",      watch_gps,      false);
    nh_node.param<std::string>("global_frame_id", global_frame_id, "/map");
    nh_node.param<std::string>("odom_frame_id",   odom_frame_id,   "/odom");
    nh_node.param<std::string>("base_frame_id",   base_frame_id,   "/base_footprint");

    sub_tf = boost::make_shared<tf::TransformListener>();
    pub_tf = boost::make_shared<tf::TransformBroadcaster>();
    sub_compass  = nh.subscribe("compass", 1, &updateCompass);
    sub_encoders = nh.subscribe("wheel_odom", 1, &updateEncoders);
    sub_gps      = nh.subscribe("gps", 1, &updateGps);
    pub_fused    = nh.advertise<nav_msgs::Odometry>("odom_fused", 100);

    ros::spin();
    return 0;
}
<|endoftext|>


<commit_before>/*
Copyright (c) 2017 Microsoft Corporation. All rights reserved.
Released under Apache 2.0 license as described in the file LICENSE.

Author: Leonardo de Moura
*/
#include <utility>
#include <random>
#include <vector>
#include <functional>
#include <unordered_set>
#include "util/test.h"
#include "util/init_module.h"
#include "util/sexpr/init_module.h"
#include "kernel/init_module.h"
#include "library/init_module.h"
#include "library/phashtable.h"
using namespace lean;

typedef phashtable<unsigned, std::hash<unsigned>, std::equal_to<unsigned>, true> unsigned_set;
typedef std::unordered_set<unsigned, std::hash<unsigned>> std_unsigned_set;

void tst1() {
    unsigned_set s;
    lean_assert(s.size() == 0);
    s.insert(10);
    s.for_each([](unsigned v) {
            lean_assert(v == 10);
        });
    lean_assert(s.contains(10));
    lean_assert(!s.contains(20));
    lean_assert(s.find(10));
    auto v = s.find(10);
    lean_assert(v && *v == 10);
    lean_assert(!s.find(20));
    s.erase(20);
    lean_assert(s.check_invariant());
    lean_assert(s.contains(10));
    s.erase(10);
    lean_assert(!s.contains(10));
    lean_assert(s.check_invariant());
}

static void tst2() {
    std::uniform_int_distribution<unsigned int> uint_dist;
    std::mt19937   rng;
    rng.seed(static_cast<unsigned int>(time(0)));
    const unsigned N = 10000;
    unsigned vals[N];
    unsigned_set s1;
    for (unsigned i = 1; i < N; i ++) {
        unsigned v = uint_dist(rng) % (N / 2);
        s1.insert(v);
        vals[i] = v;
        lean_assert(s1.contains(v));
    }
    std::cout << "step1\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i ++) {
        lean_assert(s1.contains(vals[i]));
    }
    std::cout << "step2\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i += 2) {
        s1.erase(vals[i]);
        lean_assert(!s1.contains(vals[i]));
    }
    std::cout << "step3\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i += 2) {
        s1.insert(vals[i]);
    }
    std::cout << "step4\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i ++) {
        lean_assert(s1.contains(vals[i]));
    }
}

static void tst3() {
    std::uniform_int_distribution<unsigned int> uint_dist;
    std::mt19937   rng;
    rng.seed(static_cast<unsigned int>(time(0)));
    unsigned_set     s1;
    std_unsigned_set s2;
    unsigned N = uint_dist(rng) % 1000;
    for (unsigned i = 0; i < N; i++) {
        unsigned v = uint_dist(rng)%1000;
        if (uint_dist(rng) % 3 == 2) {
            s1.erase(v);
            s2.erase(v);
            lean_assert(!s1.contains(v));
        } else {
            s1.insert(v);
            s2.insert(v);
            lean_assert(s1.contains(v));
        }
    }
    {
        std_unsigned_set::iterator it  = s2.begin();
        std_unsigned_set::iterator end = s2.end();
        for (; it != end; ++it) {
            lean_assert(s1.contains(*it));
        }
    }
    {
        unsigned n = 0;
        s1.for_each([&](unsigned v) {
                lean_assert(s2.find(v) != s2.end());
                n++;
            });
        lean_assert(n == s1.size());
    }
    lean_assert(s1.size() == s2.size());
}

static void tst4() {
    unsigned_set s1;
    s1.insert(10);
    s1.insert(20);
    unsigned_set s2 = s1;
    lean_assert(s2.contains(10));
    lean_assert(s2.contains(20));
    lean_assert(!s2.contains(30));
    s2.insert(30);
    lean_assert(s2.contains(30));
    lean_assert(!s1.contains(30));
}

int main() {
    save_stack_info();
    initialize_util_module();
    initialize_sexpr_module();
    initialize_kernel_module();
    initialize_library_core_module();
    initialize_library_module();

    tst1();
    tst2();
    for (int i = 0; i < 1000; i++)
        tst3();
    tst4();

    finalize_library_module();
    finalize_library_core_module();
    finalize_kernel_module();
    finalize_sexpr_module();
    finalize_util_module();
    return has_violations() ? 1 : 0;
}
<commit_msg>test(tests/library/phashtable): another perf test<commit_after>/*
Copyright (c) 2017 Microsoft Corporation. All rights reserved.
Released under Apache 2.0 license as described in the file LICENSE.

Author: Leonardo de Moura
*/
#include <utility>
#include <random>
#include <vector>
#include <functional>
#include <unordered_set>
#include "util/test.h"
#include "util/timeit.h"
#include "util/init_module.h"
#include "util/sexpr/init_module.h"
#include "kernel/init_module.h"
#include "library/init_module.h"
#include "library/phashtable.h"
using namespace lean;

typedef phashtable<unsigned, std::hash<unsigned>, std::equal_to<unsigned>, true> unsigned_set;
typedef std::unordered_set<unsigned, std::hash<unsigned>> std_unsigned_set;

void tst1() {
    unsigned_set s;
    lean_assert(s.size() == 0);
    s.insert(10);
    s.for_each([](unsigned v) {
            lean_assert(v == 10);
        });
    lean_assert(s.contains(10));
    lean_assert(!s.contains(20));
    lean_assert(s.find(10));
    auto v = s.find(10);
    lean_assert(v && *v == 10);
    lean_assert(!s.find(20));
    s.erase(20);
    lean_assert(s.check_invariant());
    lean_assert(s.contains(10));
    s.erase(10);
    lean_assert(!s.contains(10));
    lean_assert(s.check_invariant());
}

static void tst2() {
    std::uniform_int_distribution<unsigned int> uint_dist;
    std::mt19937   rng;
    rng.seed(static_cast<unsigned int>(time(0)));
    const unsigned N = 10000;
    unsigned vals[N];
    unsigned_set s1;
    for (unsigned i = 1; i < N; i ++) {
        unsigned v = uint_dist(rng) % (N / 2);
        s1.insert(v);
        vals[i] = v;
        lean_assert(s1.contains(v));
    }
    std::cout << "step1\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i ++) {
        lean_assert(s1.contains(vals[i]));
    }
    std::cout << "step2\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i += 2) {
        s1.erase(vals[i]);
        lean_assert(!s1.contains(vals[i]));
    }
    std::cout << "step3\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i += 2) {
        s1.insert(vals[i]);
    }
    std::cout << "step4\n"; std::cout.flush();
    for (unsigned i = 1; i < N; i ++) {
        lean_assert(s1.contains(vals[i]));
    }
}

static void tst3() {
    std::uniform_int_distribution<unsigned int> uint_dist;
    std::mt19937   rng;
    rng.seed(static_cast<unsigned int>(time(0)));
    unsigned_set     s1;
    std_unsigned_set s2;
    unsigned N = uint_dist(rng) % 1000;
    for (unsigned i = 0; i < N; i++) {
        unsigned v = uint_dist(rng)%1000;
        if (uint_dist(rng) % 3 == 2) {
            s1.erase(v);
            s2.erase(v);
            lean_assert(!s1.contains(v));
        } else {
            s1.insert(v);
            s2.insert(v);
            lean_assert(s1.contains(v));
        }
    }
    {
        std_unsigned_set::iterator it  = s2.begin();
        std_unsigned_set::iterator end = s2.end();
        for (; it != end; ++it) {
            lean_assert(s1.contains(*it));
        }
    }
    {
        unsigned n = 0;
        s1.for_each([&](unsigned v) {
                lean_assert(s2.find(v) != s2.end());
                n++;
            });
        lean_assert(n == s1.size());
    }
    lean_assert(s1.size() == s2.size());
}

static void tst4() {
    unsigned_set s1;
    s1.insert(10);
    s1.insert(20);
    unsigned_set s2 = s1;
    lean_assert(s2.contains(10));
    lean_assert(s2.contains(20));
    lean_assert(!s2.contains(30));
    s2.insert(30);
    lean_assert(s2.contains(30));
    lean_assert(!s1.contains(30));
}

static void tst5() {
    /* This example would take a few minute if we use std_unsigned_set.
       With unsigned_set, it takes a few milliseconds. */
    timeit timer(std::cout, "tst5");
    unsigned_set s1;
    for (unsigned i = 0; i < 100000; i++) {
        s1.insert(i);
    }
    for (unsigned i = 0; i < 100000; i++) {
        unsigned_set s2 = s1;
        s2.insert(99999999);
        lean_assert(s2.contains(10));
        lean_assert(s2.contains(99999999));
    }
    lean_assert(!s1.contains(99999999));
}

int main() {
    save_stack_info();
    initialize_util_module();
    initialize_sexpr_module();
    initialize_kernel_module();
    initialize_library_core_module();
    initialize_library_module();

    tst1();
    tst2();
    for (int i = 0; i < 1000; i++)
        tst3();
    tst4();
    tst5();

    finalize_library_module();
    finalize_library_core_module();
    finalize_kernel_module();
    finalize_sexpr_module();
    finalize_util_module();
    return has_violations() ? 1 : 0;
}
<|endoftext|>