<commit_before>/** @file
    @brief Implementation

    @date 2015, 2016

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015, 2016 Sensics, Inc.
//
// 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.

// Internal Includes
#include "PoseEstimator_RANSAC.h"
#include "LED.h"
#include "CameraParameters.h"
#include "cvToEigen.h"

// Library/third-party includes
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

// Standard includes
#include <algorithm>

namespace osvr {
namespace vbtracker {
    bool RANSACPoseEstimator::operator()(CameraParameters const &camParams,
                                         LedPtrList const &leds,
                                         BeaconStateVec const &beacons,
                                         std::vector<BeaconData> &beaconDebug,
                                         Eigen::Vector3d &outXlate,
                                         Eigen::Quaterniond &outQuat) {

        // We need to get a pair of matched vectors of points: 2D locations
        // with in the image and 3D locations in model space.  There needs to
        // be a correspondence between the points in these vectors, such that
        // the ith element in one matches the ith element in the other.  We
        // make these by looking up the locations of LEDs with known identifiers
        // and pushing both onto the vectors at the same time.

        std::vector<cv::Point3f> objectPoints;
        std::vector<cv::Point2f> imagePoints;
        std::vector<ZeroBasedBeaconId> beaconIds;
        for (auto const &led : leds) {
            auto id = makeZeroBased(led->getID());
            auto index = asIndex(id);
            beaconDebug[index].variance = -1;
            beaconDebug[index].measurement = led->getLocationForTracking();
            beaconIds.push_back(id);

            /// Effectively invert the image points here so we get the output of
            /// a coordinate system we want.
            imagePoints.push_back(led->getLocationForTracking());
            objectPoints.push_back(
                vec3dToCVPoint3f(beacons[index]->stateVector()));
        }

        // Make sure we have enough points to do our estimation.
        if (objectPoints.size() < m_permittedOutliers + m_requiredInliers) {
            return false;
        }

        // Produce an estimate of the translation and rotation needed to take
        // points from model space into camera space.  We allow for at most
        // m_permittedOutliers outliers. Even in simulation data, we sometimes
        // find duplicate IDs for LEDs, indicating that we are getting
        // mis-identified ones sometimes.
        // We tried using the previous guess to reduce the amount of computation
        // being done, but this got us stuck in infinite locations.  We seem to
        // do okay without using it, so leaving it out.
        // @todo Make number of iterations into a parameter.
        bool usePreviousGuess = false;
        int iterationsCount = 5;
        cv::Mat inlierIndices;

        cv::Mat rvec;
        cv::Mat tvec;
#if CV_MAJOR_VERSION == 2
        cv::solvePnPRansac(
            objectPoints, imagePoints, camParams.cameraMatrix,
            camParams.distortionParameters, rvec, tvec, usePreviousGuess,
            iterationsCount, 8.0f,
            static_cast<int>(objectPoints.size() - m_permittedOutliers),
            inlierIndices);
#elif CV_MAJOR_VERSION == 3
        // parameter added to the OpenCV 3.0 interface in place of the number of
        // inliers
        /// @todo how to determine this requested confidence from the data we're
        /// given?
        double confidence = 0.99;
        auto ransacResult = cv::solvePnPRansac(
            objectPoints, imagePoints, camParams.cameraMatrix,
            camParams.distortionParameters, rvec, tvec, usePreviousGuess,
            iterationsCount, 8.0f, confidence, inlierIndices);
        if (!ransacResult) {
            return false;
        }
#else
#error "Unrecognized OpenCV version!"
#endif

        //==========================================================================
        // Make sure we got all the inliers we needed.  Otherwise, reject this
        // pose.
        if (inlierIndices.rows < static_cast<int>(m_requiredInliers)) {
            return false;
        }

        //==========================================================================
        // Reproject the inliers into the image and make sure they are actually
        // close to the expected location; otherwise, we have a bad pose.
        const double pixelReprojectionErrorForSingleAxisMax = 4;
        if (inlierIndices.rows > 0) {
            std::vector<cv::Point3f> inlierObjectPoints;
            std::vector<cv::Point2f> inlierImagePoints;
            std::vector<ZeroBasedBeaconId> inlierBeaconIds;
            for (int i = 0; i < inlierIndices.rows; i++) {
                inlierObjectPoints.push_back(objectPoints[i]);
                inlierImagePoints.push_back(imagePoints[i]);
                inlierBeaconIds.push_back(beaconIds[i]);
            }
            std::vector<cv::Point2f> reprojectedPoints;
            cv::projectPoints(
                inlierObjectPoints, rvec, tvec, camParams.cameraMatrix,
                camParams.distortionParameters, reprojectedPoints);

            for (size_t i = 0; i < reprojectedPoints.size(); i++) {
                if (reprojectedPoints[i].x - inlierImagePoints[i].x >
                    pixelReprojectionErrorForSingleAxisMax) {
                    return false;
                }
                if (reprojectedPoints[i].y - inlierImagePoints[i].y >
                    pixelReprojectionErrorForSingleAxisMax) {
                    return false;
                }
            }

            /// Now, we will sort that vector of inlier beacon IDs so we can
            /// rapidly binary search it to flag the LEDs we used.

            // Need a custom comparator for the ID type.
            auto idComparator = [](ZeroBasedBeaconId const &lhs,
                                   ZeroBasedBeaconId const &rhs) {
                return lhs.value() < rhs.value();
            };
            std::sort(begin(inlierBeaconIds), end(inlierBeaconIds),
                      idComparator);
            // This lambda wraps binary_search to do what it says: check to see
            // if a given beacon ID is in the list of inlier beacons.
            auto isAnInlierBeacon = [&inlierBeaconIds, &idComparator](
                ZeroBasedBeaconId const &needle) {
                return std::binary_search(begin(inlierBeaconIds),
                                          end(inlierBeaconIds), needle,
                                          idComparator);
            };
            for (auto &led : leds) {
                if (isAnInlierBeacon(led->getID())) {
                    led->markAsUsed();
                }
            }
        }

        //==========================================================================
        // Convert this into an OSVR representation of the transformation that
        // gives the pose of the HDK origin in the camera coordinate system,
        // switching units to meters and encoding the angle in a unit
        // quaternion.
        // The matrix described by rvec and tvec takes points in model space
        // (the space where the LEDs are defined, which is in mm away from an
        // implied origin) into a coordinate system where the center is at the
        // camera's origin, with X to the right, Y down, and Z in the direction
        // that the camera is facing (but still in the original units of mm):
        //  |Xc|   |r11 r12 r13 t1| |Xm|
        //  |Yc| = |r21 r22 r23 t2|*|Ym|
        //  |Zc|   |r31 r32 r33 t3| |Zm|
        //                          |1 |
        //  That is, it rotates into the camera coordinate system and then adds
        // the translation, which is in the camera coordinate system.
        //  This is the transformation we want, since it reports the sensor's
        // position and orientation in camera space, except that we want to
        // convert
        // the units into meters and the orientation into a Quaternion.
        //  NOTE: This is a right-handed coordinate system with X pointing
        // towards the right from the camera center of projection, Y pointing
        // down, and Z pointing along the camera viewing direction, if the input
        // points are not inverted.

        outXlate = cvToVector3d(tvec);
        outQuat = cvRotVecToQuat(rvec);
        return true;
    }

    /// Variance in Meters^2
    static const double InitialPositionStateError = 0.;
    /// Variance in Radians^2
    static const double InitialOrientationStateError = 0.5;
    static const double InitialVelocityStateError = 0.;
    static const double InitialAngVelStateError =0.;
    static const double InitialStateError[] = {
        InitialPositionStateError,    InitialPositionStateError,
        InitialPositionStateError,    InitialOrientationStateError,
        InitialOrientationStateError, InitialOrientationStateError,
        InitialVelocityStateError,    InitialVelocityStateError,
        InitialVelocityStateError,    InitialAngVelStateError,
        InitialAngVelStateError,      InitialAngVelStateError};
    bool RANSACPoseEstimator::operator()(EstimatorInOutParams const &p,
                                         LedPtrList const &leds) {
        Eigen::Vector3d xlate;
        Eigen::Quaterniond quat;
        /// Call the main pose estimation to get the vector and quat.
        {
            auto ret = (*this)(p.camParams, leds, p.beacons, p.beaconDebug,
                               xlate, quat);
            if (!ret) {
                return false;
            }
        }
        /// OK, so if we're here, estimation succeeded and we have valid data in
        /// xlate and quat.
        p.state.position() = xlate;
        p.state.setQuaternion(quat);
        /// Zero things we can't measure.
#if 1
        p.state.incrementalOrientation() = Eigen::Vector3d::Zero();
        p.state.velocity() = Eigen::Vector3d::Zero();
        p.state.angularVelocity() = Eigen::Vector3d::Zero();
#endif
        using StateVec = kalman::types::DimVector<BodyState>;
        using StateSquareMatrix = kalman::types::DimSquareMatrix<BodyState>;

        StateSquareMatrix covariance = StateVec(InitialStateError).asDiagonal();
        /// @todo Copy the existing angular velocity error covariance
        /*
        covariance.bottomRightCorner<3, 3>() =
            p.state.errorCovariance().bottomRightCorner<3, 3>();
            */
        p.state.setErrorCovariance(covariance);
        return true;
    }

} // namespace vbtracker
} // namespace osvr
<commit_msg>Style<commit_after>/** @file
    @brief Implementation

    @date 2015, 2016

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015, 2016 Sensics, Inc.
//
// 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.

// Internal Includes
#include "PoseEstimator_RANSAC.h"
#include "LED.h"
#include "CameraParameters.h"
#include "cvToEigen.h"

// Library/third-party includes
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

// Standard includes
#include <algorithm>

namespace osvr {
namespace vbtracker {
    bool RANSACPoseEstimator::operator()(CameraParameters const &camParams,
                                         LedPtrList const &leds,
                                         BeaconStateVec const &beacons,
                                         std::vector<BeaconData> &beaconDebug,
                                         Eigen::Vector3d &outXlate,
                                         Eigen::Quaterniond &outQuat) {

        // We need to get a pair of matched vectors of points: 2D locations
        // with in the image and 3D locations in model space.  There needs to
        // be a correspondence between the points in these vectors, such that
        // the ith element in one matches the ith element in the other.  We
        // make these by looking up the locations of LEDs with known identifiers
        // and pushing both onto the vectors at the same time.

        std::vector<cv::Point3f> objectPoints;
        std::vector<cv::Point2f> imagePoints;
        std::vector<ZeroBasedBeaconId> beaconIds;
        for (auto const &led : leds) {
            auto id = makeZeroBased(led->getID());
            auto index = asIndex(id);
            beaconDebug[index].variance = -1;
            beaconDebug[index].measurement = led->getLocationForTracking();
            beaconIds.push_back(id);

            /// Effectively invert the image points here so we get the output of
            /// a coordinate system we want.
            imagePoints.push_back(led->getLocationForTracking());
            objectPoints.push_back(
                vec3dToCVPoint3f(beacons[index]->stateVector()));
        }

        // Make sure we have enough points to do our estimation.
        if (objectPoints.size() < m_permittedOutliers + m_requiredInliers) {
            return false;
        }

        // Produce an estimate of the translation and rotation needed to take
        // points from model space into camera space.  We allow for at most
        // m_permittedOutliers outliers. Even in simulation data, we sometimes
        // find duplicate IDs for LEDs, indicating that we are getting
        // mis-identified ones sometimes.
        // We tried using the previous guess to reduce the amount of computation
        // being done, but this got us stuck in infinite locations.  We seem to
        // do okay without using it, so leaving it out.
        // @todo Make number of iterations into a parameter.
        bool usePreviousGuess = false;
        int iterationsCount = 5;
        cv::Mat inlierIndices;

        cv::Mat rvec;
        cv::Mat tvec;
#if CV_MAJOR_VERSION == 2
        cv::solvePnPRansac(
            objectPoints, imagePoints, camParams.cameraMatrix,
            camParams.distortionParameters, rvec, tvec, usePreviousGuess,
            iterationsCount, 8.0f,
            static_cast<int>(objectPoints.size() - m_permittedOutliers),
            inlierIndices);
#elif CV_MAJOR_VERSION == 3
        // parameter added to the OpenCV 3.0 interface in place of the number of
        // inliers
        /// @todo how to determine this requested confidence from the data we're
        /// given?
        double confidence = 0.99;
        auto ransacResult = cv::solvePnPRansac(
            objectPoints, imagePoints, camParams.cameraMatrix,
            camParams.distortionParameters, rvec, tvec, usePreviousGuess,
            iterationsCount, 8.0f, confidence, inlierIndices);
        if (!ransacResult) {
            return false;
        }
#else
#error "Unrecognized OpenCV version!"
#endif

        //==========================================================================
        // Make sure we got all the inliers we needed.  Otherwise, reject this
        // pose.
        if (inlierIndices.rows < static_cast<int>(m_requiredInliers)) {
            return false;
        }

        //==========================================================================
        // Reproject the inliers into the image and make sure they are actually
        // close to the expected location; otherwise, we have a bad pose.
        const double pixelReprojectionErrorForSingleAxisMax = 4;
        if (inlierIndices.rows > 0) {
            std::vector<cv::Point3f> inlierObjectPoints;
            std::vector<cv::Point2f> inlierImagePoints;
            std::vector<ZeroBasedBeaconId> inlierBeaconIds;
            for (int i = 0; i < inlierIndices.rows; i++) {
                inlierObjectPoints.push_back(objectPoints[i]);
                inlierImagePoints.push_back(imagePoints[i]);
                inlierBeaconIds.push_back(beaconIds[i]);
            }
            std::vector<cv::Point2f> reprojectedPoints;
            cv::projectPoints(
                inlierObjectPoints, rvec, tvec, camParams.cameraMatrix,
                camParams.distortionParameters, reprojectedPoints);

            for (size_t i = 0; i < reprojectedPoints.size(); i++) {
                if (reprojectedPoints[i].x - inlierImagePoints[i].x >
                    pixelReprojectionErrorForSingleAxisMax) {
                    return false;
                }
                if (reprojectedPoints[i].y - inlierImagePoints[i].y >
                    pixelReprojectionErrorForSingleAxisMax) {
                    return false;
                }
            }

            /// Now, we will sort that vector of inlier beacon IDs so we can
            /// rapidly binary search it to flag the LEDs we used.

            // Need a custom comparator for the ID type.
            auto idComparator = [](ZeroBasedBeaconId const &lhs,
                                   ZeroBasedBeaconId const &rhs) {
                return lhs.value() < rhs.value();
            };
            std::sort(begin(inlierBeaconIds), end(inlierBeaconIds),
                      idComparator);
            // This lambda wraps binary_search to do what it says: check to see
            // if a given beacon ID is in the list of inlier beacons.
            auto isAnInlierBeacon = [&inlierBeaconIds, &idComparator](
                ZeroBasedBeaconId const &needle) {
                return std::binary_search(begin(inlierBeaconIds),
                                          end(inlierBeaconIds), needle,
                                          idComparator);
            };
            for (auto &led : leds) {
                if (isAnInlierBeacon(led->getID())) {
                    led->markAsUsed();
                }
            }
        }

        //==========================================================================
        // Convert this into an OSVR representation of the transformation that
        // gives the pose of the HDK origin in the camera coordinate system,
        // switching units to meters and encoding the angle in a unit
        // quaternion.
        // The matrix described by rvec and tvec takes points in model space
        // (the space where the LEDs are defined, which is in mm away from an
        // implied origin) into a coordinate system where the center is at the
        // camera's origin, with X to the right, Y down, and Z in the direction
        // that the camera is facing (but still in the original units of mm):
        //  |Xc|   |r11 r12 r13 t1| |Xm|
        //  |Yc| = |r21 r22 r23 t2|*|Ym|
        //  |Zc|   |r31 r32 r33 t3| |Zm|
        //                          |1 |
        //  That is, it rotates into the camera coordinate system and then adds
        // the translation, which is in the camera coordinate system.
        //  This is the transformation we want, since it reports the sensor's
        // position and orientation in camera space, except that we want to
        // convert
        // the units into meters and the orientation into a Quaternion.
        //  NOTE: This is a right-handed coordinate system with X pointing
        // towards the right from the camera center of projection, Y pointing
        // down, and Z pointing along the camera viewing direction, if the input
        // points are not inverted.

        outXlate = cvToVector3d(tvec);
        outQuat = cvRotVecToQuat(rvec);
        return true;
    }

    /// Variance in Meters^2
    static const double InitialPositionStateError = 0.;
    /// Variance in Radians^2
    static const double InitialOrientationStateError = 0.5;
    static const double InitialVelocityStateError = 0.;
    static const double InitialAngVelStateError = 0.;
    static const double InitialStateError[] = {
        InitialPositionStateError,    InitialPositionStateError,
        InitialPositionStateError,    InitialOrientationStateError,
        InitialOrientationStateError, InitialOrientationStateError,
        InitialVelocityStateError,    InitialVelocityStateError,
        InitialVelocityStateError,    InitialAngVelStateError,
        InitialAngVelStateError,      InitialAngVelStateError};
    bool RANSACPoseEstimator::operator()(EstimatorInOutParams const &p,
                                         LedPtrList const &leds) {
        Eigen::Vector3d xlate;
        Eigen::Quaterniond quat;
        /// Call the main pose estimation to get the vector and quat.
        {
            auto ret = (*this)(p.camParams, leds, p.beacons, p.beaconDebug,
                               xlate, quat);
            if (!ret) {
                return false;
            }
        }
        /// OK, so if we're here, estimation succeeded and we have valid data in
        /// xlate and quat.
        p.state.position() = xlate;
        p.state.setQuaternion(quat);
/// Zero things we can't measure.
#if 1
        p.state.incrementalOrientation() = Eigen::Vector3d::Zero();
        p.state.velocity() = Eigen::Vector3d::Zero();
        p.state.angularVelocity() = Eigen::Vector3d::Zero();
#endif
        using StateVec = kalman::types::DimVector<BodyState>;
        using StateSquareMatrix = kalman::types::DimSquareMatrix<BodyState>;

        StateSquareMatrix covariance = StateVec(InitialStateError).asDiagonal();
        /// @todo Copy the existing angular velocity error covariance
        /*
        covariance.bottomRightCorner<3, 3>() =
            p.state.errorCovariance().bottomRightCorner<3, 3>();
            */
        p.state.setErrorCovariance(covariance);
        return true;
    }

} // namespace vbtracker
} // namespace osvr
<|endoftext|>


<commit_before><commit_msg>fix for running externandnotdefined plugin on Ubuntu<commit_after><|endoftext|>


<commit_before>// MatrixKeyboard.hh
/*
 *	A class that creates a simple matrix keyboard
 * 	using the wiringPi library and the X11 Xtest
 * 	extension.
 */

#ifndef _MATRIX_KEYBOARD_HH
#define _MATRIX_KEYBOARD_HH

#include <wiringPi.h>
#include <X11/Xlib.h>
#include <X11/Intrinsic.h>
#include <X11/extensions/XTest.h>

#include <iostream>
#include <chrono>
#include <thread>

using namespace std;
using namespace std::chrono;
using namespace std::this_thread;

typedef unsigned int uInt;
struct Key
{
	uInt rowPin;
	uInt colPin;
	KeySym keysym;
	uInt lastState;
};

static const bool KEY_DOWN = true;
static const bool KEY_UP = false;
static const uInt nRows = 1;
static const uInt nCols = 2;

class MatrixKeyboard
{
private:

	Display * myXDisplay;
	Key keyboard[nRows][nCols];

public:
	MatrixKeyboard();

private:
	void pressKey(KeySym keysym);
	void releaseKey(KeySym keysym);

public:
	void run();
};

#endif // ! _MATRIX_KEYBOARD_HH<commit_msg>Full reworked class<commit_after>#ifndef _MATRIX_KEYBOARD_HH
#define _MATRIX_KEYBOARD_HH

#include <wiringPi.h>
#include <X11/Xlib.h>
#include <X11/Intrinsic.h>
#include <X11/extensions/XTest.h>

#include <iostream>
#include <chrono>
#include <thread>
#include <vector>

using namespace std;
using namespace std::chrono;
using namespace std::this_thread;

typedef unsigned int uInt;

class MatrixKeyboard
{
private:
	Display * myXDisplay;
	vector<uInt> rows;
	vector<uInt> cols;
	vector<KeySym> keys;
	vector<bool> lastStates;
	uInt delay;

public:
	MatrixKeyboard(vector<uInt> rows, vector<uInt> cols, vector<KeySym> keys);
	void run();

private:
	void pressKey(KeySym keysym);
	void releaseKey(KeySym keysym);

};

#endif // ! _MATRIX_KEYBOARD_HH
<|endoftext|>


<commit_before>/*
    knwidgets.cpp

    KNode, the KDE newsreader
    Copyright (c) 1999-2001 the KNode authors.
    See file AUTHORS for details

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software Foundation,
    Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, US
*/

#include <qpainter.h>

#include "knwidgets.h"

//====================================================================================

KNListBoxItem::KNListBoxItem(const QString& text, QPixmap *pm)
{
  p_m=pm;
  setText(text);
}

KNListBoxItem::~KNListBoxItem()
{
}

void KNListBoxItem::paint(QPainter *p)
{

  QFontMetrics fm = p->fontMetrics();

  int tYPos=0, tXPos=3, pYPos=0;

  tYPos = fm.ascent() + fm.leading()/2; // vertical text position

  if(p_m) {

    tXPos=p_m->width() + 6;

    if ( p_m->height() < fm.height() )  {
      //tYPos = fm.ascent() + fm.leading()/2;
      pYPos = (fm.height() - p_m->height())/2;}
    else {
      tYPos = p_m->height()/2 - fm.height()/2 + fm.ascent();
      pYPos = 0;
    }
    p->drawPixmap( 3, pYPos ,  *p_m );
  }

  p->drawText( tXPos, tYPos, text() );
}

int KNListBoxItem::height(const QListBox *lb) const
{
  if(p_m)
    return QMAX( p_m->height(), lb->fontMetrics().lineSpacing() + 1 );
  else
    return (lb->fontMetrics().lineSpacing() + 1);
}

int KNListBoxItem::width(const QListBox *lb) const
{
  if(p_m)
    return (p_m->width() + lb->fontMetrics().width( text() ) + 6);
  else
    return (lb->fontMetrics().width( text() ) + 6);
}

//====================================================================================

// **** listbox for dialogs **************************************************

KNDialogListBox::KNDialogListBox(bool alwaysIgnore, QWidget * parent, const char * name)
 : QListBox(parent, name), a_lwaysIgnore(alwaysIgnore)
{
}

KNDialogListBox::~KNDialogListBox()
{
}

void KNDialogListBox::keyPressEvent(QKeyEvent *e)
{
  if ((a_lwaysIgnore || !(hasFocus()&&isVisible()))&&((e->key()==Key_Enter)||(e->key()==Key_Return)))
    e->ignore();
  else
    QListBox::keyPressEvent(e);
}

//====================================================================================

KNProgress::KNProgress (int desiredHeight, int maxValue, int value, QWidget *parent, const char *name)
 : KProgress(parent, name), desHeight(desiredHeight)
{
  setRange(0, maxValue);
  setValue(value);
  setFixedWidth(110);
  setFrameStyle(QFrame::Box | QFrame::Raised);
  setLineWidth(1);
  setBackgroundMode(QWidget::PaletteBackground);
  disableProgressBar();
}

KNProgress::~KNProgress()
{}

// 0% and no text
void KNProgress::disableProgressBar()
{
  setFormat(QString::null);
  setValue(0);
  repaint(false);
}

// manual operation
void KNProgress::setProgressBar(int value,const QString& text)
{
  setFormat(text);
  if (value>1000) {
    setValue(1000);
    update();       // circumvent the optimization of setValue
  } else
    setValue(value);
}

QSize KNProgress::sizeHint() const
{
  return QSize(KProgress::sizeHint().width(),desHeight);
}

//====================================================================================

KNDockWidgetHeaderDrag::KNDockWidgetHeaderDrag(QWidget *focusWidget, KDockWidgetAbstractHeader* parent, KDockWidget* dock, const char* name )
  : KDockWidgetHeaderDrag(parent, dock, name), f_ocus(false)
{
  connect(focusWidget, SIGNAL(focusChanged(QFocusEvent*)), SLOT(slotFocusChanged(QFocusEvent*)));
}

KNDockWidgetHeaderDrag::~KNDockWidgetHeaderDrag()
{
}

void KNDockWidgetHeaderDrag::slotFocusChanged(QFocusEvent *e)
{
  if(e->gotFocus()) {
    f_ocus = true;
  } else if(e->lostFocus()) {
    f_ocus = false;
  }
  update();
}

void KNDockWidgetHeaderDrag::paintEvent(QPaintEvent* ev)
{
  if (!f_ocus) {
    KDockWidgetHeaderDrag::paintEvent(ev);
    return;
  }

  QPixmap drawBuffer(width(), height());
  QPainter paint;

  paint.begin(&drawBuffer);
  paint.fillRect(drawBuffer.rect(), QBrush(colorGroup().brush(QColorGroup::Background)));

  paint.setPen(palette().active().highlight());
  paint.drawLine(1, 2, width(), 2);
  paint.drawLine(1, 3, width(), 3);
  paint.drawLine(1, 5, width(), 5);
  paint.drawLine(1, 6, width(), 6);

  bitBlt( this,0,0,&drawBuffer,0,0,width(),height());
  paint.end();
}

//====================================================================================

#include "knwidgets.moc"
<commit_msg>looks better with the new linespacing behaviour of Qt -  setFrameStyle(QFrame::Box | QFrame::Raised); +  setFrameStyle(QFrame::Box);<commit_after>/*
    knwidgets.cpp

    KNode, the KDE newsreader
    Copyright (c) 1999-2001 the KNode authors.
    See file AUTHORS for details

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software Foundation,
    Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, US
*/

#include <qpainter.h>

#include "knwidgets.h"

//====================================================================================

KNListBoxItem::KNListBoxItem(const QString& text, QPixmap *pm)
{
  p_m=pm;
  setText(text);
}

KNListBoxItem::~KNListBoxItem()
{
}

void KNListBoxItem::paint(QPainter *p)
{

  QFontMetrics fm = p->fontMetrics();

  int tYPos=0, tXPos=3, pYPos=0;

  tYPos = fm.ascent() + fm.leading()/2; // vertical text position

  if(p_m) {

    tXPos=p_m->width() + 6;

    if ( p_m->height() < fm.height() )  {
      //tYPos = fm.ascent() + fm.leading()/2;
      pYPos = (fm.height() - p_m->height())/2;}
    else {
      tYPos = p_m->height()/2 - fm.height()/2 + fm.ascent();
      pYPos = 0;
    }
    p->drawPixmap( 3, pYPos ,  *p_m );
  }

  p->drawText( tXPos, tYPos, text() );
}

int KNListBoxItem::height(const QListBox *lb) const
{
  if(p_m)
    return QMAX( p_m->height(), lb->fontMetrics().lineSpacing() + 1 );
  else
    return (lb->fontMetrics().lineSpacing() + 1);
}

int KNListBoxItem::width(const QListBox *lb) const
{
  if(p_m)
    return (p_m->width() + lb->fontMetrics().width( text() ) + 6);
  else
    return (lb->fontMetrics().width( text() ) + 6);
}

//====================================================================================

// **** listbox for dialogs **************************************************

KNDialogListBox::KNDialogListBox(bool alwaysIgnore, QWidget * parent, const char * name)
 : QListBox(parent, name), a_lwaysIgnore(alwaysIgnore)
{
}

KNDialogListBox::~KNDialogListBox()
{
}

void KNDialogListBox::keyPressEvent(QKeyEvent *e)
{
  if ((a_lwaysIgnore || !(hasFocus()&&isVisible()))&&((e->key()==Key_Enter)||(e->key()==Key_Return)))
    e->ignore();
  else
    QListBox::keyPressEvent(e);
}

//====================================================================================

KNProgress::KNProgress (int desiredHeight, int maxValue, int value, QWidget *parent, const char *name)
 : KProgress(parent, name), desHeight(desiredHeight)
{
  setRange(0, maxValue);
  setValue(value);
  setFixedWidth(110);
  setFrameStyle(QFrame::Box);
  setLineWidth(1);
  setBackgroundMode(QWidget::PaletteBackground);
  disableProgressBar();
}

KNProgress::~KNProgress()
{}

// 0% and no text
void KNProgress::disableProgressBar()
{
  setFormat(QString::null);
  setValue(0);
  repaint(false);
}

// manual operation
void KNProgress::setProgressBar(int value,const QString& text)
{
  setFormat(text);
  if (value>1000) {
    setValue(1000);
    update();       // circumvent the optimization of setValue
  } else
    setValue(value);
}

QSize KNProgress::sizeHint() const
{
  return QSize(KProgress::sizeHint().width(),desHeight);
}

//====================================================================================

KNDockWidgetHeaderDrag::KNDockWidgetHeaderDrag(QWidget *focusWidget, KDockWidgetAbstractHeader* parent, KDockWidget* dock, const char* name )
  : KDockWidgetHeaderDrag(parent, dock, name), f_ocus(false)
{
  connect(focusWidget, SIGNAL(focusChanged(QFocusEvent*)), SLOT(slotFocusChanged(QFocusEvent*)));
}

KNDockWidgetHeaderDrag::~KNDockWidgetHeaderDrag()
{
}

void KNDockWidgetHeaderDrag::slotFocusChanged(QFocusEvent *e)
{
  if(e->gotFocus()) {
    f_ocus = true;
  } else if(e->lostFocus()) {
    f_ocus = false;
  }
  update();
}

void KNDockWidgetHeaderDrag::paintEvent(QPaintEvent* ev)
{
  if (!f_ocus) {
    KDockWidgetHeaderDrag::paintEvent(ev);
    return;
  }

  QPixmap drawBuffer(width(), height());
  QPainter paint;

  paint.begin(&drawBuffer);
  paint.fillRect(drawBuffer.rect(), QBrush(colorGroup().brush(QColorGroup::Background)));

  paint.setPen(palette().active().highlight());
  paint.drawLine(1, 2, width(), 2);
  paint.drawLine(1, 3, width(), 3);
  paint.drawLine(1, 5, width(), 5);
  paint.drawLine(1, 6, width(), 6);

  bitBlt( this,0,0,&drawBuffer,0,0,width(),height());
  paint.end();
}

//====================================================================================

#include "knwidgets.moc"
<|endoftext|>


<commit_before>#include <Arduino.h>

#include "DollhousePanel.h"

#include <Adafruit_TLC59711.h>
#include <Adafruit_NeoPixel.h>
#include <LiquidCrystal.h>
#include <SPI.h>
#include <Fsm.h>
#include <EnableInterrupt.h>

const char NUM_TLC59711 = 1;
const char TLC_DATA = 12;
const char TLC_CLK = 13;

const char PIXEL_COUNT = 3;
const char PIXEL_PIN = 8;

enum events {
  CHANGE_LIGHT_MODE,
  NEXT_ROOM,
  PREVIOUS_ROOM,
  RESET_ROOMS
};

// Lighting modes finite state machine
State state_lighting_mode(on_lighting_mode_enter, NULL, &on_lighting_mode_exit);
State state_party_mode(on_party_mode_enter, NULL, &on_party_mode_exit);
State state_nitelite_mode(on_nitelite_mode_enter, NULL, &on_nitelite_mode_exit);
State state_off_mode(on_off_mode_enter, NULL, &on_off_mode_exit);
Fsm modes(&state_off_mode);

enum Modes {
  LIGHTING_MODE,
  PARTY_MODE,
  NITELITE_MODE,
  OFF_MODE
};

String modeNames[] = {"Lighting", "Party", "Nitelite", "Off"};

// Rooms finite state machine
State state_all_rooms(on_all_enter, NULL, &on_all_exit);
State state_hall(on_hall_enter, NULL, &on_hall_exit);
State state_living_room(on_living_room_enter, NULL, &on_living_room_exit);
State state_kitchen(on_kitchen_enter, NULL, &on_kitchen_exit);
State state_bedroom(on_bedroom_enter, NULL, &on_bedroom_exit);
State state_bathroom(on_bathroom_enter, NULL, &on_bathroom_exit);
State state_attic(on_attic_enter, NULL, &on_attic_exit);
Fsm rooms(&state_all_rooms);

// LastROOM is included to make it easier to figure out the size of the enum
// for things like sizing the brightness state array
enum Rooms {
  ALL_ROOMS,
  LIVING_ROOM,
  HALL,
  KITCHEN,
  BEDROOM,
  BATHROOM,
  ATTIC,
  LastROOM
};

String roomNames[] = {"All", "Living", "Hall", "Kitchen", "Bedroom", "Bathroom", "Attic"};

// NeoPixels (for the attic & !!!PARTY MODE!!!)
Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, NEO_GRB + NEO_KHZ800);

// PWM board (controls the room lights)
Adafruit_TLC59711 tlc = Adafruit_TLC59711(NUM_TLC59711, TLC_CLK, TLC_DATA);

// 16x2 LCD display
LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

// Panel RGB LED pins
const char RED_PIN = 9;
const char GREEN_PIN = 10;
const char BLUE_PIN = 11;

int brightness = 90;
int deltaLevel = 30;
int minLevel = 0;
int maxLevel = 180;
int roomBrightness[LastROOM];
int currentRoom = ALL_ROOMS;
int currentMode = OFF_MODE;

int debounceDelay = 100;

void setup() {
  // Fire up the LCD display
  lcd.begin(16, 2);
  lcd.print("Doll house");
  lcd.setCursor(0,1);
  lcd.print("lighting!");

  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);

  // initialize the NeoPixel strand
  strip.begin();
  strip.show();

  // Initialize the PWM board
  tlc.begin();
  tlc.write();

  // set defualt room brightness
  setDefaultLightLevel();

  // enable interrupts on buttons
  // The button interface is a Smartmaker 5A5 (annoying, but it works)
  enableInterrupt(A0, handleButtonOne, FALLING);
  enableInterrupt(A1, handleButtonTwo, FALLING);
  enableInterrupt(A2, handleButtonThree, FALLING);
  enableInterrupt(A3, handleButtonFour, FALLING);
  enableInterrupt(A4, handleButtonFive, FALLING);

  // mode FSM transitions
  modes.add_transition(&state_off_mode, &state_lighting_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_lighting_mode, &state_party_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_party_mode, &state_nitelite_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_nitelite_mode, &state_off_mode, CHANGE_LIGHT_MODE, NULL);

  // rooms FSM transitions
  // looping "forward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_hall, NEXT_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_living_room, NEXT_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_kitchen, NEXT_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_bedroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_bathroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_attic, NEXT_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, NEXT_ROOM, NULL);

  // looping "backward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_attic, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_bathroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_bedroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_kitchen, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_living_room, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_hall, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_all_rooms, PREVIOUS_ROOM, NULL);

  // reseting to the default room (all rooms)
  rooms.add_transition(&state_hall, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_living_room, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_kitchen, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bedroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bathroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, RESET_ROOMS, NULL);
}

// ***** Button event handlers ***** //
void handleButtonOne() {
  lcd.clear();
  rooms.trigger(RESET_ROOMS);
  modes.trigger(CHANGE_LIGHT_MODE);
  delay(debounceDelay);
}

void handleButtonTwo() {
  setRoomBrightness(currentRoom, min(roomBrightness[currentRoom] + deltaLevel, maxLevel));
  printCurrentRoom();
  delay(debounceDelay);
}

void handleButtonThree() {
  lcd.clear();
  rooms.trigger(PREVIOUS_ROOM);
  delay(debounceDelay);
}

void handleButtonFour() {
  setRoomBrightness(currentRoom, max(roomBrightness[currentRoom] - deltaLevel, minLevel));
  printCurrentRoom();
  delay(debounceDelay);
}

void handleButtonFive() {
  lcd.clear();
  rooms.trigger(NEXT_ROOM);
  delay(debounceDelay);
}

// ***** helpers ***** //

void setRGBColor(int red, int green, int blue) {
  int myRed = constrain(red, 0, 255);
  int myGreen = constrain(green, 0, 255);
  int myBlue = constrain(blue, 0, 255);

  analogWrite(RED_PIN, myRed);
  analogWrite(GREEN_PIN, myGreen);
  analogWrite(BLUE_PIN, myBlue);
}

void setRoomBrightness(int room, int level) {
  setRGBColor(0,0,level);
  roomBrightness[room] = level;
  tlc.setPWM(room * 3, roomBrightness[room] * maxLevel);
  tlc.write();
}

void setDefaultLightLevel() {
  setRGBColor(0,0,brightness);
  for (int i = 0; i != LastROOM; i++) {
    roomBrightness[i] = brightness;
  }
}

void setCurrentMode(int mode) {
  currentMode = mode;
  printCurrentMode();
}

void printCurrentMode() {
  lcd.clear();
  lcd.print("Mode: ");
  lcd.print(modeNames[currentMode]);
}

void setCurrentRoom(int room) {
  currentRoom = room;
  setRGBColor(0,0,roomBrightness[room]);
  printCurrentRoom();
}

void printCurrentRoom() {
  lcd.clear();
  lcd.print("room: ");
  lcd.print(roomNames[currentRoom]);
  lcd.setCursor(0,1);
  lcd.print("brightness: ");
  lcd.print(roomBrightness[currentRoom]);
}

// ***** FSM event handlers ***** //

// ---- lighting mode states ---- //

void on_lighting_mode_enter(){
  setCurrentMode(LIGHTING_MODE);
}

void on_lighting_mode_exit(){

}

void on_party_mode_enter(){
  setCurrentMode(PARTY_MODE);
}

void on_party_mode_exit(){

}

void on_nitelite_mode_enter(){
  setCurrentMode(NITELITE_MODE);
}

void on_nitelite_mode_exit(){

}

void on_off_mode_enter(){
  setCurrentMode(OFF_MODE);
}

void on_off_mode_exit(){

}

// ---- room selection states ---- //
void on_all_enter() {
  setCurrentRoom(ALL_ROOMS);
}

void on_all_exit() {

}

void on_hall_enter() {
  setCurrentRoom(HALL);
}

void on_hall_exit() {

}

void on_living_room_enter() {
  setCurrentRoom(LIVING_ROOM);
}

void on_living_room_exit() {

}

void on_kitchen_enter() {
  setCurrentRoom(KITCHEN);
}

void on_kitchen_exit() {

}

void on_bathroom_enter() {
  setCurrentRoom(BATHROOM);
}

void on_bathroom_exit() {

}

void on_bedroom_enter() {
  setCurrentRoom(BEDROOM);
}

void on_bedroom_exit() {

}

void on_attic_enter() {
  setCurrentRoom(ATTIC);
}

void on_attic_exit() {

}

void loop() {
  // do nothing; everything is handled via FSM events.
  // We also don't need to call the ".run_machine" methods of
  // the FSMs as there are no "on_state" handlers or timed transitions
}
<commit_msg>Add notes re button functions<commit_after>#include <Arduino.h>

#include "DollhousePanel.h"

#include <Adafruit_TLC59711.h>
#include <Adafruit_NeoPixel.h>
#include <LiquidCrystal.h>
#include <SPI.h>
#include <Fsm.h>
#include <EnableInterrupt.h>

const char NUM_TLC59711 = 1;
const char TLC_DATA = 12;
const char TLC_CLK = 13;

const char PIXEL_COUNT = 3;
const char PIXEL_PIN = 8;

enum events {
  CHANGE_LIGHT_MODE,
  NEXT_ROOM,
  PREVIOUS_ROOM,
  RESET_ROOMS
};

// Lighting modes finite state machine
State state_lighting_mode(on_lighting_mode_enter, NULL, &on_lighting_mode_exit);
State state_party_mode(on_party_mode_enter, NULL, &on_party_mode_exit);
State state_nitelite_mode(on_nitelite_mode_enter, NULL, &on_nitelite_mode_exit);
State state_off_mode(on_off_mode_enter, NULL, &on_off_mode_exit);
Fsm modes(&state_off_mode);

enum Modes {
  LIGHTING_MODE,
  PARTY_MODE,
  NITELITE_MODE,
  OFF_MODE
};

String modeNames[] = {"Lighting", "Party", "Nitelite", "Off"};

// Rooms finite state machine
State state_all_rooms(on_all_enter, NULL, &on_all_exit);
State state_hall(on_hall_enter, NULL, &on_hall_exit);
State state_living_room(on_living_room_enter, NULL, &on_living_room_exit);
State state_kitchen(on_kitchen_enter, NULL, &on_kitchen_exit);
State state_bedroom(on_bedroom_enter, NULL, &on_bedroom_exit);
State state_bathroom(on_bathroom_enter, NULL, &on_bathroom_exit);
State state_attic(on_attic_enter, NULL, &on_attic_exit);
Fsm rooms(&state_all_rooms);

// LastROOM is included to make it easier to figure out the size of the enum
// for things like sizing the brightness state array
enum Rooms {
  ALL_ROOMS,
  LIVING_ROOM,
  HALL,
  KITCHEN,
  BEDROOM,
  BATHROOM,
  ATTIC,
  LastROOM
};

String roomNames[] = {"All", "Living", "Hall", "Kitchen", "Bedroom", "Bathroom", "Attic"};

// NeoPixels (for the attic & !!!PARTY MODE!!!)
Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, NEO_GRB + NEO_KHZ800);

// PWM board (controls the room lights)
Adafruit_TLC59711 tlc = Adafruit_TLC59711(NUM_TLC59711, TLC_CLK, TLC_DATA);

// 16x2 LCD display
LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

// Panel RGB LED pins
const char RED_PIN = 9;
const char GREEN_PIN = 10;
const char BLUE_PIN = 11;

int brightness = 90;
int deltaLevel = 30;
int minLevel = 0;
int maxLevel = 180;
int roomBrightness[LastROOM];
int currentRoom = ALL_ROOMS;
int currentMode = OFF_MODE;

int debounceDelay = 100;

void setup() {
  // Fire up the LCD display
  lcd.begin(16, 2);
  lcd.print("Doll house");
  lcd.setCursor(0,1);
  lcd.print("lighting!");

  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);

  // initialize the NeoPixel strand
  strip.begin();
  strip.show();

  // Initialize the PWM board
  tlc.begin();
  tlc.write();

  // set defualt room brightness
  setDefaultLightLevel();

  // enable interrupts on buttons
  // The button interface is a Smartmaker 5A5 (annoying, but it works)
  enableInterrupt(A0, handleButtonOne, FALLING);
  enableInterrupt(A1, handleButtonTwo, FALLING);
  enableInterrupt(A2, handleButtonThree, FALLING);
  enableInterrupt(A3, handleButtonFour, FALLING);
  enableInterrupt(A4, handleButtonFive, FALLING);

  // mode FSM transitions
  modes.add_transition(&state_off_mode, &state_lighting_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_lighting_mode, &state_party_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_party_mode, &state_nitelite_mode, CHANGE_LIGHT_MODE, NULL);
  modes.add_transition(&state_nitelite_mode, &state_off_mode, CHANGE_LIGHT_MODE, NULL);

  // rooms FSM transitions
  // looping "forward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_hall, NEXT_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_living_room, NEXT_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_kitchen, NEXT_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_bedroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_bathroom, NEXT_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_attic, NEXT_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, NEXT_ROOM, NULL);

  // looping "backward" through the rooms
  rooms.add_transition(&state_all_rooms, &state_attic, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_attic, &state_bathroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bathroom, &state_bedroom, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_bedroom, &state_kitchen, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_kitchen, &state_living_room, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_living_room, &state_hall, PREVIOUS_ROOM, NULL);
  rooms.add_transition(&state_hall, &state_all_rooms, PREVIOUS_ROOM, NULL);

  // resetting to the default room (all rooms)
  rooms.add_transition(&state_hall, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_living_room, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_kitchen, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bedroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_bathroom, &state_all_rooms, RESET_ROOMS, NULL);
  rooms.add_transition(&state_attic, &state_all_rooms, RESET_ROOMS, NULL);
}

// ***** Button event handlers ***** //

// Use button one to set the light mode for all rooms
void handleButtonOne() {
  lcd.clear();
  rooms.trigger(RESET_ROOMS);
  modes.trigger(CHANGE_LIGHT_MODE);
  delay(debounceDelay);
}

// Use button two to increase brightness for the current room
void handleButtonTwo() {
  setRoomBrightness(currentRoom, min(roomBrightness[currentRoom] + deltaLevel, maxLevel));
  printCurrentRoom();
  delay(debounceDelay);
}

// Use button three to select the previous room
void handleButtonThree() {
  lcd.clear();
  rooms.trigger(PREVIOUS_ROOM);
  delay(debounceDelay);
}

// Use button four to decrease brightness for the current room
void handleButtonFour() {
  setRoomBrightness(currentRoom, max(roomBrightness[currentRoom] - deltaLevel, minLevel));
  printCurrentRoom();
  delay(debounceDelay);
}

// Use button five to select the next room
void handleButtonFive() {
  lcd.clear();
  rooms.trigger(NEXT_ROOM);
  delay(debounceDelay);
}

// ***** helpers ***** //

void setRGBColor(int red, int green, int blue) {
  int myRed = constrain(red, 0, 255);
  int myGreen = constrain(green, 0, 255);
  int myBlue = constrain(blue, 0, 255);

  analogWrite(RED_PIN, myRed);
  analogWrite(GREEN_PIN, myGreen);
  analogWrite(BLUE_PIN, myBlue);
}

void setRoomBrightness(int room, int level) {
  setRGBColor(0,0,level);
  roomBrightness[room] = level;
  tlc.setPWM(room * 3, roomBrightness[room] * maxLevel);
  tlc.write();
}

void setDefaultLightLevel() {
  setRGBColor(0,0,brightness);
  for (int i = 0; i != LastROOM; i++) {
    roomBrightness[i] = brightness;
  }
}

void setCurrentMode(int mode) {
  currentMode = mode;
  printCurrentMode();
}

void printCurrentMode() {
  lcd.clear();
  lcd.print("Mode: ");
  lcd.print(modeNames[currentMode]);
}

void setCurrentRoom(int room) {
  currentRoom = room;
  setRGBColor(0,0,roomBrightness[room]);
  printCurrentRoom();
}

void printCurrentRoom() {
  lcd.clear();
  lcd.print("room: ");
  lcd.print(roomNames[currentRoom]);
  lcd.setCursor(0,1);
  lcd.print("brightness: ");
  lcd.print(roomBrightness[currentRoom]);
}

// ***** FSM event handlers ***** //

// ---- lighting mode states ---- //

void on_lighting_mode_enter(){
  setCurrentMode(LIGHTING_MODE);
}

void on_lighting_mode_exit(){

}

void on_party_mode_enter(){
  setCurrentMode(PARTY_MODE);
}

void on_party_mode_exit(){

}

void on_nitelite_mode_enter(){
  setCurrentMode(NITELITE_MODE);
}

void on_nitelite_mode_exit(){

}

void on_off_mode_enter(){
  setCurrentMode(OFF_MODE);
}

void on_off_mode_exit(){

}

// ---- room selection states ---- //
void on_all_enter() {
  setCurrentRoom(ALL_ROOMS);
}

void on_all_exit() {

}

void on_hall_enter() {
  setCurrentRoom(HALL);
}

void on_hall_exit() {

}

void on_living_room_enter() {
  setCurrentRoom(LIVING_ROOM);
}

void on_living_room_exit() {

}

void on_kitchen_enter() {
  setCurrentRoom(KITCHEN);
}

void on_kitchen_exit() {

}

void on_bathroom_enter() {
  setCurrentRoom(BATHROOM);
}

void on_bathroom_exit() {

}

void on_bedroom_enter() {
  setCurrentRoom(BEDROOM);
}

void on_bedroom_exit() {

}

void on_attic_enter() {
  setCurrentRoom(ATTIC);
}

void on_attic_exit() {

}

void loop() {
  // do nothing; everything is handled via FSM events.
  // We also don't need to call the ".run_machine" methods of
  // the FSMs as there are no "on_state" handlers or timed transitions
}
<|endoftext|>


<commit_before>/*
 * Copyright 2018 The Cartographer Authors
 *
 * 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.
 */

#include "cartographer_ros/offline_node.h"

#include <errno.h>
#include <string.h>
#include <sys/resource.h>
#include <time.h>
#include <chrono>

#include "cartographer_ros/node.h"
#include "cartographer_ros/playable_bag.h"
#include "cartographer_ros/split_string.h"
#include "cartographer_ros/urdf_reader.h"
#include "gflags/gflags.h"
#include "ros/callback_queue.h"
#include "rosgraph_msgs/Clock.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "urdf/model.h"

DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Cartographer installation to allow "
              "including files from there.");
DEFINE_string(
    configuration_basenames, "",
    "Comma-separated list of basenames, i.e. not containing any "
    "directory prefix, of the configuration files for each trajectory. "
    "The first configuration file will be used for node options. "
    "If less configuration files are specified than trajectories, the "
    "first file will be used the remaining trajectories.");
DEFINE_string(
    bag_filenames, "",
    "Comma-separated list of bags to process. One bag per trajectory. "
    "Any combination of simultaneous and sequential bags is supported.");
DEFINE_string(urdf_filenames, "",
              "Comma-separated list of one or more URDF files that contain "
              "static links for the sensor configuration(s).");
DEFINE_bool(use_bag_transforms, true,
            "Whether to read, use and republish transforms from bags.");
DEFINE_string(load_state_filename, "",
              "If non-empty, filename of a .pbstream file to load, containing "
              "a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
            "Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(keep_running, false,
            "Keep running the offline node after all messages from the bag "
            "have been processed.");

namespace cartographer_ros {

constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
// We publish tf messages one second earlier than other messages. Under
// the assumption of higher frequency tf this should ensure that tf can
// always interpolate.
const ::ros::Duration kDelay = ::ros::Duration(1.0);

void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basenames.empty())
      << "-configuration_basenames is missing.";
  CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty()))
      << "-bag_filenames and -load_state_filename cannot both be unspecified.";
  const auto bag_filenames =
      cartographer_ros::SplitString(FLAGS_bag_filenames, ',');
  cartographer_ros::NodeOptions node_options;
  const auto configuration_basenames =
      cartographer_ros::SplitString(FLAGS_configuration_basenames, ',');
  std::vector<TrajectoryOptions> bag_trajectory_options(1);
  std::tie(node_options, bag_trajectory_options.at(0)) =
      LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0));

  for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) {
    TrajectoryOptions current_trajectory_options;
    if (bag_index < configuration_basenames.size()) {
      std::tie(std::ignore, current_trajectory_options) = LoadOptions(
          FLAGS_configuration_directory, configuration_basenames.at(bag_index));
    } else {
      current_trajectory_options = bag_trajectory_options.at(0);
    }
    bag_trajectory_options.push_back(current_trajectory_options);
  }
  if (bag_filenames.size() > 0) {
    CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
  }

  // Since we preload the transform buffer, we should never have to wait for a
  // transform. When we finish processing the bag, we will simply drop any
  // remaining sensor data that cannot be transformed due to missing transforms.
  node_options.lookup_transform_timeout_sec = 0.;

  auto map_builder = map_builder_factory(node_options.map_builder_options);

  const std::chrono::time_point<std::chrono::steady_clock> start_time =
      std::chrono::steady_clock::now();

  tf2_ros::Buffer tf_buffer;

  std::vector<geometry_msgs::TransformStamped> urdf_transforms;
  for (const std::string& urdf_filename :
       cartographer_ros::SplitString(FLAGS_urdf_filenames, ',')) {
    const auto current_urdf_transforms =
        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
    urdf_transforms.insert(urdf_transforms.end(),
                           current_urdf_transforms.begin(),
                           current_urdf_transforms.end());
  }

  tf_buffer.setUsingDedicatedThread(true);

  Node node(node_options, std::move(map_builder), &tf_buffer);
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  ::ros::Publisher tf_publisher =
      node.node_handle()->advertise<tf2_msgs::TFMessage>(
          kTfTopic, kLatestOnlyPublisherQueueSize);

  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;

  ::ros::Publisher clock_publisher =
      node.node_handle()->advertise<rosgraph_msgs::Clock>(
          kClockTopic, kLatestOnlyPublisherQueueSize);

  if (urdf_transforms.size() > 0) {
    static_tf_broadcaster.sendTransform(urdf_transforms);
  }

  ros::AsyncSpinner async_spinner(kSingleThreaded);
  async_spinner.start();
  rosgraph_msgs::Clock clock;
  auto clock_republish_timer = node.node_handle()->createWallTimer(
      ::ros::WallDuration(kClockPublishFrequencySec),
      [&clock_publisher, &clock](const ::ros::WallTimerEvent&) {
        clock_publisher.publish(clock);
      },
      false /* oneshot */, false /* autostart */);

  auto bag_expected_sensor_ids =
      node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
  std::map<std::pair<int /* bag_index */, std::string>,
           cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      bag_topic_to_sensor_id;
  PlayableBagMultiplexer playable_bag_multiplexer;
  for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
       ++current_bag_index) {
    const std::string& bag_filename = bag_filenames.at(current_bag_index);
    if (!::ros::ok()) {
      return;
    }
    for (const auto& expected_sensor_id :
         bag_expected_sensor_ids.at(current_bag_index)) {
      const auto bag_resolved_topic = std::make_pair(
          static_cast<int>(current_bag_index),
          node.node_handle()->resolveName(expected_sensor_id.id));
      if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) {
        LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag "
                   << current_bag_index << " resolves to topic "
                   << bag_resolved_topic.second << " which is already used by "
                   << " sensor "
                   << bag_topic_to_sensor_id.at(bag_resolved_topic).id;
      }
      bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id;
    }

    playable_bag_multiplexer.AddPlayableBag(PlayableBag(
        bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
        // PlayableBag::FilteringEarlyMessageHandler is used to get an early
        // peek at the tf messages in the bag and insert them into 'tf_buffer'.
        // When a message is retrieved by GetNextMessage() further below,
        // we will have already inserted further 'kDelay' seconds worth of
        // transforms into 'tf_buffer' via this lambda.
        [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) {
          if (msg.isType<tf2_msgs::TFMessage>()) {
            if (FLAGS_use_bag_transforms) {
              const auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
              tf_publisher.publish(tf_message);

              for (const auto& transform : tf_message->transforms) {
                try {
                  // We need to keep 'tf_buffer' small because it becomes very
                  // inefficient otherwise. We make sure that tf_messages are
                  // published before any data messages, so that tf lookups
                  // always work.
                  tf_buffer.setTransform(transform, "unused_authority",
                                         msg.getTopic() == kTfStaticTopic);
                } catch (const tf2::TransformException& ex) {
                  LOG(WARNING) << ex.what();
                }
              }
            }
            // Tell 'PlayableBag' to filter the tf message since there is no
            // further use for it.
            return false;
          } else {
            return true;
          }
        }));
  }

  // TODO(gaschler): Warn if resolved topics are not in bags.
  std::unordered_map<int, int> bag_index_to_trajectory_id;
  while (playable_bag_multiplexer.IsMessageAvailable()) {
    const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
    const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
    const int bag_index = std::get<1>(next_msg_tuple);
    const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);

    int trajectory_id;
    // Lazily add trajectories only when the first message arrives in order
    // to avoid blocking the sensor queue.
    if (bag_index_to_trajectory_id.count(bag_index) == 0) {
      trajectory_id =
          node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index),
                                    bag_trajectory_options.at(bag_index));
      CHECK(bag_index_to_trajectory_id
                .emplace(std::piecewise_construct,
                         std::forward_as_tuple(bag_index),
                         std::forward_as_tuple(trajectory_id))
                .second);
      LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag "
                << bag_filenames.at(bag_index);
    } else {
      trajectory_id = bag_index_to_trajectory_id.at(bag_index);
    }

    if (!::ros::ok()) {
      return;
    }
    const auto bag_topic = std::make_pair(
        bag_index,
        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));
    auto it = bag_topic_to_sensor_id.find(bag_topic);
    if (it != bag_topic_to_sensor_id.end()) {
      const std::string& sensor_id = it->second.id;
      if (msg.isType<sensor_msgs::LaserScan>()) {
        node.HandleLaserScanMessage(trajectory_id, sensor_id,
                                    msg.instantiate<sensor_msgs::LaserScan>());
      }
      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
        node.HandleMultiEchoLaserScanMessage(
            trajectory_id, sensor_id,
            msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
      }
      if (msg.isType<sensor_msgs::PointCloud2>()) {
        node.HandlePointCloud2Message(
            trajectory_id, sensor_id,
            msg.instantiate<sensor_msgs::PointCloud2>());
      }
      if (msg.isType<sensor_msgs::Imu>()) {
        node.HandleImuMessage(trajectory_id, sensor_id,
                              msg.instantiate<sensor_msgs::Imu>());
      }
      if (msg.isType<nav_msgs::Odometry>()) {
        node.HandleOdometryMessage(trajectory_id, sensor_id,
                                   msg.instantiate<nav_msgs::Odometry>());
      }
      if (msg.isType<sensor_msgs::NavSatFix>()) {
        node.HandleNavSatFixMessage(trajectory_id, sensor_id,
                                    msg.instantiate<sensor_msgs::NavSatFix>());
      }
    }
    clock.clock = msg.getTime();
    clock_publisher.publish(clock);

    if (is_last_message_in_bag) {
      node.FinishTrajectory(trajectory_id);
    }
  }

  // Ensure the clock is republished after the bag has been finished, during the
  // final optimization, serialization, and optional indefinite spinning at the
  // end.
  clock_republish_timer.start();
  node.RunFinalOptimization();

  const std::chrono::time_point<std::chrono::steady_clock> end_time =
      std::chrono::steady_clock::now();
  const double wall_clock_seconds =
      std::chrono::duration_cast<std::chrono::duration<double>>(end_time -
                                                                start_time)
          .count();

  LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s";
#ifdef __linux__
  timespec cpu_timespec = {};
  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec);
  LOG(INFO) << "Elapsed CPU time: "
            << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
  rusage usage;
  CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno);
  LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
#endif

  if (::ros::ok() && bag_filenames.size() > 0) {
    const std::string output_filename = bag_filenames.front();
    const std::string suffix = ".pbstream";
    const std::string state_output_filename = output_filename + suffix;
    LOG(INFO) << "Writing state to '" << state_output_filename << "'...";
    node.SerializeState(state_output_filename);
  }
  if (FLAGS_keep_running) {
    ::ros::waitForShutdown();
  }
}

}  // namespace cartographer_ros
<commit_msg>Offline multi-trajectory: use topic names without 'bag_n_' prefix by default (#707)<commit_after>/*
 * Copyright 2018 The Cartographer Authors
 *
 * 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.
 */

#include "cartographer_ros/offline_node.h"

#include <errno.h>
#include <string.h>
#include <sys/resource.h>
#include <time.h>
#include <chrono>

#include "cartographer_ros/node.h"
#include "cartographer_ros/playable_bag.h"
#include "cartographer_ros/split_string.h"
#include "cartographer_ros/urdf_reader.h"
#include "gflags/gflags.h"
#include "ros/callback_queue.h"
#include "rosgraph_msgs/Clock.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "urdf/model.h"

DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Cartographer installation to allow "
              "including files from there.");
DEFINE_string(
    configuration_basenames, "",
    "Comma-separated list of basenames, i.e. not containing any "
    "directory prefix, of the configuration files for each trajectory. "
    "The first configuration file will be used for node options. "
    "If less configuration files are specified than trajectories, the "
    "first file will be used the remaining trajectories.");
DEFINE_string(
    bag_filenames, "",
    "Comma-separated list of bags to process. One bag per trajectory. "
    "Any combination of simultaneous and sequential bags is supported.");
DEFINE_string(urdf_filenames, "",
              "Comma-separated list of one or more URDF files that contain "
              "static links for the sensor configuration(s).");
DEFINE_bool(use_bag_transforms, true,
            "Whether to read, use and republish transforms from bags.");
DEFINE_string(load_state_filename, "",
              "If non-empty, filename of a .pbstream file to load, containing "
              "a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
            "Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(keep_running, false,
            "Keep running the offline node after all messages from the bag "
            "have been processed.");

namespace cartographer_ros {

constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
// We publish tf messages one second earlier than other messages. Under
// the assumption of higher frequency tf this should ensure that tf can
// always interpolate.
const ::ros::Duration kDelay = ::ros::Duration(1.0);

void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basenames.empty())
      << "-configuration_basenames is missing.";
  CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty()))
      << "-bag_filenames and -load_state_filename cannot both be unspecified.";
  const auto bag_filenames =
      cartographer_ros::SplitString(FLAGS_bag_filenames, ',');
  cartographer_ros::NodeOptions node_options;
  const auto configuration_basenames =
      cartographer_ros::SplitString(FLAGS_configuration_basenames, ',');
  std::vector<TrajectoryOptions> bag_trajectory_options(1);
  std::tie(node_options, bag_trajectory_options.at(0)) =
      LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0));

  for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) {
    TrajectoryOptions current_trajectory_options;
    if (bag_index < configuration_basenames.size()) {
      std::tie(std::ignore, current_trajectory_options) = LoadOptions(
          FLAGS_configuration_directory, configuration_basenames.at(bag_index));
    } else {
      current_trajectory_options = bag_trajectory_options.at(0);
    }
    bag_trajectory_options.push_back(current_trajectory_options);
  }
  if (bag_filenames.size() > 0) {
    CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
  }

  // Since we preload the transform buffer, we should never have to wait for a
  // transform. When we finish processing the bag, we will simply drop any
  // remaining sensor data that cannot be transformed due to missing transforms.
  node_options.lookup_transform_timeout_sec = 0.;

  auto map_builder = map_builder_factory(node_options.map_builder_options);

  const std::chrono::time_point<std::chrono::steady_clock> start_time =
      std::chrono::steady_clock::now();

  tf2_ros::Buffer tf_buffer;

  std::vector<geometry_msgs::TransformStamped> urdf_transforms;
  for (const std::string& urdf_filename :
       cartographer_ros::SplitString(FLAGS_urdf_filenames, ',')) {
    const auto current_urdf_transforms =
        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
    urdf_transforms.insert(urdf_transforms.end(),
                           current_urdf_transforms.begin(),
                           current_urdf_transforms.end());
  }

  tf_buffer.setUsingDedicatedThread(true);

  Node node(node_options, std::move(map_builder), &tf_buffer);
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  ::ros::Publisher tf_publisher =
      node.node_handle()->advertise<tf2_msgs::TFMessage>(
          kTfTopic, kLatestOnlyPublisherQueueSize);

  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;

  ::ros::Publisher clock_publisher =
      node.node_handle()->advertise<rosgraph_msgs::Clock>(
          kClockTopic, kLatestOnlyPublisherQueueSize);

  if (urdf_transforms.size() > 0) {
    static_tf_broadcaster.sendTransform(urdf_transforms);
  }

  ros::AsyncSpinner async_spinner(kSingleThreaded);
  async_spinner.start();
  rosgraph_msgs::Clock clock;
  auto clock_republish_timer = node.node_handle()->createWallTimer(
      ::ros::WallDuration(kClockPublishFrequencySec),
      [&clock_publisher, &clock](const ::ros::WallTimerEvent&) {
        clock_publisher.publish(clock);
      },
      false /* oneshot */, false /* autostart */);

  std::vector<
      std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
      bag_expected_sensor_ids;
  if (configuration_basenames.size() == 1) {
    const auto current_bag_expected_sensor_ids =
        node.ComputeDefaultSensorIdsForMultipleBags(
            {bag_trajectory_options.front()});
    bag_expected_sensor_ids = {bag_filenames.size(),
                               current_bag_expected_sensor_ids.front()};
  } else {
    bag_expected_sensor_ids =
        node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
  }
  CHECK_EQ(bag_expected_sensor_ids.size(), bag_filenames.size());

  std::map<std::pair<int /* bag_index */, std::string>,
           cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      bag_topic_to_sensor_id;
  PlayableBagMultiplexer playable_bag_multiplexer;
  for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
       ++current_bag_index) {
    const std::string& bag_filename = bag_filenames.at(current_bag_index);
    if (!::ros::ok()) {
      return;
    }
    for (const auto& expected_sensor_id :
         bag_expected_sensor_ids.at(current_bag_index)) {
      const auto bag_resolved_topic = std::make_pair(
          static_cast<int>(current_bag_index),
          node.node_handle()->resolveName(expected_sensor_id.id));
      if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) {
        LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag "
                   << current_bag_index << " resolves to topic "
                   << bag_resolved_topic.second << " which is already used by "
                   << " sensor "
                   << bag_topic_to_sensor_id.at(bag_resolved_topic).id;
      }
      bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id;
    }

    playable_bag_multiplexer.AddPlayableBag(PlayableBag(
        bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
        // PlayableBag::FilteringEarlyMessageHandler is used to get an early
        // peek at the tf messages in the bag and insert them into 'tf_buffer'.
        // When a message is retrieved by GetNextMessage() further below,
        // we will have already inserted further 'kDelay' seconds worth of
        // transforms into 'tf_buffer' via this lambda.
        [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) {
          if (msg.isType<tf2_msgs::TFMessage>()) {
            if (FLAGS_use_bag_transforms) {
              const auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
              tf_publisher.publish(tf_message);

              for (const auto& transform : tf_message->transforms) {
                try {
                  // We need to keep 'tf_buffer' small because it becomes very
                  // inefficient otherwise. We make sure that tf_messages are
                  // published before any data messages, so that tf lookups
                  // always work.
                  tf_buffer.setTransform(transform, "unused_authority",
                                         msg.getTopic() == kTfStaticTopic);
                } catch (const tf2::TransformException& ex) {
                  LOG(WARNING) << ex.what();
                }
              }
            }
            // Tell 'PlayableBag' to filter the tf message since there is no
            // further use for it.
            return false;
          } else {
            return true;
          }
        }));
  }

  // TODO(gaschler): Warn if resolved topics are not in bags.
  std::unordered_map<int, int> bag_index_to_trajectory_id;
  while (playable_bag_multiplexer.IsMessageAvailable()) {
    const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
    const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
    const int bag_index = std::get<1>(next_msg_tuple);
    const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);

    int trajectory_id;
    // Lazily add trajectories only when the first message arrives in order
    // to avoid blocking the sensor queue.
    if (bag_index_to_trajectory_id.count(bag_index) == 0) {
      trajectory_id =
          node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index),
                                    bag_trajectory_options.at(bag_index));
      CHECK(bag_index_to_trajectory_id
                .emplace(std::piecewise_construct,
                         std::forward_as_tuple(bag_index),
                         std::forward_as_tuple(trajectory_id))
                .second);
      LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag "
                << bag_filenames.at(bag_index);
    } else {
      trajectory_id = bag_index_to_trajectory_id.at(bag_index);
    }

    if (!::ros::ok()) {
      return;
    }
    const auto bag_topic = std::make_pair(
        bag_index,
        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));
    auto it = bag_topic_to_sensor_id.find(bag_topic);
    if (it != bag_topic_to_sensor_id.end()) {
      const std::string& sensor_id = it->second.id;
      if (msg.isType<sensor_msgs::LaserScan>()) {
        node.HandleLaserScanMessage(trajectory_id, sensor_id,
                                    msg.instantiate<sensor_msgs::LaserScan>());
      }
      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
        node.HandleMultiEchoLaserScanMessage(
            trajectory_id, sensor_id,
            msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
      }
      if (msg.isType<sensor_msgs::PointCloud2>()) {
        node.HandlePointCloud2Message(
            trajectory_id, sensor_id,
            msg.instantiate<sensor_msgs::PointCloud2>());
      }
      if (msg.isType<sensor_msgs::Imu>()) {
        node.HandleImuMessage(trajectory_id, sensor_id,
                              msg.instantiate<sensor_msgs::Imu>());
      }
      if (msg.isType<nav_msgs::Odometry>()) {
        node.HandleOdometryMessage(trajectory_id, sensor_id,
                                   msg.instantiate<nav_msgs::Odometry>());
      }
      if (msg.isType<sensor_msgs::NavSatFix>()) {
        node.HandleNavSatFixMessage(trajectory_id, sensor_id,
                                    msg.instantiate<sensor_msgs::NavSatFix>());
      }
    }
    clock.clock = msg.getTime();
    clock_publisher.publish(clock);

    if (is_last_message_in_bag) {
      node.FinishTrajectory(trajectory_id);
    }
  }

  // Ensure the clock is republished after the bag has been finished, during the
  // final optimization, serialization, and optional indefinite spinning at the
  // end.
  clock_republish_timer.start();
  node.RunFinalOptimization();

  const std::chrono::time_point<std::chrono::steady_clock> end_time =
      std::chrono::steady_clock::now();
  const double wall_clock_seconds =
      std::chrono::duration_cast<std::chrono::duration<double>>(end_time -
                                                                start_time)
          .count();

  LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s";
#ifdef __linux__
  timespec cpu_timespec = {};
  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec);
  LOG(INFO) << "Elapsed CPU time: "
            << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
  rusage usage;
  CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno);
  LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
#endif

  if (::ros::ok() && bag_filenames.size() > 0) {
    const std::string output_filename = bag_filenames.front();
    const std::string suffix = ".pbstream";
    const std::string state_output_filename = output_filename + suffix;
    LOG(INFO) << "Writing state to '" << state_output_filename << "'...";
    node.SerializeState(state_output_filename);
  }
  if (FLAGS_keep_running) {
    ::ros::waitForShutdown();
  }
}

}  // namespace cartographer_ros
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                          OpenSim:  Controller.cpp                          *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2016 Stanford University and the Authors                *
 * Author(s): Ajay Seth, Frank C. Anderson, Chand T. John, Samuel R. Hamner   *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

//=============================================================================
// INCLUDES
//=============================================================================
#include "Controller.h"
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Simulation/Model/Actuator.h>
#include <OpenSim/Common/IO.h>

//=============================================================================
// STATICS
//=============================================================================

using namespace OpenSim;
using namespace std;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 */
Controller::Controller()
{
    constructProperties();
}

//=============================================================================
// CONSTRUCTION
//=============================================================================
//_____________________________________________________________________________
//_____________________________________________________________________________
/**
 * Connect properties to local pointers.
 */
void Controller::constructProperties()
{
    setAuthors("Ajay Seth, Frank Anderson, Chand John, Samuel Hamner");
    constructProperty_enabled(true);
    constructProperty_actuator_list();

    // Set is only a reference list, not ownership
    _actuatorSet.setMemoryOwner(false);
}

void Controller::updateFromXMLNode(SimTK::Xml::Element& node,
                                   int versionNumber) {
    if(versionNumber < XMLDocument::getLatestVersion()) {
        if(versionNumber < 30510) {
            // Rename property 'isDisabled' to 'appliesForce' and
            // negate the contained value.
            std::string oldName{"isDisabled"};
            std::string newName{"enabled"};
            if(node.hasElement(oldName)) {
                auto elem = node.getRequiredElement(oldName);
                elem.setElementTag(newName);
                if(elem.getValue().find("true") != std::string::npos)
                    elem.setValue("false");
                else
                    elem.setValue("true");
            }
        }
    }

    Super::updateFromXMLNode(node, versionNumber);
}

//=============================================================================
// GET AND SET
//=============================================================================

//-----------------------------------------------------------------------------
// ON/OFF
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Get whether or not this controller is enabled.
 */
bool Controller::isEnabled() const
{
    if( getModel().getAllControllersEnabled() ) {
       return( get_enabled() );
    } else {
       return( false );
    }
}
//_____________________________________________________________________________
/**
 * Turn this controller on or off.
 */
void Controller::setEnabled(bool aTrueFalse)
{
    upd_enabled() = aTrueFalse;
}

// for any post XML deserialization initialization
void Controller::extendConnectToModel(Model& model)
{
    Super::extendConnectToModel(model);

    // TODO this custom connection code can all disappear
    // if we use a list Connector<Actuator>

    // make sure controller does not take ownership
    _actuatorSet.setMemoryOwner(false);
    _actuatorSet.setSize(0);

    int nac = getProperty_actuator_list().size();
    if (nac == 0)
        return;

    auto actuators = model.getComponentList<Actuator>();
    if (IO::Uppercase(get_actuator_list(0)) == "ALL"){
        for (auto& actuator : actuators) {
            _actuatorSet.adoptAndAppend(&actuator);
        }
        return;
    }
    else{
         for (int i = 0; i < nac; ++i) {
             bool found = false;
             for (auto& actuator : actuators) {
                if (get_actuator_list(i) == actuator.getName()) {
                    _actuatorSet.adoptAndAppend(&actuator);
                    found = true;
                    break;
                }
             }
            if (!found) {
                cerr << "WARN: Controller::connectToModel : Actuator "
                    << get_actuator_list(i) <<
                    " was not found and will be ignored." << endl;
            }
        }
    }
}

/**
 * Create a Controller in the SimTK::System
 */
void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystem(system);
}

// makes a request for which actuators a controller will control
void Controller::setActuators(const Set<Actuator>& actuators)
{
    //TODO this needs to be setting a Connector list of Actuators

    // make sure controller does NOT assume ownership
    _actuatorSet.setMemoryOwner(false);
    //Rebuild consistent set of actuator lists
    _actuatorSet.setSize(0);
    updProperty_actuator_list().clear();
    for (int i = 0; i< actuators.getSize(); i++){
        addActuator(actuators[i]);
    }
}

void Controller::addActuator(const Actuator& actuator)
{
    _actuatorSet.adoptAndAppend(&actuator);

    int found = updProperty_actuator_list().findIndex(actuator.getName());
    if (found < 0) //add if the actuator isn't already in the list
        updProperty_actuator_list().appendValue(actuator.getName());
}

Set<const Actuator>& Controller::updActuators() { return _actuatorSet; }

const Set<const Actuator>& Controller::getActuatorSet() const { return _actuatorSet; }
<commit_msg>Toggle value only if there is one.<commit_after>/* -------------------------------------------------------------------------- *
 *                          OpenSim:  Controller.cpp                          *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2016 Stanford University and the Authors                *
 * Author(s): Ajay Seth, Frank C. Anderson, Chand T. John, Samuel R. Hamner   *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */

//=============================================================================
// INCLUDES
//=============================================================================
#include "Controller.h"
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Simulation/Model/Actuator.h>
#include <OpenSim/Common/IO.h>

//=============================================================================
// STATICS
//=============================================================================

using namespace OpenSim;
using namespace std;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 */
Controller::Controller()
{
    constructProperties();
}

//=============================================================================
// CONSTRUCTION
//=============================================================================
//_____________________________________________________________________________
//_____________________________________________________________________________
/**
 * Connect properties to local pointers.
 */
void Controller::constructProperties()
{
    setAuthors("Ajay Seth, Frank Anderson, Chand John, Samuel Hamner");
    constructProperty_enabled(true);
    constructProperty_actuator_list();

    // Set is only a reference list, not ownership
    _actuatorSet.setMemoryOwner(false);
}

void Controller::updateFromXMLNode(SimTK::Xml::Element& node,
                                   int versionNumber) {
    if(versionNumber < XMLDocument::getLatestVersion()) {
        if(versionNumber < 30510) {
            // Rename property 'isDisabled' to 'appliesForce' and
            // negate the contained value.
            std::string oldName{"isDisabled"};
            std::string newName{"enabled"};
            if(node.hasElement(oldName)) {
                auto elem = node.getRequiredElement(oldName);
                elem.setElementTag(newName);
                if(elem.getValue().find("true") != std::string::npos)
                    elem.setValue("false");
                else if(elem.getValue().find("false") != std::string::npos)
                    elem.setValue("true");
            }
        }
    }

    Super::updateFromXMLNode(node, versionNumber);
}

//=============================================================================
// GET AND SET
//=============================================================================

//-----------------------------------------------------------------------------
// ON/OFF
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/**
 * Get whether or not this controller is enabled.
 */
bool Controller::isEnabled() const
{
    if( getModel().getAllControllersEnabled() ) {
       return( get_enabled() );
    } else {
       return( false );
    }
}
//_____________________________________________________________________________
/**
 * Turn this controller on or off.
 */
void Controller::setEnabled(bool aTrueFalse)
{
    upd_enabled() = aTrueFalse;
}

// for any post XML deserialization initialization
void Controller::extendConnectToModel(Model& model)
{
    Super::extendConnectToModel(model);

    // TODO this custom connection code can all disappear
    // if we use a list Connector<Actuator>

    // make sure controller does not take ownership
    _actuatorSet.setMemoryOwner(false);
    _actuatorSet.setSize(0);

    int nac = getProperty_actuator_list().size();
    if (nac == 0)
        return;

    auto actuators = model.getComponentList<Actuator>();
    if (IO::Uppercase(get_actuator_list(0)) == "ALL"){
        for (auto& actuator : actuators) {
            _actuatorSet.adoptAndAppend(&actuator);
        }
        return;
    }
    else{
         for (int i = 0; i < nac; ++i) {
             bool found = false;
             for (auto& actuator : actuators) {
                if (get_actuator_list(i) == actuator.getName()) {
                    _actuatorSet.adoptAndAppend(&actuator);
                    found = true;
                    break;
                }
             }
            if (!found) {
                cerr << "WARN: Controller::connectToModel : Actuator "
                    << get_actuator_list(i) <<
                    " was not found and will be ignored." << endl;
            }
        }
    }
}

/**
 * Create a Controller in the SimTK::System
 */
void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystem(system);
}

// makes a request for which actuators a controller will control
void Controller::setActuators(const Set<Actuator>& actuators)
{
    //TODO this needs to be setting a Connector list of Actuators

    // make sure controller does NOT assume ownership
    _actuatorSet.setMemoryOwner(false);
    //Rebuild consistent set of actuator lists
    _actuatorSet.setSize(0);
    updProperty_actuator_list().clear();
    for (int i = 0; i< actuators.getSize(); i++){
        addActuator(actuators[i]);
    }
}

void Controller::addActuator(const Actuator& actuator)
{
    _actuatorSet.adoptAndAppend(&actuator);

    int found = updProperty_actuator_list().findIndex(actuator.getName());
    if (found < 0) //add if the actuator isn't already in the list
        updProperty_actuator_list().appendValue(actuator.getName());
}

Set<const Actuator>& Controller::updActuators() { return _actuatorSet; }

const Set<const Actuator>& Controller::getActuatorSet() const { return _actuatorSet; }
<|endoftext|>


<commit_before>
#include <cstring>

#ifdef DEBUG
#include <mutex>
#endif

#include "result.h"
#include "solutions.h"

#ifdef DEBUG
extern std::mutex debug_mutex;
#endif

const Result * Solutions::find(const double *ip, const Sense sense) const {
  bool t1,t3;
  for (auto& res: store_) {

    t1 = true;
    t3 = true;
    if (sense == MIN) {
      for (int i = 0; i < objective_count; i++){
        /* t1: All values of candidate must be >= than ip */
        if (res->ip[i] < ip[i]) {
          t1 = false;
          break;
        }
        /* t3: All values of candidate result must be <= than ip */
        if (!res->infeasible) {
          if (res->result[i] > ip[i]) {
            t3 = false;
            break;
          }
        }
      }
    } else {
      for (int i = 0; i < objective_count; i++){
        /* t1: All values of candidate must be <= than ip */
        if (res->ip[i] > ip[i]) {
          t1 = false;
          break;
        }
        /* t3: All values of candidate result must be >= than ip */
        if (!res->infeasible) {
          if (res->result[i] < ip[i]) {
            t3 = false;
            break;
          }
        }
      }
    }
    /* If all conditions are met copy problem & solution and return */
    if (t1 && t3) {
#ifdef DEBUG
      debug_mutex.lock();
      std::cout << " relaxed to ";
      for(int i = 0; i < objective_count; ++i) {
        if (res->ip[i] > 1e19)
          std::cout << "∞,";
        else if (res->ip[i] < -1e19)
          std::cout << "-∞,";
        else
          std::cout << res->ip[i] << ",";
      }
      std::cout << " soln is ";
      if (res->infeasible) {
        std::cout << "infeasible";
      } else {
        for(int i = 0; i < objective_count; ++i) {
          std::cout << res->result[i] << ",";
        }
      }
      std::cout << std::endl;
      debug_mutex.unlock();
#endif
      return res;
    }
  }
  return nullptr;
}
void Solutions::insert(const double *lp, const int *result,
    const bool infeasible) {
  Result * r = new Result;
  r->objective_count = objective_count;
  r->ip = new double[objective_count];
  std::memcpy(r->ip, lp, objective_count * sizeof(double));
  if (infeasible) {
    r->infeasible = true;
    r->result = nullptr;
  } else {
    r->infeasible = false;
    r->result = new int[objective_count];
    std::memcpy(r->result, result, objective_count * sizeof(int));
  }
  store_.push_back(r);
}

Solutions::~Solutions() {
  for(auto r: store_) {
    delete[] r->ip;
    if (r->result)
      delete[] r->result;
  }
}
<commit_msg>Hide solution search information unless wanted<commit_after>
#include <cstring>

#ifdef DEBUG
#include <mutex>
#endif

#include "result.h"
#include "solutions.h"

#ifdef DEBUG
extern std::mutex debug_mutex;
#endif

const Result * Solutions::find(const double *ip, const Sense sense) const {
  bool t1,t3;
  for (auto& res: store_) {

    t1 = true;
    t3 = true;
    if (sense == MIN) {
      for (int i = 0; i < objective_count; i++){
        /* t1: All values of candidate must be >= than ip */
        if (res->ip[i] < ip[i]) {
          t1 = false;
          break;
        }
        /* t3: All values of candidate result must be <= than ip */
        if (!res->infeasible) {
          if (res->result[i] > ip[i]) {
            t3 = false;
            break;
          }
        }
      }
    } else {
      for (int i = 0; i < objective_count; i++){
        /* t1: All values of candidate must be <= than ip */
        if (res->ip[i] > ip[i]) {
          t1 = false;
          break;
        }
        /* t3: All values of candidate result must be >= than ip */
        if (!res->infeasible) {
          if (res->result[i] < ip[i]) {
            t3 = false;
            break;
          }
        }
      }
    }
    /* If all conditions are met copy problem & solution and return */
    if (t1 && t3) {
#ifdef DEBUG_SOLUTION_SEARCH
      debug_mutex.lock();
      std::cout << " relaxed to ";
      for(int i = 0; i < objective_count; ++i) {
        if (res->ip[i] > 1e19)
          std::cout << "∞,";
        else if (res->ip[i] < -1e19)
          std::cout << "-∞,";
        else
          std::cout << res->ip[i] << ",";
      }
      std::cout << " soln is ";
      if (res->infeasible) {
        std::cout << "infeasible";
      } else {
        for(int i = 0; i < objective_count; ++i) {
          std::cout << res->result[i] << ",";
        }
      }
      std::cout << std::endl;
      debug_mutex.unlock();
#endif
      return res;
    }
  }
  return nullptr;
}
void Solutions::insert(const double *lp, const int *result,
    const bool infeasible) {
  Result * r = new Result;
  r->objective_count = objective_count;
  r->ip = new double[objective_count];
  std::memcpy(r->ip, lp, objective_count * sizeof(double));
  if (infeasible) {
    r->infeasible = true;
    r->result = nullptr;
  } else {
    r->infeasible = false;
    r->result = new int[objective_count];
    std::memcpy(r->result, result, objective_count * sizeof(int));
  }
  store_.push_back(r);
}

Solutions::~Solutions() {
  for(auto r: store_) {
    delete[] r->ip;
    if (r->result)
      delete[] r->result;
  }
}
<|endoftext|>