<commit_before>/****************************************************************************\
**
**  tInputFile.cpp: Member functions for tInput objects.
**
**  Greg Tucker, November 1997
**
**  $Id: tInputFile.cpp,v 1.5 1999-02-22 17:40:12 gtucker Exp $
\****************************************************************************/

#include <iostream.h>
#include <fstream.h>
#include <assert.h>
#include <string.h>
#include "../Definitions.h"
#include "../Classes.h"
#include "tInputFile.h"
#include "../errors/errors.h"

/*
**  Constructors
*/

tInputFile::tInputFile( const char *filename )
{
   char inoutname[kMaxNameLength];

   infile.open( filename );
   if( !infile.good() )
   {
      cerr << "tInputFile::tInputFile: Unable to open '" << filename << "'." << endl;
      ReportFatalError( "The file may not exist or may be mis-named." );
   }
   strcpy( inoutname, filename );
   strcat( inoutname, "INPUTS" );
   inoutfile.open( inoutname );
   assert( inoutfile.good() );

}

/*
**  ReadItem
**
**  Reads one parameter from the file. The format is assumed to be a line
**  of text that begins with the code "code", followed by a line containing
**  the parameter to be read. The function is overloaded according to the
**  type of data desired (datType simply governs which overloaded function
**  will be called; it is not used by the routines).
**
**  IMPORTANT:
**  revised to allow arbitrary ordering of items in infile and/or ReadItem
**  calls in code; routine searches
**  through list until it either finds the right itemCode or reaches EOF.
**  This should make reading/entering parameters MUCH less complicated.
**  -12/23/97 SL
*/
int tInputFile::ReadItem( const int &datType, const char *itemCode )
{
   //cout << "ReadItem( int )...";
   int item;
   char headerLine[kMaxNameLength];

   assert( infile.good() );

   // NB: Should check for eof on reading each line

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );
   if( !( infile.eof() ) )
   {
      infile >> item;
      infile.ignore( 1, '\n' );
      inoutfile << itemCode << endl << item << endl;
   }
   else

       //if( strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 )
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   //infile.seekg( original );
   infile.seekg( 0, ios::beg );
   return item;
}

long tInputFile::ReadItem( const long &datType, const char *itemCode )
{
   //cout << "ReadItem( long )...";
   long item;
   char headerLine[kMaxNameLength];

   assert( infile.good() );

     // NB: Should check for eof on reading each line

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );
   if( !( infile.eof() ) )
   {
      infile >> item;
      infile.ignore( 1, '\n' );
      inoutfile << itemCode << endl << item << endl;
   }
   else
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   infile.seekg( 0, ios::beg );
   return item;
}

double tInputFile::ReadItem( const double &datType, const char *itemCode )
{
   //cout << "ReadItem( double )...";
   double item;
   char headerLine[kMaxNameLength];

   assert( infile.good() );

     // NB: Should check for eof on reading each line

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );
   if( !( infile.eof() ) )
   {
      infile >> item;
      infile.ignore( 1, '\n' );
      inoutfile << itemCode << endl << item << endl;
   }
   else
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   infile.seekg( 0, ios::beg );
   return item;
}

void tInputFile::ReadItem(  char * theString, const char *itemCode )
{
   //cout << "ReadItem( char )...";
   char headerLine[kMaxNameLength];

   assert( infile.good() );

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );

   if( !( infile.eof() ) )
   {
      infile.getline( theString, kMaxNameLength );
      inoutfile << itemCode << endl << theString << endl;
   }
   else
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   infile.seekg( 0, ios::beg );
}
<commit_msg>GT changed the constructor so that it now reads "OUTFILENAME" from the file, and creates a log file for inputs called <filename>.inputs. Also added inline doc.<commit_after>/****************************************************************************\
**
**  tInputFile.cpp: Member functions for class tInputFile.
**
**  (see tInputFile.h for a description of this class)
**
**  Greg Tucker, November 1997
**
**  $Id: tInputFile.cpp,v 1.6 1999-03-31 21:24:04 gtucker Exp $
\****************************************************************************/

#include <iostream.h>
#include <fstream.h>
#include <assert.h>
#include <string.h>
//#include "../Definitions.h"
//#include "../Classes.h"
#include "tInputFile.h"
#include "../errors/errors.h"

/****************************************************************************\
**
**  tInputFile Constructor
**
**  Looks for a file called filename, opens it if found or generates an
**  error if not. Then reads the base name for output files and creates
**  a file called <filename>.inputs which will act as a log file for
**  parameters read. (This is often useful when the original input file
**  gets lost or modified).
**
\****************************************************************************/
tInputFile::tInputFile( const char *filename )
{
   char inoutname[kMaxNameLength];

   // Open file
   infile.open( filename );
   if( !infile.good() )
   {
      cerr << "tInputFile::tInputFile: Unable to open '" << filename << "'." << endl;
      ReportFatalError( "The file may not exist or may be mis-named." );
   }

   // Create log file for inputs
   ReadItem( inoutname, "OUTFILENAME" );
   strcat( inoutname, ".inputs" );
   inoutfile.open( inoutname );
   assert( inoutfile.good() );

}

/****************************************************************************\
**
**  tInputFile::ReadItem
**
**  Reads one parameter from the file. The format is assumed to be a line
**  of text that begins with the code "itemCode", followed by a line containing
**  the parameter to be read. The function is overloaded according to the
**  type of data desired (datType simply governs which overloaded function
**  will be called; it is not used by the routines).
**
**  Inputs:  datType -- dummy variable indicating the data type to be read
**                      (in the case of the string version, the string read
**                      is placed here)
**           itemCode -- string that describes the parameter to be read
**  Returns:  the item read (except in the case of the string version)
**  Modifications:
**    - revised to allow arbitrary ordering of items in infile and/or
**      ReadItem calls in code; routine searches through
**      list until it either finds the right itemCode or reaches EOF.
**      12/23/97 SL
**
\****************************************************************************/
int tInputFile::ReadItem( const int &datType, const char *itemCode )
{
   //cout << "ReadItem( int )...";
   int item;
   char headerLine[kMaxNameLength];

   assert( infile.good() );

   // NB: Should check for eof on reading each line

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );
   if( !( infile.eof() ) )
   {
      infile >> item;
      infile.ignore( 1, '\n' );
      inoutfile << itemCode << endl << item << endl;
   }
   else

       //if( strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 )
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   //infile.seekg( original );
   infile.seekg( 0, ios::beg );
   return item;
}

long tInputFile::ReadItem( const long &datType, const char *itemCode )
{
   //cout << "ReadItem( long )...";
   long item;
   char headerLine[kMaxNameLength];

   assert( infile.good() );

     // NB: Should check for eof on reading each line

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );
   if( !( infile.eof() ) )
   {
      infile >> item;
      infile.ignore( 1, '\n' );
      inoutfile << itemCode << endl << item << endl;
   }
   else
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   infile.seekg( 0, ios::beg );
   return item;
}

double tInputFile::ReadItem( const double &datType, const char *itemCode )
{
   //cout << "ReadItem( double )...";
   double item;
   char headerLine[kMaxNameLength];

   assert( infile.good() );

     // NB: Should check for eof on reading each line

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );
   if( !( infile.eof() ) )
   {
      infile >> item;
      infile.ignore( 1, '\n' );
      inoutfile << itemCode << endl << item << endl;
   }
   else
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   infile.seekg( 0, ios::beg );
   return item;
}

void tInputFile::ReadItem(  char * theString, const char *itemCode )
{
   //cout << "ReadItem( char )...";
   char headerLine[kMaxNameLength];

   assert( infile.good() );

   infile.getline( headerLine, kMaxNameLength );
   while( !( infile.eof() ) &&
          ( headerLine[0]==kCommentMark ||
            strncmp( itemCode, headerLine, strlen( itemCode ) )!=0 ) )
       infile.getline( headerLine, kMaxNameLength );

   if( !( infile.eof() ) )
   {
      infile.getline( theString, kMaxNameLength );
      inoutfile << itemCode << endl << theString << endl;
   }
   else
   {
      cerr << "I expected to read the parameter '" << itemCode
           << "', but reached EOF first" << endl;
      ReportFatalError( "Missing parameter in input file" );
   }
   infile.seekg( 0, ios::beg );
}
<|endoftext|>


<commit_before>// Copyright 2021 The Pigweed Authors
//
// Licensed under the Apache License, Version 2.0 (the "License"); you may not
// use this file except in compliance with the License. You may obtain a copy of
// the License at
//
//     https://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 "pw_thread_embos/util.h"

#include "RTOS.h"
#include "pw_function/function.h"
#include "pw_status/status.h"

namespace pw::thread::embos {

namespace internal {

// Iterates through all threads that haven't been deleted, calling the provided
// callback.
Status ForEachThread(const OS_TASK& starting_thread, ThreadCallback& cb) {
  if (!OS_IsRunning()) {
    return Status::FailedPrecondition();
  }

  const OS_TASK* thread = &starting_thread;
  while (thread != nullptr) {
    if (!cb(*thread)) {
      // Early-terminate iteration if requested by the callback.
      return Status::Aborted();
    }
    thread = thread->pNext;
  }

  return OkStatus();
}

}  // namespace internal

Status ForEachThread(ThreadCallback& cb) {
  return internal::ForEachThread(*OS_GetpCurrentTask(), cb);
}

}  // namespace pw::thread::embos
<commit_msg>pw_thread_embos: Update thread iteration start<commit_after>// Copyright 2021 The Pigweed Authors
//
// Licensed under the Apache License, Version 2.0 (the "License"); you may not
// use this file except in compliance with the License. You may obtain a copy of
// the License at
//
//     https://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 "pw_thread_embos/util.h"

#include "RTOS.h"
#include "pw_function/function.h"
#include "pw_status/status.h"

namespace pw::thread::embos {

namespace internal {

// Iterates through all threads that haven't been deleted, calling the provided
// callback.
Status ForEachThread(const OS_TASK& starting_thread, ThreadCallback& cb) {
  if (!OS_IsRunning()) {
    return Status::FailedPrecondition();
  }

  const OS_TASK* thread = &starting_thread;
  while (thread != nullptr) {
    if (!cb(*thread)) {
      // Early-terminate iteration if requested by the callback.
      return Status::Aborted();
    }
    thread = thread->pNext;
  }

  return OkStatus();
}

}  // namespace internal

Status ForEachThread(ThreadCallback& cb) {
  return internal::ForEachThread(*OS_Global.pTask, cb);
}

}  // namespace pw::thread::embos
<|endoftext|>


<commit_before><commit_msg>Switch to STL sort algorithm on MSVC. Feature #3376826.<commit_after><|endoftext|>


<commit_before>/*ckwg +5
 * Copyright 2011 by Kitware, Inc. All Rights Reserved. Please refer to
 * KITWARE_LICENSE.TXT for licensing information, or contact General Counsel,
 * Kitware, Inc., 28 Corporate Drive, Clifton Park, NY 12065.
 */

#include <python/helpers/python_wrap_const_shared_ptr.h>
#include <python/helpers/python_convert_any.h>

#include <vistk/pipeline/datum.h>

#include <boost/any.hpp>
#include <boost/python.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

/**
 * \file datum.cxx
 *
 * \brief Python bindings for \link vistk::datum\endlink.
 */

using namespace boost::python;

static void translator(vistk::datum_exception const& e);

static vistk::datum_t new_datum(object const& obj);

BOOST_PYTHON_MODULE(datum)
{
  register_exception_translator<
    vistk::datum_exception>(translator);

  enum_<vistk::datum::type_t>("DatumType"
    , "A type for a datum packet.")
    .value("invalid", vistk::datum::invalid)
    .value("data", vistk::datum::data)
    .value("empty", vistk::datum::empty)
    .value("complete", vistk::datum::complete)
    .value("error", vistk::datum::error)
  ;

  class_<vistk::datum::error_t>("DatumError"
    , "The type of an error message.");

  def("new", &new_datum
    , (arg("dat"))
    , "Creates a new datum packet.");
  def("empty", &vistk::datum::empty_datum
    , "Creates an empty datum packet.");
  def("complete", &vistk::datum::complete_datum
    , "Creates a complete marker datum packet.");
  def("error", &vistk::datum::error_datum
    , (arg("err"))
    , "Creates an error datum packet.");

  class_<vistk::datum, vistk::datum_t, boost::noncopyable>("Datum"
    , "A packet of data within the pipeline."
    , no_init)
    .def("type", &vistk::datum::type
      , "The type of the datum packet.")
    .def("get_error", &vistk::datum::get_error
      , "The error contained within the datum packet.")
    .def("get_datum", &vistk::datum::get_datum<boost::any>
      , "Get the data contained within the packet.")
  ;

  implicitly_convertible<boost::shared_ptr<vistk::datum>, vistk::datum_t>();
  to_python_converter<boost::any, boost_any_to_object>();

  boost_any_to_object();
}

void
translator(vistk::datum_exception const& e)
{
  PyErr_SetString(PyExc_RuntimeError, e.what());
}

vistk::datum_t
new_datum(object const& obj)
{
  return vistk::datum::new_datum<boost::any>(extract<boost::any>(obj));
}
<commit_msg>Allow implicit conversions between any and objects<commit_after>/*ckwg +5
 * Copyright 2011 by Kitware, Inc. All Rights Reserved. Please refer to
 * KITWARE_LICENSE.TXT for licensing information, or contact General Counsel,
 * Kitware, Inc., 28 Corporate Drive, Clifton Park, NY 12065.
 */

#include <python/helpers/python_wrap_const_shared_ptr.h>
#include <python/helpers/python_convert_any.h>

#include <vistk/pipeline/datum.h>

#include <boost/any.hpp>
#include <boost/python.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

/**
 * \file datum.cxx
 *
 * \brief Python bindings for \link vistk::datum\endlink.
 */

using namespace boost::python;

static void translator(vistk::datum_exception const& e);

static vistk::datum_t new_datum(object const& obj);

BOOST_PYTHON_MODULE(datum)
{
  register_exception_translator<
    vistk::datum_exception>(translator);

  enum_<vistk::datum::type_t>("DatumType"
    , "A type for a datum packet.")
    .value("invalid", vistk::datum::invalid)
    .value("data", vistk::datum::data)
    .value("empty", vistk::datum::empty)
    .value("complete", vistk::datum::complete)
    .value("error", vistk::datum::error)
  ;

  class_<vistk::datum::error_t>("DatumError"
    , "The type of an error message.");

  def("new", &new_datum
    , (arg("dat"))
    , "Creates a new datum packet.");
  def("empty", &vistk::datum::empty_datum
    , "Creates an empty datum packet.");
  def("complete", &vistk::datum::complete_datum
    , "Creates a complete marker datum packet.");
  def("error", &vistk::datum::error_datum
    , (arg("err"))
    , "Creates an error datum packet.");

  class_<vistk::datum, vistk::datum_t, boost::noncopyable>("Datum"
    , "A packet of data within the pipeline."
    , no_init)
    .def("type", &vistk::datum::type
      , "The type of the datum packet.")
    .def("get_error", &vistk::datum::get_error
      , "The error contained within the datum packet.")
    .def("get_datum", &vistk::datum::get_datum<boost::any>
      , "Get the data contained within the packet.")
  ;

  implicitly_convertible<boost::shared_ptr<vistk::datum>, vistk::datum_t>();

  to_python_converter<boost::any, boost_any_to_object>();
  boost_any_to_object();

  implicitly_convertible<boost::any, object>();
  implicitly_convertible<object, boost::any>();
}

void
translator(vistk::datum_exception const& e)
{
  PyErr_SetString(PyExc_RuntimeError, e.what());
}

vistk::datum_t
new_datum(object const& obj)
{
  return vistk::datum::new_datum<boost::any>(extract<boost::any>(obj));
}
<|endoftext|>


<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Radu Serban, Justin Madsen
// =============================================================================
//
// Front and Rear HMMWV suspension subsystems (reduced double A-arm).
//
// These concrete suspension subsystems are defined with respect to right-handed
// frames with X pointing towards the front, Y to the left, and Z up (as imposed
// by the base class ChDoubleWishboneReduced) and origins at the midpoint
// between the lower control arms' connection points to the chassis.
//
// All point locations are provided for the left half of the supspension.
//
// =============================================================================

#include "models/hmmwv/suspension/HMMWV_DoubleWishboneReduced.h"

using namespace chrono;

namespace hmmwv {

// -----------------------------------------------------------------------------
// Static variables
// -----------------------------------------------------------------------------

static const double in2m = 0.0254;
static const double lb2kg = 0.453592;
static const double lbf2N = 4.44822162;
static const double lbfpin2Npm = 175.12677;

// entire wheel assembly = 195 lbs, includes upright, spindle and tire.
// HMMWV tires run ~ 100 lbs, so the spindle and upright should be ~ 95 lbs combined
const double     HMMWV_DoubleWishboneReducedFront::m_uprightMass = lb2kg * 60.0;
const double     HMMWV_DoubleWishboneReducedFront::m_spindleMass = lb2kg * 35.0;

const double     HMMWV_DoubleWishboneReducedFront::m_spindleRadius = 0.15;
const double     HMMWV_DoubleWishboneReducedFront::m_spindleWidth  = 0.06;
const double     HMMWV_DoubleWishboneReducedFront::m_uprightRadius = 0.02;

const ChVector<> HMMWV_DoubleWishboneReducedFront::m_spindleInertia(1, 1, 1);
const ChVector<> HMMWV_DoubleWishboneReducedFront::m_uprightInertia(5, 5, 5);

const double     HMMWV_DoubleWishboneReducedFront::m_axleInertia = 0.4;

const double     HMMWV_DoubleWishboneReducedFront::m_springCoefficient  = lbfpin2Npm * 954;
const double     HMMWV_DoubleWishboneReducedFront::m_dampingCoefficient = lbfpin2Npm * 128.25;
const double     HMMWV_DoubleWishboneReducedFront::m_springRestLength   = in2m * 13.36;

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

const double     HMMWV_DoubleWishboneReducedRear::m_uprightMass = lb2kg * 60.0;
const double     HMMWV_DoubleWishboneReducedRear::m_spindleMass = lb2kg * 35.0;

const double     HMMWV_DoubleWishboneReducedRear::m_spindleRadius = 0.15;
const double     HMMWV_DoubleWishboneReducedRear::m_spindleWidth  = 0.06;
const double     HMMWV_DoubleWishboneReducedRear::m_uprightRadius = 0.02;

const ChVector<> HMMWV_DoubleWishboneReducedRear::m_spindleInertia(1, 1, 1);
const ChVector<> HMMWV_DoubleWishboneReducedRear::m_uprightInertia(5, 5, 5);

const double     HMMWV_DoubleWishboneReducedRear::m_axleInertia = 0.4;

const double     HMMWV_DoubleWishboneReducedRear::m_springCoefficient  = lbfpin2Npm * 2108;
const double     HMMWV_DoubleWishboneReducedRear::m_dampingCoefficient = lbfpin2Npm * 200.00;
const double     HMMWV_DoubleWishboneReducedRear::m_springRestLength   = in2m * 15.03;

// -----------------------------------------------------------------------------
// Constructors
// -----------------------------------------------------------------------------
HMMWV_DoubleWishboneReducedFront::HMMWV_DoubleWishboneReducedFront(const std::string& name,
                                                                   bool               driven)
: ChDoubleWishboneReduced(name, true, driven)
{
}

HMMWV_DoubleWishboneReducedRear::HMMWV_DoubleWishboneReducedRear(const std::string& name,
                                                                 bool               driven)
: ChDoubleWishboneReduced(name, false, driven)
{
}

// -----------------------------------------------------------------------------
// Implementations of the getLocation() virtual methods.
// -----------------------------------------------------------------------------

const ChVector<> HMMWV_DoubleWishboneReducedFront::getLocation(PointId which)
{
  switch (which) {
  case SPINDLE:  return in2m * ChVector<>(-1.59, 35.815, -1.0350);
  case UPRIGHT:  return in2m * ChVector<>(-1.59, 31.81, -1.0350);
  case UCA_F:    return in2m * ChVector<>(-1.89, 17.55, 9.63);
  case UCA_B:    return in2m * ChVector<>(-10.56, 18.81, 7.69);
  case UCA_U:    return in2m * ChVector<>(-2.09, 28.16, 8.48);
  case LCA_F:    return in2m * ChVector<>(8.79, 12.09, 0);
  case LCA_B:    return in2m * ChVector<>(-8.79, 12.09, 0);
  case LCA_U:    return in2m * ChVector<>(-1.40, 30.96, -4.65);
  case SHOCK_C:  return in2m * ChVector<>(4.10, 27.86, 12.72);
  case SHOCK_U:  return in2m * ChVector<>(3.83, 30.96, -1.52);
  case TIEROD_C: return in2m * ChVector<>(-13.39, 9.8, -1.0350);
  case TIEROD_U: return in2m * ChVector<>(-6.92, 32.31, -1.0350);
  default:       return ChVector<>(0, 0, 0);
  }
}

const ChVector<> HMMWV_DoubleWishboneReducedRear::getLocation(PointId which)
{
  switch (which) {
  case SPINDLE:  return in2m * ChVector<>(1.40, 35.815, -1.035);
  case UPRIGHT:  return in2m * ChVector<>(1.40, 31.81, -1.035);
  case UCA_F:    return in2m * ChVector<>(13.78, 18.19, 8.88);
  case UCA_B:    return in2m * ChVector<>(3.07, 18.19, 8.88);
  case UCA_U:    return in2m * ChVector<>(1.40, 28.16, 8.50);
  case LCA_F:    return in2m * ChVector<>(8.79, 12.09, 0);
  case LCA_B:    return in2m * ChVector<>(-8.79, 12.09, 0);
  case LCA_U:    return in2m * ChVector<>(1.40, 30.96, -4.65);
  case SHOCK_C:  return in2m * ChVector<>(-4.09, 28.19, 12.72);
  case SHOCK_U:  return in2m * ChVector<>(-4.09, 30.96, -1.51);
  case TIEROD_C: return in2m * ChVector<>(12.70, 16.37, -0.37);
  case TIEROD_U: return in2m * ChVector<>(6.70, 32.32, -0.37);
  default:       return ChVector<>(0, 0, 0);
  }
}

} // end namespace hmmwv
<commit_msg>Modify tierod attachment points to match the full double wishbone suspension.<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Radu Serban, Justin Madsen
// =============================================================================
//
// Front and Rear HMMWV suspension subsystems (reduced double A-arm).
//
// These concrete suspension subsystems are defined with respect to right-handed
// frames with X pointing towards the front, Y to the left, and Z up (as imposed
// by the base class ChDoubleWishboneReduced) and origins at the midpoint
// between the lower control arms' connection points to the chassis.
//
// All point locations are provided for the left half of the supspension.
//
// =============================================================================

#include "models/hmmwv/suspension/HMMWV_DoubleWishboneReduced.h"

using namespace chrono;

namespace hmmwv {

// -----------------------------------------------------------------------------
// Static variables
// -----------------------------------------------------------------------------

static const double in2m = 0.0254;
static const double lb2kg = 0.453592;
static const double lbf2N = 4.44822162;
static const double lbfpin2Npm = 175.12677;

// entire wheel assembly = 195 lbs, includes upright, spindle and tire.
// HMMWV tires run ~ 100 lbs, so the spindle and upright should be ~ 95 lbs combined
const double     HMMWV_DoubleWishboneReducedFront::m_uprightMass = lb2kg * 60.0;
const double     HMMWV_DoubleWishboneReducedFront::m_spindleMass = lb2kg * 35.0;

const double     HMMWV_DoubleWishboneReducedFront::m_spindleRadius = 0.15;
const double     HMMWV_DoubleWishboneReducedFront::m_spindleWidth  = 0.06;
const double     HMMWV_DoubleWishboneReducedFront::m_uprightRadius = 0.02;

const ChVector<> HMMWV_DoubleWishboneReducedFront::m_spindleInertia(1, 1, 1);
const ChVector<> HMMWV_DoubleWishboneReducedFront::m_uprightInertia(5, 5, 5);

const double     HMMWV_DoubleWishboneReducedFront::m_axleInertia = 0.4;

const double     HMMWV_DoubleWishboneReducedFront::m_springCoefficient  = lbfpin2Npm * 954;
const double     HMMWV_DoubleWishboneReducedFront::m_dampingCoefficient = lbfpin2Npm * 128.25;
const double     HMMWV_DoubleWishboneReducedFront::m_springRestLength   = in2m * 13.36;

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

const double     HMMWV_DoubleWishboneReducedRear::m_uprightMass = lb2kg * 60.0;
const double     HMMWV_DoubleWishboneReducedRear::m_spindleMass = lb2kg * 35.0;

const double     HMMWV_DoubleWishboneReducedRear::m_spindleRadius = 0.15;
const double     HMMWV_DoubleWishboneReducedRear::m_spindleWidth  = 0.06;
const double     HMMWV_DoubleWishboneReducedRear::m_uprightRadius = 0.02;

const ChVector<> HMMWV_DoubleWishboneReducedRear::m_spindleInertia(1, 1, 1);
const ChVector<> HMMWV_DoubleWishboneReducedRear::m_uprightInertia(5, 5, 5);

const double     HMMWV_DoubleWishboneReducedRear::m_axleInertia = 0.4;

const double     HMMWV_DoubleWishboneReducedRear::m_springCoefficient  = lbfpin2Npm * 2108;
const double     HMMWV_DoubleWishboneReducedRear::m_dampingCoefficient = lbfpin2Npm * 200.00;
const double     HMMWV_DoubleWishboneReducedRear::m_springRestLength   = in2m * 15.03;

// -----------------------------------------------------------------------------
// Constructors
// -----------------------------------------------------------------------------
HMMWV_DoubleWishboneReducedFront::HMMWV_DoubleWishboneReducedFront(const std::string& name,
                                                                   bool               driven)
: ChDoubleWishboneReduced(name, true, driven)
{
}

HMMWV_DoubleWishboneReducedRear::HMMWV_DoubleWishboneReducedRear(const std::string& name,
                                                                 bool               driven)
: ChDoubleWishboneReduced(name, false, driven)
{
}

// -----------------------------------------------------------------------------
// Implementations of the getLocation() virtual methods.
// -----------------------------------------------------------------------------

const ChVector<> HMMWV_DoubleWishboneReducedFront::getLocation(PointId which)
{
  switch (which) {
  case SPINDLE:  return in2m * ChVector<>(-1.59, 35.815, -1.0350);
  case UPRIGHT:  return in2m * ChVector<>(-1.59, 31.81, -1.0350);
  case UCA_F:    return in2m * ChVector<>(-1.89, 17.55, 9.63);
  case UCA_B:    return in2m * ChVector<>(-10.56, 18.81, 7.69);
  case UCA_U:    return in2m * ChVector<>(-2.09, 28.16, 8.48);
  case LCA_F:    return in2m * ChVector<>(8.79, 12.09, 0);
  case LCA_B:    return in2m * ChVector<>(-8.79, 12.09, 0);
  case LCA_U:    return in2m * ChVector<>(-1.40, 30.96, -4.65);
  case SHOCK_C:  return in2m * ChVector<>(4.10, 27.86, 12.72);
  case SHOCK_U:  return in2m * ChVector<>(3.83, 30.96, -1.52);
  case TIEROD_C: return in2m * ChVector<>(-9.855, 17.655, 2.135);
  case TIEROD_U: return in2m * ChVector<>(-6.922, 32.327, -0.643);
  default:       return ChVector<>(0, 0, 0);
  }
}

const ChVector<> HMMWV_DoubleWishboneReducedRear::getLocation(PointId which)
{
  switch (which) {
  case SPINDLE:  return in2m * ChVector<>(1.40, 35.815, -1.035);
  case UPRIGHT:  return in2m * ChVector<>(1.40, 31.81, -1.035);
  case UCA_F:    return in2m * ChVector<>(13.78, 18.19, 8.88);
  case UCA_B:    return in2m * ChVector<>(3.07, 18.19, 8.88);
  case UCA_U:    return in2m * ChVector<>(1.40, 28.16, 8.50);
  case LCA_F:    return in2m * ChVector<>(8.79, 12.09, 0);
  case LCA_B:    return in2m * ChVector<>(-8.79, 12.09, 0);
  case LCA_U:    return in2m * ChVector<>(1.40, 30.96, -4.65);
  case SHOCK_C:  return in2m * ChVector<>(-4.09, 28.19, 12.72);
  case SHOCK_U:  return in2m * ChVector<>(-4.09, 30.96, -1.51);
  case TIEROD_C: return in2m * ChVector<>(8.790, 16.38, 2.310);
  case TIEROD_U: return in2m * ChVector<>(6.704, 32.327, -0.365);
  default:       return ChVector<>(0, 0, 0);
  }
}

} // end namespace hmmwv
<|endoftext|>


<commit_before>#include <Poco/Logger.h>
#include <Poco/URI.h>
#include <Poco/Net/NetException.h>
#include <Poco/Net/HTTPRequest.h>
#include <Poco/Net/HTTPSClientSession.h>
#include <Poco/JSON/Parser.h>
#include <Poco/JSON/Object.h>

#include "Debug.h"
#include "di/Injectable.h"
#include "ssl/SSLClient.h"
#include "provider/GoogleAuthProvider.h"

BEEEON_OBJECT_BEGIN(BeeeOn, GoogleAuthProvider)
BEEEON_OBJECT_CASTABLE(AuthProvider)
BEEEON_OBJECT_CASTABLE(AuthCodeAuthProvider)
BEEEON_OBJECT_CASTABLE(OAuth2AuthProvider)
BEEEON_OBJECT_TEXT("client_id", &GoogleAuthProvider::setClientId)
BEEEON_OBJECT_TEXT("client_secret", &GoogleAuthProvider::setClientSecret)
BEEEON_OBJECT_TEXT("redirect_uri", &GoogleAuthProvider::setRedirectURI)
BEEEON_OBJECT_REF("sslConfig", &GoogleAuthProvider::setSSLConfig)
BEEEON_OBJECT_END(BeeeOn, GoogleAuthProvider)

using namespace std;
using namespace Poco;
using namespace Poco::JSON;
using namespace Poco::Net;
using namespace BeeeOn;

bool GoogleAuthProvider::parseIdentity(const std::string &userInfo,
		const GoogleTokens &tokens,
		AuthResult &result)
{
	Parser parser;
	Object::Ptr info = parser.parse(userInfo).extract<Object::Ptr>();

	if (info->has("sub"))
		result.setProviderID(info->getValue<string>("sub"));

	if (info->has("email"))
		result.setEmail(info->getValue<string>("email"));

	if (result.email().empty() || result.providerID().empty())
		return false;

	if (info->has("given_name"))
		result.setFirstName(info->getValue<string>("given_name"));
	if (info->has("family_name"))
		result.setLastName(info->getValue<string>("family_name"));
	if (info->has("picture"))
		result.setPicture(info->getValue<string>("picture"));

	result.setAccessToken(tokens.accessToken);

	return true;
}

bool GoogleAuthProvider::verifyAuthCode(const string &authCode, AuthResult &info)
{
	GoogleTokens tokens;
	string rawInfo;
	string google_id;

	try {
		tokens = requestTokens(authCode);

		if (tokens.tokenType != "Bearer")
			throw NotAuthenticatedException("incompatible token type: " + tokens.tokenType);
		if (tokens.accessToken.empty())
			throw NotAuthenticatedException("missing access_token");
		if (tokens.expiresIn == 0)
			throw NotAuthenticatedException("token has expired");

		rawInfo = fetchUserInfo(tokens);
	} catch(const Exception &e) {
		logger().log(e, __FILE__, __LINE__);
		return false;
	}

	return parseIdentity(rawInfo, tokens, info);
}

GoogleAuthProvider::GoogleTokens GoogleAuthProvider::requestTokens(const string &authCode)
{
	TRACE_METHOD();

	SharedPtr<HTTPSClientSession> session;
	URI uri(m_tokenUrl);

	session = connectSecure(uri.getHost(), uri.getPort());

	string requestRaw = "code=" + authCode + "&"
		"redirect_uri=" + m_redirectURI + "&"
		"client_id=" + m_clientId + "&"
		"client_secret=" + m_clientSecret + "&"
		"scope=&"	// No need to specify, defaults to userinfo.profile,userinfo.email
		"grant_type=authorization_code";

	if (logger().debug())
		logger().debug("request: " + requestRaw, __FILE__, __LINE__);

	HTTPRequest req(HTTPRequest::HTTP_POST,
			uri.getPathAndQuery(),
			HTTPMessage::HTTP_1_1);
	req.setContentType("application/x-www-form-urlencoded");
	req.setContentLength(requestRaw.length());

	session->sendRequest(req) << requestRaw;
	string receiveResponse = handleResponse(*session);

	Parser parser;
	Object::Ptr object = parser.parse(receiveResponse)
			.extract<Object::Ptr>();

	GoogleTokens tokens;

	tokens.accessToken = object->optValue<string>("access_token", "");
	tokens.refreshToken = object->optValue<string>("refresh_token", "");
	tokens.expiresIn = Timespan(object->optValue<unsigned int>("expires_in", 0), 0);
	tokens.tokenType = object->optValue<string>("token_type", "");
	tokens.idToken = object->optValue<string>("id_token", "");

	return tokens;
}

string GoogleAuthProvider::fetchUserInfo(const GoogleTokens &tokens)
{
	TRACE_METHOD();

	if (tokens.idToken.empty())
		throw NotAuthenticatedException("missing id_token");

	URI uri(m_tokenInfoUrl + tokens.idToken);
	SharedPtr<HTTPSClientSession> session;

	session = connectSecure(uri.getHost(), uri.getPort());

	HTTPRequest req(HTTPRequest::HTTP_GET,
			uri.getPathAndQuery(),
			HTTPMessage::HTTP_1_1);

	session->sendRequest(req);
	return handleResponse(*session);
}
<commit_msg>GoogleAuthProvider: Simplify extracting data from JSON string<commit_after>#include <Poco/Logger.h>
#include <Poco/URI.h>
#include <Poco/Net/NetException.h>
#include <Poco/Net/HTTPRequest.h>
#include <Poco/Net/HTTPSClientSession.h>
#include <Poco/JSON/Parser.h>
#include <Poco/JSON/Object.h>

#include "Debug.h"
#include "di/Injectable.h"
#include "ssl/SSLClient.h"
#include "provider/GoogleAuthProvider.h"
#include "util/JsonUtil.h"

BEEEON_OBJECT_BEGIN(BeeeOn, GoogleAuthProvider)
BEEEON_OBJECT_CASTABLE(AuthProvider)
BEEEON_OBJECT_CASTABLE(AuthCodeAuthProvider)
BEEEON_OBJECT_CASTABLE(OAuth2AuthProvider)
BEEEON_OBJECT_TEXT("client_id", &GoogleAuthProvider::setClientId)
BEEEON_OBJECT_TEXT("client_secret", &GoogleAuthProvider::setClientSecret)
BEEEON_OBJECT_TEXT("redirect_uri", &GoogleAuthProvider::setRedirectURI)
BEEEON_OBJECT_REF("sslConfig", &GoogleAuthProvider::setSSLConfig)
BEEEON_OBJECT_END(BeeeOn, GoogleAuthProvider)

using namespace std;
using namespace Poco;
using namespace Poco::JSON;
using namespace Poco::Net;
using namespace BeeeOn;

bool GoogleAuthProvider::parseIdentity(const std::string &userInfo,
		const GoogleTokens &tokens,
		AuthResult &result)
{
	Object::Ptr info = JsonUtil::parse(userInfo);

	if (info->has("sub"))
		result.setProviderID(info->getValue<string>("sub"));

	if (info->has("email"))
		result.setEmail(info->getValue<string>("email"));

	if (result.email().empty() || result.providerID().empty())
		return false;

	if (info->has("given_name"))
		result.setFirstName(info->getValue<string>("given_name"));
	if (info->has("family_name"))
		result.setLastName(info->getValue<string>("family_name"));
	if (info->has("picture"))
		result.setPicture(info->getValue<string>("picture"));

	result.setAccessToken(tokens.accessToken);

	return true;
}

bool GoogleAuthProvider::verifyAuthCode(const string &authCode, AuthResult &info)
{
	GoogleTokens tokens;
	string rawInfo;
	string google_id;

	try {
		tokens = requestTokens(authCode);

		if (tokens.tokenType != "Bearer")
			throw NotAuthenticatedException("incompatible token type: " + tokens.tokenType);
		if (tokens.accessToken.empty())
			throw NotAuthenticatedException("missing access_token");
		if (tokens.expiresIn == 0)
			throw NotAuthenticatedException("token has expired");

		rawInfo = fetchUserInfo(tokens);
	} catch(const Exception &e) {
		logger().log(e, __FILE__, __LINE__);
		return false;
	}

	return parseIdentity(rawInfo, tokens, info);
}

GoogleAuthProvider::GoogleTokens GoogleAuthProvider::requestTokens(const string &authCode)
{
	TRACE_METHOD();

	SharedPtr<HTTPSClientSession> session;
	URI uri(m_tokenUrl);

	session = connectSecure(uri.getHost(), uri.getPort());

	string requestRaw = "code=" + authCode + "&"
		"redirect_uri=" + m_redirectURI + "&"
		"client_id=" + m_clientId + "&"
		"client_secret=" + m_clientSecret + "&"
		"scope=&"	// No need to specify, defaults to userinfo.profile,userinfo.email
		"grant_type=authorization_code";

	if (logger().debug())
		logger().debug("request: " + requestRaw, __FILE__, __LINE__);

	HTTPRequest req(HTTPRequest::HTTP_POST,
			uri.getPathAndQuery(),
			HTTPMessage::HTTP_1_1);
	req.setContentType("application/x-www-form-urlencoded");
	req.setContentLength(requestRaw.length());

	session->sendRequest(req) << requestRaw;
	string receiveResponse = handleResponse(*session);

	Object::Ptr object = JsonUtil::parse(receiveResponse);

	GoogleTokens tokens;

	tokens.accessToken = object->optValue<string>("access_token", "");
	tokens.refreshToken = object->optValue<string>("refresh_token", "");
	tokens.expiresIn = Timespan(object->optValue<unsigned int>("expires_in", 0), 0);
	tokens.tokenType = object->optValue<string>("token_type", "");
	tokens.idToken = object->optValue<string>("id_token", "");

	return tokens;
}

string GoogleAuthProvider::fetchUserInfo(const GoogleTokens &tokens)
{
	TRACE_METHOD();

	if (tokens.idToken.empty())
		throw NotAuthenticatedException("missing id_token");

	URI uri(m_tokenInfoUrl + tokens.idToken);
	SharedPtr<HTTPSClientSession> session;

	session = connectSecure(uri.getHost(), uri.getPort());

	HTTPRequest req(HTTPRequest::HTTP_GET,
			uri.getPathAndQuery(),
			HTTPMessage::HTTP_1_1);

	session->sendRequest(req);
	return handleResponse(*session);
}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 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/scenarios/side_pass/side_pass_stage.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::math::Vec2d;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

/*
 * @brief:
 * STAGE: SidePassApproachObstacle
 */
Stage::StageStatus SidePassApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  // check the status of side pass scenario
  const SLBoundary& adc_sl_boundary =
      frame->reference_line_info().front().AdcSlBoundary();
  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();

  bool has_blocking_obstacle = false;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
      continue;
    }
    CHECK(obstacle->IsStatic());

    if (obstacle->PerceptionSLBoundary().start_s() <=
        adc_sl_boundary.end_s()) {  // such vehicles are behind the ego car.
      continue;
    }
    constexpr double kAdcDistanceThreshold = 15.0;  // unit: m
    if (obstacle->PerceptionSLBoundary().start_s() >
        adc_sl_boundary.end_s() +
            kAdcDistanceThreshold) {  // vehicles are far away
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_l() > 1.0 ||
        obstacle->PerceptionSLBoundary().end_l() < -1.0) {
      continue;
    }
    has_blocking_obstacle = true;
  }
  if (!has_blocking_obstacle) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  // do path planning
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }

  double max_stop_velocity =
      GetContext()->scenario_config_.approach_obstacle_max_stop_speed();
  double min_stop_obstacle_distance =
      GetContext()->scenario_config_.approach_obstacle_min_stop_distance();

  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassGeneratePath
 */
Stage::StageStatus SidePassGeneratePath::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
    AERROR << "Fail to plan on reference_line.";
    return Stage::ERROR;
  }
  GetContext()->path_data_ = frame->reference_line_info().front().path_data();
  if (frame->reference_line_info().front().trajectory().NumOfPoints() > 0) {
    next_stage_ = ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassDetectSafety
 */

Stage::StageStatus SidePassDetectSafety::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
    return Stage::ERROR;
  }
  bool is_safe = true;
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    // TODO(All): check according to neighbor lane.
    if (obstacle->IsVirtual() && obstacle->Id().substr(0, 3) == "SP_" &&
        obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
      is_safe = false;
      break;
    }
  }
  if (is_safe) {
    next_stage_ = ScenarioConfig::SIDE_PASS_PASS_OBSTACLE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassPassObstacle
 */
Stage::StageStatus SidePassPassObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  // TODO(All):
  // Have to use DpPolyPathOptimizer to show in dreamview. Need change to
  // correct name.
  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Fail to plan on reference line.";
    return Stage::ERROR;
  }

  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  const auto& end_point =
      reference_line_info.path_data().discretized_path().EndPoint();
  Vec2d last_xy_point(end_point.x(), end_point.y());
  // get s of last point on path
  common::SLPoint sl_point;
  if (!reference_line_info.reference_line().XYToSL(last_xy_point, &sl_point)) {
    AERROR << "Fail to transfer cartesian point to frenet point.";
    return Stage::ERROR;
  }

  double distance_to_path_end =
      sl_point.s() - GetContext()->scenario_config_.side_pass_exit_distance();
  if (adc_sl_boundary.end_s() > distance_to_path_end) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>planning: fixed side pass approach ostacle stage<commit_after>/******************************************************************************
 * Copyright 2018 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/scenarios/side_pass/side_pass_stage.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::math::Vec2d;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

/*
 * @brief:
 * STAGE: SidePassApproachObstacle
 */
Stage::StageStatus SidePassApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  // check the status of side pass scenario
  const SLBoundary& adc_sl_boundary =
      frame->reference_line_info().front().AdcSlBoundary();
  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();

  bool has_blocking_obstacle = false;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
      continue;
    }
    CHECK(obstacle->IsStatic());
    if (obstacle->speed() >
        GetContext()->scenario_config_.block_obstacle_min_speed()) {
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_s() <=
        adc_sl_boundary.end_s()) {  // such vehicles are behind the ego car.
      continue;
    }
    constexpr double kAdcDistanceThreshold = 15.0;  // unit: m
    if (obstacle->PerceptionSLBoundary().start_s() >
        adc_sl_boundary.end_s() +
            kAdcDistanceThreshold) {  // vehicles are far away
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_l() > 1.0 ||
        obstacle->PerceptionSLBoundary().end_l() < -1.0) {
      continue;
    }
    has_blocking_obstacle = true;
  }
  if (!has_blocking_obstacle) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  // do path planning
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }

  double max_stop_velocity =
      GetContext()->scenario_config_.approach_obstacle_max_stop_speed();
  double min_stop_obstacle_distance =
      GetContext()->scenario_config_.approach_obstacle_min_stop_distance();

  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassGeneratePath
 */
Stage::StageStatus SidePassGeneratePath::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
    AERROR << "Fail to plan on reference_line.";
    return Stage::ERROR;
  }
  GetContext()->path_data_ = frame->reference_line_info().front().path_data();
  if (frame->reference_line_info().front().trajectory().NumOfPoints() > 0) {
    next_stage_ = ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassDetectSafety
 */

Stage::StageStatus SidePassDetectSafety::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
    return Stage::ERROR;
  }
  bool is_safe = true;
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    // TODO(All): check according to neighbor lane.
    if (obstacle->IsVirtual() && obstacle->Id().substr(0, 3) == "SP_" &&
        obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
      is_safe = false;
      break;
    }
  }
  if (is_safe) {
    next_stage_ = ScenarioConfig::SIDE_PASS_PASS_OBSTACLE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassPassObstacle
 */
Stage::StageStatus SidePassPassObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  // TODO(All):
  // Have to use DpPolyPathOptimizer to show in dreamview. Need change to
  // correct name.
  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Fail to plan on reference line.";
    return Stage::ERROR;
  }

  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  const auto& end_point =
      reference_line_info.path_data().discretized_path().EndPoint();
  Vec2d last_xy_point(end_point.x(), end_point.y());
  // get s of last point on path
  common::SLPoint sl_point;
  if (!reference_line_info.reference_line().XYToSL(last_xy_point, &sl_point)) {
    AERROR << "Fail to transfer cartesian point to frenet point.";
    return Stage::ERROR;
  }

  double distance_to_path_end =
      sl_point.s() - GetContext()->scenario_config_.side_pass_exit_distance();
  if (adc_sl_boundary.end_s() > distance_to_path_end) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

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


<commit_before>/*
 * Image.cpp
 *
 *  Created on: Sep 8, 2009
 *      Author: rasmussn
 */

#include "Image.hpp"
#include "../io/imageio.hpp";

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

namespace PV {

Image::Image(const char * name, HyPerCol * hc)
{
   initialize_base(name, hc);
}

Image::Image(const char * name, HyPerCol * hc, const char * filename)
{
   initialize_base(name, hc);

   // get size info from image so that data buffer can be allocated
   int status = getImageInfo(filename, comm, &loc);

   // create mpi_datatypes for border transfer
   mpi_datatypes = Communicator::newDatatypes(&loc);

   if (status) return;

   initialize_data(&loc);

   read(filename);

   // for now convert images to grayscale
   if (loc.nBands > 1) {
      this->toGrayScale();
   }

   // exchange border information
   exchange();
}

Image::~Image()
{
   free(name);

   if (data != NULL) {
      free(data);
      data = NULL;
   }
}

int Image::initialize_base(const char * name, HyPerCol * hc)
{
   this->name = strdup(name);
   this->data = NULL;
   this->comm = hc->icCommunicator();
   mpi_datatypes = NULL;

   PVParams * params = hc->parameters();

   loc = hc->getImageLoc();
   loc.nPad   = 0;
   loc.nBands = 1;

   if (params->present(name, "marginWidth")) {
      loc.nPad = (int) params->value(name, "marginWidth");
   }

   return 0;
}
/**
 * data lives in an extended frame of size
 * (nx+2*nPad)*(ny+2*nPad)*nBands
 */
int Image::initialize_data(const LayerLoc * imageLoc)
{
   const int N = (imageLoc->nx + 2*imageLoc->nPad) *
                 (imageLoc->ny + 2*imageLoc->nPad) * imageLoc->nBands;
   data = (pvdata_t *) calloc(sizeof(pvdata_t), N);
   assert(data != NULL);
   return 0;
}

pvdata_t * Image::getImageBuffer()
{
   return data;
}

LayerLoc Image::getImageLoc()
{
   return loc;
}

/**
 * update the image buffers
 *
 * return true if buffers have changed
 */
bool Image::updateImage(float time, float dt)
{
   // default is to do nothing for now
   // eventually could go through a list of images
   return false;
}

//! CLEAR IMAGE
/*!
 * this is Image specific.
 * NOTE:
 *      - Shall I modify this method to return a bool as
 * updateImage() does?
 *      - If I do so, I should also modify clearImage() in ImagrCreator().
 *      .
 */
int Image::clearImage()
{
   // default is to do nothing for now
   // it could, for example, set the data buffer to zero.

   return 0;
}

int Image::read(const char * filename)
{
   int status = 0;

   const int n = loc.nx * loc.ny * loc.nBands;
   unsigned char * buf = new unsigned char[n];
   assert(buf != NULL);

   // read the image and scatter the local portions
   status = scatterImageFile(filename, comm, &loc, buf);

   if (status == 0) {
      status = copyFromInteriorBuffer(buf);
   }
   delete buf;

   return status;
}

int Image::write(const char * filename)
{
   int status = 0;

   const int n = loc.nx * loc.ny * loc.nBands;
   unsigned char * buf = new unsigned char[n];
   assert(buf != NULL);

   status = copyToInteriorBuffer(buf);

   // gather the local portions and write the image
   status = gatherImageFile(filename, comm, &loc, buf);

   delete buf;

   return status;
}

int Image::exchange()
{
   return comm->exchange(data, mpi_datatypes, &loc);
}

int Image::copyToInteriorBuffer(unsigned char * buf)
{
   const int nx = loc.nx;
   const int ny = loc.ny;

   const int nxBorder = loc.nPad;
   const int nyBorder = loc.nPad;

   const int sy = nx + 2*nxBorder;
   const int sb = sy * (ny + 2*nyBorder);

   int ii = 0;
   for (int b = 0; b < loc.nBands; b++) {
      for (int j = 0; j < ny; j++) {
         int jex = j + nyBorder;
         for (int i = 0; i < nx; i++) {
            int iex = i + nxBorder;
            buf[ii++] = (unsigned char) data[iex + jex*sy + b*sb];
         }
      }
   }
   return 0;
}

int Image::copyFromInteriorBuffer(const unsigned char * buf)
{
   float * a = data;

   const int nx = loc.nx;
   const int ny = loc.ny;

   const int nxBorder = loc.nPad;
   const int nyBorder = loc.nPad;

   const int sy = nx + 2*nxBorder;
   const int sb = sy * (ny + 2*nyBorder);

   int ii = 0;
   for (int b = 0; b < loc.nBands; b++) {
      for (int j = 0; j < ny; j++) {
         int jex = j + nyBorder;
         for (int i = 0; i < nx; i++) {
            int iex = i + nxBorder;
            data[iex + jex*sy + b*sb] = (pvdata_t) buf[ii++];
         }
      }
   }
   return 0;
}

int Image::toGrayScale()
{
   const int nx_ex = loc.nx + 2*loc.nPad;
   const int ny_ex = loc.ny + 2*loc.nPad;

   const int numBands = loc.nBands;

   const int sx = 1;
   const int sy = nx_ex;
   const int sb = nx_ex * ny_ex;

   if (numBands < 2) return 0;

   for (int j = 0; j < ny_ex; j++) {
      for (int i = 0; i < nx_ex; i++) {
         float val = 0;
         for (int b = 0; b < numBands; b++) {
            float d = data[i*sx + j*sy + b*sb];
            val += d*d;
//            val += d;
         }
         // store the converted image in the first color band
         data[i*sx + j*sy + 0*sb] = sqrt(val/numBands);
//         data[i*sx + j*sy + 0*sb] = val/numBands;
      }
   }

   // turn off the color
   loc.nBands = 1;

   return 0;
}

int Image::convertToGrayScale(LayerLoc * loc, unsigned char * buf)
{
   const int nx = loc->nx;
   const int ny = loc->ny;

   const int numBands = loc->nBands;

   const int sx = 1;
   const int sy = nx;
   const int sb = nx * ny;

   if (numBands < 2) return 0;

   for (int j = 0; j < ny; j++) {
      for (int i = 0; i < nx; i++) {
         float val = 0;
         for (int b = 0; b < numBands; b++) {
            float d = buf[i*sx + j*sy + b*sb];
            val += d*d;
         }
         // store the converted image in the first color band
         buf[i*sx + j*sy + 0*sb] = sqrt(val/numBands);
      }
   }

   // turn off the color
   loc->nBands = 1;

   return 0;
}

int Image::convolve(int width)
{
   const int nx_ex = loc.nx + 2*loc.nPad;
   const int ny_ex = loc.ny + 2*loc.nPad;
   const int nb = loc.nBands;

   const int size_ex = nx_ex * ny_ex;

   // an image is different from normal layers as features (bands) vary last
   const int sx = 1;
   const int sy = nx_ex;
   const int sb = nx_ex * ny_ex;

   const int npx = width;
   const int npy = width;
   const int npx_2 = width/2;
   const int npy_2 = width/2;

   assert(npx <= loc.nPad);
   assert(npy <= loc.nPad);

   float * buf = new float[size_ex];
   //for (int i = 0; i < size_ex; i++) buf[i] = 0;

   float max = -1.0e9;
   float min = -max;

   // ignore image bands for now
   for (int jex = npy_2; jex < ny_ex - npy_2; jex++) {
      for (int iex = npx_2; iex < nx_ex - npx_2; iex++) {
         float av = 0;
         float sq = 0;
         for (int jp = 0; jp < npy; jp++) {
            for (int ip = 0; ip < npx; ip++) {
   //            int ix = i + ip - npx_2;
   //            int iy = j + jp - npy_2;
   //            float val = data[ix*sx + iy*sy];
   //            av += val;
   //            sq += val * val;
            }
         }
         av = av / (npx*npy);
         min = (av < min) ? av : min;
         max = (av > max) ? av : max;
//         sq  = sqrt( sq/(nPad*nPad) - av*av ) + tau;
//         buf[i*sx + j*sy] = data[i*sx + j*sy] + mid - av;
         buf[iex*sx + jex*sy] = .95*255 * (data[iex*sx + jex*sy] - .95*av) / sq;
      }
   }

   printf("min==%f max==%f\n", min, max);

   for (int k = 0; k < size_ex; k++) data[k] = buf[k];

   return 0;
}

} // namespace PV
<commit_msg>Added lastUpdateTime so that subscribers can find out the last time an Image has been updated and take appropriate action.<commit_after>/*
 * Image.cpp
 *
 *  Created on: Sep 8, 2009
 *      Author: rasmussn
 */

#include "Image.hpp"
#include "../io/imageio.hpp";

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

namespace PV {

Image::Image(const char * name, HyPerCol * hc)
{
   initialize_base(name, hc);
}

Image::Image(const char * name, HyPerCol * hc, const char * filename)
{
   initialize_base(name, hc);

   // get size info from image so that data buffer can be allocated
   int status = getImageInfo(filename, comm, &imageLoc);

   // create mpi_datatypes for border transfer
   mpi_datatypes = Communicator::newDatatypes(&loc);

   if (status) return;

   // need all image bands until converted to gray scale
   loc.nBands = imageLoc.nBands;

   initialize_data(&loc);

   read(filename);

   // for now convert images to grayscale
   if (loc.nBands > 1) {
      this->toGrayScale();
   }

   // exchange border information
   exchange();
}

Image::~Image()
{
   free(name);

   if (data != NULL) {
      free(data);
      data = NULL;
   }
}

int Image::initialize_base(const char * name, HyPerCol * hc)
{
   this->name = strdup(name);
   this->data = NULL;
   this->comm = hc->icCommunicator();
   this->lastUpdateTime = 0.0;

   mpi_datatypes = NULL;

   PVParams * params = hc->parameters();

   loc = hc->getImageLoc();
   loc.nPad   = 0;
   loc.nBands = 1;

   if (params->present(name, "marginWidth")) {
      loc.nPad = (int) params->value(name, "marginWidth");
   }

   return 0;
}
/**
 * data lives in an extended frame of size
 * (nx+2*nPad)*(ny+2*nPad)*nBands
 */
int Image::initialize_data(const LayerLoc * loc)
{
   // allocate storage for actual image
   //
//   int N = imageLoc.nx * imageLoc.ny * imageLoc.nBands;
//   imageData = (pvdata_t *) calloc(sizeof(pvdata_t), N);
//   assert(imageData != NULL);

   // allocate storage for layer data buffer
   //
   int N = (loc->nx + 2*loc->nPad) * (loc->ny + 2*loc->nPad) * loc->nBands;
   data = (pvdata_t *) calloc(sizeof(pvdata_t), N);
   assert(data != NULL);

   return 0;
}

pvdata_t * Image::getImageBuffer()
{
   return data;
}

LayerLoc Image::getImageLoc()
{
   return loc;
}

//pvdata_t * Image::getDataBuffer()
//{
//   return data;
//}

//LayerLoc Image::getDataLoc()
//{
//   return loc;
//}

/**
 * update the image buffers
 *
 * return true if buffers have changed
 */
bool Image::updateImage(float time, float dt)
{
   // default is to do nothing for now
   // eventually could go through a list of images
   return false;
}

//! CLEAR IMAGE
/*!
 * this is Image specific.
 * NOTE:
 *      - Shall I modify this method to return a bool as
 * updateImage() does?
 *      - If I do so, I should also modify clearImage() in ImagrCreator().
 *      .
 */
int Image::clearImage()
{
   // default is to do nothing for now
   // it could, for example, set the data buffer to zero.

   return 0;
}

int Image::read(const char * filename)
{
   int status = 0;

   const int n = loc.nx * loc.ny * loc.nBands;
   unsigned char * buf = new unsigned char[n];
   assert(buf != NULL);

   // read the image and scatter the local portions
   status = scatterImageFile(filename, comm, &loc, buf);

   if (status == 0) {
      status = copyFromInteriorBuffer(buf);
   }
   delete buf;

   return status;
}

int Image::write(const char * filename)
{
   int status = 0;

   const int n = loc.nx * loc.ny * loc.nBands;
   unsigned char * buf = new unsigned char[n];
   assert(buf != NULL);

   status = copyToInteriorBuffer(buf);

   // gather the local portions and write the image
   status = gatherImageFile(filename, comm, &loc, buf);

   delete buf;

   return status;
}

int Image::exchange()
{
   return comm->exchange(data, mpi_datatypes, &loc);
}

int Image::copyToInteriorBuffer(unsigned char * buf)
{
   const int nx = loc.nx;
   const int ny = loc.ny;

   const int nxBorder = loc.nPad;
   const int nyBorder = loc.nPad;

   const int sy = nx + 2*nxBorder;
   const int sb = sy * (ny + 2*nyBorder);

   int ii = 0;
   for (int b = 0; b < loc.nBands; b++) {
      for (int j = 0; j < ny; j++) {
         int jex = j + nyBorder;
         for (int i = 0; i < nx; i++) {
            int iex = i + nxBorder;
            buf[ii++] = (unsigned char) data[iex + jex*sy + b*sb];
         }
      }
   }
   return 0;
}

int Image::copyFromInteriorBuffer(const unsigned char * buf)
{
   float * a = data;

   const int nx = loc.nx;
   const int ny = loc.ny;

   const int nxBorder = loc.nPad;
   const int nyBorder = loc.nPad;

   const int sy = nx + 2*nxBorder;
   const int sb = sy * (ny + 2*nyBorder);

   int ii = 0;
   for (int b = 0; b < loc.nBands; b++) {
      for (int j = 0; j < ny; j++) {
         int jex = j + nyBorder;
         for (int i = 0; i < nx; i++) {
            int iex = i + nxBorder;
            data[iex + jex*sy + b*sb] = (pvdata_t) buf[ii++];
         }
      }
   }
   return 0;
}

int Image::toGrayScale()
{
   const int nx_ex = loc.nx + 2*loc.nPad;
   const int ny_ex = loc.ny + 2*loc.nPad;

   const int numBands = loc.nBands;

   const int sx = 1;
   const int sy = nx_ex;
   const int sb = nx_ex * ny_ex;

   if (numBands < 2) return 0;

   for (int j = 0; j < ny_ex; j++) {
      for (int i = 0; i < nx_ex; i++) {
         float val = 0;
         for (int b = 0; b < numBands; b++) {
            float d = data[i*sx + j*sy + b*sb];
            val += d*d;
//            val += d;
         }
         // store the converted image in the first color band
         data[i*sx + j*sy + 0*sb] = sqrt(val/numBands);
//         data[i*sx + j*sy + 0*sb] = val/numBands;
      }
   }

   // turn off the color
   loc.nBands = 1;

   return 0;
}

int Image::convertToGrayScale(LayerLoc * loc, unsigned char * buf)
{
   const int nx = loc->nx;
   const int ny = loc->ny;

   const int numBands = loc->nBands;

   const int sx = 1;
   const int sy = nx;
   const int sb = nx * ny;

   if (numBands < 2) return 0;

   for (int j = 0; j < ny; j++) {
      for (int i = 0; i < nx; i++) {
         float val = 0;
         for (int b = 0; b < numBands; b++) {
            float d = buf[i*sx + j*sy + b*sb];
            val += d*d;
         }
         // store the converted image in the first color band
         buf[i*sx + j*sy + 0*sb] = sqrt(val/numBands);
      }
   }

   // turn off the color
   loc->nBands = 1;

   return 0;
}

int Image::convolve(int width)
{
   const int nx_ex = loc.nx + 2*loc.nPad;
   const int ny_ex = loc.ny + 2*loc.nPad;
   const int nb = loc.nBands;

   const int size_ex = nx_ex * ny_ex;

   // an image is different from normal layers as features (bands) vary last
   const int sx = 1;
   const int sy = nx_ex;
   const int sb = nx_ex * ny_ex;

   const int npx = width;
   const int npy = width;
   const int npx_2 = width/2;
   const int npy_2 = width/2;

   assert(npx <= loc.nPad);
   assert(npy <= loc.nPad);

   float * buf = new float[size_ex];
   //for (int i = 0; i < size_ex; i++) buf[i] = 0;

   float max = -1.0e9;
   float min = -max;

   // ignore image bands for now
   for (int jex = npy_2; jex < ny_ex - npy_2; jex++) {
      for (int iex = npx_2; iex < nx_ex - npx_2; iex++) {
         float av = 0;
         float sq = 0;
         for (int jp = 0; jp < npy; jp++) {
            for (int ip = 0; ip < npx; ip++) {
   //            int ix = i + ip - npx_2;
   //            int iy = j + jp - npy_2;
   //            float val = data[ix*sx + iy*sy];
   //            av += val;
   //            sq += val * val;
            }
         }
         av = av / (npx*npy);
         min = (av < min) ? av : min;
         max = (av > max) ? av : max;
//         sq  = sqrt( sq/(nPad*nPad) - av*av ) + tau;
//         buf[i*sx + j*sy] = data[i*sx + j*sy] + mid - av;
         buf[iex*sx + jex*sy] = .95*255 * (data[iex*sx + jex*sy] - .95*av) / sq;
      }
   }

   printf("min==%f max==%f\n", min, max);

   for (int k = 0; k < size_ex; k++) data[k] = buf[k];

   return 0;
}

} // namespace PV
<|endoftext|>


<commit_before>#include "PythonParser.h"

PythonParser::PythonParser(std::shared_ptr<StorageInterface> storage,
                           std::shared_ptr<const std::vector<ColumnMeta> > metadatas) {
    this->metas = metadatas;
    this->parsers = std::vector<UnitParser *>(metadatas->size());
    uint32_t meta_i = 0;
    for (const ColumnMeta &CM : *metadatas) {
        if (CM.type == CASS_VALUE_TYPE_INT) {
            parsers[meta_i] = new Int32Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_BIGINT || CM.type == CASS_VALUE_TYPE_VARINT) {
            parsers[meta_i] = new Int64Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_BOOLEAN) {
            parsers[meta_i] = new BoolParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_TEXT || CM.type == CASS_VALUE_TYPE_VARCHAR ||
                   CM.type == CASS_VALUE_TYPE_ASCII) {
            parsers[meta_i] = new TextParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_BLOB) {
            parsers[meta_i] = new BytesParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_DOUBLE || CM.type == CASS_VALUE_TYPE_FLOAT) {
            parsers[meta_i] = new DoubleParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_UUID) {
            parsers[meta_i] = new UuidParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_SMALL_INT) {
            parsers[meta_i] = new Int16Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_TINY_INT) {
            parsers[meta_i] = new Int8Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_UDT) {
            throw ModuleException("Support for UDT other than Numpy not implemented");
        } else parsers[meta_i] = new UnitParser(CM);
        ++meta_i;
    }
}

PythonParser::~PythonParser() {
    for (UnitParser *parser : this->parsers) {
        delete (parser);
    }
}
/*** TUPLE BUILDERS ***/

/***
 * Build a tuple from the given Python object using the stored metadata
 * @param obj Python List containing exactly the same number of objects that parsers/metadata sued
 * to setup the PythonParser
 * @return TupleRow with a copy of the values in obj
 * @post The python object can be garbage collected
 */
TupleRow *PythonParser::make_tuple(PyObject *obj) const {
    if (!PyList_Check(obj)) throw ModuleException("PythonParser: Make tuple: Expected python list");
    if (size_t(PyList_Size(obj)) != parsers.size())
        throw ModuleException("PythonParser: Got less python elements than columns configured");

    uint32_t total_bytes = 0;
    char *buffer = nullptr;
    if (!metas->empty()) {
        total_bytes = metas->at(metas->size() - 1).position + metas->at(metas->size() - 1).size;
        buffer = (char *) malloc(total_bytes);
    }

    uint32_t total_bytes = 0;
    char *buffer = nullptr;
    if (!metas->empty()) {
        total_bytes = metas->at(metas->size() - 1).position + metas->at(metas->size() - 1).size;
        buffer = (char *) malloc(total_bytes);
    }

    TupleRow *new_tuple = new TupleRow(metas, total_bytes, buffer);

    for (uint32_t i = 0; i < PyList_Size(obj); ++i) {
        PyObject *some = PyList_GetItem(obj, i);
        if (this->parsers[i]->py_to_c(some, buffer + metas->at(i).position) < 0) new_tuple->setNull(i);
    }
    return new_tuple;
}

/***
 * Builds a Python list from the data being held inside the TupleRow
 * @param tuple
 * @return A list with the information from tuple preserving its order
 */
PyObject *PythonParser::make_pylist(std::vector<const TupleRow *> &values) const {
    //TODO design behaviour, should we expect N tuples or just one?
    const TupleRow *tuple = values[0];
    if (tuple == nullptr)
        throw ModuleException("PythonParser: Marshalling from c to python a NULL tuple, unsupported");
    if (tuple->n_elem() != parsers.size())
        throw ModuleException("PythonParser: Found " + std::to_string(tuple->n_elem()) +
                              " elements from a max of " + std::to_string(parsers.size()));

    PyObject *list = PyList_New(tuple->n_elem());
    for (uint16_t i = 0; i < tuple->n_elem(); i++) {
        if (!tuple->isNull(i)) PyList_SetItem(list, i, this->parsers[i]->c_to_py(tuple->get_element(i)));
        else PyList_SetItem(list, i, Py_None);
    }
    return list;
}
<commit_msg>conflict with master solved<commit_after>#include "PythonParser.h"

PythonParser::PythonParser(std::shared_ptr<StorageInterface> storage,
                           std::shared_ptr<const std::vector<ColumnMeta> > metadatas) {
    this->metas = metadatas;
    this->parsers = std::vector<UnitParser *>(metadatas->size());
    uint32_t meta_i = 0;
    for (const ColumnMeta &CM : *metadatas) {
        if (CM.type == CASS_VALUE_TYPE_INT) {
            parsers[meta_i] = new Int32Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_BIGINT || CM.type == CASS_VALUE_TYPE_VARINT) {
            parsers[meta_i] = new Int64Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_BOOLEAN) {
            parsers[meta_i] = new BoolParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_TEXT || CM.type == CASS_VALUE_TYPE_VARCHAR ||
                   CM.type == CASS_VALUE_TYPE_ASCII) {
            parsers[meta_i] = new TextParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_BLOB) {
            parsers[meta_i] = new BytesParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_DOUBLE || CM.type == CASS_VALUE_TYPE_FLOAT) {
            parsers[meta_i] = new DoubleParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_UUID) {
            parsers[meta_i] = new UuidParser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_SMALL_INT) {
            parsers[meta_i] = new Int16Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_TINY_INT) {
            parsers[meta_i] = new Int8Parser(CM);
        } else if (CM.type == CASS_VALUE_TYPE_UDT) {
            throw ModuleException("Support for UDT other than Numpy not implemented");
        } else parsers[meta_i] = new UnitParser(CM);
        ++meta_i;
    }
}

PythonParser::~PythonParser() {
    for (UnitParser *parser : this->parsers) {
        delete (parser);
    }
}
/*** TUPLE BUILDERS ***/

/***
 * Build a tuple from the given Python object using the stored metadata
 * @param obj Python List containing exactly the same number of objects that parsers/metadata sued
 * to setup the PythonParser
 * @return TupleRow with a copy of the values in obj
 * @post The python object can be garbage collected
 */
TupleRow *PythonParser::make_tuple(PyObject *obj) const {
    if (!PyList_Check(obj)) throw ModuleException("PythonParser: Make tuple: Expected python list");
    if (size_t(PyList_Size(obj)) != parsers.size())
        throw ModuleException("PythonParser: Got less python elements than columns configured");

    uint32_t total_bytes = 0;
    char *buffer = nullptr;
    if (!metas->empty()) {
        total_bytes = metas->at(metas->size() - 1).position + metas->at(metas->size() - 1).size;
        buffer = (char *) malloc(total_bytes);
    }

    TupleRow *new_tuple = new TupleRow(metas, total_bytes, buffer);

    for (uint32_t i = 0; i < PyList_Size(obj); ++i) {
        PyObject *some = PyList_GetItem(obj, i);
        if (this->parsers[i]->py_to_c(some, buffer + metas->at(i).position) < 0) new_tuple->setNull(i);
    }
    return new_tuple;
}

/***
 * Builds a Python list from the data being held inside the TupleRow
 * @param tuple
 * @return A list with the information from tuple preserving its order
 */
PyObject *PythonParser::make_pylist(std::vector<const TupleRow *> &values) const {
    //TODO design behaviour, should we expect N tuples or just one?
    const TupleRow *tuple = values[0];
    if (tuple == nullptr)
        throw ModuleException("PythonParser: Marshalling from c to python a NULL tuple, unsupported");
    if (tuple->n_elem() != parsers.size())
        throw ModuleException("PythonParser: Found " + std::to_string(tuple->n_elem()) +
                              " elements from a max of " + std::to_string(parsers.size()));

    PyObject *list = PyList_New(tuple->n_elem());
    for (uint16_t i = 0; i < tuple->n_elem(); i++) {
        if (!tuple->isNull(i)) PyList_SetItem(list, i, this->parsers[i]->c_to_py(tuple->get_element(i)));
        else PyList_SetItem(list, i, Py_None);
    }
    return list;
}
<|endoftext|>


<commit_before>
typedef struct {
	struct pt pt;
	uint8_t count;
	uint8_t idleCount;
	uint8_t idleState;
} State;

State state;

static struct pt pt_jump;

static PT_THREAD(jump(struct pt *pt))
{
	// TODO: Fix jump animation
	PT_BEGIN(pt);
	showColor(0, LOOKUP(31), 0);
	PT_YIELD(pt);
	showColor(0, LOOKUP(23), 0);
	PT_YIELD(pt);
	showColor(0, LOOKUP(15), 0);
	PT_YIELD(pt);
	showColor(0, LOOKUP(7), 0);
	PT_YIELD(pt);
	showColor(0, LOOKUP(0), 0);

	PT_END(pt);
}

static PT_THREAD(next(State *s, volatile uint8_t controller[]))
{
	PT_BEGIN(&s->pt);

	if(controller[4] & 0x02 && controller[7] < 80) {	// down-b
		for(s->count = 0; s->count < 21; s->count++) {
			if(s->count > 2 && controller[4] & 0x0c) {		// jump
				PT_SPAWN(&s->pt, &pt_jump, jump(&pt_jump));
				PT_EXIT(&s->pt);
			}

			showColor(0, 0, SAW_DESC(s->count, 6));
			PT_YIELD(&s->pt);
		}
	}

	switch(s->idleState) {
	case 0:
		showColor(255, SAW_DESC(s->idleCount, 256), 0);		// green goes up
		break;
	case 1:
		showColor(SAW_ASC(s->idleCount, 256), 255, 0);		// red goes down
		break;
	case 2:
		showColor(0, 255, SAW_DESC(s->idleCount, 256));		// blue goes up
		break;
	case 3:
		showColor(0, SAW_ASC(s->idleCount, 256), 255);		// green goes down
		break;
	case 4:
		showColor(SAW_DESC(s->idleCount, 256), 0, 255);		// red goes up
		break;
	case 5:
		showColor(255, 0, SAW_ASC(s->idleCount, 256));		// blue goes down
		break;
	default:
		s->idleState = 0;
		break;
	}

	if(!s->idleCount--) {
		s->idleState = (s->idleState + 1) % 6;
	}

	PT_END(&s->pt);
}

void nextFrame(volatile uint8_t controller[])
{
	next(&state, controller);
}
<commit_msg>Fixed jump animation<commit_after>
typedef struct {
	struct pt pt;
	uint8_t count;
	uint8_t idleCount;
	uint8_t idleState;
} State;

State state;

static PT_THREAD(next(State *s, volatile uint8_t controller[]))
{
	PT_BEGIN(&s->pt);

	if(controller[4] & 0x02 && controller[7] < 80) {	// down-b
		for(s->count = 0; s->count < 21; s->count++) {
			if(s->count > 2 && controller[4] & 0x0c) {		// jump
				for(s->count = 0; s->count < 30; s->count++) {
					showColor(0, SAW_DESC(s->count, 31), 0);
					PT_YIELD(&s->pt);
				}
				PT_EXIT(&s->pt);
			}

			showColor(0, 0, SAW_DESC(s->count, 6));
			PT_YIELD(&s->pt);
		}
	}

	switch(s->idleState) {
	case 0:
		showColor(255, SAW_DESC(s->idleCount, 256), 0);		// green goes up
		break;
	case 1:
		showColor(SAW_ASC(s->idleCount, 256), 255, 0);		// red goes down
		break;
	case 2:
		showColor(0, 255, SAW_DESC(s->idleCount, 256));		// blue goes up
		break;
	case 3:
		showColor(0, SAW_ASC(s->idleCount, 256), 255);		// green goes down
		break;
	case 4:
		showColor(SAW_DESC(s->idleCount, 256), 0, 255);		// red goes up
		break;
	case 5:
		showColor(255, 0, SAW_ASC(s->idleCount, 256));		// blue goes down
		break;
	default:
		s->idleState = 0;
		break;
	}

	if(!s->idleCount--) {
		s->idleState = (s->idleState + 1) % 6;
	}

	PT_END(&s->pt);
}

void nextFrame(volatile uint8_t controller[])
{
	next(&state, controller);
}
<|endoftext|>