<commit_before>//
// Copyright (C) 2006 SIPez LLC.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// Copyright (C) 2004-2006 SIPfoundry Inc.
// Licensed by SIPfoundry under the LGPL license.
//
// Copyright (C) 2004-2006 Pingtel Corp.  All rights reserved.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// $$
///////////////////////////////////////////////////////////////////////////////

// SYSTEM INCLUDES

// APPLICATION INCLUDES
#include "os/Wnt/OsFileSystemWnt.h"
#include "os/Wnt/OsFileWnt.h"
#include "os/Wnt/OsPathWnt.h"
#include "os/Wnt/OsFileInfoWnt.h"

// EXTERNAL FUNCTIONS
// EXTERNAL VARIABLES
// CONSTANTS
const unsigned long CopyBufLen = 32768;

// STATIC VARIABLE INITIALIZATIONS

/* //////////////////////////// PUBLIC //////////////////////////////////// */

/* ============================ CREATORS ================================== */

// Constructor
OsFileWnt::OsFileWnt(const OsPathBase& filename) :
OsFileBase(filename)
{
}

// Copy constructor
OsFileWnt::OsFileWnt(const OsFileWnt& rOsFileWnt) :
OsFileBase(rOsFileWnt)
{
    OsPathWnt path;
    rOsFileWnt.getFileName(path);
    mFilename = path;
    mOsFileHandle = rOsFileWnt.mOsFileHandle;
}

// Destructor
OsFileWnt::~OsFileWnt()
{
    if (mOsFileHandle)
        close(); //call our close
}

/* ============================ MANIPULATORS ============================== */

OsStatus OsFileWnt::setLength(unsigned long newLength)
{
    OsStatus stat = OS_SUCCESS;

    return stat;
}

OsStatus OsFileWnt::fileunlock()
{
    OsStatus retval = OS_SUCCESS;

    return retval;
}

OsStatus OsFileWnt::filelock(const bool wait)
{
    OsStatus retval = OS_FAILED;

    return retval;
}

OsStatus OsFileWnt::getFileInfo(OsFileInfoBase& fileinfo) const
{
    OsStatus ret = OS_INVALID;

#ifdef WINCE
	printf( "JEP - TODO in OsFileWnt::getFileInfo( )\n" );
    //  JEP - TODO - implement this...
#else
    struct stat stats;
    if (stat(mFilename,&stats) == 0)
    {
        ret = OS_SUCCESS;
        if (stats.st_mode & _S_IWRITE)
            fileinfo.mbIsReadOnly = FALSE;
        else
            fileinfo.mbIsReadOnly = TRUE;

        OsTime createTime(stats.st_ctime,0);
        fileinfo.mCreateTime = createTime;

        OsTime modifiedTime(stats.st_ctime,0);
        fileinfo.mCreateTime = modifiedTime;

        fileinfo.mSize = stats.st_size;
    }
#endif

    return ret;

}
/* ============================ ACCESSORS ================================= */

/* ============================ INQUIRY =================================== */

UtlBoolean OsFileWnt::isReadonly() const
{
    UtlBoolean retval = FALSE;

    OsFileInfoWnt info;
    getFileInfo(info);

    return info.mbIsReadOnly;
}

/* //////////////////////////// PROTECTED ///////////////////////////////// */
// Assignment operator
OsFileWnt&
OsFileWnt::operator=(const OsFileWnt& rhs)
{
   if (this == &rhs)            // handle the assignment to self case
      return *this;

   return *this;
}

/* //////////////////////////// PRIVATE /////////////////////////////////// */

/* ============================ FUNCTIONS ================================= */

<commit_msg>Bugfix: OsFileWnt::getFileInfo() returned incorrect file modification time.<commit_after>//
// Copyright (C) 2006 SIPez LLC.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// Copyright (C) 2004-2006 SIPfoundry Inc.
// Licensed by SIPfoundry under the LGPL license.
//
// Copyright (C) 2004-2006 Pingtel Corp.  All rights reserved.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// $$
///////////////////////////////////////////////////////////////////////////////

// SYSTEM INCLUDES

// APPLICATION INCLUDES
#include "os/Wnt/OsFileSystemWnt.h"
#include "os/Wnt/OsFileWnt.h"
#include "os/Wnt/OsPathWnt.h"
#include "os/Wnt/OsFileInfoWnt.h"

// EXTERNAL FUNCTIONS
// EXTERNAL VARIABLES
// CONSTANTS
const unsigned long CopyBufLen = 32768;

// STATIC VARIABLE INITIALIZATIONS

/* //////////////////////////// PUBLIC //////////////////////////////////// */

/* ============================ CREATORS ================================== */

// Constructor
OsFileWnt::OsFileWnt(const OsPathBase& filename) :
OsFileBase(filename)
{
}

// Copy constructor
OsFileWnt::OsFileWnt(const OsFileWnt& rOsFileWnt) :
OsFileBase(rOsFileWnt)
{
    OsPathWnt path;
    rOsFileWnt.getFileName(path);
    mFilename = path;
    mOsFileHandle = rOsFileWnt.mOsFileHandle;
}

// Destructor
OsFileWnt::~OsFileWnt()
{
    if (mOsFileHandle)
        close(); //call our close
}

/* ============================ MANIPULATORS ============================== */

OsStatus OsFileWnt::setLength(unsigned long newLength)
{
    OsStatus stat = OS_SUCCESS;

    return stat;
}

OsStatus OsFileWnt::fileunlock()
{
    OsStatus retval = OS_SUCCESS;

    return retval;
}

OsStatus OsFileWnt::filelock(const bool wait)
{
    OsStatus retval = OS_FAILED;

    return retval;
}

OsStatus OsFileWnt::getFileInfo(OsFileInfoBase& fileinfo) const
{
    OsStatus ret = OS_INVALID;

#ifdef WINCE
	printf( "JEP - TODO in OsFileWnt::getFileInfo( )\n" );
    //  JEP - TODO - implement this...
#else
    struct stat stats;
    if (stat(mFilename,&stats) == 0)
    {
        ret = OS_SUCCESS;
        if (stats.st_mode & _S_IWRITE)
            fileinfo.mbIsReadOnly = FALSE;
        else
            fileinfo.mbIsReadOnly = TRUE;

        OsTime createTime(stats.st_ctime,0);
        fileinfo.mCreateTime = createTime;

        OsTime modifiedTime(stats.st_mtime,0);
        fileinfo.mModifiedTime = modifiedTime;

        fileinfo.mSize = stats.st_size;
    }
#endif

    return ret;
}

/* ============================ ACCESSORS ================================= */

/* ============================ INQUIRY =================================== */

UtlBoolean OsFileWnt::isReadonly() const
{
    UtlBoolean retval = FALSE;

    OsFileInfoWnt info;
    getFileInfo(info);

    return info.mbIsReadOnly;
}

/* //////////////////////////// PROTECTED ///////////////////////////////// */
// Assignment operator
OsFileWnt&
OsFileWnt::operator=(const OsFileWnt& rhs)
{
   if (this == &rhs)            // handle the assignment to self case
      return *this;

   return *this;
}

/* //////////////////////////// PRIVATE /////////////////////////////////// */

/* ============================ FUNCTIONS ================================= */

<|endoftext|>


<commit_before>#include "linear_interpolate_d.hpp"
#include "find_barycentric_coords.hpp"

#include <algorithm>
#include <map>
#include <vector>

typedef CGAL::Cartesian_d<double> R;
typedef CGAL::Delaunay_d<R>       Delaunay;
typedef Delaunay::Point_d         Point;
typedef Delaunay::Vertex_handle   Vertex_handle;

// returns beta_1, ..., beta_d+1, beta_0
std::vector<double> linear_coef(size_t d,
                                const std::vector<Point> &points,
                                const std::vector<double> &values) {
  std::vector<double> a((d+1) * (d+1));
  for (size_t i = 0; i <= d; ++i) {
    for (size_t j = 0; j < d; ++j) {
      a[i + j*(d+1)] = *(points[i].cartesian_begin() + j);
    }

    a[i + d*(d+1)] = 1;
  }

  std::vector<double> b(values);

  for (auto el : b) {
    if (ISNAN(el)) {
      return b;
    }
  }

  // additional variables needed for LAPACK
  std::vector<int> ipiv(d + 1);
  int info;
  int size_int = d + 1;
  int b_size = 1;

  // solve a*x = b
  // result will be in b
  F77_NAME(dgesv)(&size_int, &b_size, &a[0], &size_int, &ipiv[0],
    &b[0], &size_int, &info);

  if (info != 0)
  {
    // error("Bad matrix");
  }

  return b;
}

class LinearInterpolator_d {
public:
  LinearInterpolator_d(int d,
                       const double *points,
                       const double *values,
                       size_t npoints) : d{d}, triangulation{d}, npoints{npoints} {
    for (size_t i{0}; i < npoints; ++i) {
      Point point(d, points + i*d, points + (i + 1)*d);
      Vertex_handle vertex_handle = triangulation.insert(point);
      vertices_to_values[vertex_handle] = values[i];
    }

    for (const auto &simplex : triangulation.all_simplices()) {
      if (simplex == Delaunay::Simplex_handle()) continue;

      std::vector<Point>  vertices;
      std::vector<double> vertex_values;
      bool flag = true;
      for (size_t i{0}; i <= d; ++i) {
        auto vertex = triangulation.vertex_of_simplex(simplex, i);
        double vertex_val = vertices_to_values[vertex];
        if (vertex == Delaunay::Vertex_handle()) {
          flag = false;
          break;
        }
        vertices.push_back(vertex->point());
        vertex_values.push_back(vertex_val);
      }
      if (!flag) continue;
      m[simplex] = linear_coef(d, vertices, vertex_values);
    }
  }

  double get(const double *x) {
    const double double_fill_value = NA_REAL;

    Point point(d, x, x + d);
    auto simplex = triangulation.locate(point);
    if (!m.count(simplex)) {
      return double_fill_value;
    }

    auto beta = m[simplex];
    return std::inner_product(x, x + d, beta.begin(), beta[d]);
  }

  double operator()(const double *x) {
    const double double_fill_value = NA_REAL;

    Point point(d, x, x + d);
    auto simplex = triangulation.locate(point);
    if (simplex == Delaunay::Simplex_handle()) {
      return double_fill_value;
    }
    std::vector<Point>  vertices(d + 1);
    std::vector<double> vertex_values(d + 1);
    for (size_t i{0}; i <= triangulation.current_dimension(); ++i) {
      auto vertex = triangulation.vertex_of_simplex(simplex, i);
      double vertex_val = vertices_to_values[vertex];
      if (ISNAN(vertex_val)) {
        return double_fill_value;
      }
      vertices[i] = triangulation.associated_point(vertex);
      vertex_values[i] = vertex_val;
    }
    auto barycentric = find_barycentric_coords(triangulation.current_dimension(), point, vertices);
    return std::inner_product(vertex_values.begin(), vertex_values.end(), barycentric.begin(), 0.);
  }

private:
  const int d;
  size_t npoints;
  Delaunay triangulation;
  std::map<Vertex_handle, double> vertices_to_values;
  std::map<Delaunay::Simplex_handle, std::vector<double>> m;
};

SEXP linear_interpolate_d(SEXP dimentions,
                          SEXP points,
                          SEXP values,
                          SEXP xi) {
  int d = INTEGER(dimentions)[0];

  LinearInterpolator_d li(d, REAL(points), REAL(values), length(values));

  int result_length = length(xi) / d;
  SEXP results = PROTECT(allocVector(REALSXP, result_length));

  for (size_t i{0}; i < result_length; ++i) {
    // REAL(results)[i] = li(REAL(xi) + i*d);
    REAL(results)[i] = li.get(REAL(xi) + i*d);
  }

  UNPROTECT(1);
  return results;
}
<commit_msg>Drop exact coef computation staff<commit_after>#include "linear_interpolate_d.hpp"
#include "find_barycentric_coords.hpp"

#include <algorithm>
#include <map>
#include <vector>

typedef CGAL::Cartesian_d<double> R;
typedef CGAL::Delaunay_d<R>       Delaunay;
typedef Delaunay::Point_d         Point;
typedef Delaunay::Vertex_handle   Vertex_handle;

class LinearInterpolator_d {
public:
  LinearInterpolator_d(int d,
                       const double *points,
                       const double *values,
                       size_t npoints) : d{d}, triangulation{d}, npoints{npoints} {
    for (size_t i{0}; i < npoints; ++i) {
      Point point(d, points + i*d, points + (i + 1)*d);
      Vertex_handle vertex_handle = triangulation.insert(point);
      vertices_to_values[vertex_handle] = values[i];
    }
  }

  double operator()(const double *x) {
    const double double_fill_value = NA_REAL;

    Point point(d, x, x + d);
    auto simplex = triangulation.locate(point);
    if (simplex == Delaunay::Simplex_handle()) {
      return double_fill_value;
    }
    std::vector<Point>  vertices(d + 1);
    std::vector<double> vertex_values(d + 1);
    for (size_t i{0}; i <= triangulation.current_dimension(); ++i) {
      auto vertex = triangulation.vertex_of_simplex(simplex, i);
      double vertex_val = vertices_to_values[vertex];
      if (ISNAN(vertex_val)) {
        return double_fill_value;
      }
      vertices[i] = triangulation.associated_point(vertex);
      vertex_values[i] = vertex_val;
    }
    auto barycentric = find_barycentric_coords(triangulation.current_dimension(), point, vertices);
    return std::inner_product(vertex_values.begin(), vertex_values.end(), barycentric.begin(), 0.);
  }

private:
  const int d;
  size_t npoints;
  Delaunay triangulation;
  std::map<Vertex_handle, double> vertices_to_values;
};

SEXP linear_interpolate_d(SEXP dimentions,
                          SEXP points,
                          SEXP values,
                          SEXP xi) {
  int d = INTEGER(dimentions)[0];

  LinearInterpolator_d li(d, REAL(points), REAL(values), length(values));

  int result_length = length(xi) / d;
  SEXP results = PROTECT(allocVector(REALSXP, result_length));

  for (size_t i{0}; i < result_length; ++i) {
    REAL(results)[i] = li(REAL(xi) + i*d);
  }

  UNPROTECT(1);
  return results;
}
<|endoftext|>


<commit_before>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2000, 2010 Oracle and/or its affiliates.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_sal.hxx"

#include <cppunit/TestFixture.h>
#include <cppunit/TestAssert.h>
#include <cppunit/extensions/HelperMacros.h>
#include "rtl/ustrbuf.hxx"
#include "rtl/ustring.hxx"

namespace test { namespace oustringbuffer {

class ToString: public CppUnit::TestFixture {
private:
    void testToString();

    CPPUNIT_TEST_SUITE(ToString);
    CPPUNIT_TEST(testToString);
    CPPUNIT_TEST_SUITE_END();
};

} }

CPPUNIT_TEST_SUITE_REGISTRATION(test::oustringbuffer::ToString);

void test::oustringbuffer::ToString::testToString() {
    rtl::OUStringBuffer sb(rtl::OUString(RTL_CONSTASCII_USTRINGPARAM("test string")));
    rtl::OUString str = sb.toString();
    CPPUNIT_ASSERT(str.equalsAsciiL(RTL_CONSTASCII_STRINGPARAM("test string")));
    // returned OUString must be independent from sb
    sb.append('a');
    CPPUNIT_ASSERT(str.equalsAsciiL(RTL_CONSTASCII_STRINGPARAM("test string")));
    sb.setLength(0);
    CPPUNIT_ASSERT(str.equalsAsciiL(RTL_CONSTASCII_STRINGPARAM("test string")));
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<commit_msg>Fix: call of overloaded 'append(char)' is ambiguous<commit_after>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2000, 2010 Oracle and/or its affiliates.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_sal.hxx"

#include <cppunit/TestFixture.h>
#include <cppunit/TestAssert.h>
#include <cppunit/extensions/HelperMacros.h>
#include "rtl/ustrbuf.hxx"
#include "rtl/ustring.hxx"

namespace test { namespace oustringbuffer {

class ToString: public CppUnit::TestFixture {
private:
    void testToString();

    CPPUNIT_TEST_SUITE(ToString);
    CPPUNIT_TEST(testToString);
    CPPUNIT_TEST_SUITE_END();
};

} }

CPPUNIT_TEST_SUITE_REGISTRATION(test::oustringbuffer::ToString);

void test::oustringbuffer::ToString::testToString() {
    rtl::OUStringBuffer sb(rtl::OUString(RTL_CONSTASCII_USTRINGPARAM("test string")));
    rtl::OUString str = sb.toString();
    CPPUNIT_ASSERT(str.equalsAsciiL(RTL_CONSTASCII_STRINGPARAM("test string")));
    // returned OUString must be independent from sb
    sb.append( (sal_Unicode)'a' );
    CPPUNIT_ASSERT(str.equalsAsciiL(RTL_CONSTASCII_STRINGPARAM("test string")));
    sb.setLength(0);
    CPPUNIT_ASSERT(str.equalsAsciiL(RTL_CONSTASCII_STRINGPARAM("test string")));
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<|endoftext|>


<commit_before>/****************************************************************************
 *
 * Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file df_mpu9250_wrapper.cpp
 * Lightweight driver to access the MPU9250 of the DriverFramework.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include <px4_config.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_getopt.h>
#include <errno.h>

#include <systemlib/perf_counter.h>
#include <systemlib/err.h>

#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>

#include <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h>

#include <mpu9250/MPU9250.hpp>
#include <DevMgr.hpp>

extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }

using namespace DriverFramework;

class DfMpu9250Wrapper : public MPU9250
{
public:
	DfMpu9250Wrapper(/*enum Rotation rotation*/);
	~DfMpu9250Wrapper();

	/**
	 * Start automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		start();

	/**
	 * Stop automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		stop();

private:
	int _publish(struct imu_sensor_data &data);

	//enum Rotation		_rotation;

	orb_advert_t		_accel_topic;
	orb_advert_t		_gyro_topic;

	int			_accel_orb_class_instance;
	int			_gyro_orb_class_instance;

	perf_counter_t		_accel_sample_perf;
	perf_counter_t		_gyro_sample_perf;

};

DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
	MPU9250(IMU_DEVICE_PATH),
	_accel_topic(nullptr),
	_gyro_topic(nullptr),
	_accel_orb_class_instance(-1),
	_gyro_orb_class_instance(-1),
	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
	_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
	/*_rotation(rotation)*/
{
}

DfMpu9250Wrapper::~DfMpu9250Wrapper()
{
	perf_free(_accel_sample_perf);
	perf_free(_gyro_sample_perf);
}

int DfMpu9250Wrapper::start()
{
	// TODO: don't publish garbage here
	accel_report accel_report = {};
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
	if (_accel_topic == nullptr) {
		PX4_ERR("sensor_accel advert fail");
		return -1;
	}

	// TODO: don't publish garbage here
	gyro_report gyro_report = {};
	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
					  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
	if (_gyro_topic == nullptr) {
		PX4_ERR("sensor_gyro advert fail");
		return -1;
	}

	/* Init device and start sensor. */
	int ret = init();

	if (ret != 0) {
		PX4_ERR("MPU9250 init fail: %d", ret);
		return ret;
	}

	ret = MPU9250::start();

	if (ret != 0) {
		PX4_ERR("MPU9250 start fail: %d", ret);
		return ret;
	}

	return 0;
}

int DfMpu9250Wrapper::stop()
{
	/* Stop sensor. */
	int ret = MPU9250::stop();

	if (ret != 0) {
		PX4_ERR("MPU9250 stop fail: %d", ret);
		return ret;
	}

	return 0;
}

int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
{
	/* Publish accel first. */
	perf_begin(_accel_sample_perf);

	accel_report accel_report = {};
	accel_report.timestamp = data.last_read_time_usec;

	// TODO: remove these (or get the values)
	accel_report.x_raw = NAN;
	accel_report.y_raw = NAN;
	accel_report.z_raw = NAN;
	accel_report.x = data.accel_m_s2_x;
	accel_report.y = data.accel_m_s2_y;
	accel_report.z = data.accel_m_s2_z;

	// TODO: get these right
	accel_report.scaling = -1.0f;
	accel_report.range_m_s2 = -1.0f;

	// TODO: when is this ever blocked?
	if (!(m_pub_blocked)) {

		if (_accel_topic != nullptr) {
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
		}
	}
	perf_end(_accel_sample_perf);

	/* Then publish gyro. */
	perf_begin(_gyro_sample_perf);

	gyro_report gyro_report = {};
	gyro_report.timestamp = data.last_read_time_usec;

	// TODO: remove these (or get the values)
	gyro_report.x_raw = NAN;
	gyro_report.y_raw = NAN;
	gyro_report.z_raw = NAN;
	gyro_report.x = data.gyro_rad_s_x;
	gyro_report.y = data.gyro_rad_s_y;
	gyro_report.z = data.gyro_rad_s_z;

	// TODO: get these right
	gyro_report.scaling = -1.0f;
	gyro_report.range_rad_s = -1.0f;

	// TODO: when is this ever blocked?
	if (!(m_pub_blocked)) {

		if (_gyro_topic != nullptr) {
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
		}
	}
	perf_end(_gyro_sample_perf);

	/* Notify anyone waiting for data. */
	DevMgr::updateNotify(*this);

	return 0;
};

namespace df_mpu9250_wrapper
{

DfMpu9250Wrapper *g_dev = nullptr;

int start(/* enum Rotation rotation */);
int stop();
int info();
void usage();

int start(/*enum Rotation rotation*/)
{
	g_dev = new DfMpu9250Wrapper(/*rotation*/);

	if (g_dev == nullptr) {
		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
		return -1;
	}

	int ret = g_dev->start();
	if (ret != 0) {
		PX4_ERR("DfMpu9250Wrapper start failed");
		return ret;
	}

	// Open the IMU sensor
	DevHandle h;
	DevMgr::getHandle(IMU_DEVICE_PATH, h);
	if (!h.isValid())
	{
		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
			IMU_DEVICE_PATH, h.getError());
		return -1;
	}

	DevMgr::releaseHandle(h);

	return 0;
}

int stop()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	int ret = g_dev->stop();

	if (ret != 0) {
		PX4_ERR("driver could not be stopped");
		return ret;
	}

	delete g_dev;
	g_dev = nullptr;
	return 0;
}

/**
 * Print a little info about the driver.
 */
int
info()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	PX4_DEBUG("state @ %p", g_dev);

	return 0;
}

void
usage()
{
	PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'info', 'stop'");
	PX4_WARN("options:");
	//PX4_WARN("    -R rotation");
}

} // namespace df_mpu9250_wrapper

int
df_mpu9250_wrapper_main(int argc, char *argv[])
{
	int ch;
	// enum Rotation rotation = ROTATION_NONE;
	int ret = 0;
	int myoptind = 1;
	const char *myoptarg = NULL;

	/* jump over start/off/etc and look at options first */
	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		//case 'R':
		//	rotation = (enum Rotation)atoi(myoptarg);
		//	break;

		default:
			df_mpu9250_wrapper::usage();
			return 0;
		}
	}

	if (argc <= 1) {
		df_mpu9250_wrapper::usage();
		return 1;
	}

	const char *verb = argv[myoptind];

	if (!strcmp(verb, "start")) {
		ret = df_mpu9250_wrapper::start(/*rotation*/);
	}

	else if (!strcmp(verb, "stop")) {
		ret = df_mpu9250_wrapper::stop();
	}

	else if (!strcmp(verb, "info")) {
		ret = df_mpu9250_wrapper::info();
	}

	else {
		df_mpu9250_wrapper::usage();
		return 1;
	}

	return ret;
}
<commit_msg>df_mpu9250_wrapper: astyle<commit_after>/****************************************************************************
 *
 * Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file df_mpu9250_wrapper.cpp
 * Lightweight driver to access the MPU9250 of the DriverFramework.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include <px4_config.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_getopt.h>
#include <errno.h>

#include <systemlib/perf_counter.h>
#include <systemlib/err.h>

#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>

#include <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h>

#include <mpu9250/MPU9250.hpp>
#include <DevMgr.hpp>

extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }

using namespace DriverFramework;

class DfMpu9250Wrapper : public MPU9250
{
public:
	DfMpu9250Wrapper(/*enum Rotation rotation*/);
	~DfMpu9250Wrapper();

	/**
	 * Start automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		start();

	/**
	 * Stop automatic measurement.
	 *
	 * @return 0 on success
	 */
	int		stop();

private:
	int _publish(struct imu_sensor_data &data);

	//enum Rotation		_rotation;

	orb_advert_t		_accel_topic;
	orb_advert_t		_gyro_topic;

	int			_accel_orb_class_instance;
	int			_gyro_orb_class_instance;

	perf_counter_t		_accel_sample_perf;
	perf_counter_t		_gyro_sample_perf;

};

DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
	MPU9250(IMU_DEVICE_PATH),
	_accel_topic(nullptr),
	_gyro_topic(nullptr),
	_accel_orb_class_instance(-1),
	_gyro_orb_class_instance(-1),
	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
	_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
	/*_rotation(rotation)*/
{
}

DfMpu9250Wrapper::~DfMpu9250Wrapper()
{
	perf_free(_accel_sample_perf);
	perf_free(_gyro_sample_perf);
}

int DfMpu9250Wrapper::start()
{
	// TODO: don't publish garbage here
	accel_report accel_report = {};
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);

	if (_accel_topic == nullptr) {
		PX4_ERR("sensor_accel advert fail");
		return -1;
	}

	// TODO: don't publish garbage here
	gyro_report gyro_report = {};
	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
					  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);

	if (_gyro_topic == nullptr) {
		PX4_ERR("sensor_gyro advert fail");
		return -1;
	}

	/* Init device and start sensor. */
	int ret = init();

	if (ret != 0) {
		PX4_ERR("MPU9250 init fail: %d", ret);
		return ret;
	}

	ret = MPU9250::start();

	if (ret != 0) {
		PX4_ERR("MPU9250 start fail: %d", ret);
		return ret;
	}

	return 0;
}

int DfMpu9250Wrapper::stop()
{
	/* Stop sensor. */
	int ret = MPU9250::stop();

	if (ret != 0) {
		PX4_ERR("MPU9250 stop fail: %d", ret);
		return ret;
	}

	return 0;
}

int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
{
	/* Publish accel first. */
	perf_begin(_accel_sample_perf);

	accel_report accel_report = {};
	accel_report.timestamp = data.last_read_time_usec;

	// TODO: remove these (or get the values)
	accel_report.x_raw = NAN;
	accel_report.y_raw = NAN;
	accel_report.z_raw = NAN;
	accel_report.x = data.accel_m_s2_x;
	accel_report.y = data.accel_m_s2_y;
	accel_report.z = data.accel_m_s2_z;

	// TODO: get these right
	accel_report.scaling = -1.0f;
	accel_report.range_m_s2 = -1.0f;

	// TODO: when is this ever blocked?
	if (!(m_pub_blocked)) {

		if (_accel_topic != nullptr) {
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
		}
	}

	perf_end(_accel_sample_perf);

	/* Then publish gyro. */
	perf_begin(_gyro_sample_perf);

	gyro_report gyro_report = {};
	gyro_report.timestamp = data.last_read_time_usec;

	// TODO: remove these (or get the values)
	gyro_report.x_raw = NAN;
	gyro_report.y_raw = NAN;
	gyro_report.z_raw = NAN;
	gyro_report.x = data.gyro_rad_s_x;
	gyro_report.y = data.gyro_rad_s_y;
	gyro_report.z = data.gyro_rad_s_z;

	// TODO: get these right
	gyro_report.scaling = -1.0f;
	gyro_report.range_rad_s = -1.0f;

	// TODO: when is this ever blocked?
	if (!(m_pub_blocked)) {

		if (_gyro_topic != nullptr) {
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
		}
	}

	perf_end(_gyro_sample_perf);

	/* Notify anyone waiting for data. */
	DevMgr::updateNotify(*this);

	return 0;
};

namespace df_mpu9250_wrapper
{

DfMpu9250Wrapper *g_dev = nullptr;

int start(/* enum Rotation rotation */);
int stop();
int info();
void usage();

int start(/*enum Rotation rotation*/)
{
	g_dev = new DfMpu9250Wrapper(/*rotation*/);

	if (g_dev == nullptr) {
		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
		return -1;
	}

	int ret = g_dev->start();

	if (ret != 0) {
		PX4_ERR("DfMpu9250Wrapper start failed");
		return ret;
	}

	// Open the IMU sensor
	DevHandle h;
	DevMgr::getHandle(IMU_DEVICE_PATH, h);

	if (!h.isValid()) {
		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
			    IMU_DEVICE_PATH, h.getError());
		return -1;
	}

	DevMgr::releaseHandle(h);

	return 0;
}

int stop()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	int ret = g_dev->stop();

	if (ret != 0) {
		PX4_ERR("driver could not be stopped");
		return ret;
	}

	delete g_dev;
	g_dev = nullptr;
	return 0;
}

/**
 * Print a little info about the driver.
 */
int
info()
{
	if (g_dev == nullptr) {
		PX4_ERR("driver not running");
		return 1;
	}

	PX4_DEBUG("state @ %p", g_dev);

	return 0;
}

void
usage()
{
	PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'info', 'stop'");
	PX4_WARN("options:");
	//PX4_WARN("    -R rotation");
}

} // namespace df_mpu9250_wrapper

int
df_mpu9250_wrapper_main(int argc, char *argv[])
{
	int ch;
	// enum Rotation rotation = ROTATION_NONE;
	int ret = 0;
	int myoptind = 1;
	const char *myoptarg = NULL;

	/* jump over start/off/etc and look at options first */
	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		//case 'R':
		//	rotation = (enum Rotation)atoi(myoptarg);
		//	break;

		default:
			df_mpu9250_wrapper::usage();
			return 0;
		}
	}

	if (argc <= 1) {
		df_mpu9250_wrapper::usage();
		return 1;
	}

	const char *verb = argv[myoptind];

	if (!strcmp(verb, "start")) {
		ret = df_mpu9250_wrapper::start(/*rotation*/);
	}

	else if (!strcmp(verb, "stop")) {
		ret = df_mpu9250_wrapper::stop();
	}

	else if (!strcmp(verb, "info")) {
		ret = df_mpu9250_wrapper::info();
	}

	else {
		df_mpu9250_wrapper::usage();
		return 1;
	}

	return ret;
}
<|endoftext|>


<commit_before>// The MIT License (MIT)

// Copyright (c) 2013 lailongwei<lailongwei@126.com>
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to
// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
// the Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
// FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
// IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include "llbc/common/Export.h"
#include "llbc/common/BeforeIncl.h"

#include "llbc/common/ThirdHeader.h"
#include "llbc/common/Config.h"
#include "llbc/common/Compiler.h"
#include "llbc/common/Version.h"

__LLBC_NS_BEGIN

int LLBC_majorVersion       = 1;
int LLBC_minorVersion       = 0;
int LLBC_updateNo           = 2;

#ifdef LLBC_DEBUG
bool LLBC_isDebugVer        = true;
#else
bool LLBC_isDebugVer        = false;
#endif

LLBC_String LLBC_GetVersionInfo(bool verbose)
{
    LLBC_String desc;
    desc.format("%d.%d.%d_%s", LLBC_majorVersion,
        LLBC_minorVersion, LLBC_updateNo, LLBC_isDebugVer ? "debug" : "release");

#if LLBC_TARGET_PLATFORM_WIN32
    desc.append_format("(%s", "WIN32");
#elif LLBC_TARGET_PLATFORM_LINUX
    desc.append_format("(%s", "LINUX");
#elif LLBC_TARGET_PLATFORM_IPHONE
    desc.append_format("(%s", "IPHONE");
#elif LLBC_TARGET_PLATFORM_MAC
    desc.append_format("(%s", "MAC");
#elif LLBC_TARGET_PLATFORM_ANDROID
    desc.append_format("(%s", "ANDROID");
#endif

#if LLBC_CUR_COMP == LLBC_COMP_MSVC
    desc.append_format(", compiled with: %s, version: %d)", LLBC_CUR_COMP_DESC, LLBC_COMP_VER);
#elif LLBC_CUR_COMP == LLBC_COMP_GCC
    desc.append_format(", compiled with: %s, version %d.%d.%d)",
        LLBC_CUR_COMP_DESC, LLBC_COMP_MAJOR_VER, LLBC_COMP_MINOR_VER, LLBC_COMP_PATCH_LEVEL);
#else
    desc.append_format(", compiled with: %s)", LLBC_CUR_COMP_DESC);
#endif

    if (!verbose)
        return desc;

    // Append core info.
    desc.append_format("\n");
    desc.append_format("core info: \n");
    desc.append_format("  thread info: \n");
    desc.append_format("    max thread num: %d\n", LLBC_CFG_THREAD_MAX_THREAD_NUM);
    desc.append_format("    default thread stack size: %d\n", LLBC_CFG_THREAD_DFT_STACK_SIZE);
    desc.append_format("    miximum thread stack size: %d\n", LLBC_CFG_THREAD_MINIMUM_STACK_SIZE);
    desc.append_format("    message block size: %d\n", LLBC_CFG_THREAD_MSG_BLOCK_DFT_SIZE);
    desc.append_format("    enabled guard debug: %s\n", LLBC_CFG_THREAD_GUARD_DEBUG ? "true" : "false");
    desc.append_format("  logger info: \n");
    desc.append_format("    default level: %d\n", LLBC_CFG_LOG_DEFAULT_LEVEL);
    desc.append_format("    default asynchronous mode: %s\n", (LLBC_CFG_LOG_DEFAULT_ASYNC_MODE) ? "true" : "false");
    desc.append_format("    default log to console: %s\n", (LLBC_CFG_LOG_DEFAULT_LOG_TO_CONSOLE) ? "true" : "false");
    desc.append_format("    default console log pattern: %s\n", LLBC_CFG_LOG_DEFAULT_CONSOLE_LOG_PATTERN);
    desc.append_format("    default log to file: %s\n", (LLBC_CFG_LOG_DEFAULT_LOG_TO_FILE) ? "true" : "false");
    desc.append_format("    default log file name: %s\n", LLBC_CFG_LOG_DEFAULT_LOG_FILE_NAME);
    desc.append_format("    default file log pattern: %s\n", LLBC_CFG_LOG_DEFAULT_FILE_LOG_PATTERN);
    desc.append_format("    default daily mode enabled(available in file log): %s\n", (LLBC_CFG_LOG_DEFAULT_DAILY_MODE) ? "true" : "false");
    desc.append_format("    default max log file size: %d\n", LLBC_CFG_LOG_MAX_FILE_SIZE);
    desc.append_format("    default max backup index: %d\n", LLBC_CFG_LOG_MAX_BACKUP_INDEX);
    desc.append_format("    default flush interval(ms)(only available in asyn mode): %d\n", LLBC_CFG_LOG_DEFAULT_LOG_FLUSH_INTERVAL);
    desc.append_format("    max flush interval(ms)(only available in asyn mode): %d\n", LLBC_CFG_LOG_MAX_LOG_FLUSH_INTERVAL);
    desc.append_format("    default log file buffer size: %d\n", LLBC_CFG_LOG_DEFAULT_LOG_FILE_BUFFER_SIZE);
    desc.append_format("    take over unknown logger message?: %s\n", LLBC_CFG_LOG_ROOT_LOGGER_TAKE_OVER_UNCONFIGED ? "true" : "false");
    desc.append_format("  timer info: \n");
    desc.append_format("    strict timer schedule: %s\n", LLBC_CFG_CORE_TIMER_STRICT_SCHEDULE ? "true" : "false");

    // Append communication info.
    desc.append_format("communication info: \n");
    desc.append_format("  poller model: %s\n", LLBC_CFG_COMM_POLLER_MODEL);
    desc.append_format("  default service FPS: %d\n", LLBC_CFG_COMM_DFT_SERVICE_FPS);
    desc.append_format("  per thread max drive services count: %d\n", LLBC_CFG_COMM_PER_THREAD_DRIVE_MAX_SVC_COUNT);
    desc.append_format("  full protocol-stack support(let your program more efficient): %s\n", LLBC_CFG_COMM_USE_FULL_STACK ? "true" : "false");
    desc.append_format("  enabled register status code handler support: %s\n", LLBC_CFG_COMM_ENABLE_STATUS_HANDLER ? "true" : "false");
    desc.append_format("  enabled register status code describe string support: %s\n", LLBC_CFG_COMM_ENABLE_STATUS_DESC ? "true" : "false");
    desc.append_format("  enabled unify pre-subscribe support: %s\n", LLBC_CFG_COMM_ENABLE_UNIFY_PRESUBSCRIBE ? "true" : "false");
    desc.append_format("  sampler support: %s\n", LLBC_CFG_COMM_ENABLE_SAMPLER_SUPPORT ? "true" : "false");

    // Append third-party info.
    // desc.append_format("3rd party info: \n");
    // desc.append_format("  zlib version: %s(ver-num:%d)\n", ZLIB_VERSION, ZLIB_VERNUM);

    return desc;
}

__LLBC_NS_END

#include "llbc/common/AfterIncl.h"
<commit_msg>Update version to 1.0.3<commit_after>// The MIT License (MIT)

// Copyright (c) 2013 lailongwei<lailongwei@126.com>
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to
// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
// the Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
// FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
// IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include "llbc/common/Export.h"
#include "llbc/common/BeforeIncl.h"

#include "llbc/common/ThirdHeader.h"
#include "llbc/common/Config.h"
#include "llbc/common/Compiler.h"
#include "llbc/common/Version.h"

__LLBC_NS_BEGIN

int LLBC_majorVersion       = 1;
int LLBC_minorVersion       = 0;
int LLBC_updateNo           = 3;

#ifdef LLBC_DEBUG
bool LLBC_isDebugVer        = true;
#else
bool LLBC_isDebugVer        = false;
#endif

LLBC_String LLBC_GetVersionInfo(bool verbose)
{
    LLBC_String desc;
    desc.format("%d.%d.%d_%s", LLBC_majorVersion,
        LLBC_minorVersion, LLBC_updateNo, LLBC_isDebugVer ? "debug" : "release");

#if LLBC_TARGET_PLATFORM_WIN32
    desc.append_format("(%s", "WIN32");
#elif LLBC_TARGET_PLATFORM_LINUX
    desc.append_format("(%s", "LINUX");
#elif LLBC_TARGET_PLATFORM_IPHONE
    desc.append_format("(%s", "IPHONE");
#elif LLBC_TARGET_PLATFORM_MAC
    desc.append_format("(%s", "MAC");
#elif LLBC_TARGET_PLATFORM_ANDROID
    desc.append_format("(%s", "ANDROID");
#endif

#if LLBC_CUR_COMP == LLBC_COMP_MSVC
    desc.append_format(", compiled with: %s, version: %d)", LLBC_CUR_COMP_DESC, LLBC_COMP_VER);
#elif LLBC_CUR_COMP == LLBC_COMP_GCC
    desc.append_format(", compiled with: %s, version %d.%d.%d)",
        LLBC_CUR_COMP_DESC, LLBC_COMP_MAJOR_VER, LLBC_COMP_MINOR_VER, LLBC_COMP_PATCH_LEVEL);
#else
    desc.append_format(", compiled with: %s)", LLBC_CUR_COMP_DESC);
#endif

    if (!verbose)
        return desc;

    // Append core info.
    desc.append_format("\n");
    desc.append_format("core info: \n");
    desc.append_format("  thread info: \n");
    desc.append_format("    max thread num: %d\n", LLBC_CFG_THREAD_MAX_THREAD_NUM);
    desc.append_format("    default thread stack size: %d\n", LLBC_CFG_THREAD_DFT_STACK_SIZE);
    desc.append_format("    miximum thread stack size: %d\n", LLBC_CFG_THREAD_MINIMUM_STACK_SIZE);
    desc.append_format("    message block size: %d\n", LLBC_CFG_THREAD_MSG_BLOCK_DFT_SIZE);
    desc.append_format("    enabled guard debug: %s\n", LLBC_CFG_THREAD_GUARD_DEBUG ? "true" : "false");
    desc.append_format("  logger info: \n");
    desc.append_format("    default level: %d\n", LLBC_CFG_LOG_DEFAULT_LEVEL);
    desc.append_format("    default asynchronous mode: %s\n", (LLBC_CFG_LOG_DEFAULT_ASYNC_MODE) ? "true" : "false");
    desc.append_format("    default log to console: %s\n", (LLBC_CFG_LOG_DEFAULT_LOG_TO_CONSOLE) ? "true" : "false");
    desc.append_format("    default console log pattern: %s\n", LLBC_CFG_LOG_DEFAULT_CONSOLE_LOG_PATTERN);
    desc.append_format("    default log to file: %s\n", (LLBC_CFG_LOG_DEFAULT_LOG_TO_FILE) ? "true" : "false");
    desc.append_format("    default log file name: %s\n", LLBC_CFG_LOG_DEFAULT_LOG_FILE_NAME);
    desc.append_format("    default file log pattern: %s\n", LLBC_CFG_LOG_DEFAULT_FILE_LOG_PATTERN);
    desc.append_format("    default daily mode enabled(available in file log): %s\n", (LLBC_CFG_LOG_DEFAULT_DAILY_MODE) ? "true" : "false");
    desc.append_format("    default max log file size: %d\n", LLBC_CFG_LOG_MAX_FILE_SIZE);
    desc.append_format("    default max backup index: %d\n", LLBC_CFG_LOG_MAX_BACKUP_INDEX);
    desc.append_format("    default flush interval(ms)(only available in asyn mode): %d\n", LLBC_CFG_LOG_DEFAULT_LOG_FLUSH_INTERVAL);
    desc.append_format("    max flush interval(ms)(only available in asyn mode): %d\n", LLBC_CFG_LOG_MAX_LOG_FLUSH_INTERVAL);
    desc.append_format("    default log file buffer size: %d\n", LLBC_CFG_LOG_DEFAULT_LOG_FILE_BUFFER_SIZE);
    desc.append_format("    take over unknown logger message?: %s\n", LLBC_CFG_LOG_ROOT_LOGGER_TAKE_OVER_UNCONFIGED ? "true" : "false");
    desc.append_format("  timer info: \n");
    desc.append_format("    strict timer schedule: %s\n", LLBC_CFG_CORE_TIMER_STRICT_SCHEDULE ? "true" : "false");

    // Append communication info.
    desc.append_format("communication info: \n");
    desc.append_format("  poller model: %s\n", LLBC_CFG_COMM_POLLER_MODEL);
    desc.append_format("  default service FPS: %d\n", LLBC_CFG_COMM_DFT_SERVICE_FPS);
    desc.append_format("  per thread max drive services count: %d\n", LLBC_CFG_COMM_PER_THREAD_DRIVE_MAX_SVC_COUNT);
    desc.append_format("  full protocol-stack support(let your program more efficient): %s\n", LLBC_CFG_COMM_USE_FULL_STACK ? "true" : "false");
    desc.append_format("  enabled register status code handler support: %s\n", LLBC_CFG_COMM_ENABLE_STATUS_HANDLER ? "true" : "false");
    desc.append_format("  enabled register status code describe string support: %s\n", LLBC_CFG_COMM_ENABLE_STATUS_DESC ? "true" : "false");
    desc.append_format("  enabled unify pre-subscribe support: %s\n", LLBC_CFG_COMM_ENABLE_UNIFY_PRESUBSCRIBE ? "true" : "false");
    desc.append_format("  sampler support: %s\n", LLBC_CFG_COMM_ENABLE_SAMPLER_SUPPORT ? "true" : "false");

    // Append third-party info.
    // desc.append_format("3rd party info: \n");
    // desc.append_format("  zlib version: %s(ver-num:%d)\n", ZLIB_VERSION, ZLIB_VERNUM);

    return desc;
}

__LLBC_NS_END

#include "llbc/common/AfterIncl.h"
<|endoftext|>


<commit_before>//
// Copyright (C) 2007 Jaroslav Libak
// Licensed under the LGPL license.
//
// Copyright (C) 2005-2007 SIPez LLC.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// Copyright (C) 2004-2007 SIPfoundry Inc.
// Licensed by SIPfoundry under the LGPL license.
//
// Copyright (C) 2004-2006 Pingtel Corp.  All rights reserved.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// $$
///////////////////////////////////////////////////////////////////////////////

// SYSTEM INCLUDES
#include <assert.h>

// APPLICATION INCLUDES
#include <net/SipTransport.h>
#include <net/Url.h>

// DEFINES
// EXTERNAL FUNCTIONS
// EXTERNAL VARIABLES
// CONSTANTS
// STATIC VARIABLE INITIALIZATIONS
// MACROS
// GLOBAL VARIABLES
// GLOBAL FUNCTIONS

/* //////////////////////////// PUBLIC //////////////////////////////////// */

/* ============================ CREATORS ================================== */

/* ============================ MANIPULATORS ============================== */

SIPXSTACK_TRANSPORT_TYPE SipTransport::getSipTransport(const Url& url)
{
   return getSipTransport(url.toString());
}

SIPXSTACK_TRANSPORT_TYPE SipTransport::getSipTransport(const UtlString& strUrl)
{
   SIPXSTACK_TRANSPORT_TYPE transport = SIPXSTACK_TRANSPORT_UDP;

   if (strUrl.contains("sips:") || strUrl.contains("transport=tls"))
   {
      transport = SIPXSTACK_TRANSPORT_TLS;
   }
   if (strUrl.contains("transport=tcp"))
   {
      transport = SIPXSTACK_TRANSPORT_TCP;
   }

   return transport;
}

SIPXSTACK_TRANSPORT_TYPE SipTransport::getSipTransport(const char* szUrl)
{
   UtlString strUrl(szUrl);
   return getSipTransport(strUrl);
}

/* ============================ ACCESSORS ================================= */

/* ============================ INQUIRY =================================== */

/* //////////////////////////// PROTECTED ///////////////////////////////// */

/* //////////////////////////// PRIVATE /////////////////////////////////// */

/* ============================ FUNCTIONS ================================= */

<commit_msg>SipTransport now ignores case of transport=tls etc.<commit_after>//
// Copyright (C) 2007 Jaroslav Libak
// Licensed under the LGPL license.
//
// Copyright (C) 2005-2007 SIPez LLC.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// Copyright (C) 2004-2007 SIPfoundry Inc.
// Licensed by SIPfoundry under the LGPL license.
//
// Copyright (C) 2004-2006 Pingtel Corp.  All rights reserved.
// Licensed to SIPfoundry under a Contributor Agreement.
//
// $$
///////////////////////////////////////////////////////////////////////////////

// SYSTEM INCLUDES
#include <assert.h>

// APPLICATION INCLUDES
#include <net/SipTransport.h>
#include <net/Url.h>

// DEFINES
// EXTERNAL FUNCTIONS
// EXTERNAL VARIABLES
// CONSTANTS
// STATIC VARIABLE INITIALIZATIONS
// MACROS
// GLOBAL VARIABLES
// GLOBAL FUNCTIONS

/* //////////////////////////// PUBLIC //////////////////////////////////// */

/* ============================ CREATORS ================================== */

/* ============================ MANIPULATORS ============================== */

SIPXSTACK_TRANSPORT_TYPE SipTransport::getSipTransport(const Url& url)
{
   return getSipTransport(url.toString());
}

SIPXSTACK_TRANSPORT_TYPE SipTransport::getSipTransport(const UtlString& strUrl)
{
   SIPXSTACK_TRANSPORT_TYPE transport = SIPXSTACK_TRANSPORT_UDP;

   UtlString lowerStrUrl(strUrl);
   lowerStrUrl.toLower();

   if (lowerStrUrl.contains("sips:") || lowerStrUrl.contains("transport=tls"))
   {
      transport = SIPXSTACK_TRANSPORT_TLS;
   }
   if (lowerStrUrl.contains("transport=tcp"))
   {
      transport = SIPXSTACK_TRANSPORT_TCP;
   }

   return transport;
}

SIPXSTACK_TRANSPORT_TYPE SipTransport::getSipTransport(const char* szUrl)
{
   UtlString strUrl(szUrl);
   return getSipTransport(strUrl);
}

/* ============================ ACCESSORS ================================= */

/* ============================ INQUIRY =================================== */

/* //////////////////////////// PROTECTED ///////////////////////////////// */

/* //////////////////////////// PRIVATE /////////////////////////////////// */

/* ============================ FUNCTIONS ================================= */

<|endoftext|>


<commit_before>#include "loci/PopulationLocus.hpp"
#include "exception/ValueOutOfRangeException.hpp"
#include <boost/any.hpp>
#include <sstream>

using namespace std;

PopulationLocus::PopulationLocus(PopulationNode * node) {
	this->node = node;
}

PopulationLocus::~PopulationLocus() {}

Genome * PopulationLocus::getIndex(int index) {
	return this->node->getIndex(index);
}

int PopulationLocus::topIndex() {
	return this->node->populationSize() - 1;
}

int PopulationLocus::randomIndex() {
	uniform_int_distribution<int> dist(0, this->node->populationSize());
	return dist(this->generator);
}

bool PopulationLocus::isConstructive() {
	return true;
}

bool PopulationLocus::outOfRange(int i) {
	return i < 0 || i >= this->node->populationSize();
}

string PopulationLocus::toString() {
	stringstream ss;

	for (int i = 0; i < population.size(); i++) {
		ss << "Locus uses node " << this->node->name();
	}

	ss << "\nRandom Seed: " << seed << "\n";

	return ss.str();
}

string PopulationLocus::flatten(int index) {
	if (outOfRange(index)) throw ValueOutOfRangeException();

	stringstream ss;
	ss << this->getIndex(index)->flatten();
	return ss.str();
}
<commit_msg>[PopulationLocus]: Fixed index-out-of-range error<commit_after>#include "loci/PopulationLocus.hpp"
#include "exception/ValueOutOfRangeException.hpp"
#include <boost/any.hpp>
#include <sstream>

using namespace std;

PopulationLocus::PopulationLocus(PopulationNode * node) {
	this->node = node;
}

PopulationLocus::~PopulationLocus() {}

Genome * PopulationLocus::getIndex(int index) {
	if (this->outOfRange(index)) throw ValueOutOfRangeException();
	return this->node->getIndex(index);
}

int PopulationLocus::topIndex() {
	return this->node->populationSize() - 1;
}

int PopulationLocus::randomIndex() {
	uniform_int_distribution<int> dist(0, this->node->populationSize()-1);
	return dist(this->generator);
}

bool PopulationLocus::isConstructive() {
	return true;
}

bool PopulationLocus::outOfRange(int i) {
	return i < 0 || i >= this->node->populationSize();
}

string PopulationLocus::toString() {
	stringstream ss;

	for (int i = 0; i < population.size(); i++) {
		ss << "Locus uses node " << this->node->name();
	}

	ss << "\nRandom Seed: " << seed << "\n";

	return ss.str();
}

string PopulationLocus::flatten(int index) {
	if (outOfRange(index)) throw ValueOutOfRangeException();

	stringstream ss;
	ss << this->getIndex(index)->flatten();
	return ss.str();
}
<|endoftext|>


<commit_before>#include <protobuf/LogFrame.pb.h>
#include "InterpolatedPath.hpp"
#include "Utils.hpp"
#include "LogUtils.hpp"
#include <stdexcept>

#pragma mark InterpolatedPath

using namespace std;
using namespace Geometry2d;

namespace Planning {
	InterpolatedPath::InterpolatedPath(const Geometry2d::Point &p0) {
		points.push_back(p0);
	}

	InterpolatedPath::InterpolatedPath(const Geometry2d::Point &p0, const Geometry2d::Point &p1) {
		points.push_back(p0);
		points.push_back(p1);
	}

	float InterpolatedPath::length(unsigned int start) const {
		if (points.empty() || start >= (points.size() - 1)) {
			return 0;
		}

		float length = 0;
		for (unsigned int i = start; i < (points.size() - 1); ++i) {
			length += (points[i + 1] - points[i]).mag();
		}
		return length;
	}

	float InterpolatedPath::length(unsigned int start, unsigned int end) const {
		if (points.empty() || start >= (points.size() - 1)) {
			return 0;
		}

		float length = 0;
		for (unsigned int i = start; i < end; ++i) {
			length += (points[i + 1] - points[i]).mag();
		}
		return length;
	}

	boost::optional<Geometry2d::Point> InterpolatedPath::start() const {
		if (points.empty())
			return boost::none;
		else
			return points.front();
	}

	boost::optional<Geometry2d::Point> InterpolatedPath::destination() const {
		if (points.empty())
			return boost::none;
		else
			return points.back();
	}

	// Returns the index of the point in this path nearest to pt.
	int InterpolatedPath::nearestIndex(const Geometry2d::Point &pt) const {
		if (points.size() == 0) {
			return -1;
		}

		int index = 0;
		float dist = pt.distTo(points[0]);

		for (unsigned int i = 1; i < points.size(); ++i) {
			float d = pt.distTo(points[i]);
			if (d < dist) {
				dist = d;
				index = i;
			}
		}

		return index;
	}

	bool InterpolatedPath::hit(const Geometry2d::CompositeShape &obstacles, float startTime) const {
		int start = 0;
		for (float t: times) {
			start++;
			if (t > startTime) {
				start--;
				break;
			}
		}

		if (start >= points.size()) {
			// Empty path or starting beyond end of path
			return false;
		}

		//This code disreguards obstacles if which we start in. This the robot to move out a obstacle if it is already in one.
		// The set of obstacles the starting point was inside of
		std::set<std::shared_ptr<Geometry2d::Shape>> startHitSet;
		obstacles.hit(points[start], startHitSet);

		for (size_t i = 0; i < points.size() - 1; i++) {
			std::set<std::shared_ptr<Geometry2d::Shape>> newHitSet;
			if (obstacles.hit(Geometry2d::Segment(points[i], points[i+1]), newHitSet)) {
				for (std::shared_ptr<Geometry2d::Shape> hit : newHitSet) {
					//If it hits something, check if the hit was in hte origional hitSet
					if (startHitSet.find(hit) == startHitSet.end()) {
						return true;
					}
				}
			}
		}
		return false;
	}

	float InterpolatedPath::distanceTo(const Geometry2d::Point &pt) const {
		int i = nearestIndex(pt);
		if (i < 0) {
			return 0;
		}

		float dist = -1;
		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);
			const float d = s.distTo(pt);

			if (dist < 0 || d < dist) {
				dist = d;
			}
		}

		return dist;
	}

	Geometry2d::Segment InterpolatedPath::nearestSegment(const Geometry2d::Point &pt) const {
		Geometry2d::Segment best;
		float dist = -1;
		if (points.empty()) {
			return best;
		}

		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);
			const float d = s.distTo(pt);

			if (dist < 0 || d < dist) {
				best = s;
				dist = d;
			}
		}

		return best;
	}

	void InterpolatedPath::startFrom(const Geometry2d::Point &pt, InterpolatedPath &result) const {

		// path will start at the current robot pose
		result.clear();
		result.points.push_back(pt);

		if (points.empty())
			return;

		// handle simple paths
		if (points.size() == 1) {
			result.points.push_back(points.front());
			return;
		}

		// find where to start the path
		Geometry2d::Segment close_segment;
		float dist = -1;
		unsigned int i = (points.front().nearPoint(pt, 0.02)) ? 1 : 0;
		vector<Geometry2d::Point>::const_iterator path_start = ++points.begin();
		for (; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);
			const float d = s.distTo(pt);
			if (dist < 0 || d < dist) {
				close_segment = s;
				dist = d;
			}
		}

		// slice path
		// new path will be pt, [closest point on nearest segment], [i+1 to end]
		if (dist > 0.0 && dist < 0.02) {
			Geometry2d::Point intersection_pt = close_segment.nearestPoint(pt);
			result.points.push_back(intersection_pt);
		}

		result.points.insert(result.points.end(), path_start, points.end());

	}

	float InterpolatedPath::length(const Geometry2d::Point &pt) const {
		float dist = -1;
		float length = 0;
		if (points.empty()) {
			return 0;
		}

		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);

			//add the segment length
			length += s.length();

			const float d = s.distTo(pt);

			//if point closer to this segment
			if (dist < 0 || d < dist) {
				//closest point on segment
				Geometry2d::Point p = s.nearestPoint(pt);

				//new best distance
				dist = d;

				//reset running length count
				length = 0;
				length += p.distTo(s.pt[1]);
			}
		}

		return length;
	}

	bool InterpolatedPath::getPoint(float distance, Geometry2d::Point &position,
											  Geometry2d::Point &direction) const {
		if (distance <= 0) {
			position = points.front();
			return false;
		}
		if (points.empty()) {
			return false;
		}
		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Point vector(points[i + 1] - points[i]);

			float vectorLength = vector.mag();
			distance -= vectorLength;

			if (distance <= 0) {
				distance += vectorLength;
				position = points[i] + (vector * (distance / vectorLength));
				direction = vector.normalized();
				return true;
			}
		}
		position = points.back();
		return false;

	}

	void InterpolatedPath::draw(SystemState *const state, const QColor &col = Qt::black,
										  const QString &layer = "Motion") const {
		Packet::DebugPath *dbg = state->logFrame->add_debug_paths();
		dbg->set_layer(state->findDebugLayer(layer));
		for (Geometry2d::Point pt : points) {
			*dbg->add_points() = pt;
		}
		dbg->set_color(color(col));
		return;
	}

	bool InterpolatedPath::evaluate(float t, Geometry2d::Point &targetPosOut,
											  Geometry2d::Point &targetVelOut) const {
		/*
		float linearPos;
		float linearSpeed;
		bool pathIsValid = TrapezoidalMotion(
			length(),
			maxSpeed,
			maxAcceleration,
			t,
			startSpeed,
			endSpeed,
			linearPos,      //  these are set by reference since C++ can't return multiple values
			linearSpeed);   //

		Geometry2d::Point direction;
		if(!getPoint(linearPos, targetPosOut, direction)) {
			return false;
		}

		targetVelOut = direction * linearSpeed;
		*/
		if (times.size() == 0) {
			targetPosOut = Geometry2d::Point(0, 0);
			targetVelOut = Geometry2d::Point(0, 0);
			return false;
		}
		if (times.size() == 1) {
			targetPosOut = points[0];
			targetVelOut = Geometry2d::Point(0, 0);
			return false;
		}
		if (t < times[0]) {
			targetPosOut = points[0];
			targetVelOut = vels[0];
			return false;
		}

		int i = 0;
		while (times[i] <= t) {
			if (times[i] == t) {
				targetPosOut = points[i];
				targetVelOut = vels[i];
				return true;
			}
			i++;
			if (i == size()) {
				targetPosOut = points[i - 1];
				targetVelOut = vels[i - 1];
				return false;
			}
		}
		float deltaT = (times[i] - times[i - 1]);
		if (deltaT == 0) {
			targetPosOut = points[i];
			targetVelOut = vels[i];
			return true;
		}
		float constant = (t - times[i - 1]) / deltaT;

		targetPosOut = points[i - 1] * (1 - constant) + points[i] * (constant);
		targetVelOut = vels[i - 1] * (1 - constant) + vels[i] * (constant);

		return true;
	}

	size_t InterpolatedPath::size() const {
		return points.size();
	}

	bool InterpolatedPath::valid() const {
		return !points.empty();
	}

	float InterpolatedPath::getTime(int index) const {
		return times[index];
	}

	float InterpolatedPath::getDuration() const {
		if (times.size() > 0) {
			return times.back();
		} else {
			return 0;
		}
	}

	unique_ptr<Path> InterpolatedPath::subPath(float startTime, float endTime) const {
		//Check for valid arguments
		if (startTime < 0) {
			throw invalid_argument("InterpolatedPath::subPath(): startTime(" + to_string(startTime) + ") can't be less than zero");
		}

		if (endTime < 0) {
			throw invalid_argument("InterpolatedPath::subPath(): endTime(" + to_string(endTime) + ") can't be less than zero");
		}

		if (startTime > endTime) {
			throw invalid_argument("InterpolatedPath::subPath(): startTime(" + to_string(startTime) + ") can't be after endTime(" + to_string(endTime) + ")");
		}

		if (startTime >= getDuration()) {
			debugThrow(invalid_argument("InterpolatedPath::subPath(): startTime(" + to_string(startTime) + ") can't be greater than the duration(" + to_string(getDuration()) + ") of the path"));
			return unique_ptr<Path>(new InterpolatedPath());
		}

		if (startTime == 0 && endTime >= getDuration()) {
			return this->clone();
		}

		InterpolatedPath *path = new InterpolatedPath();

		//Bound the endTime to a reasonable time.
		endTime = min(endTime, getDuration());

		//Find the first point in the vector of points which will be included in the subPath
		size_t start = 0;
		while (times[start] <= startTime) {
			start++;
		}
		start--;

		//Add the first points to the InterpolatedPath
		path->times.push_back(0);
		if (times[start] == startTime) {
			path->points.push_back(points[start]);
			path->vels.push_back(vels[start]);
		} else {
			float deltaT = (times[start + 1] - times[start]);
			float constant = (times[start + 1] - startTime) / deltaT;
			Point startPos = points[start + 1] * (1 - constant) + points[start] * (constant);
			Point vi = vels[start + 1] * (1 - constant) + vels[start] * (constant);
			path->points.push_back(startPos);
			path->vels.push_back(vi);
		}

		//Find the last point in the InterpolatedPath
		Point vf;
		Point endPos;
		size_t end;
		if (endTime >= getDuration()) {
			end = size() - 1;
			vf = vels[end];
			endPos = points[end];
		} else {
			end = start + 1;
			while (times[end] < endTime) {
				end++;
			}
			float deltaT = (times[end] - times[end - 1]);
			float constant = (times[end] - endTime) / deltaT;
			//endTime = times[end];
			vf = vels[end] * (1 - constant) + vels[end - 1] * (constant);
			endPos = points[end] * (1 - constant) + points[end - 1] * (constant);
		}

		//Add all the points in the middle
		size_t i = start + 1;
		while (i < end) {
			path->points.push_back(points[i]);
			path->vels.push_back(vels[i]);
			path->times.push_back(times[i] - startTime);
			i++;
		}

		//Add the last point
		path->points.push_back(endPos);
		path->vels.push_back(vf);
		path->times.push_back(endTime - startTime);

		return unique_ptr<Path>(path);

	}

	unique_ptr<Path> InterpolatedPath::clone() const {
		return unique_ptr<Path>(new InterpolatedPath(*this));
	}
}<commit_msg>InterpolatedPath.hit() starts from @start, not zero<commit_after>#include <protobuf/LogFrame.pb.h>
#include "InterpolatedPath.hpp"
#include "Utils.hpp"
#include "LogUtils.hpp"
#include <stdexcept>

#pragma mark InterpolatedPath

using namespace std;
using namespace Geometry2d;

namespace Planning {
	InterpolatedPath::InterpolatedPath(const Geometry2d::Point &p0) {
		points.push_back(p0);
	}

	InterpolatedPath::InterpolatedPath(const Geometry2d::Point &p0, const Geometry2d::Point &p1) {
		points.push_back(p0);
		points.push_back(p1);
	}

	float InterpolatedPath::length(unsigned int start) const {
		if (points.empty() || start >= (points.size() - 1)) {
			return 0;
		}

		float length = 0;
		for (unsigned int i = start; i < (points.size() - 1); ++i) {
			length += (points[i + 1] - points[i]).mag();
		}
		return length;
	}

	float InterpolatedPath::length(unsigned int start, unsigned int end) const {
		if (points.empty() || start >= (points.size() - 1)) {
			return 0;
		}

		float length = 0;
		for (unsigned int i = start; i < end; ++i) {
			length += (points[i + 1] - points[i]).mag();
		}
		return length;
	}

	boost::optional<Geometry2d::Point> InterpolatedPath::start() const {
		if (points.empty())
			return boost::none;
		else
			return points.front();
	}

	boost::optional<Geometry2d::Point> InterpolatedPath::destination() const {
		if (points.empty())
			return boost::none;
		else
			return points.back();
	}

	// Returns the index of the point in this path nearest to pt.
	int InterpolatedPath::nearestIndex(const Geometry2d::Point &pt) const {
		if (points.size() == 0) {
			return -1;
		}

		int index = 0;
		float dist = pt.distTo(points[0]);

		for (unsigned int i = 1; i < points.size(); ++i) {
			float d = pt.distTo(points[i]);
			if (d < dist) {
				dist = d;
				index = i;
			}
		}

		return index;
	}

	bool InterpolatedPath::hit(const Geometry2d::CompositeShape &obstacles, float startTime) const {
		int start = 0;
		for (float t: times) {
			start++;
			if (t > startTime) {
				start--;
				break;
			}
		}

		if (start >= points.size()) {
			// Empty path or starting beyond end of path
			return false;
		}

		//This code disreguards obstacles if which we start in. This the robot to move out a obstacle if it is already in one.
		// The set of obstacles the starting point was inside of
		std::set<std::shared_ptr<Geometry2d::Shape>> startHitSet;
		obstacles.hit(points[start], startHitSet);

		for (size_t i = start; i < points.size() - 1; i++) {
			std::set<std::shared_ptr<Geometry2d::Shape>> newHitSet;
			if (obstacles.hit(Geometry2d::Segment(points[i], points[i+1]), newHitSet)) {
				for (std::shared_ptr<Geometry2d::Shape> hit : newHitSet) {
					//If it hits something, check if the hit was in hte origional hitSet
					if (startHitSet.find(hit) == startHitSet.end()) {
						return true;
					}
				}
			}
		}
		return false;
	}

	float InterpolatedPath::distanceTo(const Geometry2d::Point &pt) const {
		int i = nearestIndex(pt);
		if (i < 0) {
			return 0;
		}

		float dist = -1;
		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);
			const float d = s.distTo(pt);

			if (dist < 0 || d < dist) {
				dist = d;
			}
		}

		return dist;
	}

	Geometry2d::Segment InterpolatedPath::nearestSegment(const Geometry2d::Point &pt) const {
		Geometry2d::Segment best;
		float dist = -1;
		if (points.empty()) {
			return best;
		}

		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);
			const float d = s.distTo(pt);

			if (dist < 0 || d < dist) {
				best = s;
				dist = d;
			}
		}

		return best;
	}

	void InterpolatedPath::startFrom(const Geometry2d::Point &pt, InterpolatedPath &result) const {

		// path will start at the current robot pose
		result.clear();
		result.points.push_back(pt);

		if (points.empty())
			return;

		// handle simple paths
		if (points.size() == 1) {
			result.points.push_back(points.front());
			return;
		}

		// find where to start the path
		Geometry2d::Segment close_segment;
		float dist = -1;
		unsigned int i = (points.front().nearPoint(pt, 0.02)) ? 1 : 0;
		vector<Geometry2d::Point>::const_iterator path_start = ++points.begin();
		for (; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);
			const float d = s.distTo(pt);
			if (dist < 0 || d < dist) {
				close_segment = s;
				dist = d;
			}
		}

		// slice path
		// new path will be pt, [closest point on nearest segment], [i+1 to end]
		if (dist > 0.0 && dist < 0.02) {
			Geometry2d::Point intersection_pt = close_segment.nearestPoint(pt);
			result.points.push_back(intersection_pt);
		}

		result.points.insert(result.points.end(), path_start, points.end());

	}

	float InterpolatedPath::length(const Geometry2d::Point &pt) const {
		float dist = -1;
		float length = 0;
		if (points.empty()) {
			return 0;
		}

		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Segment s(points[i], points[i + 1]);

			//add the segment length
			length += s.length();

			const float d = s.distTo(pt);

			//if point closer to this segment
			if (dist < 0 || d < dist) {
				//closest point on segment
				Geometry2d::Point p = s.nearestPoint(pt);

				//new best distance
				dist = d;

				//reset running length count
				length = 0;
				length += p.distTo(s.pt[1]);
			}
		}

		return length;
	}

	bool InterpolatedPath::getPoint(float distance, Geometry2d::Point &position,
											  Geometry2d::Point &direction) const {
		if (distance <= 0) {
			position = points.front();
			return false;
		}
		if (points.empty()) {
			return false;
		}
		for (unsigned int i = 0; i < (points.size() - 1); ++i) {
			Geometry2d::Point vector(points[i + 1] - points[i]);

			float vectorLength = vector.mag();
			distance -= vectorLength;

			if (distance <= 0) {
				distance += vectorLength;
				position = points[i] + (vector * (distance / vectorLength));
				direction = vector.normalized();
				return true;
			}
		}
		position = points.back();
		return false;

	}

	void InterpolatedPath::draw(SystemState *const state, const QColor &col = Qt::black,
										  const QString &layer = "Motion") const {
		Packet::DebugPath *dbg = state->logFrame->add_debug_paths();
		dbg->set_layer(state->findDebugLayer(layer));
		for (Geometry2d::Point pt : points) {
			*dbg->add_points() = pt;
		}
		dbg->set_color(color(col));
		return;
	}

	bool InterpolatedPath::evaluate(float t, Geometry2d::Point &targetPosOut,
											  Geometry2d::Point &targetVelOut) const {
		/*
		float linearPos;
		float linearSpeed;
		bool pathIsValid = TrapezoidalMotion(
			length(),
			maxSpeed,
			maxAcceleration,
			t,
			startSpeed,
			endSpeed,
			linearPos,      //  these are set by reference since C++ can't return multiple values
			linearSpeed);   //

		Geometry2d::Point direction;
		if(!getPoint(linearPos, targetPosOut, direction)) {
			return false;
		}

		targetVelOut = direction * linearSpeed;
		*/
		if (times.size() == 0) {
			targetPosOut = Geometry2d::Point(0, 0);
			targetVelOut = Geometry2d::Point(0, 0);
			return false;
		}
		if (times.size() == 1) {
			targetPosOut = points[0];
			targetVelOut = Geometry2d::Point(0, 0);
			return false;
		}
		if (t < times[0]) {
			targetPosOut = points[0];
			targetVelOut = vels[0];
			return false;
		}

		int i = 0;
		while (times[i] <= t) {
			if (times[i] == t) {
				targetPosOut = points[i];
				targetVelOut = vels[i];
				return true;
			}
			i++;
			if (i == size()) {
				targetPosOut = points[i - 1];
				targetVelOut = vels[i - 1];
				return false;
			}
		}
		float deltaT = (times[i] - times[i - 1]);
		if (deltaT == 0) {
			targetPosOut = points[i];
			targetVelOut = vels[i];
			return true;
		}
		float constant = (t - times[i - 1]) / deltaT;

		targetPosOut = points[i - 1] * (1 - constant) + points[i] * (constant);
		targetVelOut = vels[i - 1] * (1 - constant) + vels[i] * (constant);

		return true;
	}

	size_t InterpolatedPath::size() const {
		return points.size();
	}

	bool InterpolatedPath::valid() const {
		return !points.empty();
	}

	float InterpolatedPath::getTime(int index) const {
		return times[index];
	}

	float InterpolatedPath::getDuration() const {
		if (times.size() > 0) {
			return times.back();
		} else {
			return 0;
		}
	}

	unique_ptr<Path> InterpolatedPath::subPath(float startTime, float endTime) const {
		//Check for valid arguments
		if (startTime < 0) {
			throw invalid_argument("InterpolatedPath::subPath(): startTime(" + to_string(startTime) + ") can't be less than zero");
		}

		if (endTime < 0) {
			throw invalid_argument("InterpolatedPath::subPath(): endTime(" + to_string(endTime) + ") can't be less than zero");
		}

		if (startTime > endTime) {
			throw invalid_argument("InterpolatedPath::subPath(): startTime(" + to_string(startTime) + ") can't be after endTime(" + to_string(endTime) + ")");
		}

		if (startTime >= getDuration()) {
			debugThrow(invalid_argument("InterpolatedPath::subPath(): startTime(" + to_string(startTime) + ") can't be greater than the duration(" + to_string(getDuration()) + ") of the path"));
			return unique_ptr<Path>(new InterpolatedPath());
		}

		if (startTime == 0 && endTime >= getDuration()) {
			return this->clone();
		}

		InterpolatedPath *path = new InterpolatedPath();

		//Bound the endTime to a reasonable time.
		endTime = min(endTime, getDuration());

		//Find the first point in the vector of points which will be included in the subPath
		size_t start = 0;
		while (times[start] <= startTime) {
			start++;
		}
		start--;

		//Add the first points to the InterpolatedPath
		path->times.push_back(0);
		if (times[start] == startTime) {
			path->points.push_back(points[start]);
			path->vels.push_back(vels[start]);
		} else {
			float deltaT = (times[start + 1] - times[start]);
			float constant = (times[start + 1] - startTime) / deltaT;
			Point startPos = points[start + 1] * (1 - constant) + points[start] * (constant);
			Point vi = vels[start + 1] * (1 - constant) + vels[start] * (constant);
			path->points.push_back(startPos);
			path->vels.push_back(vi);
		}

		//Find the last point in the InterpolatedPath
		Point vf;
		Point endPos;
		size_t end;
		if (endTime >= getDuration()) {
			end = size() - 1;
			vf = vels[end];
			endPos = points[end];
		} else {
			end = start + 1;
			while (times[end] < endTime) {
				end++;
			}
			float deltaT = (times[end] - times[end - 1]);
			float constant = (times[end] - endTime) / deltaT;
			//endTime = times[end];
			vf = vels[end] * (1 - constant) + vels[end - 1] * (constant);
			endPos = points[end] * (1 - constant) + points[end - 1] * (constant);
		}

		//Add all the points in the middle
		size_t i = start + 1;
		while (i < end) {
			path->points.push_back(points[i]);
			path->vels.push_back(vels[i]);
			path->times.push_back(times[i] - startTime);
			i++;
		}

		//Add the last point
		path->points.push_back(endPos);
		path->vels.push_back(vf);
		path->times.push_back(endTime - startTime);

		return unique_ptr<Path>(path);

	}

	unique_ptr<Path> InterpolatedPath::clone() const {
		return unique_ptr<Path>(new InterpolatedPath(*this));
	}
}
<|endoftext|>


<commit_before>// Copyright (c) 2006, Google Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
//     * Neither the name of Google Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "common/linux/guid_creator.h"

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

//
// GUIDGenerator
//
// This class is used to generate random GUID.
// Currently use random number to generate a GUID since Linux has
// no native GUID generator. This should be OK since we don't expect
// crash to happen very offen.
//
class GUIDGenerator {
 public:
  GUIDGenerator() {
    srandom(time(NULL));
  }

  static u_int32_t BytesToUInt32(const u_int8_t bytes[]) {
    return ((u_int32_t) bytes[0]
            | ((u_int32_t) bytes[1] << 8)
            | ((u_int32_t) bytes[2] << 16)
            | ((u_int32_t) bytes[3] << 24));
  }

  static void UInt32ToBytes(u_int8_t bytes[], u_int32_t n) {
    bytes[0] = n & 0xff;
    bytes[1] = (n >> 8) & 0xff;
    bytes[2] = (n >> 16) & 0xff;
    bytes[3] = (n >> 24) & 0xff;
  }

  bool CreateGUID(GUID *guid) const {
    guid->data1 = random();
    guid->data2 = (u_int16_t)(random());
    guid->data3 = (u_int16_t)(random());
    UInt32ToBytes(&guid->data4[0], random());
    UInt32ToBytes(&guid->data4[4], random());
    return true;
  }
};

// Guid generator.
const GUIDGenerator kGuidGenerator;

bool CreateGUID(GUID *guid) {
  return kGuidGenerator.CreateGUID(guid);
}

// Parse guid to string.
bool GUIDToString(const GUID *guid, char *buf, int buf_len) {
  // Should allow more space the the max length of GUID.
  assert(buf_len > kGUIDStringLength);
  int num = snprintf(buf, buf_len, kGUIDFormatString,
                     guid->data1, guid->data2, guid->data3,
                     GUIDGenerator::BytesToUInt32(&(guid->data4[0])),
                     GUIDGenerator::BytesToUInt32(&(guid->data4[4])));
  if (num != kGUIDStringLength)
    return false;

  buf[num] = '\0';
  return true;
}
<commit_msg> Remove static initializer in linux/guid_creator.cc.<commit_after>// Copyright (c) 2006, Google Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
//     * Neither the name of Google Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "common/linux/guid_creator.h"

#include <assert.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

//
// GUIDGenerator
//
// This class is used to generate random GUID.
// Currently use random number to generate a GUID since Linux has
// no native GUID generator. This should be OK since we don't expect
// crash to happen very offen.
//
class GUIDGenerator {
 public:
  static u_int32_t BytesToUInt32(const u_int8_t bytes[]) {
    return ((u_int32_t) bytes[0]
            | ((u_int32_t) bytes[1] << 8)
            | ((u_int32_t) bytes[2] << 16)
            | ((u_int32_t) bytes[3] << 24));
  }

  static void UInt32ToBytes(u_int8_t bytes[], u_int32_t n) {
    bytes[0] = n & 0xff;
    bytes[1] = (n >> 8) & 0xff;
    bytes[2] = (n >> 16) & 0xff;
    bytes[3] = (n >> 24) & 0xff;
  }

  static bool CreateGUID(GUID *guid) {
    InitOnce();
    guid->data1 = random();
    guid->data2 = (u_int16_t)(random());
    guid->data3 = (u_int16_t)(random());
    UInt32ToBytes(&guid->data4[0], random());
    UInt32ToBytes(&guid->data4[4], random());
    return true;
  }

 private:
  static void InitOnce() {
    pthread_once(&once_control, &InitOnceImpl);
  }

  static void InitOnceImpl() {
    srandom(time(NULL));
  }

  static pthread_once_t once_control;
};

pthread_once_t GUIDGenerator::once_control = PTHREAD_ONCE_INIT;

bool CreateGUID(GUID *guid) {
  return GUIDGenerator::CreateGUID(guid);
}

// Parse guid to string.
bool GUIDToString(const GUID *guid, char *buf, int buf_len) {
  // Should allow more space the the max length of GUID.
  assert(buf_len > kGUIDStringLength);
  int num = snprintf(buf, buf_len, kGUIDFormatString,
                     guid->data1, guid->data2, guid->data3,
                     GUIDGenerator::BytesToUInt32(&(guid->data4[0])),
                     GUIDGenerator::BytesToUInt32(&(guid->data4[4])));
  if (num != kGUIDStringLength)
    return false;

  buf[num] = '\0';
  return true;
}
<|endoftext|>