<commit_before>/** @file test out feature data visitors,
 * some fixtures may be scattered somewhat awkwardly in this file due to conditional support (e.g. PCL support)
 */
#include "core/featuredatavisitors.h"

#include "core/exception.h"
#include "ts/ts.h"

using namespace std;
using namespace cv;
using namespace sem;

namespace {

/**
 * @brief Type-Parameterized tests around POD visitor class
 */
template <class T>
class FeatDataVisitorPOD_Test : public ::testing::Test
{
protected:
};
TYPED_TEST_CASE_P(FeatDataVisitorPOD_Test);

TYPED_TEST_P(FeatDataVisitorPOD_Test, Invalid_Mat) {

    FeatDataVisitorPOD_<TypeParam > to;

    // empty
    EXPECT_THROW(to(Mat_f()), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(0, 0)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(1, 0)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(0, 1)), ExceptionBadDims);

    // multiple elements
    EXPECT_THROW(to(Mat_f(2, 1)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(1, 2)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(2, 2)), ExceptionBadDims);

    EXPECT_THROW(to(Mat_f(2, 1, 1.f)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(1, 2, 0.f)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(2, 1, 3.f)), ExceptionBadDims);
}

TYPED_TEST_P(FeatDataVisitorPOD_Test, Value)
{
    FeatDataVisitorPOD_<TypeParam > to;

    float _v = 256; // to cover a range of values common between all of our PODs
    while(--_v >= 0) {

        EXPECT_EQ(static_cast<TypeParam >(_v), to(Mat_f(1, 1, _v))) << "Value mismatch.";
    }
}

#ifdef __WITH_PCL // PCL support required for these tests

TYPED_TEST_P(FeatDataVisitorPOD_Test, Invalid_Cloud)
{
    FeatDataVisitorPOD_<TypeParam > to;

    // empty
    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_THROW(to(cld), ExceptionTypeError);

    // multi-row, 3-col
    cld = Mat2PointCloud(Mat1f(4, 3, 0.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat1f(4, 3, 1.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat1f(4, 3, 2.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    // single row
    cld = Mat2PointCloud(Mat1f(1, 3, 0.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat1f(1, 3, 1.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    // 3-channel
    cld = Mat2PointCloud(Mat3f(1, 1, 0.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat3f(1, 1, 1.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);
}
#else // __WITH_PCL

TYPED_TEST_P(FeatDataVisitorPOD_Test, DISABLED_Invalid_Cloud)
{
}

#endif // __WITH_PCL

/** Write additional type+value parameterized tests here.
 *  Acquaint yourself with the values passed to along with each type.
 *
 *  Register test names:
 * @todo: add define macro guard for PCL support
 */
REGISTER_TYPED_TEST_CASE_P(FeatDataVisitorPOD_Test,
                           Invalid_Mat,
                           Value,
                           Invalid_Cloud
                           ); ///< register additional typed_test_p (i.e. unit test) routines here

typedef ::testing::Types<float, int, uchar> PODTypes;  ///< lists the usual suspects of plain old data types
INSTANTIATE_TYPED_TEST_CASE_P(FeatureDataVisitorPOD_Test, FeatDataVisitorPOD_Test, PODTypes);

/**
 * @brief test class around FeatDataVisitorMat_f
 */
class FeatDataVisitorMat_fTest : public ::testing::Test
{
protected:
    FeatDataVisitorMat_f to_;    ///< test object
};

TEST_F(FeatDataVisitorMat_fTest, EmptyMat)
{
    EXPECT_TRUE(to_(Mat_f()).empty());
    EXPECT_TRUE(to_(Mat_f(0, 0)).empty());
    EXPECT_TRUE(to_(Mat_f(1, 0)).empty());
    EXPECT_TRUE(to_(Mat_f(0, 1)).empty());
}

TEST_F(FeatDataVisitorMat_fTest, EmptyMatSize)
{
    EXPECT_MAT_DIMS_EQ(to_(Mat_f()), Size2i(0, 0));
    EXPECT_MAT_DIMS_EQ(to_(Mat_f(0, 0)), Size2i(0, 0));
    EXPECT_MAT_DIMS_EQ(to_(Mat_f(1, 0)), Size2i(0, 1));
    EXPECT_MAT_DIMS_EQ(to_(Mat_f(0, 1)), Size2i(1, 0));
}

TEST_F(FeatDataVisitorMat_fTest, FromMat_f)
{
    int N=5;

    while(N--) {

        const int MAT_ROWS = abs(randu<int>()) % 10 + 1;
        const int MAT_COLS = abs(randu<int>()) % 100 + 1;

        Mat_f m(MAT_ROWS, MAT_COLS);
        randn(m, 0.f, 100.f);

        Mat_f m2 = to_(m);

        EXPECT_MAT_EQ(m, m2) << "Matrices are not equal";
        EXPECT_EQ(m.data, m2.data) << "Mats are not pointing to the same data in memory. Expecting shared copy.";
    }
}

TEST_F(FeatDataVisitorMat_fTest, Reset)
{
    EXPECT_NO_THROW(to_.Reset());

    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    Mat_f m2 = to_(m);
    EXPECT_NO_THROW(to_.Reset());
}

#ifndef __WITH_PCL
    #warning "Disabling unit tests that require PCL support."
#else // __WITH_PCL

TEST_F(FeatDataVisitorMat_fTest, EmptyCLoud)
{
    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_TRUE(to_(cld).empty());
}

TEST_F(FeatDataVisitorMat_fTest, EmptyCloudSize)
{
    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_MAT_DIMS_EQ(to_(cld), Size2i(0, 0));
}

TEST_F(FeatDataVisitorMat_fTest, FromCloud)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);
    CloudXYZ::Ptr cloud = Mat2PointCloud(m);

    Mat_f m2 = to_(cloud);
    hconcat(m, Mat1f(m.rows, 1, 1), m);

    EXPECT_MAT_EQ(m, m2);
    EXPECT_NE(m.data, m2.data) << "Expecting deep copy. If intentionally optimized to be a shared copy, please update test.";
}

TEST_F(FeatDataVisitorMat_fTest, Reset_with_cloud)
{
    EXPECT_NO_THROW(to_.Reset());

    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    Mat_f m2 = to_(m);
    EXPECT_NO_THROW(to_.Reset());

    CloudXYZ::Ptr cloud = Mat2PointCloud(m);
    m2 = to_(cloud);
    EXPECT_NO_THROW(to_.Reset());

    m2 = to_(m);
    EXPECT_NO_THROW(to_.Reset());
}

/**
 * @brief test class around FeatDataVisitorCloud
 */
class FeatDataVisitorCloudTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        to_.Reset();
    }

    FeatDataVisitorCloud to_;    ///< test object
};

TEST_F(FeatDataVisitorCloudTest, Empty)
{
    EXPECT_TRUE(to_(Mat_f())->empty());
    EXPECT_TRUE(to_(Mat_f(0, 0))->empty());
    EXPECT_TRUE(to_(Mat_f(1, 0))->empty());
    EXPECT_TRUE(to_(Mat_f(0, 1))->empty());

    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_TRUE(to_(cld)->empty());
}

TEST_F(FeatDataVisitorCloudTest, EmptySize)
{
    EXPECT_EQ(size_t(0), to_(Mat_f())->size());
    EXPECT_EQ(size_t(0), to_(Mat_f(0, 0))->size());
    EXPECT_EQ(size_t(0), to_(Mat_f(1, 0))->size());
    EXPECT_EQ(size_t(0), to_(Mat_f(0, 1))->size());

    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_EQ(size_t(0), to_(cld)->size());
}

TEST_F(FeatDataVisitorCloudTest, FromMat_f)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    CloudXYZ::Ptr cld = to_(m);
    Mat1f m2 = PointCloud2Mat(cld);
    hconcat(m, Mat1f(m.rows, 1, 1), m);

    EXPECT_MAT_EQ(m, m2);
    EXPECT_NE(m.data, m2.data) << "Expecting deep copy. If intentionally optimized to be a shared copy, please update test.";
}

TEST_F(FeatDataVisitorCloudTest, Cloud)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);
    CloudXYZ::Ptr cld = Mat2PointCloud(m);

    CloudXYZ::Ptr cld2 = to_(cld);

    EXPECT_EQ(cld->size(), cld2->size()) << "Size mismatch.";
    EXPECT_EQ(cld->width, cld2->width) << "Width mismatch.";
    EXPECT_EQ(cld->height, cld2->height) << "Height mismatch.";

    for(CloudXYZ::iterator itr1=cld->begin(), itr2=cld2->begin();
        itr1 != cld->end(); ++itr1, ++itr2) {

        pcl::PointXYZ _p1 = *itr1;
        pcl::PointXYZ _p2 = *itr2;
        EXPECT_FLOAT_EQ(_p1.x, _p2.x) << "Unexpected value for x coordinate.";
        EXPECT_FLOAT_EQ(_p1.y, _p2.y) << "Unexpected value for y coordinate.";
        EXPECT_FLOAT_EQ(_p1.z, _p2.z) << "Unexpected value for z coordinate.";
    }
}

TEST_F(FeatDataVisitorCloudTest, ResetValid)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    CloudXYZ::Ptr cld1 = to_(m);
    EXPECT_NO_THROW(to_.Reset());

    CloudXYZ::Ptr cld2 = Mat2PointCloud(m);
    CloudXYZ::Ptr cld3 = to_(cld2);
    EXPECT_NO_THROW(to_.Reset());

    cld1 = to_(m);
    EXPECT_NO_THROW(to_.Reset());
}

/**
 * @brief Thest this visitor's caching
 */
TEST_F(FeatDataVisitorCloudTest, Reset)
{
    Mat1f m(4, 3, 1.f);
    Mat1f m0 = m.clone();
    hconcat(m0, Mat1f(m0.rows, 1, 1), m0);

    CloudXYZ::Ptr cld = to_(m);
    EXPECT_MAT_EQ(PointCloud2Mat(cld), m0);

    cld = to_(m); // result of first call is cached.

    const int N=2;
    ASSERT_GT(N, 1) << "This test must iterate at least twice to see effect of Reset() on cached reference.";
    for(int i=0; i<2; i++) {

        cld = to_(m+1);

        if(i == 0) {

            EXPECT_MAT_EQ(PointCloud2Mat(cld), m0) << "Not suing cahced cloud.";
        }
        else {

            Mat1f m2 = m0.clone();
            m2.colRange(0, m2.cols-1).setTo(2.f);
            EXPECT_MAT_EQ(PointCloud2Mat(cld), m2) << "Still using cached cloud";
        }
        to_.Reset();
    }
}

#endif // __WITH_PCL

} // annonymous namespace for visitors' test fixtures
<commit_msg>explicit float notation<commit_after>/** @file test out feature data visitors,
 * some fixtures may be scattered somewhat awkwardly in this file due to conditional support (e.g. PCL support)
 */
#include "core/featuredatavisitors.h"

#include "core/exception.h"
#include "ts/ts.h"

using namespace std;
using namespace cv;
using namespace sem;

namespace {

/**
 * @brief Type-Parameterized tests around POD visitor class
 */
template <class T>
class FeatDataVisitorPOD_Test : public ::testing::Test
{
protected:
};
TYPED_TEST_CASE_P(FeatDataVisitorPOD_Test);

TYPED_TEST_P(FeatDataVisitorPOD_Test, Invalid_Mat) {

    FeatDataVisitorPOD_<TypeParam > to;

    // empty
    EXPECT_THROW(to(Mat_f()), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(0, 0)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(1, 0)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(0, 1)), ExceptionBadDims);

    // multiple elements
    EXPECT_THROW(to(Mat_f(2, 1)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(1, 2)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(2, 2)), ExceptionBadDims);

    EXPECT_THROW(to(Mat_f(2, 1, 1.f)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(1, 2, 0.f)), ExceptionBadDims);
    EXPECT_THROW(to(Mat_f(2, 1, 3.f)), ExceptionBadDims);
}

TYPED_TEST_P(FeatDataVisitorPOD_Test, Value)
{
    FeatDataVisitorPOD_<TypeParam > to;

    float _v = 256.f; // to cover a range of values common between all of our PODs
    while(--_v >= 0.f) {

        EXPECT_EQ(static_cast<TypeParam >(_v), to(Mat_f(1, 1, _v))) << "Value mismatch.";
    }
}

#ifdef __WITH_PCL // PCL support required for these tests

TYPED_TEST_P(FeatDataVisitorPOD_Test, Invalid_Cloud)
{
    FeatDataVisitorPOD_<TypeParam > to;

    // empty
    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_THROW(to(cld), ExceptionTypeError);

    // multi-row, 3-col
    cld = Mat2PointCloud(Mat1f(4, 3, 0.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat1f(4, 3, 1.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat1f(4, 3, 2.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    // single row
    cld = Mat2PointCloud(Mat1f(1, 3, 0.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat1f(1, 3, 1.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    // 3-channel
    cld = Mat2PointCloud(Mat3f(1, 1, 0.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);

    cld = Mat2PointCloud(Mat3f(1, 1, 1.f));
    EXPECT_THROW(to(cld), ExceptionTypeError);
}
#else // __WITH_PCL

TYPED_TEST_P(FeatDataVisitorPOD_Test, DISABLED_Invalid_Cloud)
{
}

#endif // __WITH_PCL

/** Write additional type+value parameterized tests here.
 *  Acquaint yourself with the values passed to along with each type.
 *
 *  Register test names:
 * @todo: add define macro guard for PCL support
 */
REGISTER_TYPED_TEST_CASE_P(FeatDataVisitorPOD_Test,
                           Invalid_Mat,
                           Value,
                           Invalid_Cloud
                           ); ///< register additional typed_test_p (i.e. unit test) routines here

typedef ::testing::Types<float, int, uchar> PODTypes;  ///< lists the usual suspects of plain old data types
INSTANTIATE_TYPED_TEST_CASE_P(FeatureDataVisitorPOD_Test, FeatDataVisitorPOD_Test, PODTypes);

/**
 * @brief test class around FeatDataVisitorMat_f
 */
class FeatDataVisitorMat_fTest : public ::testing::Test
{
protected:
    FeatDataVisitorMat_f to_;    ///< test object
};

TEST_F(FeatDataVisitorMat_fTest, EmptyMat)
{
    EXPECT_TRUE(to_(Mat_f()).empty());
    EXPECT_TRUE(to_(Mat_f(0, 0)).empty());
    EXPECT_TRUE(to_(Mat_f(1, 0)).empty());
    EXPECT_TRUE(to_(Mat_f(0, 1)).empty());
}

TEST_F(FeatDataVisitorMat_fTest, EmptyMatSize)
{
    EXPECT_MAT_DIMS_EQ(to_(Mat_f()), Size2i(0, 0));
    EXPECT_MAT_DIMS_EQ(to_(Mat_f(0, 0)), Size2i(0, 0));
    EXPECT_MAT_DIMS_EQ(to_(Mat_f(1, 0)), Size2i(0, 1));
    EXPECT_MAT_DIMS_EQ(to_(Mat_f(0, 1)), Size2i(1, 0));
}

TEST_F(FeatDataVisitorMat_fTest, FromMat_f)
{
    int N=5;

    while(N--) {

        const int MAT_ROWS = abs(randu<int>()) % 10 + 1;
        const int MAT_COLS = abs(randu<int>()) % 100 + 1;

        Mat_f m(MAT_ROWS, MAT_COLS);
        randn(m, 0.f, 100.f);

        Mat_f m2 = to_(m);

        EXPECT_MAT_EQ(m, m2) << "Matrices are not equal";
        EXPECT_EQ(m.data, m2.data) << "Mats are not pointing to the same data in memory. Expecting shared copy.";
    }
}

TEST_F(FeatDataVisitorMat_fTest, Reset)
{
    EXPECT_NO_THROW(to_.Reset());

    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    Mat_f m2 = to_(m);
    EXPECT_NO_THROW(to_.Reset());
}

#ifndef __WITH_PCL
    #warning "Disabling unit tests that require PCL support."
#else // __WITH_PCL

TEST_F(FeatDataVisitorMat_fTest, EmptyCLoud)
{
    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_TRUE(to_(cld).empty());
}

TEST_F(FeatDataVisitorMat_fTest, EmptyCloudSize)
{
    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_MAT_DIMS_EQ(to_(cld), Size2i(0, 0));
}

TEST_F(FeatDataVisitorMat_fTest, FromCloud)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);
    CloudXYZ::Ptr cloud = Mat2PointCloud(m);

    Mat_f m2 = to_(cloud);
    hconcat(m, Mat1f(m.rows, 1, 1), m);

    EXPECT_MAT_EQ(m, m2);
    EXPECT_NE(m.data, m2.data) << "Expecting deep copy. If intentionally optimized to be a shared copy, please update test.";
}

TEST_F(FeatDataVisitorMat_fTest, Reset_with_cloud)
{
    EXPECT_NO_THROW(to_.Reset());

    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    Mat_f m2 = to_(m);
    EXPECT_NO_THROW(to_.Reset());

    CloudXYZ::Ptr cloud = Mat2PointCloud(m);
    m2 = to_(cloud);
    EXPECT_NO_THROW(to_.Reset());

    m2 = to_(m);
    EXPECT_NO_THROW(to_.Reset());
}

/**
 * @brief test class around FeatDataVisitorCloud
 */
class FeatDataVisitorCloudTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        to_.Reset();
    }

    FeatDataVisitorCloud to_;    ///< test object
};

TEST_F(FeatDataVisitorCloudTest, Empty)
{
    EXPECT_TRUE(to_(Mat_f())->empty());
    EXPECT_TRUE(to_(Mat_f(0, 0))->empty());
    EXPECT_TRUE(to_(Mat_f(1, 0))->empty());
    EXPECT_TRUE(to_(Mat_f(0, 1))->empty());

    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_TRUE(to_(cld)->empty());
}

TEST_F(FeatDataVisitorCloudTest, EmptySize)
{
    EXPECT_EQ(size_t(0), to_(Mat_f())->size());
    EXPECT_EQ(size_t(0), to_(Mat_f(0, 0))->size());
    EXPECT_EQ(size_t(0), to_(Mat_f(1, 0))->size());
    EXPECT_EQ(size_t(0), to_(Mat_f(0, 1))->size());

    CloudXYZ::Ptr cld(new CloudXYZ);
    EXPECT_EQ(size_t(0), to_(cld)->size());
}

TEST_F(FeatDataVisitorCloudTest, FromMat_f)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    CloudXYZ::Ptr cld = to_(m);
    Mat1f m2 = PointCloud2Mat(cld);
    hconcat(m, Mat1f(m.rows, 1, 1), m);

    EXPECT_MAT_EQ(m, m2);
    EXPECT_NE(m.data, m2.data) << "Expecting deep copy. If intentionally optimized to be a shared copy, please update test.";
}

TEST_F(FeatDataVisitorCloudTest, Cloud)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);
    CloudXYZ::Ptr cld = Mat2PointCloud(m);

    CloudXYZ::Ptr cld2 = to_(cld);

    EXPECT_EQ(cld->size(), cld2->size()) << "Size mismatch.";
    EXPECT_EQ(cld->width, cld2->width) << "Width mismatch.";
    EXPECT_EQ(cld->height, cld2->height) << "Height mismatch.";

    for(CloudXYZ::iterator itr1=cld->begin(), itr2=cld2->begin();
        itr1 != cld->end(); ++itr1, ++itr2) {

        pcl::PointXYZ _p1 = *itr1;
        pcl::PointXYZ _p2 = *itr2;
        EXPECT_FLOAT_EQ(_p1.x, _p2.x) << "Unexpected value for x coordinate.";
        EXPECT_FLOAT_EQ(_p1.y, _p2.y) << "Unexpected value for y coordinate.";
        EXPECT_FLOAT_EQ(_p1.z, _p2.z) << "Unexpected value for z coordinate.";
    }
}

TEST_F(FeatDataVisitorCloudTest, ResetValid)
{
    Mat1f m(4, 3);
    randn(m, 0.f, 100.f);

    CloudXYZ::Ptr cld1 = to_(m);
    EXPECT_NO_THROW(to_.Reset());

    CloudXYZ::Ptr cld2 = Mat2PointCloud(m);
    CloudXYZ::Ptr cld3 = to_(cld2);
    EXPECT_NO_THROW(to_.Reset());

    cld1 = to_(m);
    EXPECT_NO_THROW(to_.Reset());
}

/**
 * @brief Thest this visitor's caching
 */
TEST_F(FeatDataVisitorCloudTest, Reset)
{
    Mat1f m(4, 3, 1.f);
    Mat1f m0 = m.clone();
    hconcat(m0, Mat1f(m0.rows, 1, 1), m0);

    CloudXYZ::Ptr cld = to_(m);
    EXPECT_MAT_EQ(PointCloud2Mat(cld), m0);

    cld = to_(m); // result of first call is cached.

    const int N=2;
    ASSERT_GT(N, 1) << "This test must iterate at least twice to see effect of Reset() on cached reference.";
    for(int i=0; i<2; i++) {

        cld = to_(m+1);

        if(i == 0) {

            EXPECT_MAT_EQ(PointCloud2Mat(cld), m0) << "Not suing cahced cloud.";
        }
        else {

            Mat1f m2 = m0.clone();
            m2.colRange(0, m2.cols-1).setTo(2.f);
            EXPECT_MAT_EQ(PointCloud2Mat(cld), m2) << "Still using cached cloud";
        }
        to_.Reset();
    }
}

#endif // __WITH_PCL

} // annonymous namespace for visitors' test fixtures
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2013 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/desktop_capture/mouse_cursor_monitor.h"

#include <assert.h>

#include <memory>

#include "webrtc/modules/desktop_capture/desktop_frame.h"
#include "webrtc/modules/desktop_capture/mouse_cursor.h"
#include "webrtc/modules/desktop_capture/win/cursor.h"
#include "webrtc/modules/desktop_capture/win/window_capture_utils.h"
#include "webrtc/system_wrappers/include/logging.h"

namespace webrtc {

class MouseCursorMonitorWin : public MouseCursorMonitor {
 public:
  explicit MouseCursorMonitorWin(HWND window);
  explicit MouseCursorMonitorWin(ScreenId screen);
  virtual ~MouseCursorMonitorWin();

  void Init(Callback* callback, Mode mode) override;
  void Capture() override;

 private:
  // Get the rect of the currently selected screen, relative to the primary
  // display's top-left. If the screen is disabled or disconnected, or any error
  // happens, an empty rect is returned.
  DesktopRect GetScreenRect();

  HWND window_;
  ScreenId screen_;

  Callback* callback_;
  Mode mode_;

  HDC desktop_dc_;

  HCURSOR last_cursor_;
};

MouseCursorMonitorWin::MouseCursorMonitorWin(HWND window)
    : window_(window),
      screen_(kInvalidScreenId),
      callback_(NULL),
      mode_(SHAPE_AND_POSITION),
      desktop_dc_(NULL),
      last_cursor_(NULL) {
}

MouseCursorMonitorWin::MouseCursorMonitorWin(ScreenId screen)
    : window_(NULL),
      screen_(screen),
      callback_(NULL),
      mode_(SHAPE_AND_POSITION),
      desktop_dc_(NULL),
      last_cursor_(NULL) {
  assert(screen >= kFullDesktopScreenId);
}

MouseCursorMonitorWin::~MouseCursorMonitorWin() {
  if (desktop_dc_)
    ReleaseDC(NULL, desktop_dc_);
}

void MouseCursorMonitorWin::Init(Callback* callback, Mode mode) {
  assert(!callback_);
  assert(callback);

  callback_ = callback;
  mode_ = mode;

  desktop_dc_ = GetDC(NULL);
}

void MouseCursorMonitorWin::Capture() {
  assert(callback_);

  CURSORINFO cursor_info;
  cursor_info.cbSize = sizeof(CURSORINFO);
  if (!GetCursorInfo(&cursor_info)) {
    LOG_F(LS_ERROR) << "Unable to get cursor info. Error = " << GetLastError();
    return;
  }

  if (last_cursor_ != cursor_info.hCursor) {
    last_cursor_ = cursor_info.hCursor;
    // Note that |cursor_info.hCursor| does not need to be freed.
    std::unique_ptr<MouseCursor> cursor(
        CreateMouseCursorFromHCursor(desktop_dc_, cursor_info.hCursor));
    if (cursor.get())
      callback_->OnMouseCursor(cursor.release());
  }

  if (mode_ != SHAPE_AND_POSITION)
    return;

  DesktopVector position(cursor_info.ptScreenPos.x, cursor_info.ptScreenPos.y);
  bool inside = cursor_info.flags == CURSOR_SHOWING;

  if (window_) {
    DesktopRect original_rect;
    DesktopRect cropped_rect;
    if (!GetCroppedWindowRect(window_, &cropped_rect, &original_rect)) {
      position.set(0, 0);
      inside = false;
    } else {
      if (inside) {
        HWND windowUnderCursor = WindowFromPoint(cursor_info.ptScreenPos);
        inside = windowUnderCursor ?
            (window_ == GetAncestor(windowUnderCursor, GA_ROOT)) : false;
      }
      position = position.subtract(cropped_rect.top_left());
    }
  } else {
    assert(screen_ != kInvalidScreenId);
    DesktopRect rect = GetScreenRect();
    if (inside)
      inside = rect.Contains(position);
    position = position.subtract(rect.top_left());
  }

  callback_->OnMouseCursorPosition(inside ? INSIDE : OUTSIDE, position);
}

DesktopRect MouseCursorMonitorWin::GetScreenRect() {
  assert(screen_ != kInvalidScreenId);
  if (screen_ == kFullDesktopScreenId) {
    return DesktopRect::MakeXYWH(
        GetSystemMetrics(SM_XVIRTUALSCREEN),
        GetSystemMetrics(SM_YVIRTUALSCREEN),
        GetSystemMetrics(SM_CXVIRTUALSCREEN),
        GetSystemMetrics(SM_CYVIRTUALSCREEN));
  }
  DISPLAY_DEVICE device;
  device.cb = sizeof(device);
  BOOL result = EnumDisplayDevices(NULL, screen_, &device, 0);
  if (!result)
    return DesktopRect();

  DEVMODE device_mode;
  device_mode.dmSize = sizeof(device_mode);
  device_mode.dmDriverExtra = 0;
  result = EnumDisplaySettingsEx(
      device.DeviceName, ENUM_CURRENT_SETTINGS, &device_mode, 0);
  if (!result)
    return DesktopRect();

  return DesktopRect::MakeXYWH(device_mode.dmPosition.x,
                               device_mode.dmPosition.y,
                               device_mode.dmPelsWidth,
                               device_mode.dmPelsHeight);
}

MouseCursorMonitor* MouseCursorMonitor::CreateForWindow(
    const DesktopCaptureOptions& options, WindowId window) {
  return new MouseCursorMonitorWin(reinterpret_cast<HWND>(window));
}

MouseCursorMonitor* MouseCursorMonitor::CreateForScreen(
    const DesktopCaptureOptions& options,
    ScreenId screen) {
  return new MouseCursorMonitorWin(screen);
}

}  // namespace webrtc
<commit_msg>CURSORINFO.flags should be checked before capturing its bitmap<commit_after>/*
 *  Copyright (c) 2013 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/desktop_capture/mouse_cursor_monitor.h"

#include <assert.h>
#include <string.h>

#include <memory>

#include "webrtc/modules/desktop_capture/desktop_frame.h"
#include "webrtc/modules/desktop_capture/desktop_geometry.h"
#include "webrtc/modules/desktop_capture/mouse_cursor.h"
#include "webrtc/modules/desktop_capture/win/cursor.h"
#include "webrtc/modules/desktop_capture/win/window_capture_utils.h"
#include "webrtc/system_wrappers/include/logging.h"

namespace webrtc {

namespace {

bool IsSameCursorShape(const CURSORINFO& left, const CURSORINFO& right) {
  // If the cursors are not showing, we do not care the hCursor handle.
  return left.flags == right.flags &&
         (left.flags != CURSOR_SHOWING ||
          left.hCursor == right.hCursor);
}

}  // namespace

class MouseCursorMonitorWin : public MouseCursorMonitor {
 public:
  explicit MouseCursorMonitorWin(HWND window);
  explicit MouseCursorMonitorWin(ScreenId screen);
  virtual ~MouseCursorMonitorWin();

  void Init(Callback* callback, Mode mode) override;
  void Capture() override;

 private:
  // Get the rect of the currently selected screen, relative to the primary
  // display's top-left. If the screen is disabled or disconnected, or any error
  // happens, an empty rect is returned.
  DesktopRect GetScreenRect();

  HWND window_;
  ScreenId screen_;

  Callback* callback_;
  Mode mode_;

  HDC desktop_dc_;

  // The last CURSORINFO (converted to MouseCursor) we have sent to the client.
  CURSORINFO last_cursor_;
};

MouseCursorMonitorWin::MouseCursorMonitorWin(HWND window)
    : window_(window),
      screen_(kInvalidScreenId),
      callback_(NULL),
      mode_(SHAPE_AND_POSITION),
      desktop_dc_(NULL) {
  memset(&last_cursor_, 0, sizeof(CURSORINFO));
}

MouseCursorMonitorWin::MouseCursorMonitorWin(ScreenId screen)
    : window_(NULL),
      screen_(screen),
      callback_(NULL),
      mode_(SHAPE_AND_POSITION),
      desktop_dc_(NULL) {
  assert(screen >= kFullDesktopScreenId);
  memset(&last_cursor_, 0, sizeof(CURSORINFO));
}

MouseCursorMonitorWin::~MouseCursorMonitorWin() {
  if (desktop_dc_)
    ReleaseDC(NULL, desktop_dc_);
}

void MouseCursorMonitorWin::Init(Callback* callback, Mode mode) {
  assert(!callback_);
  assert(callback);

  callback_ = callback;
  mode_ = mode;

  desktop_dc_ = GetDC(NULL);
}

void MouseCursorMonitorWin::Capture() {
  assert(callback_);

  CURSORINFO cursor_info;
  cursor_info.cbSize = sizeof(CURSORINFO);
  if (!GetCursorInfo(&cursor_info)) {
    LOG_F(LS_ERROR) << "Unable to get cursor info. Error = " << GetLastError();
    return;
  }

  if (!IsSameCursorShape(cursor_info, last_cursor_)) {
    if (cursor_info.flags == CURSOR_SUPPRESSED) {
      // The cursor is intentionally hidden now, send an empty bitmap.
      last_cursor_ = cursor_info;
      callback_->OnMouseCursor(new MouseCursor(
          new BasicDesktopFrame(DesktopSize()), DesktopVector()));
    } else {
      // According to MSDN https://goo.gl/u6gyuC, HCURSOR instances returned by
      // functions other than CreateCursor do not need to be actively destroyed.
      // And CloseHandle function (https://goo.gl/ja5ycW) does not close a
      // cursor, so assume a HCURSOR does not need to be closed.
      if (cursor_info.flags == 0) {
        // Host machine does not have a hardware mouse attached, we will send a
        // default one instead.
        // Note, Windows automatically caches cursor resource, so we do not need
        // to cache the result of LoadCursor.
        cursor_info.hCursor = LoadCursor(nullptr, IDC_ARROW);
      }
      std::unique_ptr<MouseCursor> cursor(
          CreateMouseCursorFromHCursor(desktop_dc_, cursor_info.hCursor));
      if (cursor) {
        last_cursor_ = cursor_info;
        callback_->OnMouseCursor(cursor.release());
      }
    }
  }

  if (mode_ != SHAPE_AND_POSITION)
    return;

  DesktopVector position(cursor_info.ptScreenPos.x, cursor_info.ptScreenPos.y);
  bool inside = cursor_info.flags == CURSOR_SHOWING;

  if (window_) {
    DesktopRect original_rect;
    DesktopRect cropped_rect;
    if (!GetCroppedWindowRect(window_, &cropped_rect, &original_rect)) {
      position.set(0, 0);
      inside = false;
    } else {
      if (inside) {
        HWND windowUnderCursor = WindowFromPoint(cursor_info.ptScreenPos);
        inside = windowUnderCursor ?
            (window_ == GetAncestor(windowUnderCursor, GA_ROOT)) : false;
      }
      position = position.subtract(cropped_rect.top_left());
    }
  } else {
    assert(screen_ != kInvalidScreenId);
    DesktopRect rect = GetScreenRect();
    if (inside)
      inside = rect.Contains(position);
    position = position.subtract(rect.top_left());
  }

  callback_->OnMouseCursorPosition(inside ? INSIDE : OUTSIDE, position);
}

DesktopRect MouseCursorMonitorWin::GetScreenRect() {
  assert(screen_ != kInvalidScreenId);
  if (screen_ == kFullDesktopScreenId) {
    return DesktopRect::MakeXYWH(
        GetSystemMetrics(SM_XVIRTUALSCREEN),
        GetSystemMetrics(SM_YVIRTUALSCREEN),
        GetSystemMetrics(SM_CXVIRTUALSCREEN),
        GetSystemMetrics(SM_CYVIRTUALSCREEN));
  }
  DISPLAY_DEVICE device;
  device.cb = sizeof(device);
  BOOL result = EnumDisplayDevices(NULL, screen_, &device, 0);
  if (!result)
    return DesktopRect();

  DEVMODE device_mode;
  device_mode.dmSize = sizeof(device_mode);
  device_mode.dmDriverExtra = 0;
  result = EnumDisplaySettingsEx(
      device.DeviceName, ENUM_CURRENT_SETTINGS, &device_mode, 0);
  if (!result)
    return DesktopRect();

  return DesktopRect::MakeXYWH(device_mode.dmPosition.x,
                               device_mode.dmPosition.y,
                               device_mode.dmPelsWidth,
                               device_mode.dmPelsHeight);
}

MouseCursorMonitor* MouseCursorMonitor::CreateForWindow(
    const DesktopCaptureOptions& options, WindowId window) {
  return new MouseCursorMonitorWin(reinterpret_cast<HWND>(window));
}

MouseCursorMonitor* MouseCursorMonitor::CreateForScreen(
    const DesktopCaptureOptions& options,
    ScreenId screen) {
  return new MouseCursorMonitorWin(screen);
}

}  // namespace webrtc
<|endoftext|>


<commit_before>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2022  Miodrag Milanovic <micko@yosyshq.com>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/fstdata.h"

USING_YOSYS_NAMESPACE

static std::string file_base_name(std::string const & path)
{
	return path.substr(path.find_last_of("/\\") + 1);
}

FstData::FstData(std::string filename) : ctx(nullptr)
{
	#if !defined(YOSYS_DISABLE_SPAWN)
	std::string filename_trim = file_base_name(filename);
	if (filename_trim.size() > 4 && filename_trim.compare(filename_trim.size()-4, std::string::npos, ".vcd") == 0) {
		filename_trim.erase(filename_trim.size()-4);
		tmp_file = stringf("/tmp/converted_%s.fst", filename_trim.c_str());
		std::string cmd = stringf("vcd2fst %s %s", filename.c_str(), tmp_file.c_str());
		log("Exec: %s\n", cmd.c_str());
		if (run_command(cmd) != 0)
			log_cmd_error("Shell command failed!\n");
		filename = tmp_file;
	}
	#endif
	const std::vector<std::string> g_units = { "s", "ms", "us", "ns", "ps", "fs", "as", "zs" };
	ctx = (fstReaderContext *)fstReaderOpen(filename.c_str());
	if (!ctx)
		log_error("Error opening '%s' as FST file\n", filename.c_str());
	int scale = (int)fstReaderGetTimescale(ctx);
	timescale = pow(10.0, scale);
	timescale_str = "";
	int unit = 0;
	int zeros = 0;
	if (scale > 0)  {
		zeros = scale;
	} else {
		if ((scale % 3) == 0) {
			zeros = (-scale % 3);
			unit = (-scale / 3);
		} else {
			zeros = 3 - (-scale % 3);
			unit = (-scale / 3) + 1;
		}
	}
	for (int i=0;i<zeros; i++) timescale_str += "0";
	timescale_str += g_units[unit];
	extractVarNames();
}

FstData::~FstData()
{
	if (ctx)
		fstReaderClose(ctx);
	if (!tmp_file.empty())
		remove(tmp_file.c_str());
}

uint64_t FstData::getStartTime() { return fstReaderGetStartTime(ctx); }

uint64_t FstData::getEndTime() { return fstReaderGetEndTime(ctx); }

fstHandle FstData::getHandle(std::string name) {
	if (name_to_handle.find(name) != name_to_handle.end())
		return name_to_handle[name];
	else
		return 0;
};

dict<int,fstHandle> FstData::getMemoryHandles(std::string name) {
	if (memory_to_handle.find(name) != memory_to_handle.end())
		return memory_to_handle[name];
	else
		return dict<int,fstHandle>();
};

static std::string remove_spaces(std::string str)
{
	str.erase(std::remove(str.begin(), str.end(), ' '), str.end());
	return str;
}

void FstData::extractVarNames()
{
	struct fstHier *h;
	std::string fst_scope_name;

	while ((h = fstReaderIterateHier(ctx))) {
		switch (h->htyp) {
			case FST_HT_SCOPE: {
				fst_scope_name = fstReaderPushScope(ctx, h->u.scope.name, NULL);
				break;
			}
			case FST_HT_UPSCOPE: {
				fst_scope_name = fstReaderPopScope(ctx);
				break;
			}
			case FST_HT_VAR: {
				FstVar var;
				var.id = h->u.var.handle;
				var.is_alias = h->u.var.is_alias;
				var.is_reg = (fstVarType)h->u.var.typ == FST_VT_VCD_REG;
				var.name = remove_spaces(h->u.var.name);
				var.scope = fst_scope_name;
				var.width = h->u.var.length;
				vars.push_back(var);
				if (!var.is_alias)
					handle_to_var[h->u.var.handle] = var;
				std::string clean_name;
				for(size_t i=0;i<strlen(h->u.var.name);i++)
				{
					char c = h->u.var.name[i];
					if(c==' ') break;
					clean_name += c;
				}
				if (clean_name[0]=='\\')
					clean_name = clean_name.substr(1);
				size_t pos = clean_name.find_last_of("<");
				if (pos != std::string::npos) {
					std::string mem_cell = clean_name.substr(0, pos);
					std::string addr = clean_name.substr(pos+1);
					addr.pop_back(); // remove closing bracket
					char *endptr;
					int mem_addr = strtol(addr.c_str(), &endptr, 16);
					if (*endptr) {
						log_error("Error parsing memory address in : %s\n", clean_name.c_str());
					}
					memory_to_handle[var.scope+"."+mem_cell][mem_addr] = var.id;
				}
				pos = clean_name.find_last_of("[");
				if (pos != std::string::npos) {
					std::string mem_cell = clean_name.substr(0, pos);
					std::string addr = clean_name.substr(pos+1);
					addr.pop_back(); // remove closing bracket
					char *endptr;
					int mem_addr = strtol(addr.c_str(), &endptr, 10);
					if (*endptr) {
						log_error("Error parsing memory address in : %s\n", clean_name.c_str());
					}
					memory_to_handle[var.scope+"."+mem_cell][mem_addr] = var.id;
				}
				name_to_handle[var.scope+"."+clean_name] = h->u.var.handle;
				break;
			}
		}
	}
}

static void reconstruct_clb_varlen_attimes(void *user_data, uint64_t pnt_time, fstHandle pnt_facidx, const unsigned char *pnt_value, uint32_t plen)
{
	FstData *ptr = (FstData*)user_data;
	ptr->reconstruct_callback_attimes(pnt_time, pnt_facidx, pnt_value, plen);
}

static void reconstruct_clb_attimes(void *user_data, uint64_t pnt_time, fstHandle pnt_facidx, const unsigned char *pnt_value)
{
	FstData *ptr = (FstData*)user_data;
	uint32_t plen = (pnt_value) ?  strlen((const char *)pnt_value) : 0;
	ptr->reconstruct_callback_attimes(pnt_time, pnt_facidx, pnt_value, plen);
}

void FstData::reconstruct_callback_attimes(uint64_t pnt_time, fstHandle pnt_facidx, const unsigned char *pnt_value, uint32_t /* plen */)
{
	if (pnt_time > end_time) return;
	// if we are past the timestamp
	bool is_clock = false;
	if (!all_samples) {
		for(auto &s : clk_signals) {
			if (s==pnt_facidx)  {
				is_clock=true;
				break;
			}
		}
	}

	if (pnt_time > past_time) {
		past_data = last_data;
		past_time = pnt_time;
	}

	if (pnt_time > last_time) {
		if (all_samples) {
			callback(last_time);
			last_time = pnt_time;
		} else {
			if (is_clock) {
				std::string val = std::string((const char *)pnt_value);
				std::string prev = past_data[pnt_facidx];
				if ((prev!="1" && val=="1") || (prev!="0" && val=="0")) {
					callback(last_time);
					last_time = pnt_time;
				}
			}
		}
	}
	// always update last_data
	last_data[pnt_facidx] =  std::string((const char *)pnt_value);
}

void FstData::reconstructAllAtTimes(std::vector<fstHandle> &signal, uint64_t start, uint64_t end, CallbackFunction cb)
{
	clk_signals = signal;
	callback = cb;
	start_time = start;
	end_time = end;
	last_data.clear();
	last_time = start_time;
	past_data.clear();
	past_time = start_time;
	all_samples = clk_signals.empty();

	fstReaderSetUnlimitedTimeRange(ctx);
	fstReaderSetFacProcessMaskAll(ctx);
	fstReaderIterBlocks2(ctx, reconstruct_clb_attimes, reconstruct_clb_varlen_attimes, this, nullptr);
	if (last_time!=end_time) {
		past_data = last_data;
		callback(last_time);
	}
	callback(end_time);
}

std::string FstData::valueOf(fstHandle signal)
{
	if (past_data.find(signal) == past_data.end())
		log_error("Signal id %d not found\n", (int)signal);
	return past_data[signal];
}
<commit_msg>map memory location to wire value, if memory is converted to FFs<commit_after>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2022  Miodrag Milanovic <micko@yosyshq.com>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/fstdata.h"

USING_YOSYS_NAMESPACE

static std::string file_base_name(std::string const & path)
{
	return path.substr(path.find_last_of("/\\") + 1);
}

FstData::FstData(std::string filename) : ctx(nullptr)
{
	#if !defined(YOSYS_DISABLE_SPAWN)
	std::string filename_trim = file_base_name(filename);
	if (filename_trim.size() > 4 && filename_trim.compare(filename_trim.size()-4, std::string::npos, ".vcd") == 0) {
		filename_trim.erase(filename_trim.size()-4);
		tmp_file = stringf("/tmp/converted_%s.fst", filename_trim.c_str());
		std::string cmd = stringf("vcd2fst %s %s", filename.c_str(), tmp_file.c_str());
		log("Exec: %s\n", cmd.c_str());
		if (run_command(cmd) != 0)
			log_cmd_error("Shell command failed!\n");
		filename = tmp_file;
	}
	#endif
	const std::vector<std::string> g_units = { "s", "ms", "us", "ns", "ps", "fs", "as", "zs" };
	ctx = (fstReaderContext *)fstReaderOpen(filename.c_str());
	if (!ctx)
		log_error("Error opening '%s' as FST file\n", filename.c_str());
	int scale = (int)fstReaderGetTimescale(ctx);
	timescale = pow(10.0, scale);
	timescale_str = "";
	int unit = 0;
	int zeros = 0;
	if (scale > 0)  {
		zeros = scale;
	} else {
		if ((scale % 3) == 0) {
			zeros = (-scale % 3);
			unit = (-scale / 3);
		} else {
			zeros = 3 - (-scale % 3);
			unit = (-scale / 3) + 1;
		}
	}
	for (int i=0;i<zeros; i++) timescale_str += "0";
	timescale_str += g_units[unit];
	extractVarNames();
}

FstData::~FstData()
{
	if (ctx)
		fstReaderClose(ctx);
	if (!tmp_file.empty())
		remove(tmp_file.c_str());
}

uint64_t FstData::getStartTime() { return fstReaderGetStartTime(ctx); }

uint64_t FstData::getEndTime() { return fstReaderGetEndTime(ctx); }

fstHandle FstData::getHandle(std::string name) {
	if (name_to_handle.find(name) != name_to_handle.end())
		return name_to_handle[name];
	else
		return 0;
};

dict<int,fstHandle> FstData::getMemoryHandles(std::string name) {
	if (memory_to_handle.find(name) != memory_to_handle.end())
		return memory_to_handle[name];
	else
		return dict<int,fstHandle>();
};

static std::string remove_spaces(std::string str)
{
	str.erase(std::remove(str.begin(), str.end(), ' '), str.end());
	return str;
}

void FstData::extractVarNames()
{
	struct fstHier *h;
	std::string fst_scope_name;

	while ((h = fstReaderIterateHier(ctx))) {
		switch (h->htyp) {
			case FST_HT_SCOPE: {
				fst_scope_name = fstReaderPushScope(ctx, h->u.scope.name, NULL);
				break;
			}
			case FST_HT_UPSCOPE: {
				fst_scope_name = fstReaderPopScope(ctx);
				break;
			}
			case FST_HT_VAR: {
				FstVar var;
				var.id = h->u.var.handle;
				var.is_alias = h->u.var.is_alias;
				var.is_reg = (fstVarType)h->u.var.typ == FST_VT_VCD_REG;
				var.name = remove_spaces(h->u.var.name);
				var.scope = fst_scope_name;
				var.width = h->u.var.length;
				vars.push_back(var);
				if (!var.is_alias)
					handle_to_var[h->u.var.handle] = var;
				std::string clean_name;
				for(size_t i=0;i<strlen(h->u.var.name);i++)
				{
					char c = h->u.var.name[i];
					if(c==' ') break;
					clean_name += c;
				}
				if (clean_name[0]=='\\')
					clean_name = clean_name.substr(1);
				size_t pos = clean_name.find_last_of("<");
				if (pos != std::string::npos) {
					std::string mem_cell = clean_name.substr(0, pos);
					std::string addr = clean_name.substr(pos+1);
					addr.pop_back(); // remove closing bracket
					char *endptr;
					int mem_addr = strtol(addr.c_str(), &endptr, 16);
					if (*endptr) {
						log_error("Error parsing memory address in : %s\n", clean_name.c_str());
					}
					memory_to_handle[var.scope+"."+mem_cell][mem_addr] = var.id;
					name_to_handle[stringf("%s.%s[%d]",var.scope.c_str(),mem_cell.c_str(),mem_addr)] = h->u.var.handle;
					continue;
				}
				pos = clean_name.find_last_of("[");
				if (pos != std::string::npos) {
					std::string mem_cell = clean_name.substr(0, pos);
					std::string addr = clean_name.substr(pos+1);
					addr.pop_back(); // remove closing bracket
					char *endptr;
					int mem_addr = strtol(addr.c_str(), &endptr, 10);
					if (*endptr) {
						log_error("Error parsing memory address in : %s\n", clean_name.c_str());
					}
					memory_to_handle[var.scope+"."+mem_cell][mem_addr] = var.id;
					name_to_handle[stringf("%s.%s[%d]",var.scope.c_str(),mem_cell.c_str(),mem_addr)] = h->u.var.handle;
					continue;
				}
				name_to_handle[var.scope+"."+clean_name] = h->u.var.handle;
				break;
			}
		}
	}
}

static void reconstruct_clb_varlen_attimes(void *user_data, uint64_t pnt_time, fstHandle pnt_facidx, const unsigned char *pnt_value, uint32_t plen)
{
	FstData *ptr = (FstData*)user_data;
	ptr->reconstruct_callback_attimes(pnt_time, pnt_facidx, pnt_value, plen);
}

static void reconstruct_clb_attimes(void *user_data, uint64_t pnt_time, fstHandle pnt_facidx, const unsigned char *pnt_value)
{
	FstData *ptr = (FstData*)user_data;
	uint32_t plen = (pnt_value) ?  strlen((const char *)pnt_value) : 0;
	ptr->reconstruct_callback_attimes(pnt_time, pnt_facidx, pnt_value, plen);
}

void FstData::reconstruct_callback_attimes(uint64_t pnt_time, fstHandle pnt_facidx, const unsigned char *pnt_value, uint32_t /* plen */)
{
	if (pnt_time > end_time) return;
	// if we are past the timestamp
	bool is_clock = false;
	if (!all_samples) {
		for(auto &s : clk_signals) {
			if (s==pnt_facidx)  {
				is_clock=true;
				break;
			}
		}
	}

	if (pnt_time > past_time) {
		past_data = last_data;
		past_time = pnt_time;
	}

	if (pnt_time > last_time) {
		if (all_samples) {
			callback(last_time);
			last_time = pnt_time;
		} else {
			if (is_clock) {
				std::string val = std::string((const char *)pnt_value);
				std::string prev = past_data[pnt_facidx];
				if ((prev!="1" && val=="1") || (prev!="0" && val=="0")) {
					callback(last_time);
					last_time = pnt_time;
				}
			}
		}
	}
	// always update last_data
	last_data[pnt_facidx] =  std::string((const char *)pnt_value);
}

void FstData::reconstructAllAtTimes(std::vector<fstHandle> &signal, uint64_t start, uint64_t end, CallbackFunction cb)
{
	clk_signals = signal;
	callback = cb;
	start_time = start;
	end_time = end;
	last_data.clear();
	last_time = start_time;
	past_data.clear();
	past_time = start_time;
	all_samples = clk_signals.empty();

	fstReaderSetUnlimitedTimeRange(ctx);
	fstReaderSetFacProcessMaskAll(ctx);
	fstReaderIterBlocks2(ctx, reconstruct_clb_attimes, reconstruct_clb_varlen_attimes, this, nullptr);
	if (last_time!=end_time) {
		past_data = last_data;
		callback(last_time);
	}
	callback(end_time);
}

std::string FstData::valueOf(fstHandle signal)
{
	if (past_data.find(signal) == past_data.end())
		log_error("Signal id %d not found\n", (int)signal);
	return past_data[signal];
}
<|endoftext|>


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

/**
 * @file
 **/

#include "modules/planning/tasks/path_decider/path_decider.h"

#include <algorithm>
#include <cmath>
#include <string>
#include <utility>
#include <vector>

#include "modules/planning/proto/decision.pb.h"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleConfigHelper;

PathDecider::PathDecider() : Task("PathDecider") {}

apollo::common::Status PathDecider::Execute(
    Frame *, ReferenceLineInfo *reference_line_info) {
  Task::Execute(nullptr, reference_line_info);
  return Process(reference_line_info->path_data(),
                 reference_line_info->path_decision());
}

Status PathDecider::Process(const PathData &path_data,
                            PathDecision *const path_decision) {
  CHECK_NOTNULL(path_decision);
  if (!MakeObjectDecision(path_data, path_decision)) {
    AERROR << "Failed to make decision based on tunnel";
    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph decision ");
  }
  return Status::OK();
}

bool PathDecider::MakeObjectDecision(const PathData &path_data,
                                     PathDecision *const path_decision) {
  DCHECK_NOTNULL(path_decision);
  if (!MakeStaticObstacleDecision(path_data, path_decision)) {
    AERROR << "Failed to make decisions for static obstacles";
    return false;
  }
  return true;
}

bool PathDecider::MakeStaticObstacleDecision(
    const PathData &path_data, PathDecision *const path_decision) {
  DCHECK_NOTNULL(path_decision);
  const auto &frenet_path = path_data.frenet_frame_path();
  const auto &frenet_points = frenet_path.points();
  if (frenet_points.empty()) {
    AERROR << "Path is empty.";
    return false;
  }

  const double half_width =
      common::VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;

  const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer;

  const double lateral_stop_radius =
      half_width + FLAGS_static_decision_nudge_l_buffer;

  for (const auto *path_obstacle : path_decision->path_obstacles().Items()) {
    const auto &obstacle = *path_obstacle->obstacle();
    if (!obstacle.IsStatic()) {
      continue;
    }
    if (path_obstacle->HasLongitudinalDecision() &&
        path_obstacle->LongitudinalDecision().has_ignore() &&
        path_obstacle->HasLateralDecision() &&
        path_obstacle->LateralDecision().has_ignore()) {
      continue;
    }
    if (path_obstacle->HasLongitudinalDecision() &&
        path_obstacle->LongitudinalDecision().has_stop()) {
      continue;
    }

    if (path_obstacle->st_boundary().boundary_type() ==
        StBoundary::BoundaryType::KEEP_CLEAR) {
      continue;
    }

    // IGNORE by default
    ObjectDecisionType object_decision;
    object_decision.mutable_ignore();

    const auto &sl_boundary = path_obstacle->perception_sl_boundary();

    if (sl_boundary.start_s() < frenet_points.front().s() ||
        sl_boundary.start_s() > frenet_points.back().s()) {
      path_decision->AddLongitudinalDecision("PathDecider", obstacle.Id(),
                                             object_decision);
      path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                        object_decision);
      continue;
    }

    const auto frenet_point = frenet_path.EvaluateByS(sl_boundary.start_s());
    const double curr_l = frenet_point.l();
    if (curr_l - lateral_radius > sl_boundary.end_l() ||
        curr_l + lateral_radius < sl_boundary.start_l()) {
      // ignore
      path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                        object_decision);
    } else if (curr_l - lateral_stop_radius < sl_boundary.end_l() &&
               curr_l + lateral_stop_radius > sl_boundary.start_l()) {
      *object_decision.mutable_stop() =
          GenerateObjectStopDecision(*path_obstacle);
      path_decision->AddLongitudinalDecision("PathDecider", obstacle.Id(),
                                             object_decision);
    } else if (FLAGS_enable_nudge_decision &&
               (curr_l - lateral_stop_radius > sl_boundary.end_l())) {
      ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
      object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);
      object_nudge_ptr->set_distance_l(FLAGS_nudge_distance_obstacle);
      path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                        object_decision);
    } else {
      if (FLAGS_enable_nudge_decision) {
        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
        object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);
        object_nudge_ptr->set_distance_l(-FLAGS_nudge_distance_obstacle);
        path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                          object_decision);
      }
    }
  }

  return true;
}

double PathDecider::MinimumRadiusStopDistance(
    const PathObstacle &path_obstacle) const {
  constexpr double stop_distance_buffer = 0.5;
  const auto &vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
  const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();
  double lateral_diff =
      std::max(std::fabs(path_obstacle.perception_sl_boundary().start_l() -
                         reference_line_info_->AdcSlBoundary().end_l()),
               std::fabs(path_obstacle.perception_sl_boundary().end_l() -
                         reference_line_info_->AdcSlBoundary().start_l()));
  lateral_diff = std::max(lateral_diff, vehicle_param.width());
  lateral_diff = std::min(lateral_diff,
                          vehicle_param.width() +
                              path_obstacle.perception_sl_boundary().end_l() -
                              path_obstacle.perception_sl_boundary().start_l());
  double stop_distance = std::sqrt(min_turn_radius * min_turn_radius -
                                   (min_turn_radius - lateral_diff) *
                                       (min_turn_radius - lateral_diff)) +
                         stop_distance_buffer;
  stop_distance -= vehicle_param.front_edge_to_center();
  stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle);
  stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle);
  return stop_distance;
}

ObjectStop PathDecider::GenerateObjectStopDecision(
    const PathObstacle &path_obstacle) const {
  ObjectStop object_stop;

  double stop_distance = FLAGS_max_stop_distance_obstacle;

  if (path_obstacle.obstacle()->Id() == FLAGS_destination_obstacle_id) {
    // destination
    object_stop.set_reason_code(StopReasonCode::STOP_REASON_DESTINATION);
    stop_distance = FLAGS_stop_distance_destination;
  } else {
    stop_distance = MinimumRadiusStopDistance(path_obstacle);
    object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
  }
  object_stop.set_distance_s(-stop_distance);

  const double stop_ref_s =
      path_obstacle.perception_sl_boundary().start_s() - stop_distance;
  const auto stop_ref_point =
      reference_line_info_->reference_line().GetReferencePoint(stop_ref_s);
  object_stop.mutable_stop_point()->set_x(stop_ref_point.x());
  object_stop.mutable_stop_point()->set_y(stop_ref_point.y());
  object_stop.set_stop_heading(stop_ref_point.heading());
  return object_stop;
}

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

/**
 * @file
 **/

#include "modules/planning/tasks/path_decider/path_decider.h"

#include <algorithm>
#include <cmath>
#include <string>
#include <utility>
#include <vector>

#include "modules/planning/proto/decision.pb.h"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/util.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleConfigHelper;

PathDecider::PathDecider() : Task("PathDecider") {}

apollo::common::Status PathDecider::Execute(
    Frame *, ReferenceLineInfo *reference_line_info) {
  Task::Execute(nullptr, reference_line_info);
  return Process(reference_line_info->path_data(),
                 reference_line_info->path_decision());
}

Status PathDecider::Process(const PathData &path_data,
                            PathDecision *const path_decision) {
  CHECK_NOTNULL(path_decision);
  if (!MakeObjectDecision(path_data, path_decision)) {
    AERROR << "Failed to make decision based on tunnel";
    return Status(ErrorCode::PLANNING_ERROR, "dp_road_graph decision ");
  }
  return Status::OK();
}

bool PathDecider::MakeObjectDecision(const PathData &path_data,
                                     PathDecision *const path_decision) {
  DCHECK_NOTNULL(path_decision);
  if (!MakeStaticObstacleDecision(path_data, path_decision)) {
    AERROR << "Failed to make decisions for static obstacles";
    return false;
  }
  return true;
}

bool PathDecider::MakeStaticObstacleDecision(
    const PathData &path_data, PathDecision *const path_decision) {
  DCHECK_NOTNULL(path_decision);
  const auto &frenet_path = path_data.frenet_frame_path();
  const auto &frenet_points = frenet_path.points();
  if (frenet_points.empty()) {
    AERROR << "Path is empty.";
    return false;
  }

  const double half_width =
      common::VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;

  const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer;

  const double lateral_stop_radius =
      half_width + FLAGS_static_decision_nudge_l_buffer;

  for (const auto *path_obstacle : path_decision->path_obstacles().Items()) {
    const auto &obstacle = *path_obstacle->obstacle();
    if (!obstacle.IsStatic()) {
      continue;
    }
    if (path_obstacle->HasLongitudinalDecision() &&
        path_obstacle->LongitudinalDecision().has_ignore() &&
        path_obstacle->HasLateralDecision() &&
        path_obstacle->LateralDecision().has_ignore()) {
      continue;
    }
    if (path_obstacle->HasLongitudinalDecision() &&
        path_obstacle->LongitudinalDecision().has_stop()) {
      continue;
    }

    if (path_obstacle->st_boundary().boundary_type() ==
        StBoundary::BoundaryType::KEEP_CLEAR) {
      continue;
    }

    // IGNORE by default
    ObjectDecisionType object_decision;
    object_decision.mutable_ignore();

    const auto &sl_boundary = path_obstacle->perception_sl_boundary();

    if (sl_boundary.start_s() < frenet_points.front().s() ||
        sl_boundary.start_s() > frenet_points.back().s()) {
      path_decision->AddLongitudinalDecision("PathDecider", obstacle.Id(),
                                             object_decision);
      path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                        object_decision);
      continue;
    }

    const auto frenet_point = frenet_path.EvaluateByS(sl_boundary.start_s());
    const double curr_l = frenet_point.l();
    if (curr_l - lateral_radius > sl_boundary.end_l() ||
        curr_l + lateral_radius < sl_boundary.start_l()) {
      // ignore
      path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                        object_decision);
    } else if (curr_l - lateral_stop_radius < sl_boundary.end_l() &&
               curr_l + lateral_stop_radius > sl_boundary.start_l()) {
      *object_decision.mutable_stop() =
          GenerateObjectStopDecision(*path_obstacle);
      path_decision->AddLongitudinalDecision("PathDecider", obstacle.Id(),
                                             object_decision);
    } else if (FLAGS_enable_nudge_decision &&
               (curr_l - lateral_stop_radius > sl_boundary.end_l())) {
      ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
      object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);
      object_nudge_ptr->set_distance_l(FLAGS_nudge_distance_obstacle);
      path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                        object_decision);
    } else {
      if (FLAGS_enable_nudge_decision) {
        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
        object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);
        object_nudge_ptr->set_distance_l(-FLAGS_nudge_distance_obstacle);
        path_decision->AddLateralDecision("PathDecider", obstacle.Id(),
                                          object_decision);
      }
    }
}

  return true;
}

double PathDecider::MinimumRadiusStopDistance(
    const PathObstacle &path_obstacle) const {
  constexpr double stop_distance_buffer = 0.5;
  const auto &vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
  const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();
  double lateral_diff =
      std::max(std::fabs(path_obstacle.perception_sl_boundary().start_l() -
                         reference_line_info_->AdcSlBoundary().end_l()),
               std::fabs(path_obstacle.perception_sl_boundary().end_l() -
                         reference_line_info_->AdcSlBoundary().start_l()));
  lateral_diff = std::max(lateral_diff, vehicle_param.width());
  const double kEpison = 1e-5;
  lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);
  double stop_distance = std::sqrt(std::fabs(min_turn_radius * min_turn_radius -
                                   (min_turn_radius - lateral_diff) *
                                       (min_turn_radius - lateral_diff))) +
                         stop_distance_buffer;
  stop_distance -= vehicle_param.front_edge_to_center();
  stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle);
  stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle);
  return stop_distance;
}

ObjectStop PathDecider::GenerateObjectStopDecision(
    const PathObstacle &path_obstacle) const {
  ObjectStop object_stop;

  double stop_distance = FLAGS_max_stop_distance_obstacle;

  if (path_obstacle.obstacle()->Id() == FLAGS_destination_obstacle_id) {
    // destination
    object_stop.set_reason_code(StopReasonCode::STOP_REASON_DESTINATION);
    stop_distance = FLAGS_stop_distance_destination;
  } else {
    stop_distance = MinimumRadiusStopDistance(path_obstacle);
    object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
  }
  object_stop.set_distance_s(-stop_distance);

  const double stop_ref_s =
      path_obstacle.perception_sl_boundary().start_s() - stop_distance;
  const auto stop_ref_point =
      reference_line_info_->reference_line().GetReferencePoint(stop_ref_s);
  object_stop.mutable_stop_point()->set_x(stop_ref_point.x());
  object_stop.mutable_stop_point()->set_y(stop_ref_point.y());
  object_stop.set_stop_heading(stop_ref_point.heading());
  return object_stop;
}

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


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

#include "modules/prediction/container/pose/pose_container.h"

#include "modules/common/log.h"

namespace apollo {
namespace prediction {

using apollo::perception::PerceptionObstacle;
using apollo::perception::Point;
using apollo::localization::LocalizationEstimate;

std::mutex PoseContainer::g_mutex_;

void PoseContainer::Insert(const ::google::protobuf::Message& message) {
  Update(dynamic_cast<const LocalizationEstimate&>(message));
}

void PoseContainer::Update(
    const localization::LocalizationEstimate &localization) {
  if (!localization.header().has_timestamp_sec()) {
    AERROR << "Localization message has no timestamp ["
           << localization.ShortDebugString() << "].";
    return;
  } else if (!localization.has_pose()) {
    AERROR << "Localization message has no pose ["
           << localization.ShortDebugString() << "].";
  } else if (!localization.pose().has_position() ||
             !localization.pose().has_linear_velocity()) {
    AERROR << "Localization message has no position or linear velocity ["
           << localization.ShortDebugString() << "].";
    return;
  }

  std::lock_guard<std::mutex> lock(g_mutex_);
  if (obstacle_ptr_.get() == nullptr) {
    obstacle_ptr_.reset(new PerceptionObstacle());
  }
  obstacle_ptr_->set_id(ID);
  Point position;
  position.set_x(localization.pose().position().x());
  position.set_y(localization.pose().position().y());
  position.set_z(localization.pose().position().z());
  obstacle_ptr_->mutable_position()->CopyFrom(position);

  Point velocity;
  velocity.set_x(localization.pose().linear_velocity().x());
  velocity.set_y(localization.pose().linear_velocity().y());
  velocity.set_z(localization.pose().linear_velocity().z());
  obstacle_ptr_->mutable_velocity()->CopyFrom(velocity);

  obstacle_ptr_->set_type(type_);
  obstacle_ptr_->set_timestamp(localization.header().timestamp_sec());

  ADEBUG << "ADC obstacle [" << obstacle_ptr_->ShortDebugString() << "].";
}

double PoseContainer::GetTimestamp() {
  if (obstacle_ptr_ != nullptr) {
    return obstacle_ptr_->timestamp();
  } else {
    return 0.0;
  }
}

PerceptionObstacle* PoseContainer::ToPerceptionObstacle() {
  std::lock_guard<std::mutex> lock(g_mutex_);
  return obstacle_ptr_.get();
}

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

#include "modules/prediction/container/pose/pose_container.h"

#include "modules/common/log.h"

namespace apollo {
namespace prediction {

using apollo::perception::PerceptionObstacle;
using apollo::perception::Point;
using apollo::localization::LocalizationEstimate;

std::mutex PoseContainer::g_mutex_;

void PoseContainer::Insert(const ::google::protobuf::Message& message) {
  Update(dynamic_cast<const LocalizationEstimate&>(message));
}

void PoseContainer::Update(
    const localization::LocalizationEstimate &localization) {
  if (!localization.has_header() ||
      !localization.header().has_timestamp_sec()) {
    AERROR << "Localization message has no timestamp ["
           << localization.ShortDebugString() << "].";
    return;
  } else if (!localization.has_pose()) {
    AERROR << "Localization message has no pose ["
           << localization.ShortDebugString() << "].";
  } else if (!localization.pose().has_position() ||
             !localization.pose().has_linear_velocity()) {
    AERROR << "Localization message has no position or linear velocity ["
           << localization.ShortDebugString() << "].";
    return;
  }

  std::lock_guard<std::mutex> lock(g_mutex_);
  if (obstacle_ptr_.get() == nullptr) {
    obstacle_ptr_.reset(new PerceptionObstacle());
  }
  obstacle_ptr_->set_id(ID);
  Point position;
  position.set_x(localization.pose().position().x());
  position.set_y(localization.pose().position().y());
  position.set_z(localization.pose().position().z());
  obstacle_ptr_->mutable_position()->CopyFrom(position);

  Point velocity;
  velocity.set_x(localization.pose().linear_velocity().x());
  velocity.set_y(localization.pose().linear_velocity().y());
  velocity.set_z(localization.pose().linear_velocity().z());
  obstacle_ptr_->mutable_velocity()->CopyFrom(velocity);

  obstacle_ptr_->set_type(type_);
  obstacle_ptr_->set_timestamp(localization.header().timestamp_sec());

  ADEBUG << "ADC obstacle [" << obstacle_ptr_->ShortDebugString() << "].";
}

double PoseContainer::GetTimestamp() {
  if (obstacle_ptr_ != nullptr) {
    return obstacle_ptr_->timestamp();
  } else {
    return 0.0;
  }
}

PerceptionObstacle* PoseContainer::ToPerceptionObstacle() {
  std::lock_guard<std::mutex> lock(g_mutex_);
  return obstacle_ptr_.get();
}

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


<commit_before>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkLevelWindowManager.h"
#include <itkCommand.h>
#include "mitkDataStorage.h"
#include "mitkNodePredicateBase.h"
#include "mitkNodePredicateProperty.h"
#include "mitkNodePredicateDataType.h"
#include "mitkNodePredicateAND.h"
#include "mitkNodePredicateNOT.h"
#include "mitkProperties.h"
#include "mitkMessage.h"

mitk::LevelWindowManager::LevelWindowManager()
: m_DataStorage(NULL), m_LevelWindowProperty(NULL), m_AutoTopMost(true),
  m_IsObserverTagSet(false), m_CurrentImage(NULL),m_IsPropertyModifiedTagSet(false)
{
}

mitk::LevelWindowManager::~LevelWindowManager()
{
  if (m_DataStorage.IsNotNull())
  {
    m_DataStorage->AddNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
    m_DataStorage->RemoveNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
    m_DataStorage = NULL;
  }
  if (m_IsPropertyModifiedTagSet && m_LevelWindowProperty.IsNotNull())
  {
    m_LevelWindowProperty->RemoveObserver(m_PropertyModifiedTag);
    m_IsPropertyModifiedTagSet = false;
  }
  for( std::map<unsigned long, mitk::BaseProperty::Pointer>::iterator iter = m_PropObserverToNode.begin(); iter != m_PropObserverToNode.end(); iter++ ) {
    (*iter).second->RemoveObserver((*iter).first);
  }
}

void mitk::LevelWindowManager::SetDataStorage( mitk::DataStorage* ds )
{
  if (ds == NULL)
    return;

  /* remove listeners of old DataStorage */
  if (m_DataStorage.IsNotNull())
  {
    m_DataStorage->AddNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
    m_DataStorage->RemoveNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
  }
  /* register listener for new DataStorage */
  m_DataStorage = ds;  // register
  m_DataStorage->AddNodeEvent.AddListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
  m_DataStorage->RemoveNodeEvent.AddListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
  this->DataStorageChanged(); // update us with new DataStorage
}

void mitk::LevelWindowManager::OnPropertyModified(const itk::EventObject& )
{
  Modified();
}

void mitk::LevelWindowManager::SetAutoTopMostImage(bool autoTopMost)
{
  m_AutoTopMost = autoTopMost;
  if (m_AutoTopMost == false)
    return;

  if (m_IsPropertyModifiedTagSet && m_LevelWindowProperty.IsNotNull())
  {
    m_LevelWindowProperty->RemoveObserver(m_PropertyModifiedTag);
    m_IsPropertyModifiedTagSet = false;
  }
  /* search topmost image */
  if (m_DataStorage.IsNull())
  {
    itkExceptionMacro("DataStorage not set");
  }
  int maxLayer = itk::NumericTraits<int>::min();
  m_LevelWindowProperty = NULL;
  mitk::NodePredicateDataType::Pointer p1 = mitk::NodePredicateDataType::New("Image");
  mitk::NodePredicateProperty::Pointer p2 = mitk::NodePredicateProperty::New("layer");
  mitk::NodePredicateProperty::Pointer p3 = mitk::NodePredicateProperty::New("visible", mitk::BoolProperty::New(true));
  mitk::NodePredicateAND::Pointer pred = mitk::NodePredicateAND::New(p1, p2, p3);
  mitk::DataStorage::SetOfObjects::ConstPointer all = m_DataStorage->GetSubset(pred);
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = all->Begin(); it != all->End(); ++it)
  {
    mitk::DataTreeNode::Pointer node = it->Value();
    if (node.IsNull())
      continue;
    if (node->IsVisible(NULL) == false)
      continue;
    int layer = 0;
    node->GetIntProperty("layer", layer);
    if ( layer < maxLayer )
      continue;
    mitk::LevelWindowProperty::Pointer levelWindowProperty = dynamic_cast<mitk::LevelWindowProperty*>(node->GetProperty("levelwindow"));
    if (levelWindowProperty.IsNull())
      continue;
    m_LevelWindowProperty = levelWindowProperty;
    m_CurrentImage = dynamic_cast<mitk::Image*>(node->GetData());
    maxLayer = layer;
  }
}

// sets an specific LevelWindowProperty, all changes will affect the image belonging to this property.
// sets m_AutoTopMost to false
void mitk::LevelWindowManager::SetLevelWindowProperty(LevelWindowProperty::Pointer levelWindowProperty)
{
  if (levelWindowProperty.IsNull())
    return;

  if (m_IsPropertyModifiedTagSet)  // remove listener for old property
  {
    m_LevelWindowProperty->RemoveObserver(m_PropertyModifiedTag);
    m_IsPropertyModifiedTagSet = false;
  }
  m_LevelWindowProperty = levelWindowProperty;

  itk::ReceptorMemberCommand<LevelWindowManager>::Pointer command = itk::ReceptorMemberCommand<LevelWindowManager>::New();  // register listener for new property
  command->SetCallbackFunction(this, &LevelWindowManager::OnPropertyModified);
  m_PropertyModifiedTag = m_LevelWindowProperty->AddObserver( itk::ModifiedEvent(), command );
  m_IsPropertyModifiedTagSet = true;
  m_AutoTopMost = false;

  /* search image than belongs to the property */
  mitk::NodePredicateProperty::Pointer p = mitk::NodePredicateProperty::New("levelwindow", m_LevelWindowProperty);
  mitk::DataTreeNode* n = m_DataStorage->GetNode(p);
  if (n == NULL)
  {
    itkExceptionMacro("No Image in DataStorage that belongs to LevelWindow property " << m_LevelWindowProperty);
  }
  m_CurrentImage = dynamic_cast<mitk::Image*>(n->GetData());
  this->Modified();
}

// returns the current mitkLevelWindowProperty object from the image that is affected by changes
mitk::LevelWindowProperty::Pointer mitk::LevelWindowManager::GetLevelWindowProperty()
{
  return m_LevelWindowProperty;
}

// returns Level/Window values for the current image
const mitk::LevelWindow& mitk::LevelWindowManager::GetLevelWindow()
{
  if (m_LevelWindowProperty.IsNotNull())
    return m_LevelWindowProperty->GetLevelWindow();
  else
  {
    itkExceptionMacro("No LevelWindow available!");
  }
}

// sets new Level/Window values and informs all listeners about changes
void mitk::LevelWindowManager::SetLevelWindow(const mitk::LevelWindow& levelWindow)
{
  if (m_LevelWindowProperty.IsNotNull())
  {
    m_LevelWindowProperty->SetLevelWindow(levelWindow);
  }
  this->Modified();
}

void mitk::LevelWindowManager::DataStorageChanged( const mitk::DataTreeNode* itkNotUsed(n) )
{
  /* remove old observers */
  for (ObserverToPropertyMap::iterator iter = m_PropObserverToNode.begin(); iter != m_PropObserverToNode.end(); iter++ )
    (*iter).second->RemoveObserver((*iter).first);
  m_PropObserverToNode.clear();

  if (m_DataStorage.IsNull())
  {
    itkExceptionMacro("DataStorage not set");
  }

  /* listen to changes  in visible property of all images */
  mitk::NodePredicateDataType::Pointer p = mitk::NodePredicateDataType::New("Image");
  mitk::DataStorage::SetOfObjects::ConstPointer all = m_DataStorage->GetSubset(p);
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = all->Begin(); it != all->End(); ++it)
  {
    if (it->Value().IsNull())
      continue;
    /* register listener for changes in visible property */
    itk::ReceptorMemberCommand<LevelWindowManager>::Pointer command = itk::ReceptorMemberCommand<LevelWindowManager>::New();
    command->SetCallbackFunction(this, &LevelWindowManager::Update);
    m_PropObserverToNode[it->Value()->GetProperty("visible")->AddObserver( itk::ModifiedEvent(), command )] = it->Value()->GetProperty("visible");
  }

  /* search image than belongs to the property */
  if (m_LevelWindowProperty.IsNull())
    SetAutoTopMostImage(true);
  else
  {
    mitk::NodePredicateProperty::Pointer p2 = mitk::NodePredicateProperty::New("levelwindow", m_LevelWindowProperty);
    mitk::DataTreeNode* n = m_DataStorage->GetNode(p2);
    if (n == NULL) // if node was deleted, change our behaviour to AutoTopMost
      SetAutoTopMostImage(true);
   }
}

mitk::DataStorage* mitk::LevelWindowManager::GetDataStorage()
{
  return m_DataStorage.GetPointer();
}

// true if changes on slider or line-edits will affect always the topmost layer image
bool mitk::LevelWindowManager::isAutoTopMost()
{
  return m_AutoTopMost;
}

void mitk::LevelWindowManager::Update(const itk::EventObject&)  // visible property of a image has changed
{
  if (m_LevelWindowProperty.IsNull() || m_AutoTopMost)
    return;

  mitk::DataStorage::SetOfObjects::ConstPointer all = this->GetRelevantNodes();
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = all->Begin(); it != all->End(); ++it)
  {
    mitk::DataTreeNode* node = it->Value();
    if (node == NULL)
      continue;
    mitk::LevelWindowProperty* levelWindowProperty = dynamic_cast<mitk::LevelWindowProperty*>(node->GetProperty("levelwindow"));
    if (levelWindowProperty == NULL)
      continue;
    if (m_LevelWindowProperty.GetPointer() == levelWindowProperty) // we found our m_LevelWindowProperty
    {
      SetAutoTopMostImage(true);
      return;
    }
  }
}

mitk::DataStorage::SetOfObjects::ConstPointer mitk::LevelWindowManager::GetRelevantNodes()
{
  if (m_DataStorage.IsNull())
    return mitk::DataStorage::SetOfObjects::ConstPointer(mitk::DataStorage::SetOfObjects::New());  // return empty set

  mitk::BoolProperty::Pointer trueProp = mitk::BoolProperty::New(true);
  mitk::NodePredicateProperty::Pointer isVisible = mitk::NodePredicateProperty::New("visible", mitk::BoolProperty::New(true));
  mitk::NodePredicateProperty::Pointer notBinary = mitk::NodePredicateProperty::New("binary", mitk::BoolProperty::New(false));
  mitk::NodePredicateProperty::Pointer hasLevelWindow = mitk::NodePredicateProperty::New("levelwindow", NULL);
  mitk::NodePredicateDataType::Pointer isImage = mitk::NodePredicateDataType::New("Image");
  mitk::NodePredicateAND::Pointer predicate = mitk::NodePredicateAND::New();
  predicate->AddPredicate(isVisible);
  predicate->AddPredicate(notBinary);
  predicate->AddPredicate(hasLevelWindow);
  predicate->AddPredicate(isImage);

  mitk::DataStorage::SetOfObjects::ConstPointer relevantNodes = m_DataStorage->GetSubset( predicate );
  return relevantNodes;
}

mitk::Image* mitk::LevelWindowManager::GetCurrentImage()
{
  return m_CurrentImage;
}
<commit_msg>FIX (#2071): LevelWindow widget does not show up on CoreApp. Added missing Modified() in SetAutoTopMostImage().<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkLevelWindowManager.h"
#include <itkCommand.h>
#include "mitkDataStorage.h"
#include "mitkNodePredicateBase.h"
#include "mitkNodePredicateProperty.h"
#include "mitkNodePredicateDataType.h"
#include "mitkNodePredicateAND.h"
#include "mitkNodePredicateNOT.h"
#include "mitkProperties.h"
#include "mitkMessage.h"

mitk::LevelWindowManager::LevelWindowManager()
: m_DataStorage(NULL), m_LevelWindowProperty(NULL), m_AutoTopMost(true),
  m_IsObserverTagSet(false), m_CurrentImage(NULL),m_IsPropertyModifiedTagSet(false)
{
}

mitk::LevelWindowManager::~LevelWindowManager()
{
  if (m_DataStorage.IsNotNull())
  {
    m_DataStorage->AddNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
    m_DataStorage->RemoveNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
    m_DataStorage = NULL;
  }
  if (m_IsPropertyModifiedTagSet && m_LevelWindowProperty.IsNotNull())
  {
    m_LevelWindowProperty->RemoveObserver(m_PropertyModifiedTag);
    m_IsPropertyModifiedTagSet = false;
  }
  for( std::map<unsigned long, mitk::BaseProperty::Pointer>::iterator iter = m_PropObserverToNode.begin(); iter != m_PropObserverToNode.end(); iter++ ) {
    (*iter).second->RemoveObserver((*iter).first);
  }
}

void mitk::LevelWindowManager::SetDataStorage( mitk::DataStorage* ds )
{
  if (ds == NULL)
    return;

  /* remove listeners of old DataStorage */
  if (m_DataStorage.IsNotNull())
  {
    m_DataStorage->AddNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
    m_DataStorage->RemoveNodeEvent.RemoveListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
  }
  /* register listener for new DataStorage */
  m_DataStorage = ds;  // register
  m_DataStorage->AddNodeEvent.AddListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
  m_DataStorage->RemoveNodeEvent.AddListener(MessageDelegate1<LevelWindowManager, const mitk::DataTreeNode*>( this, &LevelWindowManager::DataStorageChanged ));
  this->DataStorageChanged(); // update us with new DataStorage
}

void mitk::LevelWindowManager::OnPropertyModified(const itk::EventObject& )
{
  Modified();
}

void mitk::LevelWindowManager::SetAutoTopMostImage(bool autoTopMost)
{
  m_AutoTopMost = autoTopMost;
  if (m_AutoTopMost == false)
    return;

  if (m_IsPropertyModifiedTagSet && m_LevelWindowProperty.IsNotNull())
  {
    m_LevelWindowProperty->RemoveObserver(m_PropertyModifiedTag);
    m_IsPropertyModifiedTagSet = false;
  }
  /* search topmost image */
  if (m_DataStorage.IsNull())
  {
    itkExceptionMacro("DataStorage not set");
  }
  int maxLayer = itk::NumericTraits<int>::min();
  m_LevelWindowProperty = NULL;
  mitk::NodePredicateDataType::Pointer p1 = mitk::NodePredicateDataType::New("Image");
  mitk::NodePredicateProperty::Pointer p2 = mitk::NodePredicateProperty::New("layer");
  mitk::NodePredicateProperty::Pointer p3 = mitk::NodePredicateProperty::New("visible", mitk::BoolProperty::New(true));
  mitk::NodePredicateAND::Pointer pred = mitk::NodePredicateAND::New(p1, p2, p3);
  mitk::DataStorage::SetOfObjects::ConstPointer all = m_DataStorage->GetSubset(pred);
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = all->Begin(); it != all->End(); ++it)
  {
    mitk::DataTreeNode::Pointer node = it->Value();
    if (node.IsNull())
      continue;
    if (node->IsVisible(NULL) == false)
      continue;
    int layer = 0;
    node->GetIntProperty("layer", layer);
    if ( layer < maxLayer )
      continue;
    mitk::LevelWindowProperty::Pointer levelWindowProperty = dynamic_cast<mitk::LevelWindowProperty*>(node->GetProperty("levelwindow"));
    if (levelWindowProperty.IsNull())
      continue;
    m_LevelWindowProperty = levelWindowProperty;
    m_CurrentImage = dynamic_cast<mitk::Image*>(node->GetData());
    maxLayer = layer;
  }
  Modified();
}

// sets an specific LevelWindowProperty, all changes will affect the image belonging to this property.
// sets m_AutoTopMost to false
void mitk::LevelWindowManager::SetLevelWindowProperty(LevelWindowProperty::Pointer levelWindowProperty)
{
  if (levelWindowProperty.IsNull())
    return;

  if (m_IsPropertyModifiedTagSet)  // remove listener for old property
  {
    m_LevelWindowProperty->RemoveObserver(m_PropertyModifiedTag);
    m_IsPropertyModifiedTagSet = false;
  }
  m_LevelWindowProperty = levelWindowProperty;

  itk::ReceptorMemberCommand<LevelWindowManager>::Pointer command = itk::ReceptorMemberCommand<LevelWindowManager>::New();  // register listener for new property
  command->SetCallbackFunction(this, &LevelWindowManager::OnPropertyModified);
  m_PropertyModifiedTag = m_LevelWindowProperty->AddObserver( itk::ModifiedEvent(), command );
  m_IsPropertyModifiedTagSet = true;
  m_AutoTopMost = false;

  /* search image than belongs to the property */
  mitk::NodePredicateProperty::Pointer p = mitk::NodePredicateProperty::New("levelwindow", m_LevelWindowProperty);
  mitk::DataTreeNode* n = m_DataStorage->GetNode(p);
  if (n == NULL)
  {
    itkExceptionMacro("No Image in DataStorage that belongs to LevelWindow property " << m_LevelWindowProperty);
  }
  m_CurrentImage = dynamic_cast<mitk::Image*>(n->GetData());
  this->Modified();
}

// returns the current mitkLevelWindowProperty object from the image that is affected by changes
mitk::LevelWindowProperty::Pointer mitk::LevelWindowManager::GetLevelWindowProperty()
{
  return m_LevelWindowProperty;
}

// returns Level/Window values for the current image
const mitk::LevelWindow& mitk::LevelWindowManager::GetLevelWindow()
{
  if (m_LevelWindowProperty.IsNotNull())
    return m_LevelWindowProperty->GetLevelWindow();
  else
  {
    itkExceptionMacro("No LevelWindow available!");
  }
}

// sets new Level/Window values and informs all listeners about changes
void mitk::LevelWindowManager::SetLevelWindow(const mitk::LevelWindow& levelWindow)
{
  if (m_LevelWindowProperty.IsNotNull())
  {
    m_LevelWindowProperty->SetLevelWindow(levelWindow);
  }
  this->Modified();
}

void mitk::LevelWindowManager::DataStorageChanged( const mitk::DataTreeNode* itkNotUsed(n) )
{
  /* remove old observers */
  for (ObserverToPropertyMap::iterator iter = m_PropObserverToNode.begin(); iter != m_PropObserverToNode.end(); iter++ )
    (*iter).second->RemoveObserver((*iter).first);
  m_PropObserverToNode.clear();

  if (m_DataStorage.IsNull())
  {
    itkExceptionMacro("DataStorage not set");
  }

  /* listen to changes  in visible property of all images */
  mitk::NodePredicateDataType::Pointer p = mitk::NodePredicateDataType::New("Image");
  mitk::DataStorage::SetOfObjects::ConstPointer all = m_DataStorage->GetSubset(p);
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = all->Begin(); it != all->End(); ++it)
  {
    if (it->Value().IsNull())
      continue;
    /* register listener for changes in visible property */
    itk::ReceptorMemberCommand<LevelWindowManager>::Pointer command = itk::ReceptorMemberCommand<LevelWindowManager>::New();
    command->SetCallbackFunction(this, &LevelWindowManager::Update);
    m_PropObserverToNode[it->Value()->GetProperty("visible")->AddObserver( itk::ModifiedEvent(), command )] = it->Value()->GetProperty("visible");
  }

  /* search image than belongs to the property */
  if (m_LevelWindowProperty.IsNull())
    SetAutoTopMostImage(true);
  else
  {
    mitk::NodePredicateProperty::Pointer p2 = mitk::NodePredicateProperty::New("levelwindow", m_LevelWindowProperty);
    mitk::DataTreeNode* n = m_DataStorage->GetNode(p2);
    if (n == NULL) // if node was deleted, change our behaviour to AutoTopMost
      SetAutoTopMostImage(true);
   }
}

mitk::DataStorage* mitk::LevelWindowManager::GetDataStorage()
{
  return m_DataStorage.GetPointer();
}

// true if changes on slider or line-edits will affect always the topmost layer image
bool mitk::LevelWindowManager::isAutoTopMost()
{
  return m_AutoTopMost;
}

void mitk::LevelWindowManager::Update(const itk::EventObject&)  // visible property of a image has changed
{
  if (m_LevelWindowProperty.IsNull() || m_AutoTopMost)
    return;

  mitk::DataStorage::SetOfObjects::ConstPointer all = this->GetRelevantNodes();
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = all->Begin(); it != all->End(); ++it)
  {
    mitk::DataTreeNode* node = it->Value();
    if (node == NULL)
      continue;
    mitk::LevelWindowProperty* levelWindowProperty = dynamic_cast<mitk::LevelWindowProperty*>(node->GetProperty("levelwindow"));
    if (levelWindowProperty == NULL)
      continue;
    if (m_LevelWindowProperty.GetPointer() == levelWindowProperty) // we found our m_LevelWindowProperty
    {
      SetAutoTopMostImage(true);
      return;
    }
  }
}

mitk::DataStorage::SetOfObjects::ConstPointer mitk::LevelWindowManager::GetRelevantNodes()
{
  if (m_DataStorage.IsNull())
    return mitk::DataStorage::SetOfObjects::ConstPointer(mitk::DataStorage::SetOfObjects::New());  // return empty set

  mitk::BoolProperty::Pointer trueProp = mitk::BoolProperty::New(true);
  mitk::NodePredicateProperty::Pointer isVisible = mitk::NodePredicateProperty::New("visible", mitk::BoolProperty::New(true));
  mitk::NodePredicateProperty::Pointer notBinary = mitk::NodePredicateProperty::New("binary", mitk::BoolProperty::New(false));
  mitk::NodePredicateProperty::Pointer hasLevelWindow = mitk::NodePredicateProperty::New("levelwindow", NULL);
  mitk::NodePredicateDataType::Pointer isImage = mitk::NodePredicateDataType::New("Image");
  mitk::NodePredicateAND::Pointer predicate = mitk::NodePredicateAND::New();
  predicate->AddPredicate(isVisible);
  predicate->AddPredicate(notBinary);
  predicate->AddPredicate(hasLevelWindow);
  predicate->AddPredicate(isImage);

  mitk::DataStorage::SetOfObjects::ConstPointer relevantNodes = m_DataStorage->GetSubset( predicate );
  return relevantNodes;
}

mitk::Image* mitk::LevelWindowManager::GetCurrentImage()
{
  return m_CurrentImage;
}
<|endoftext|>


<commit_before>#include <core/log/logdevice_console.h>

namespace fd {
namespace core {
namespace log {

LogDeviceConsole::LogDeviceConsole() : LogDevice(LogDeviceType::Console) {

}

void LogDeviceConsole::Log(LogLevel level, const char* message, va_list list) {
    switch(level) {
        case LogLevel::Info:
            printf("\033[1;37mINFO: ");
            break;
        case LogLevel::Debug:
            printf("\033[1;32mDEBUG: ");
            break;
        case LogLevel::Warning:
            printf("\033[1;33mWARNING: ");
            break;
        case LogLevel::Fatal:
            printf("\033[1;31mINFO: ");
            break;
    }

    vprintf(message, list);
    printf("\033[0m\n");
}

}
}
}<commit_msg>LNX: fixed nigga bitch typo<commit_after>#include <core/log/logdevice_console.h>

namespace fd {
namespace core {
namespace log {

LogDeviceConsole::LogDeviceConsole() : LogDevice(LogDeviceType::Console) {

}

void LogDeviceConsole::Log(LogLevel level, const char* message, va_list list) {
    switch(level) {
        case LogLevel::Info:
            printf("\033[1;37mINFO: ");
            break;
        case LogLevel::Debug:
            printf("\033[1;32mDEBUG: ");
            break;
        case LogLevel::Warning:
            printf("\033[1;33mWARNING: ");
            break;
        case LogLevel::Fatal:
            printf("\033[1;31mFATAL: ");
            break;
    }

    vprintf(message, list);
    printf("\033[0m\n");
}

}
}
}<|endoftext|>


<commit_before>/*
 * Copyright 2009 Google 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.
 */

#include <string.h>
#include "arp.h"
#include "conduit.h"
#include "dix.h"
#include "inet.h"
#include "inet4.h"
#include "inet4address.h"
#include "tcp.h"

extern int esInit(Object** nameSpace);
extern void esRegisterInternetProtocol(es::Context* context);

Handle<es::Resolver> resolver;

int main()
{
    Object* root = NULL;
    esInit(&root);
    Handle<es::Context> context(root);

    esRegisterInternetProtocol(context);

    // Create resolver object
    resolver = context->lookup("network/resolver");

    // Create internet config object
    Handle<es::InternetConfig> config = context->lookup("network/config");

    // Setup DIX interface
    Handle<es::NetworkInterface> nic = context->lookup("device/ethernet");
    nic->start();
    int dixID = config->addInterface(nic);// add Interface
    esReport("dixID: %d\n", dixID);

    ASSERT(config->getScopeID(nic) == dixID);

    // 192.168.2.40 Register host address
    InAddr addr = { htonl(192 << 24 | 168 << 16 | 2 << 8 | 20) };
    Handle<es::InternetAddress> host = resolver->getHostByAddress(&addr.addr, sizeof addr, dixID);
    config->addAddress(host, 16);
    esSleep(90000000);  // Wait for the host address to be settled.

    Handle<es::Socket> serverSocket = host->socket(AF_INET, es::Socket::Stream, 9);

    serverSocket->listen(5);

    es::Socket* Server;
    while (true)
    {
        while ((Server = serverSocket->accept()) == 0)
        {
        }

        esReport("accepted\n");

        char data[9];
        int length;
        memset (data, 0, 9);

        while (true)
        {

	    // server checks whether the socket has entered urgent mode at every read
            if (Server->isUrgent())
            {
                esReport("Urgent Mode\n\n");
            }
            else
            {
                esReport("Not in urgent Mode\n\n");
            }

            // Tells whether the next read delivers an urgent byte
            if (Server->sockAtMark())
            {
                esReport("An urgent byte will be read now\n");
            }

            length = Server->read(data, 3);
            esReport("Received data and length.......... ............ %s        %d\n", data, length);
            memset (data, 0, 3);
            if (length == 0)
            {
                break;
            }
        }
        Server->close();
        Server->release();
    }

    config->removeAddress(host);
    nic->stop();

    esReport("done.\n");
}

<commit_msg>clean up<commit_after>/*
 * Copyright 2009 Google 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.
 */

#include <string.h>
#include "arp.h"
#include "conduit.h"
#include "dix.h"
#include "inet.h"
#include "inet4.h"
#include "inet4address.h"
#include "tcp.h"

extern int esInit(Object** nameSpace);
extern void esRegisterInternetProtocol(es::Context* context);

Handle<es::Resolver> resolver;

int main()
{
    Object* root = NULL;
    esInit(&root);
    Handle<es::Context> context(root);

    esRegisterInternetProtocol(context);

    // Create resolver object
    resolver = context->lookup("network/resolver");

    // Create internet config object
    Handle<es::InternetConfig> config = context->lookup("network/config");

    // Setup DIX interface
    Handle<es::NetworkInterface> nic = context->lookup("device/ethernet");
    nic->start();
    int dixID = config->addInterface(nic);// add Interface
    esReport("dixID: %d\n", dixID);

    ASSERT(config->getScopeID(nic) == dixID);

    // 192.168.2.40 Register host address
    InAddr addr = { htonl(192 << 24 | 168 << 16 | 2 << 8 | 20) };
    Handle<es::InternetAddress> host = resolver->getHostByAddress(&addr.addr, sizeof addr, dixID);
    config->addAddress(host, 16);
    esSleep(90000000);  // Wait for the host address to be settled.

    Handle<es::Socket> serverSocket = host->socket(AF_INET, es::Socket::Stream, 9);

    serverSocket->listen(5);

    es::Socket* Server;
    while (true)
    {
        while ((Server = serverSocket->accept()) == 0)
        {
        }

        esReport("accepted\n");

        char data[9];
        int length;
        memset (data, 0, 9);

        while (true)
        {

            // server checks whether the socket has entered urgent mode at every read
            if (Server->isUrgent())
            {
                esReport("Urgent Mode\n\n");
            }
            else
            {
                esReport("Not in urgent Mode\n\n");
            }

            // Tells whether the next read delivers an urgent byte
            if (Server->sockAtMark())
            {
                esReport("An urgent byte will be read now\n");
            }

            length = Server->read(data, 3);
            esReport("Received data and length.......... ............ %s        %d\n", data, length);
            memset(data, 0, 3);
            if (length == 0)
            {
                break;
            }
        }
        Server->close();
        Server->release();
    }

    config->removeAddress(host);
    nic->stop();

    esReport("done.\n");
}

<|endoftext|>