<commit_before>/*
 * Copyright (c) 2011-2016, Graphics Lab, Georgia Tech Research Corporation
 * Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation
 * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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 HOLDER 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 "MyWindow.hpp"

MyWindow::MyWindow()
  : SimWindow() {
}

MyWindow::~MyWindow() {
}

void MyWindow::drawWorld() const {
  glEnable(GL_LIGHTING);
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

  SimWindow::drawWorld();
}

void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
  switch (_key) {
    case ' ':  // use space key to play or stop the motion
      mSimulating = !mSimulating;
      if (mSimulating)
        mPlay = false;
      break;
    case 'q':  // Spawn a cube
    case 'Q': {  // Spawn a cube
      Eigen::Vector3d position = Eigen::Vector3d(dart::math::random(-1.0, 1.0),
                                                 dart::math::random( 0.5, 1.0),
                                                 dart::math::random(-1.0, 1.0));
      Eigen::Vector3d size = Eigen::Vector3d(dart::math::random(0.1, 0.5),
                                             dart::math::random(0.1, 0.5),
                                             dart::math::random(0.1, 0.5));
      spawnCube(position, size);
      break;
    }
    case 'w':    // Spawn a cube
    case 'W': {  // Spawn a cube
      if (mWorld->getNumSkeletons() > 1)
        mWorld->removeSkeleton(mWorld->getSkeleton(mWorld->getNumSkeletons() - 1));
      break;
    }
    default:
      Win3D::keyboard(_key, _x, _y);
  }
  glutPostRedisplay();
}

void MyWindow::spawnCube(const Eigen::Vector3d& _position,
                         const Eigen::Vector3d& _size,
                         double _mass)
{
  dart::dynamics::SkeletonPtr newCubeSkeleton =
      dart::dynamics::Skeleton::create();

  dart::dynamics::BodyNode::Properties body;
  body.mName = "cube_link";
  body.mInertia.setMass(_mass);
  dart::dynamics::ShapePtr newBoxShape(new dart::dynamics::BoxShape(_size));

  dart::dynamics::FreeJoint::Properties joint;
  joint.mName = "cube_joint";
  joint.mT_ParentBodyToJoint = Eigen::Translation3d(_position);

  auto pair
      = newCubeSkeleton->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(
        nullptr, joint, body);
  auto shapeNode = pair.second->createShapeNodeWith<
      dart::dynamics::VisualAspect,
      dart::dynamics::CollisionAspect,
      dart::dynamics::DynamicsAspect>(newBoxShape);
  shapeNode->getVisualAspect()->setColor(dart::math::randomVector<3>(0.0, 1.0));

  mWorld->addSkeleton(newCubeSkeleton);
}
<commit_msg>[addDeleteSkel] Set inertia according to size and mass<commit_after>/*
 * Copyright (c) 2011-2016, Graphics Lab, Georgia Tech Research Corporation
 * Copyright (c) 2011-2016, Humanoid Lab, Georgia Tech Research Corporation
 * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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 HOLDER 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 "MyWindow.hpp"

MyWindow::MyWindow()
  : SimWindow() {
}

MyWindow::~MyWindow() {
}

void MyWindow::drawWorld() const {
  glEnable(GL_LIGHTING);
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

  SimWindow::drawWorld();
}

void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
  switch (_key) {
    case ' ':  // use space key to play or stop the motion
      mSimulating = !mSimulating;
      if (mSimulating)
        mPlay = false;
      break;
    case 'q':  // Spawn a cube
    case 'Q': {  // Spawn a cube
      Eigen::Vector3d position = Eigen::Vector3d(dart::math::random(-1.0, 1.0),
                                                 dart::math::random( 0.5, 1.0),
                                                 dart::math::random(-1.0, 1.0));
      Eigen::Vector3d size = Eigen::Vector3d(dart::math::random(0.1, 0.5),
                                             dart::math::random(0.1, 0.5),
                                             dart::math::random(0.1, 0.5));
      spawnCube(position, size);
      break;
    }
    case 'w':    // Spawn a cube
    case 'W': {  // Spawn a cube
      if (mWorld->getNumSkeletons() > 1)
        mWorld->removeSkeleton(mWorld->getSkeleton(mWorld->getNumSkeletons() - 1));
      break;
    }
    default:
      Win3D::keyboard(_key, _x, _y);
  }
  glutPostRedisplay();
}

void MyWindow::spawnCube(const Eigen::Vector3d& _position,
                         const Eigen::Vector3d& _size,
                         double _mass)
{
  dart::dynamics::SkeletonPtr newCubeSkeleton =
      dart::dynamics::Skeleton::create();

  dart::dynamics::BodyNode::Properties body;
  body.mName = "cube_link";
  body.mInertia.setMass(_mass);
  body.mInertia.setMoment(
      dart::dynamics::BoxShape::computeInertia(_size, _mass));
  dart::dynamics::ShapePtr newBoxShape(new dart::dynamics::BoxShape(_size));

  dart::dynamics::FreeJoint::Properties joint;
  joint.mName = "cube_joint";
  joint.mT_ParentBodyToJoint = Eigen::Translation3d(_position);

  auto pair
      = newCubeSkeleton->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(
        nullptr, joint, body);
  auto shapeNode = pair.second->createShapeNodeWith<
      dart::dynamics::VisualAspect,
      dart::dynamics::CollisionAspect,
      dart::dynamics::DynamicsAspect>(newBoxShape);
  shapeNode->getVisualAspect()->setColor(dart::math::randomVector<3>(0.0, 1.0));

  mWorld->addSkeleton(newCubeSkeleton);
}
<|endoftext|>


<commit_before>/*-------------------------------------------------------------------------
 *
 *   FILE
 *	result.cxx
 *
 *   DESCRIPTION
 *      implementation of the pqxx::result class and support classes.
 *   pqxx::result represents the set of result tuples from a database query
 *
 * Copyright (c) 2001-2005, Jeroen T. Vermeulen <jtv@xs4all.nl>
 *
 * See COPYING for copyright license.  If you did not receive a file called
 * COPYING with this source code, please notify the distributor of this mistake,
 * or contact the author.
 *
 *-------------------------------------------------------------------------
 */
#include "pqxx/compiler.h"

#include <stdexcept>

#include "libpq-fe.h"

#include "pqxx/except"
#include "pqxx/result"

using namespace PGSTD;

bool pqxx::result::operator==(const result &rhs) const throw ()
{
  if (&rhs == this) return true;
  const size_type s(size());
  if (rhs.size() != s) return false;
  for (size_type i=0; i<s; ++i)
    if ((*this)[i] != rhs[i]) return false;
  return true;
}

bool pqxx::result::tuple::operator==(const tuple &rhs) const throw ()
{
  if (&rhs == this) return true;
  const size_type s(size());
  if (rhs.size() != s) return false;
  // TODO: Depends on how null is handled!
  for (size_type i=0; i<s; ++i) if ((*this)[i] != rhs[i]) return false;
  return true;
}

void pqxx::result::tuple::swap(tuple &rhs) throw ()
{
  const result *const h(m_Home);
  const result::size_type i(m_Index);
  m_Home = rhs.m_Home;
  m_Index = rhs.m_Index;
  rhs.m_Home = h;
  rhs.m_Index = i;
}

bool pqxx::result::field::operator==(const field &rhs) const
{
  if (is_null() != rhs.is_null()) return false;
  // TODO: Verify null handling decision
  const size_type s = size();
  if (s != rhs.size()) return false;
  const char *const l(c_str()), *const r(rhs.c_str());
  for (size_type i = 0; i < s; ++i) if (l[i] != r[i]) return false;
  return true;
}

pqxx::result::size_type pqxx::result::size() const throw ()
{
  return c_ptr() ? PQntuples(c_ptr()) : 0;
}

bool pqxx::result::empty() const throw ()
{
  return !c_ptr() || !PQntuples(c_ptr());
}

void pqxx::result::swap(result &rhs) throw ()
{
  super::swap(rhs);
}

const pqxx::result::tuple pqxx::result::at(pqxx::result::size_type i) const
  throw (out_of_range)
{
  if (i >= size())
    throw out_of_range("Tuple number out of range");

  return operator[](i);
}

void pqxx::result::CheckStatus(const string &Query) const
{
  const string Err = StatusError();
  if (!Err.empty()) throw sql_error(Err, Query);
}

void pqxx::result::CheckStatus(const char Query[]) const
{
  const string Err = StatusError();
  if (!Err.empty()) throw sql_error(Err, string(Query ? Query : ""));
}

string pqxx::result::StatusError() const
{
  if (!c_ptr())
    throw runtime_error("No result");

  string Err;

  switch (PQresultStatus(c_ptr()))
  {
  case PGRES_EMPTY_QUERY: // The string sent to the backend was empty.
  case PGRES_COMMAND_OK: // Successful completion of a command returning no data
  case PGRES_TUPLES_OK: // The query successfully executed
    break;

  case PGRES_COPY_OUT: // Copy Out (from server) data transfer started
  case PGRES_COPY_IN: // Copy In (to server) data transfer started
    break;

  case PGRES_BAD_RESPONSE: // The server's response was not understood
  case PGRES_NONFATAL_ERROR:
  case PGRES_FATAL_ERROR:
    Err = PQresultErrorMessage(c_ptr());
    break;

  default:
    throw logic_error("libpqxx internal error: "
		      "pqxx::result: Unrecognized response code " +
		      to_string(int(PQresultStatus(c_ptr()))));
  }
  return Err;
}

const char *pqxx::result::CmdStatus() const throw ()
{
  return PQcmdStatus(c_ptr());
}

pqxx::oid pqxx::result::inserted_oid() const
{
  if (!c_ptr())
    throw logic_error("Attempt to read oid of inserted row without an INSERT "
	"result");
  return PQoidValue(c_ptr());
}

pqxx::result::size_type pqxx::result::affected_rows() const
{
  const char *const RowsStr = PQcmdTuples(c_ptr());
  return RowsStr[0] ? atoi(RowsStr) : 0;
}

const char *pqxx::result::GetValue(pqxx::result::size_type Row,
		                 pqxx::result::tuple::size_type Col) const
{
  return PQgetvalue(c_ptr(), Row, Col);
}

bool pqxx::result::GetIsNull(pqxx::result::size_type Row,
		           pqxx::result::tuple::size_type Col) const
{
  return PQgetisnull(c_ptr(), Row, Col) != 0;
}

pqxx::result::field::size_type
pqxx::result::GetLength(pqxx::result::size_type Row,
                        pqxx::result::tuple::size_type Col) const
{
  return PQgetlength(c_ptr(), Row, Col);
}

pqxx::oid pqxx::result::column_type(tuple::size_type ColNum) const
{
  const oid T = PQftype(c_ptr(), ColNum);
  if (T == oid_none)
    throw PGSTD::invalid_argument(
	"Attempt to retrieve type of nonexistant column " +
	to_string(ColNum) + " of query result");
  return T;
}

#ifdef PQXX_HAVE_PQFTABLE
pqxx::oid pqxx::result::column_table(tuple::size_type ColNum) const
{
  const oid T = PQftable(c_ptr(), ColNum);

  /* If we get oid_none, it may be because the column is computed, or because we
   * got an invalid row number.
   */
  if (T == oid_none && ColNum >= columns())
    throw PGSTD::invalid_argument("Attempt to retrieve table ID for column " +
	to_string(ColNum) + " out of " + to_string(columns()));

  return T;
}
#endif

int pqxx::result::errorposition() const throw ()
{
  int pos = -1;
#if defined(PQXX_HAVE_PQRESULTERRORFIELD)
  if (c_ptr())
  {
    const char *p = PQresultErrorField(c_ptr(), PG_DIAG_STATEMENT_POSITION);
    if (p) from_string(p, pos);
  }
#endif // PQXX_HAVE_PQRESULTERRORFIELD
  return pos;
}

// tuple

pqxx::result::field pqxx::result::tuple::operator[](const char f[]) const
{
  return field(*this, m_Home->column_number(f));
}

pqxx::result::field pqxx::result::tuple::at(const char f[]) const
{
  const int fnum = m_Home->column_number(f);
  if (fnum == -1)
    throw invalid_argument(string("Unknown field '") + f + "'");

  return field(*this, fnum);
}

pqxx::result::field
pqxx::result::tuple::at(pqxx::result::tuple::size_type i) const throw (out_of_range)
{
  if (i >= size())
    throw out_of_range("Invalid field number");

  return operator[](i);
}

const char *
pqxx::result::column_name(pqxx::result::tuple::size_type Number) const
{
  const char *const N = PQfname(c_ptr(), Number);
  if (!N)
    throw out_of_range("Invalid column number: " + to_string(Number));

  return N;
}

pqxx::result::tuple::size_type pqxx::result::columns() const throw ()
{
  return c_ptr() ? PQnfields(c_ptr()) : 0;
}

pqxx::result::tuple::size_type
pqxx::result::column_number(const char ColName[]) const
{
  const int N = PQfnumber(c_ptr(), ColName);
  if (N == -1)
    throw invalid_argument("Unknown column name: '" + string(ColName) + "'");

  return tuple::size_type(N);
}

// const_iterator

pqxx::result::const_iterator
pqxx::result::const_iterator::operator++(int)
{
  const_iterator old(*this);
  m_Index++;
  return old;
}

pqxx::result::const_iterator
pqxx::result::const_iterator::operator--(int)
{
  const_iterator old(*this);
  m_Index--;
  return old;
}

// const_fielditerator

pqxx::result::const_fielditerator
pqxx::result::const_fielditerator::operator++(int)
{
  const_fielditerator old(*this);
  m_col++;
  return old;
}

pqxx::result::const_fielditerator
pqxx::result::const_fielditerator::operator--(int)
{
  const_fielditerator old(*this);
  m_col--;
  return old;
}

pqxx::result::const_reverse_iterator
pqxx::result::const_reverse_iterator::operator++(int)
{
  const_reverse_iterator tmp(*this);
  iterator_type::operator--();
  return tmp;
}

pqxx::result::const_reverse_iterator
pqxx::result::const_reverse_iterator::operator--(int)
{
  const_reverse_iterator tmp(*this);
  iterator_type::operator++();
  return tmp;
}

pqxx::result::const_reverse_fielditerator
pqxx::result::const_reverse_fielditerator::operator++(int)
{
  const_reverse_fielditerator tmp(*this);
  iterator_type::operator--();
  return tmp;
}

pqxx::result::const_reverse_fielditerator
pqxx::result::const_reverse_fielditerator::operator--(int)
{
  const_reverse_fielditerator tmp(*this);
  iterator_type::operator++();
  return tmp;
}

<commit_msg>Clearer error message<commit_after>/*-------------------------------------------------------------------------
 *
 *   FILE
 *	result.cxx
 *
 *   DESCRIPTION
 *      implementation of the pqxx::result class and support classes.
 *   pqxx::result represents the set of result tuples from a database query
 *
 * Copyright (c) 2001-2005, Jeroen T. Vermeulen <jtv@xs4all.nl>
 *
 * See COPYING for copyright license.  If you did not receive a file called
 * COPYING with this source code, please notify the distributor of this mistake,
 * or contact the author.
 *
 *-------------------------------------------------------------------------
 */
#include "pqxx/compiler.h"

#include <stdexcept>

#include "libpq-fe.h"

#include "pqxx/except"
#include "pqxx/result"

using namespace PGSTD;

bool pqxx::result::operator==(const result &rhs) const throw ()
{
  if (&rhs == this) return true;
  const size_type s(size());
  if (rhs.size() != s) return false;
  for (size_type i=0; i<s; ++i)
    if ((*this)[i] != rhs[i]) return false;
  return true;
}

bool pqxx::result::tuple::operator==(const tuple &rhs) const throw ()
{
  if (&rhs == this) return true;
  const size_type s(size());
  if (rhs.size() != s) return false;
  // TODO: Depends on how null is handled!
  for (size_type i=0; i<s; ++i) if ((*this)[i] != rhs[i]) return false;
  return true;
}

void pqxx::result::tuple::swap(tuple &rhs) throw ()
{
  const result *const h(m_Home);
  const result::size_type i(m_Index);
  m_Home = rhs.m_Home;
  m_Index = rhs.m_Index;
  rhs.m_Home = h;
  rhs.m_Index = i;
}

bool pqxx::result::field::operator==(const field &rhs) const
{
  if (is_null() != rhs.is_null()) return false;
  // TODO: Verify null handling decision
  const size_type s = size();
  if (s != rhs.size()) return false;
  const char *const l(c_str()), *const r(rhs.c_str());
  for (size_type i = 0; i < s; ++i) if (l[i] != r[i]) return false;
  return true;
}

pqxx::result::size_type pqxx::result::size() const throw ()
{
  return c_ptr() ? PQntuples(c_ptr()) : 0;
}

bool pqxx::result::empty() const throw ()
{
  return !c_ptr() || !PQntuples(c_ptr());
}

void pqxx::result::swap(result &rhs) throw ()
{
  super::swap(rhs);
}

const pqxx::result::tuple pqxx::result::at(pqxx::result::size_type i) const
  throw (out_of_range)
{
  if (i >= size())
    throw out_of_range("Tuple number out of range");

  return operator[](i);
}

void pqxx::result::CheckStatus(const string &Query) const
{
  const string Err = StatusError();
  if (!Err.empty()) throw sql_error(Err, Query);
}

void pqxx::result::CheckStatus(const char Query[]) const
{
  const string Err = StatusError();
  if (!Err.empty()) throw sql_error(Err, string(Query ? Query : ""));
}

string pqxx::result::StatusError() const
{
  if (!c_ptr())
    throw runtime_error("No result set given");

  string Err;

  switch (PQresultStatus(c_ptr()))
  {
  case PGRES_EMPTY_QUERY: // The string sent to the backend was empty.
  case PGRES_COMMAND_OK: // Successful completion of a command returning no data
  case PGRES_TUPLES_OK: // The query successfully executed
    break;

  case PGRES_COPY_OUT: // Copy Out (from server) data transfer started
  case PGRES_COPY_IN: // Copy In (to server) data transfer started
    break;

  case PGRES_BAD_RESPONSE: // The server's response was not understood
  case PGRES_NONFATAL_ERROR:
  case PGRES_FATAL_ERROR:
    Err = PQresultErrorMessage(c_ptr());
    break;

  default:
    throw logic_error("libpqxx internal error: "
		      "pqxx::result: Unrecognized response code " +
		      to_string(int(PQresultStatus(c_ptr()))));
  }
  return Err;
}

const char *pqxx::result::CmdStatus() const throw ()
{
  return PQcmdStatus(c_ptr());
}

pqxx::oid pqxx::result::inserted_oid() const
{
  if (!c_ptr())
    throw logic_error("Attempt to read oid of inserted row without an INSERT "
	"result");
  return PQoidValue(c_ptr());
}

pqxx::result::size_type pqxx::result::affected_rows() const
{
  const char *const RowsStr = PQcmdTuples(c_ptr());
  return RowsStr[0] ? atoi(RowsStr) : 0;
}

const char *pqxx::result::GetValue(pqxx::result::size_type Row,
		                 pqxx::result::tuple::size_type Col) const
{
  return PQgetvalue(c_ptr(), Row, Col);
}

bool pqxx::result::GetIsNull(pqxx::result::size_type Row,
		           pqxx::result::tuple::size_type Col) const
{
  return PQgetisnull(c_ptr(), Row, Col) != 0;
}

pqxx::result::field::size_type
pqxx::result::GetLength(pqxx::result::size_type Row,
                        pqxx::result::tuple::size_type Col) const
{
  return PQgetlength(c_ptr(), Row, Col);
}

pqxx::oid pqxx::result::column_type(tuple::size_type ColNum) const
{
  const oid T = PQftype(c_ptr(), ColNum);
  if (T == oid_none)
    throw PGSTD::invalid_argument(
	"Attempt to retrieve type of nonexistant column " +
	to_string(ColNum) + " of query result");
  return T;
}

#ifdef PQXX_HAVE_PQFTABLE
pqxx::oid pqxx::result::column_table(tuple::size_type ColNum) const
{
  const oid T = PQftable(c_ptr(), ColNum);

  /* If we get oid_none, it may be because the column is computed, or because we
   * got an invalid row number.
   */
  if (T == oid_none && ColNum >= columns())
    throw PGSTD::invalid_argument("Attempt to retrieve table ID for column " +
	to_string(ColNum) + " out of " + to_string(columns()));

  return T;
}
#endif

int pqxx::result::errorposition() const throw ()
{
  int pos = -1;
#if defined(PQXX_HAVE_PQRESULTERRORFIELD)
  if (c_ptr())
  {
    const char *p = PQresultErrorField(c_ptr(), PG_DIAG_STATEMENT_POSITION);
    if (p) from_string(p, pos);
  }
#endif // PQXX_HAVE_PQRESULTERRORFIELD
  return pos;
}

// tuple

pqxx::result::field pqxx::result::tuple::operator[](const char f[]) const
{
  return field(*this, m_Home->column_number(f));
}

pqxx::result::field pqxx::result::tuple::at(const char f[]) const
{
  const int fnum = m_Home->column_number(f);
  // TODO: Should this be an out_of_range?
  if (fnum == -1)
    throw invalid_argument(string("Unknown field '") + f + "'");

  return field(*this, fnum);
}

pqxx::result::field
pqxx::result::tuple::at(pqxx::result::tuple::size_type i) const throw (out_of_range)
{
  if (i >= size())
    throw out_of_range("Invalid field number");

  return operator[](i);
}

const char *
pqxx::result::column_name(pqxx::result::tuple::size_type Number) const
{
  const char *const N = PQfname(c_ptr(), Number);
  if (!N)
    throw out_of_range("Invalid column number: " + to_string(Number));

  return N;
}

pqxx::result::tuple::size_type pqxx::result::columns() const throw ()
{
  return c_ptr() ? PQnfields(c_ptr()) : 0;
}

pqxx::result::tuple::size_type
pqxx::result::column_number(const char ColName[]) const
{
  const int N = PQfnumber(c_ptr(), ColName);
  // TODO: Should this be an out_of_range?
  if (N == -1)
    throw invalid_argument("Unknown column name: '" + string(ColName) + "'");

  return tuple::size_type(N);
}

// const_iterator

pqxx::result::const_iterator
pqxx::result::const_iterator::operator++(int)
{
  const_iterator old(*this);
  m_Index++;
  return old;
}

pqxx::result::const_iterator
pqxx::result::const_iterator::operator--(int)
{
  const_iterator old(*this);
  m_Index--;
  return old;
}

// const_fielditerator

pqxx::result::const_fielditerator
pqxx::result::const_fielditerator::operator++(int)
{
  const_fielditerator old(*this);
  m_col++;
  return old;
}

pqxx::result::const_fielditerator
pqxx::result::const_fielditerator::operator--(int)
{
  const_fielditerator old(*this);
  m_col--;
  return old;
}

pqxx::result::const_reverse_iterator
pqxx::result::const_reverse_iterator::operator++(int)
{
  const_reverse_iterator tmp(*this);
  iterator_type::operator--();
  return tmp;
}

pqxx::result::const_reverse_iterator
pqxx::result::const_reverse_iterator::operator--(int)
{
  const_reverse_iterator tmp(*this);
  iterator_type::operator++();
  return tmp;
}

pqxx::result::const_reverse_fielditerator
pqxx::result::const_reverse_fielditerator::operator++(int)
{
  const_reverse_fielditerator tmp(*this);
  iterator_type::operator--();
  return tmp;
}

pqxx::result::const_reverse_fielditerator
pqxx::result::const_reverse_fielditerator::operator--(int)
{
  const_reverse_fielditerator tmp(*this);
  iterator_type::operator++();
  return tmp;
}

<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: retstrm.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: obo $ $Date: 2004-09-09 17:23:14 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2002
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2002 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2002 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef _RETSTRM_HXX
#define _RETSTRM_HXX

#ifndef _SBXVAR_HXX //autogen
#include <svtools/sbxvar.hxx>
#endif
#ifndef _SMARTID_HXX_
#include <vcl/smartid.hxx>
#endif
#include "cmdbasestream.hxx"

class SvStream;

class RetStream: public CmdBaseStream
{

public:
    RetStream();
    ~RetStream();

//  CmdBaseStream::GenError;
//    void GenError( comm_ULONG nError, const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::GenError( nError, aString, nLenInChars );}
//  new
    void GenError( SmartId aUId, String aString );

//  CmdBaseStream::GenReturn;
    void GenReturn( comm_USHORT nRet, comm_ULONG nNr ){CmdBaseStream::GenReturn( nRet, nNr );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_ULONG nNr ){CmdBaseStream::GenReturn( nRet, &aUId, nNr );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::GenReturn( nRet, nUId, aString, nLenInChars );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, &aUId, bBool );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, comm_ULONG nNr, const comm_UniChar* aString, comm_USHORT nLenInChars, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, nUId, nNr, aString, nLenInChars, bBool );}
// MacroRecorder
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_USHORT nMethod ){CmdBaseStream::GenReturn( nRet, &aUId, nMethod );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, comm_USHORT nMethod, const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::GenReturn( nRet, nUId, nMethod, aString, nLenInChars );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, comm_USHORT nMethod, const comm_UniChar* aString, comm_USHORT nLenInChars, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, nUId, nMethod, aString, nLenInChars, bBool );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_USHORT nMethod, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, &aUId, nMethod, bBool );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_USHORT nMethod, comm_ULONG nNr ){CmdBaseStream::GenReturn( nRet, &aUId, nMethod, nNr );}

//  new
    void GenReturn( USHORT nRet, SmartId aUId, String aString );
    void GenReturn( USHORT nRet, SmartId aUId, SbxValue &aValue );
    void GenReturn( USHORT nRet, SmartId aUId, ULONG nNr, String aString, BOOL bBool );
// MacroRecorder
    void GenReturn( USHORT nRet, SmartId aUId, USHORT nMethod, String aString );
    void GenReturn( USHORT nRet, SmartId aUId, USHORT nMethod, String aString, BOOL bBool );

    void Reset();
    SvStream* GetStream();

//  CmdBaseStream::Write;
    void Write( comm_USHORT nNr ){CmdBaseStream::Write( nNr );}
    void Write( comm_ULONG nNr ){CmdBaseStream::Write( nNr );}
//  void Write( const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::Write( aString, nLenInChars );}
    void Write( comm_BOOL bBool ){CmdBaseStream::Write( bBool );}
//  new
    void Write( SbxValue &aValue );

// Complex Datatypes to be handled system dependent
    virtual void Write( SmartId* pId );
    virtual void Write( String *pString );

    SvStream *pSammel;
};

#endif
<commit_msg>INTEGRATION: CWS visibility03 (1.3.10); FILE MERGED 2005/04/01 16:59:57 mhu 1.3.10.1: #i45006# Replaced includes svtools/sbx*.hxx with basic/sbx*.hxx<commit_after>/*************************************************************************
 *
 *  $RCSfile: retstrm.hxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: obo $ $Date: 2005-04-13 09:54:27 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2002
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2002 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2002 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/
#ifndef _RETSTRM_HXX
#define _RETSTRM_HXX

#ifndef _SBXVAR_HXX //autogen
#include <basic/sbxvar.hxx>
#endif
#ifndef _SMARTID_HXX_
#include <vcl/smartid.hxx>
#endif
#include "cmdbasestream.hxx"

class SvStream;

class RetStream: public CmdBaseStream
{

public:
    RetStream();
    ~RetStream();

//  CmdBaseStream::GenError;
//    void GenError( comm_ULONG nError, const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::GenError( nError, aString, nLenInChars );}
//  new
    void GenError( SmartId aUId, String aString );

//  CmdBaseStream::GenReturn;
    void GenReturn( comm_USHORT nRet, comm_ULONG nNr ){CmdBaseStream::GenReturn( nRet, nNr );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_ULONG nNr ){CmdBaseStream::GenReturn( nRet, &aUId, nNr );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::GenReturn( nRet, nUId, aString, nLenInChars );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, &aUId, bBool );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, comm_ULONG nNr, const comm_UniChar* aString, comm_USHORT nLenInChars, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, nUId, nNr, aString, nLenInChars, bBool );}
// MacroRecorder
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_USHORT nMethod ){CmdBaseStream::GenReturn( nRet, &aUId, nMethod );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, comm_USHORT nMethod, const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::GenReturn( nRet, nUId, nMethod, aString, nLenInChars );}
//  void GenReturn( comm_USHORT nRet, comm_ULONG nUId, comm_USHORT nMethod, const comm_UniChar* aString, comm_USHORT nLenInChars, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, nUId, nMethod, aString, nLenInChars, bBool );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_USHORT nMethod, comm_BOOL bBool ){CmdBaseStream::GenReturn( nRet, &aUId, nMethod, bBool );}
    void GenReturn( comm_USHORT nRet, SmartId aUId, comm_USHORT nMethod, comm_ULONG nNr ){CmdBaseStream::GenReturn( nRet, &aUId, nMethod, nNr );}

//  new
    void GenReturn( USHORT nRet, SmartId aUId, String aString );
    void GenReturn( USHORT nRet, SmartId aUId, SbxValue &aValue );
    void GenReturn( USHORT nRet, SmartId aUId, ULONG nNr, String aString, BOOL bBool );
// MacroRecorder
    void GenReturn( USHORT nRet, SmartId aUId, USHORT nMethod, String aString );
    void GenReturn( USHORT nRet, SmartId aUId, USHORT nMethod, String aString, BOOL bBool );

    void Reset();
    SvStream* GetStream();

//  CmdBaseStream::Write;
    void Write( comm_USHORT nNr ){CmdBaseStream::Write( nNr );}
    void Write( comm_ULONG nNr ){CmdBaseStream::Write( nNr );}
//  void Write( const comm_UniChar* aString, comm_USHORT nLenInChars ){CmdBaseStream::Write( aString, nLenInChars );}
    void Write( comm_BOOL bBool ){CmdBaseStream::Write( bBool );}
//  new
    void Write( SbxValue &aValue );

// Complex Datatypes to be handled system dependent
    virtual void Write( SmartId* pId );
    virtual void Write( String *pString );

    SvStream *pSammel;
};

#endif
<|endoftext|>


<commit_before>#include "newarc.h"

ArchiveModuleManager::ArchiveModuleManager(const TCHAR* lpCurrentLanguage)
{
	m_bLoaded = false;
	m_pFilter = new ArchiveFilter(this, true);
	m_strCurrentLanguage = lpCurrentLanguage;
}

bool ArchiveModuleManager::LoadIfNeeded()
{
	if ( !m_bLoaded )
	{
		string strModulesPath = Info.ModuleName;

		CutToSlash(strModulesPath);
		strModulesPath += _T("Plugins"); //BUGBUG, to change

		FSF.FarRecursiveSearch(strModulesPath, _T("*.module"), (FRSUSERFUNC)LoadModules, FRS_RECUR, this);

		CutToSlash(strModulesPath);
		strModulesPath += _T("filters.ini");

		m_pFilter->Load(strModulesPath);

		CutToSlash(strModulesPath);
		strModulesPath += _T("templates.ini");

		LoadTemplates(strModulesPath);
	/*	ArchiveFormatInfoArray formats;

		GetFormats(formats);

		__debug(_T("formats - %d"), formats.count());
		*/
		m_bLoaded = true;
	}

	return m_bLoaded;
}

int __stdcall ArchiveModuleManager::LoadModules(
		const FAR_FIND_DATA* pFindData,
		const TCHAR* lpFullName,
		ArchiveModuleManager* pManager
		)
{
	if ( pFindData->dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY )
		return TRUE;

	ArchiveModule *pModule = new ArchiveModule(pManager);

	if ( pModule )
	{
		if ( pModule->Load(lpFullName, pManager->m_strCurrentLanguage) )
		{
			pManager->m_pModules.add(pModule);
			return TRUE;
		}

		delete pModule;
	}

	return TRUE;
}

ArchiveModuleManager::~ArchiveModuleManager()
{
	delete m_pFilter;
}

ArchiveFilter* ArchiveModuleManager::GetFilter()
{
	return m_pFilter;
}

void ArchiveModuleManager::SetCurrentLanguage(const TCHAR* lpLanguage)
{
	if ( FSF.LStricmp(lpLanguage, m_strCurrentLanguage) )
	{
		for (int i = 0; i < m_pModules.count(); i++)
			m_pModules[i]->ReloadLanguage (lpLanguage);

		m_strCurrentLanguage = lpLanguage;
	}
}

//,
int ArchiveModuleManager::QueryArchives(
		const TCHAR *lpFileName,
		const unsigned char *pBuffer,
		DWORD dwBufferSize,
		Array<ArchiveFormat*>& result
		)
{
	bool bStopped = false;

	Array<ArchiveFilterEntry*> filters;

	m_pFilter->Reset();
	m_pFilter->QueryFilters(lpFileName, filters, bStopped);

	for (int i = 0; i < filters.count(); i++)
	{
		ArchiveFilterEntry* pFE = filters[i];
		ArchiveModule* pModule = pFE->pModule;

		bool bNoPluginsFiltered = true;
		bool bNoFormatsFiltered = true;

		if ( !m_pFilter->Filtered(&pModule->GetUID(), NULL, NULL) )
		{
			if ( pFE->bAllPlugins )
			{
				Array<ArchivePlugin*>& plugins = pModule->GetPlugins();

				for (int i = 0; i < plugins.count(); i++)
				{
					ArchivePlugin* pPlugin = plugins[i];

					if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), NULL) )
					{
						if ( pModule->QueryCapability(AMF_SUPPORT_SINGLE_PLUGIN_QUERY) )
						{
							Array<ArchiveFormat*>& formats = pPlugin->GetFormats();

				    		for (int j = 0; j < formats.count(); j++)
				    		{
				    			ArchiveFormat* pFormat = formats[i];

	        		    		if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
					    		{
					    			if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
										pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
								}
								else
									bNoFormatsFiltered = false;
				    		}

				    		if ( bNoFormatsFiltered && !pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
				    			pModule->QueryArchives(&pPlugin->GetUID(), NULL, lpFileName, pBuffer, dwBufferSize, result);
						}
						else
							bNoPluginsFiltered = false;
					}

					if ( bNoPluginsFiltered && !pModule->QueryCapability(AMF_SUPPORT_SINGLE_PLUGIN_QUERY) )
						pModule->QueryArchives(NULL, NULL, lpFileName, pBuffer, dwBufferSize, result);
				}
			}
			else
			{
				if ( pFE->bAllFormats )
				{
			    	ArchivePlugin* pPlugin = pFE->pPlugin;

					if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), NULL) )
					{
						Array<ArchiveFormat*>& formats = pPlugin->GetFormats();

						for (int i = 0; i < formats.count(); i++)
						{
							ArchiveFormat* pFormat = formats[i];

							if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
							{
								if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
									pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
							}
							else
								bNoFormatsFiltered = false;

						}

						if ( bNoFormatsFiltered && !pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
			    			pModule->QueryArchives(&pPlugin->GetUID(), NULL, lpFileName, pBuffer, dwBufferSize, result);
					}
				}
				else
				{
					ArchiveFormat* pFormat = pFE->pFormat;
					ArchivePlugin* pPlugin = pFE->pPlugin;

					if  ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
					{
						if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
							pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
					}
				}
			}
		}

		m_pFilter->AddStopFilter(filters[i]);
	}

	if ( !bStopped && m_pFilter->UseRemaining() )
	{
		for (int i = 0; i < m_pModules.count(); i++)
		{
			bool bNoPluginsFiltered = true;
			bool bNoFormatsFiltered = true;

			ArchiveModule* pModule = m_pModules[i];

			if ( !m_pFilter->Filtered(&pModule->GetUID(), NULL, NULL) )
			{
				Array<ArchivePlugin*>& plugins = pModule->GetPlugins();

				for (unsigned int j = 0; j < plugins.count(); j++)
				{
					ArchivePlugin* pPlugin = plugins[j];

					if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), NULL) )
					{
						Array<ArchiveFormat*>& formats = pPlugin->GetFormats();

						for (unsigned int k = 0; k < formats.count(); k++)
						{
							ArchiveFormat* pFormat = formats[k];

							if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
							{
								if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
									pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
							}
							else
								bNoFormatsFiltered = false;
						}

						if ( bNoFormatsFiltered && !pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
							pModule->QueryArchives(&pPlugin->GetUID(), NULL, lpFileName, pBuffer, dwBufferSize, result);

					}
					else
						bNoPluginsFiltered = false;
				}

				if ( bNoPluginsFiltered && !pModule->QueryCapability(AMF_SUPPORT_SINGLE_PLUGIN_QUERY) )
					pModule->QueryArchives(NULL, NULL, lpFileName, pBuffer, dwBufferSize, result);
			}
		}
	}

	return 0;
}

ArchiveModule* ArchiveModuleManager::GetModule(const GUID &uid)
{
	for (int i = 0; i < m_pModules.count(); i++)
	{
		if ( m_pModules[i]->GetUID() == uid )
			return m_pModules[i];
	}

	return NULL;
}

int ArchiveModuleManager::GetPlugins(Array<ArchivePlugin*>& plugins)
{
	for (int i = 0; i < m_pModules.count(); i++)
		m_pModules[i]->GetPlugins(plugins);

	return 0;
}

ArchiveFormat* ArchiveModuleManager::GetFormat(const GUID& uidModule, const GUID& uidPlugin, const GUID& uidFormat)
{
	ArchiveModule* pModule = GetModule(uidModule);

	if ( pModule )
		return pModule->GetFormat(uidPlugin, uidFormat);

	return NULL;
}

int ArchiveModuleManager::GetFormats(Array<ArchiveFormat*>& formats)
{
	for (int i = 0; i < m_pModules.count(); i++)
		m_pModules[i]->GetFormats(formats);

	return 0;
}

Array<ArchiveModule*>& ArchiveModuleManager::GetModules()
{
	return m_pModules;
}

Archive* ArchiveModuleManager::OpenCreateArchive(
		ArchiveFormat* pFormat,
		const TCHAR* lpFileName,
		HANDLE hCallback,
		ARCHIVECALLBACK pfnCallback,
		bool bCreate
		)
{
	HANDLE hArchive = pFormat->GetModule()->OpenCreateArchive(
				pFormat->GetPlugin()->GetUID(),
				pFormat->GetUID(),
				lpFileName,
				hCallback,
				pfnCallback,
				bCreate
				);

	if ( hArchive )
		return new Archive(hArchive, pFormat, lpFileName);

	if ( bCreate )
		return new Archive(NULL, pFormat, lpFileName);

	return NULL;
}

void ArchiveModuleManager::CloseArchive(Archive* pArchive)
{
	if ( pArchive->GetHandle() )
		pArchive->GetModule()->CloseArchive(pArchive->GetPlugin()->GetUID(), pArchive->GetHandle());

	delete pArchive;
}

bool ArchiveModuleManager::LoadTemplates(const TCHAR* lpFileName)
{
	TCHAR szNames[4096];

	if ( !GetPrivateProfileSectionNames(szNames, 4096, lpFileName) )
		return false;

	m_pTemplates.reset();
	Array<const TCHAR*> names;

	const TCHAR *lpName = szNames;

	while ( *lpName )
	{
		names.add(lpName);
		lpName += _tcslen (lpName)+1;
	}

	for (int i = 0; i < names.count(); i++)
	{
		string strName = names[i];
		string strParams;

		TCHAR* pBuffer = strParams.GetBuffer(260); //BUGBUG
		GetPrivateProfileString(strName, _T("Params"), _T(""), pBuffer, 260, lpFileName);
		strParams.ReleaseBuffer();

		TCHAR szGUID[64];

		GetPrivateProfileString(strName, _T("ModuleUID"), _T(""), szGUID, 64, lpFileName);
		GUID uidModule = STR2GUID(szGUID);

		GetPrivateProfileString(strName, _T("PluginUID"), _T(""), szGUID, 64, lpFileName);
		GUID uidPlugin = STR2GUID(szGUID);

		GetPrivateProfileString(strName, _T("FormatUID"), _T(""), szGUID, 64, lpFileName);
		GUID uidFormat = STR2GUID(szGUID);

		m_pTemplates.add(new ArchiveTemplate(this, strName, strParams, uidModule, uidPlugin, uidFormat));
	}

	return true;
}

bool ArchiveModuleManager::SaveTemplates(const TCHAR* lpFileName)
{
	DeleteFile(lpFileName);

	string strFileName = Info.ModuleName;

	CutToSlash(strFileName);
	strFileName += lpFileName;

	for (int i = 0; i < m_pTemplates.count(); i++)
	{
		ArchiveTemplate* pAT = m_pTemplates[i];

		WritePrivateProfileString(pAT->GetName(), _T("Params"), pAT->GetParams(), strFileName);
		WritePrivateProfileString(pAT->GetName(), _T("ModuleUID"), GUID2STR(pAT->GetModuleUID()), strFileName);
		WritePrivateProfileString(pAT->GetName(), _T("PluginUID"), GUID2STR(pAT->GetPluginUID()), strFileName);
		WritePrivateProfileString(pAT->GetName(), _T("FormatUID"), GUID2STR(pAT->GetFormatUID()), strFileName);
	}

	return true;
}

Array<ArchiveTemplate*>& ArchiveModuleManager::GetTemplates()
{
	return m_pTemplates;
}<commit_msg>modules<commit_after>#include "newarc.h"

ArchiveModuleManager::ArchiveModuleManager(const TCHAR* lpCurrentLanguage)
{
	m_bLoaded = false;
	m_pFilter = new ArchiveFilter(this, true);
	m_strCurrentLanguage = lpCurrentLanguage;
}

bool ArchiveModuleManager::LoadIfNeeded()
{
	if ( !m_bLoaded )
	{
		string strModulesPath = Info.ModuleName;

		CutToSlash(strModulesPath);
		strModulesPath += _T("Modules");

		FSF.FarRecursiveSearch(strModulesPath, _T("*.module"), (FRSUSERFUNC)LoadModules, FRS_RECUR, this);

		CutToSlash(strModulesPath);
		strModulesPath += _T("filters.ini");

		m_pFilter->Load(strModulesPath);

		CutToSlash(strModulesPath);
		strModulesPath += _T("templates.ini");

		LoadTemplates(strModulesPath);
	/*	ArchiveFormatInfoArray formats;

		GetFormats(formats);

		__debug(_T("formats - %d"), formats.count());
		*/
		m_bLoaded = true;
	}

	return m_bLoaded;
}

int __stdcall ArchiveModuleManager::LoadModules(
		const FAR_FIND_DATA* pFindData,
		const TCHAR* lpFullName,
		ArchiveModuleManager* pManager
		)
{
	if ( pFindData->dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY )
		return TRUE;

	ArchiveModule *pModule = new ArchiveModule(pManager);

	if ( pModule )
	{
		if ( pModule->Load(lpFullName, pManager->m_strCurrentLanguage) )
		{
			pManager->m_pModules.add(pModule);
			return TRUE;
		}

		delete pModule;
	}

	return TRUE;
}

ArchiveModuleManager::~ArchiveModuleManager()
{
	delete m_pFilter;
}

ArchiveFilter* ArchiveModuleManager::GetFilter()
{
	return m_pFilter;
}

void ArchiveModuleManager::SetCurrentLanguage(const TCHAR* lpLanguage)
{
	if ( FSF.LStricmp(lpLanguage, m_strCurrentLanguage) )
	{
		for (int i = 0; i < m_pModules.count(); i++)
			m_pModules[i]->ReloadLanguage (lpLanguage);

		m_strCurrentLanguage = lpLanguage;
	}
}

//,
int ArchiveModuleManager::QueryArchives(
		const TCHAR *lpFileName,
		const unsigned char *pBuffer,
		DWORD dwBufferSize,
		Array<ArchiveFormat*>& result
		)
{
	bool bStopped = false;

	Array<ArchiveFilterEntry*> filters;

	m_pFilter->Reset();
	m_pFilter->QueryFilters(lpFileName, filters, bStopped);

	for (int i = 0; i < filters.count(); i++)
	{
		ArchiveFilterEntry* pFE = filters[i];
		ArchiveModule* pModule = pFE->pModule;

		bool bNoPluginsFiltered = true;
		bool bNoFormatsFiltered = true;

		if ( !m_pFilter->Filtered(&pModule->GetUID(), NULL, NULL) )
		{
			if ( pFE->bAllPlugins )
			{
				Array<ArchivePlugin*>& plugins = pModule->GetPlugins();

				for (int i = 0; i < plugins.count(); i++)
				{
					ArchivePlugin* pPlugin = plugins[i];

					if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), NULL) )
					{
						if ( pModule->QueryCapability(AMF_SUPPORT_SINGLE_PLUGIN_QUERY) )
						{
							Array<ArchiveFormat*>& formats = pPlugin->GetFormats();

				    		for (int j = 0; j < formats.count(); j++)
				    		{
				    			ArchiveFormat* pFormat = formats[i];

	        		    		if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
					    		{
					    			if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
										pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
								}
								else
									bNoFormatsFiltered = false;
				    		}

				    		if ( bNoFormatsFiltered && !pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
				    			pModule->QueryArchives(&pPlugin->GetUID(), NULL, lpFileName, pBuffer, dwBufferSize, result);
						}
						else
							bNoPluginsFiltered = false;
					}

					if ( bNoPluginsFiltered && !pModule->QueryCapability(AMF_SUPPORT_SINGLE_PLUGIN_QUERY) )
						pModule->QueryArchives(NULL, NULL, lpFileName, pBuffer, dwBufferSize, result);
				}
			}
			else
			{
				if ( pFE->bAllFormats )
				{
			    	ArchivePlugin* pPlugin = pFE->pPlugin;

					if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), NULL) )
					{
						Array<ArchiveFormat*>& formats = pPlugin->GetFormats();

						for (int i = 0; i < formats.count(); i++)
						{
							ArchiveFormat* pFormat = formats[i];

							if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
							{
								if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
									pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
							}
							else
								bNoFormatsFiltered = false;

						}

						if ( bNoFormatsFiltered && !pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
			    			pModule->QueryArchives(&pPlugin->GetUID(), NULL, lpFileName, pBuffer, dwBufferSize, result);
					}
				}
				else
				{
					ArchiveFormat* pFormat = pFE->pFormat;
					ArchivePlugin* pPlugin = pFE->pPlugin;

					if  ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
					{
						if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
							pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
					}
				}
			}
		}

		m_pFilter->AddStopFilter(filters[i]);
	}

	if ( !bStopped && m_pFilter->UseRemaining() )
	{
		for (int i = 0; i < m_pModules.count(); i++)
		{
			bool bNoPluginsFiltered = true;
			bool bNoFormatsFiltered = true;

			ArchiveModule* pModule = m_pModules[i];

			if ( !m_pFilter->Filtered(&pModule->GetUID(), NULL, NULL) )
			{
				Array<ArchivePlugin*>& plugins = pModule->GetPlugins();

				for (unsigned int j = 0; j < plugins.count(); j++)
				{
					ArchivePlugin* pPlugin = plugins[j];

					if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), NULL) )
					{
						Array<ArchiveFormat*>& formats = pPlugin->GetFormats();

						for (unsigned int k = 0; k < formats.count(); k++)
						{
							ArchiveFormat* pFormat = formats[k];

							if ( !m_pFilter->Filtered(&pModule->GetUID(), &pPlugin->GetUID(), &pFormat->GetUID()) )
							{
								if ( pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
									pModule->QueryArchives(&pPlugin->GetUID(), &pFormat->GetUID(), lpFileName, pBuffer, dwBufferSize, result);
							}
							else
								bNoFormatsFiltered = false;
						}

						if ( bNoFormatsFiltered && !pPlugin->QueryCapability(APF_SUPPORT_SINGLE_FORMAT_QUERY) )
							pModule->QueryArchives(&pPlugin->GetUID(), NULL, lpFileName, pBuffer, dwBufferSize, result);

					}
					else
						bNoPluginsFiltered = false;
				}

				if ( bNoPluginsFiltered && !pModule->QueryCapability(AMF_SUPPORT_SINGLE_PLUGIN_QUERY) )
					pModule->QueryArchives(NULL, NULL, lpFileName, pBuffer, dwBufferSize, result);
			}
		}
	}

	return 0;
}

ArchiveModule* ArchiveModuleManager::GetModule(const GUID &uid)
{
	for (int i = 0; i < m_pModules.count(); i++)
	{
		if ( m_pModules[i]->GetUID() == uid )
			return m_pModules[i];
	}

	return NULL;
}

int ArchiveModuleManager::GetPlugins(Array<ArchivePlugin*>& plugins)
{
	for (int i = 0; i < m_pModules.count(); i++)
		m_pModules[i]->GetPlugins(plugins);

	return 0;
}

ArchiveFormat* ArchiveModuleManager::GetFormat(const GUID& uidModule, const GUID& uidPlugin, const GUID& uidFormat)
{
	ArchiveModule* pModule = GetModule(uidModule);

	if ( pModule )
		return pModule->GetFormat(uidPlugin, uidFormat);

	return NULL;
}

int ArchiveModuleManager::GetFormats(Array<ArchiveFormat*>& formats)
{
	for (int i = 0; i < m_pModules.count(); i++)
		m_pModules[i]->GetFormats(formats);

	return 0;
}

Array<ArchiveModule*>& ArchiveModuleManager::GetModules()
{
	return m_pModules;
}

Archive* ArchiveModuleManager::OpenCreateArchive(
		ArchiveFormat* pFormat,
		const TCHAR* lpFileName,
		HANDLE hCallback,
		ARCHIVECALLBACK pfnCallback,
		bool bCreate
		)
{
	HANDLE hArchive = pFormat->GetModule()->OpenCreateArchive(
				pFormat->GetPlugin()->GetUID(),
				pFormat->GetUID(),
				lpFileName,
				hCallback,
				pfnCallback,
				bCreate
				);

	if ( hArchive )
		return new Archive(hArchive, pFormat, lpFileName);

	if ( bCreate )
		return new Archive(NULL, pFormat, lpFileName);

	return NULL;
}

void ArchiveModuleManager::CloseArchive(Archive* pArchive)
{
	if ( pArchive->GetHandle() )
		pArchive->GetModule()->CloseArchive(pArchive->GetPlugin()->GetUID(), pArchive->GetHandle());

	delete pArchive;
}

bool ArchiveModuleManager::LoadTemplates(const TCHAR* lpFileName)
{
	TCHAR szNames[4096];

	if ( !GetPrivateProfileSectionNames(szNames, 4096, lpFileName) )
		return false;

	m_pTemplates.reset();
	Array<const TCHAR*> names;

	const TCHAR *lpName = szNames;

	while ( *lpName )
	{
		names.add(lpName);
		lpName += _tcslen (lpName)+1;
	}

	for (int i = 0; i < names.count(); i++)
	{
		string strName = names[i];
		string strParams;

		TCHAR* pBuffer = strParams.GetBuffer(260); //BUGBUG
		GetPrivateProfileString(strName, _T("Params"), _T(""), pBuffer, 260, lpFileName);
		strParams.ReleaseBuffer();

		TCHAR szGUID[64];

		GetPrivateProfileString(strName, _T("ModuleUID"), _T(""), szGUID, 64, lpFileName);
		GUID uidModule = STR2GUID(szGUID);

		GetPrivateProfileString(strName, _T("PluginUID"), _T(""), szGUID, 64, lpFileName);
		GUID uidPlugin = STR2GUID(szGUID);

		GetPrivateProfileString(strName, _T("FormatUID"), _T(""), szGUID, 64, lpFileName);
		GUID uidFormat = STR2GUID(szGUID);

		m_pTemplates.add(new ArchiveTemplate(this, strName, strParams, uidModule, uidPlugin, uidFormat));
	}

	return true;
}

bool ArchiveModuleManager::SaveTemplates(const TCHAR* lpFileName)
{
	DeleteFile(lpFileName);

	string strFileName = Info.ModuleName;

	CutToSlash(strFileName);
	strFileName += lpFileName;

	for (int i = 0; i < m_pTemplates.count(); i++)
	{
		ArchiveTemplate* pAT = m_pTemplates[i];

		WritePrivateProfileString(pAT->GetName(), _T("Params"), pAT->GetParams(), strFileName);
		WritePrivateProfileString(pAT->GetName(), _T("ModuleUID"), GUID2STR(pAT->GetModuleUID()), strFileName);
		WritePrivateProfileString(pAT->GetName(), _T("PluginUID"), GUID2STR(pAT->GetPluginUID()), strFileName);
		WritePrivateProfileString(pAT->GetName(), _T("FormatUID"), GUID2STR(pAT->GetFormatUID()), strFileName);
	}

	return true;
}

Array<ArchiveTemplate*>& ArchiveModuleManager::GetTemplates()
{
	return m_pTemplates;
}<|endoftext|>


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

#include "modules/planning/planning.h"

#include <algorithm>
#include <vector>

#include "google/protobuf/repeated_field.h"

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/reference_line/reference_line_provider.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const {
  return "planning";
}

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

Status Planning::InitFrame(const uint32_t sequence_num, const double timestamp,
                           const TrajectoryPoint& init_adc_point) {
  frame_.reset(new Frame(sequence_num));
  frame_->SetPlanningStartPoint(init_adc_point);

  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return Status(ErrorCode::PLANNING_ERROR, "routing is empty");
  }
  frame_->UpdateRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());

  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction: " << prediction.DebugString();
  }

  auto status = frame_->Init(config_, timestamp);
  if (!status.ok()) {
    AERROR << "failed to init frame";
    return Status(ErrorCode::PLANNING_ERROR, "init frame failed");
  }
  return Status::OK();
}

bool Planning::HasSignalLight(const PlanningConfig& config) {
  for (const auto& rule_config : config.rule_config()) {
    if (rule_config.rule_id() == RuleConfig::SIGNAL_LIGHT) {
      return true;
    }
  }
  return false;
}

Status Planning::Init() {
  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  CHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
  Frame::SetMap(hdmap_);

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                               &config_))
      << "failed to load planning config file " << FLAGS_planning_config_file;
  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_filename);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_prediction && AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (HasSignalLight(config_) &&
      AdapterManager::GetTrafficLightDetection() == nullptr) {
    std::string error_msg("Traffic Light Detection is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_reference_line_provider_thread) {
    ReferenceLineProvider::instance()->Init(
        hdmap_, config_.reference_line_smoother_config());
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

bool Planning::IsVehicleStateValid(const common::VehicleState& vehicle_state) {
  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
      std::isnan(vehicle_state.kappa()) ||
      std::isnan(vehicle_state.linear_velocity()) ||
      std::isnan(vehicle_state.linear_acceleration())) {
    return false;
  }
  return true;
}

Status Planning::Start() {
  if (FLAGS_enable_reference_line_provider_thread) {
    ReferenceLineProvider::instance()->Start();
  }
  timer_ = AdapterManager::CreateTimer(
      ros::Duration(1.0 / FLAGS_planning_loop_rate), &Planning::OnTimer, this);
  return Status::OK();
}

void Planning::OnTimer(const ros::TimerEvent&) {
  RunOnce();
  if (frame_) {
    auto seq_num = frame_->SequenceNum();
    FrameHistory::instance()->Add(seq_num, std::move(frame_));
  }
}

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
                                 double timestamp) {
  AdapterManager::FillPlanningHeader(Name(), trajectory_pb);
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  AdapterManager::PublishPlanning(*trajectory_pb);
}

void Planning::RunOnce() {
  const double start_timestamp = Clock::NowInSecond();
  AdapterManager::Observe();
  ADCTrajectory not_ready_pb;
  auto* not_ready = not_ready_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
  if (AdapterManager::GetLocalization()->Empty()) {
    not_ready->set_reason("localization not ready");
  } else if (AdapterManager::GetChassis()->Empty()) {
    not_ready->set_reason("chassis not ready");
  } else if (AdapterManager::GetRoutingResponse()->Empty()) {
    not_ready->set_reason("routing not ready");
  } else if (FLAGS_enable_prediction &&
             AdapterManager::GetPrediction()->Empty()) {
    not_ready->set_reason("prediction not ready");
  }
  if (not_ready->has_reason()) {
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::Status status =
      common::VehicleState::instance()->Update(localization, chassis);
  DCHECK(IsVehicleStateValid(*common::VehicleState::instance()));

  if (!status.ok()) {
    AERROR << "Update VehicleState failed.";
    not_ready->set_reason("Update VehicleState failed.");
    status.Save(not_ready_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }
  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool is_replan = false;
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_auto_mode, start_timestamp, planning_cycle_time,
          last_publishable_trajectory_.get(), &is_replan);

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  status = InitFrame(frame_num, start_timestamp, stitching_trajectory.back());
  ADCTrajectory trajectory_pb;
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(trajectory_pb.mutable_debug());
  }
  trajectory_pb.mutable_latency_stats()->set_init_frame_time_ms(
      Clock::NowInSecond() - start_timestamp);
  if (!status.ok()) {
    AERROR << "Init frame failed";
    if (FLAGS_publish_estop) {
      ADCTrajectory estop;
      estop.mutable_estop();
      status.Save(estop.mutable_header()->mutable_status());
      PublishPlanningPb(&estop, start_timestamp);
    }
    return;
  }

  status = Plan(start_timestamp, stitching_trajectory, &trajectory_pb);

  const auto time_diff_ms = (Clock::NowInSecond() - start_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";

  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (status.ok()) {
    trajectory_pb.set_is_replan(is_replan);
    PublishPlanningPb(&trajectory_pb, start_timestamp);
    ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
  } else if (FLAGS_publish_estop) {
    trajectory_pb.mutable_estop();
    status.Save(trajectory_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&trajectory_pb, start_timestamp);
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {
  AERROR << "Planning Stop is called";
  if (FLAGS_enable_reference_line_provider_thread) {
    ReferenceLineProvider::instance()->Stop();
  }
  last_publishable_trajectory_.reset(nullptr);
  frame_.reset(nullptr);
  planner_.reset(nullptr);
}

void Planning::SetLastPublishableTrajectory(
    const ADCTrajectory& adc_trajectory) {
  last_publishable_trajectory_.reset(new PublishableTrajectory(adc_trajectory));
}

common::Status Planning::Plan(
    const double current_time_stamp,
    const std::vector<common::TrajectoryPoint>& stitching_trajectory,
    ADCTrajectory* trajectory_pb) {
  auto* ptr_debug = trajectory_pb->mutable_debug();
  if (FLAGS_enable_record_debug) {
    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
        stitching_trajectory.back());
  }
  auto status = Status::OK();
  for (auto& reference_line_info : frame_->reference_line_info()) {
    status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                            &reference_line_info);
    AERROR_IF(!status.ok()) << "planner failed to make a driving plan.";
  }

  const auto* best_reference_line = frame_->FindDriveReferenceLineInfo();
  if (!best_reference_line) {
    std::string msg(
        "planner failed to make a driving plan because NO best_reference_line "
        "can be provided.");
    AERROR << msg;
    last_publishable_trajectory_->Clear();
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  ptr_debug->MergeFrom(best_reference_line->debug());
  trajectory_pb->mutable_latency_stats()->MergeFrom(
      best_reference_line->latency_stats());

  best_reference_line->ExportDecision(trajectory_pb->mutable_decision());

  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_reference_line->reference_line().reference_points();
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
    }
  }

  last_publishable_trajectory_.reset(new PublishableTrajectory(
      current_time_stamp, best_reference_line->trajectory()));

  last_publishable_trajectory_->PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);

  return status;
}

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

#include "modules/planning/planning.h"

#include <algorithm>
#include <vector>

#include "google/protobuf/repeated_field.h"

#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/reference_line/reference_line_provider.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const {
  return "planning";
}

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

Status Planning::InitFrame(const uint32_t sequence_num, const double timestamp,
                           const TrajectoryPoint& init_adc_point) {
  frame_.reset(new Frame(sequence_num));
  frame_->SetPlanningStartPoint(init_adc_point);

  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return Status(ErrorCode::PLANNING_ERROR, "routing is empty");
  }
  frame_->UpdateRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());

  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction: " << prediction.DebugString();
  }

  auto status = frame_->Init(config_, timestamp);
  if (!status.ok()) {
    AERROR << "failed to init frame";
    return Status(ErrorCode::PLANNING_ERROR, "init frame failed");
  }
  return Status::OK();
}

bool Planning::HasSignalLight(const PlanningConfig& config) {
  for (const auto& rule_config : config.rule_config()) {
    if (rule_config.rule_id() == RuleConfig::SIGNAL_LIGHT) {
      return true;
    }
  }
  return false;
}

Status Planning::Init() {
  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  CHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
  Frame::SetMap(hdmap_);

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                               &config_))
      << "failed to load planning config file " << FLAGS_planning_config_file;
  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_filename);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_prediction && AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (HasSignalLight(config_) &&
      AdapterManager::GetTrafficLightDetection() == nullptr) {
    std::string error_msg("Traffic Light Detection is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (FLAGS_enable_reference_line_provider_thread) {
    ReferenceLineProvider::instance()->Init(
        hdmap_, config_.reference_line_smoother_config());
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

bool Planning::IsVehicleStateValid(const common::VehicleState& vehicle_state) {
  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
      std::isnan(vehicle_state.kappa()) ||
      std::isnan(vehicle_state.linear_velocity()) ||
      std::isnan(vehicle_state.linear_acceleration())) {
    return false;
  }
  return true;
}

Status Planning::Start() {
  if (FLAGS_enable_reference_line_provider_thread) {
    ReferenceLineProvider::instance()->Start();
  }
  timer_ = AdapterManager::CreateTimer(
      ros::Duration(1.0 / FLAGS_planning_loop_rate), &Planning::OnTimer, this);
  return Status::OK();
}

void Planning::OnTimer(const ros::TimerEvent&) {
  RunOnce();
}

void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
                                 double timestamp) {
  AdapterManager::FillPlanningHeader(Name(), trajectory_pb);
  trajectory_pb->mutable_header()->set_timestamp_sec(timestamp);
  // TODO(all): integrate reverse gear
  trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
  AdapterManager::PublishPlanning(*trajectory_pb);
}

void Planning::RunOnce() {
  const double start_timestamp = Clock::NowInSecond();
  AdapterManager::Observe();
  ADCTrajectory not_ready_pb;
  auto* not_ready = not_ready_pb.mutable_decision()
                        ->mutable_main_decision()
                        ->mutable_not_ready();
  if (AdapterManager::GetLocalization()->Empty()) {
    not_ready->set_reason("localization not ready");
  } else if (AdapterManager::GetChassis()->Empty()) {
    not_ready->set_reason("chassis not ready");
  } else if (AdapterManager::GetRoutingResponse()->Empty()) {
    not_ready->set_reason("routing not ready");
  } else if (FLAGS_enable_prediction &&
             AdapterManager::GetPrediction()->Empty()) {
    not_ready->set_reason("prediction not ready");
  }
  if (not_ready->has_reason()) {
    AERROR << not_ready->reason() << "; skip the planning cycle.";
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::Status status =
      common::VehicleState::instance()->Update(localization, chassis);
  DCHECK(IsVehicleStateValid(*common::VehicleState::instance()));

  if (!status.ok()) {
    AERROR << "Update VehicleState failed.";
    not_ready->set_reason("Update VehicleState failed.");
    status.Save(not_ready_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&not_ready_pb, start_timestamp);
    return;
  }
  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool is_replan = false;
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_auto_mode, start_timestamp, planning_cycle_time,
          last_publishable_trajectory_.get(), &is_replan);

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  status = InitFrame(frame_num, start_timestamp, stitching_trajectory.back());
  ADCTrajectory trajectory_pb;
  if (FLAGS_enable_record_debug) {
    frame_->RecordInputDebug(trajectory_pb.mutable_debug());
  }
  trajectory_pb.mutable_latency_stats()->set_init_frame_time_ms(
      Clock::NowInSecond() - start_timestamp);
  if (!status.ok()) {
    AERROR << "Init frame failed";
    if (FLAGS_publish_estop) {
      ADCTrajectory estop;
      estop.mutable_estop();
      status.Save(estop.mutable_header()->mutable_status());
      PublishPlanningPb(&estop, start_timestamp);
    }
    if (frame_) {
      auto seq_num = frame_->SequenceNum();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    return;
  }

  status = Plan(start_timestamp, stitching_trajectory, &trajectory_pb);

  const auto time_diff_ms = (Clock::NowInSecond() - start_timestamp) * 1000;
  ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";

  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (status.ok()) {
    trajectory_pb.set_is_replan(is_replan);
    PublishPlanningPb(&trajectory_pb, start_timestamp);
    ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
  } else if (FLAGS_publish_estop) {
    trajectory_pb.mutable_estop();
    status.Save(trajectory_pb.mutable_header()->mutable_status());
    PublishPlanningPb(&trajectory_pb, start_timestamp);
    AERROR << "Planning failed";
  }
  if (frame_) {
    auto seq_num = frame_->SequenceNum();
    FrameHistory::instance()->Add(seq_num, std::move(frame_));
  }
}

void Planning::Stop() {
  AERROR << "Planning Stop is called";
  if (FLAGS_enable_reference_line_provider_thread) {
    ReferenceLineProvider::instance()->Stop();
  }
  last_publishable_trajectory_.reset(nullptr);
  frame_.reset(nullptr);
  planner_.reset(nullptr);
}

void Planning::SetLastPublishableTrajectory(
    const ADCTrajectory& adc_trajectory) {
  last_publishable_trajectory_.reset(new PublishableTrajectory(adc_trajectory));
}

common::Status Planning::Plan(
    const double current_time_stamp,
    const std::vector<common::TrajectoryPoint>& stitching_trajectory,
    ADCTrajectory* trajectory_pb) {
  auto* ptr_debug = trajectory_pb->mutable_debug();
  if (FLAGS_enable_record_debug) {
    ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom(
        stitching_trajectory.back());
  }
  auto status = Status::OK();
  for (auto& reference_line_info : frame_->reference_line_info()) {
    status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                            &reference_line_info);
    AERROR_IF(!status.ok()) << "planner failed to make a driving plan.";
  }

  const auto* best_reference_line = frame_->FindDriveReferenceLineInfo();
  if (!best_reference_line) {
    std::string msg(
        "planner failed to make a driving plan because NO best_reference_line "
        "can be provided.");
    AERROR << msg;
    last_publishable_trajectory_->Clear();
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  ptr_debug->MergeFrom(best_reference_line->debug());
  trajectory_pb->mutable_latency_stats()->MergeFrom(
      best_reference_line->latency_stats());

  best_reference_line->ExportDecision(trajectory_pb->mutable_decision());

  // Add debug information.
  if (FLAGS_enable_record_debug) {
    auto* reference_line = ptr_debug->mutable_planning_data()->add_path();
    reference_line->set_name("planning_reference_line");
    const auto& reference_points =
        best_reference_line->reference_line().reference_points();
    for (const auto& reference_point : reference_points) {
      auto* path_point = reference_line->add_path_point();
      path_point->set_x(reference_point.x());
      path_point->set_y(reference_point.y());
      path_point->set_theta(reference_point.heading());
      path_point->set_kappa(reference_point.kappa());
      path_point->set_dkappa(reference_point.dkappa());
    }
  }

  last_publishable_trajectory_.reset(new PublishableTrajectory(
      current_time_stamp, best_reference_line->trajectory()));

  last_publishable_trajectory_->PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  last_publishable_trajectory_->PopulateTrajectoryProtobuf(trajectory_pb);

  return status;
}

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