<commit_before>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * 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 files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// TriMesh code by Erwin de Vries.

#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
	dxTriMesh* TriMesh = (dxTriMesh*)g1;

	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);

	RayCollider& Collider = TriMesh->_RayCollider;

	dReal Length = dGeomRayGetLength(RayGeom);

	int FirstContact, BackfaceCull;
	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
	int ClosestHit = dGeomRayGetClosestHit(RayGeom);

	Collider.SetFirstContact(FirstContact != 0);
	Collider.SetClosestHit(ClosestHit != 0);
	Collider.SetCulling(BackfaceCull != 0);
	Collider.SetMaxDist(Length);

	dVector3 Origin, Direction;
	dGeomRayGet(RayGeom, Origin, Direction);

	/* Make Ray */
	Ray WorldRay;
	WorldRay.mOrig.x = Origin[0];
	WorldRay.mOrig.y = Origin[1];
	WorldRay.mOrig.z = Origin[2];
	WorldRay.mDir.x = Direction[0];
	WorldRay.mDir.y = Direction[1];
	WorldRay.mDir.z = Direction[2];

	/* Intersect */
	Matrix4x4 amatrix;
	Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix));

	/* Retrieve data */
	int TriCount = TriMesh->Faces.GetNbFaces();

	if (TriCount != 0){
		const CollisionFace* Faces = TriMesh->Faces.GetFaces();

		int OutTriCount = 0;
		for (int i = 0; i < TriCount; i++){
			if (OutTriCount == (Flags & 0xffff)){
				break;
			}

			if (TriMesh->RayCallback == null || TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, Faces[i].mU, Faces[i].mV)){
				const int& TriIndex = Faces[i].mFaceID;

				if (!Callback(TriMesh, RayGeom, TriIndex)) continue;

				dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);

				dVector3 dv[3];
				FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);

				float T = Faces[i].mDistance;
				Contact->pos[0] = Origin[0] + (Direction[0] * T);
				Contact->pos[1] = Origin[1] + (Direction[1] * T);
				Contact->pos[2] = Origin[2] + (Direction[2] * T);
				Contact->pos[3] = REAL(0.0);

				dVector3 vu;
				vu[0] = dv[1][0] - dv[0][0];
				vu[1] = dv[1][1] - dv[0][1];
				vu[2] = dv[1][2] - dv[0][2];
				vu[3] = REAL(0.0);

				dVector3 vv;
				vv[0] = dv[2][0] - dv[0][0];
				vv[1] = dv[2][1] - dv[0][1];
				vv[2] = dv[2][2] - dv[0][2];
				vv[3] = REAL(0.0);

				dCROSS(Contact->normal, =, vv, vu);	// Reversed

				dNormalize3(Contact->normal);

				Contact->depth = T;
				Contact->g1 = TriMesh;
				Contact->g2 = RayGeom;

				OutTriCount++;
			}
		}
		return OutTriCount;
	}
	else return 0;
}
<commit_msg>ray-triangle collider correctness fix from Joe Ante <joe@uti.is><commit_after>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * 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 files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// TriMesh code by Erwin de Vries.

#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"

#define TRIMESH_INTERNAL
#include "collision_trimesh_internal.h"

int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
	dxTriMesh* TriMesh = (dxTriMesh*)g1;

	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);

	RayCollider& Collider = TriMesh->_RayCollider;

	dReal Length = dGeomRayGetLength(RayGeom);

	int FirstContact, BackfaceCull;
	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
	int ClosestHit = dGeomRayGetClosestHit(RayGeom);

	Collider.SetFirstContact(FirstContact != 0);
	Collider.SetClosestHit(ClosestHit != 0);
	Collider.SetCulling(BackfaceCull != 0);
	Collider.SetMaxDist(Length);

	dVector3 Origin, Direction;
	dGeomRayGet(RayGeom, Origin, Direction);

	/* Make Ray */
	Ray WorldRay;
	WorldRay.mOrig.x = Origin[0];
	WorldRay.mOrig.y = Origin[1];
	WorldRay.mOrig.z = Origin[2];
	WorldRay.mDir.x = Direction[0];
	WorldRay.mDir.y = Direction[1];
	WorldRay.mDir.z = Direction[2];

	/* Intersect */
	Matrix4x4 amatrix;
        int TriCount = 0;
        if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
                TriCount = TriMesh->Faces.GetNbFaces();
        }

        if (TriCount == 0) {
                return 0;
        }

	const CollisionFace* Faces = TriMesh->Faces.GetFaces();

	int OutTriCount = 0;
	for (int i = 0; i < TriCount; i++) {
		if (OutTriCount == (Flags & 0xffff)) {
			break;
		}
		if (TriMesh->RayCallback == null ||
                    TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
                                         Faces[i].mU, Faces[i].mV)) {
			const int& TriIndex = Faces[i].mFaceID;
			if (!Callback(TriMesh, RayGeom, TriIndex)) {
                                continue;
                        }

			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);

			dVector3 dv[3];
			FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);

			float T = Faces[i].mDistance;
			Contact->pos[0] = Origin[0] + (Direction[0] * T);
			Contact->pos[1] = Origin[1] + (Direction[1] * T);
			Contact->pos[2] = Origin[2] + (Direction[2] * T);
			Contact->pos[3] = REAL(0.0);

			dVector3 vu;
			vu[0] = dv[1][0] - dv[0][0];
			vu[1] = dv[1][1] - dv[0][1];
			vu[2] = dv[1][2] - dv[0][2];
			vu[3] = REAL(0.0);

			dVector3 vv;
			vv[0] = dv[2][0] - dv[0][0];
			vv[1] = dv[2][1] - dv[0][1];
			vv[2] = dv[2][2] - dv[0][2];
			vv[3] = REAL(0.0);

			dCROSS(Contact->normal, =, vv, vu);	// Reversed

			dNormalize3(Contact->normal);

			Contact->depth = T;
			Contact->g1 = TriMesh;
			Contact->g2 = RayGeom;

			OutTriCount++;
		}
	}
	return OutTriCount;
}
<|endoftext|>


<commit_before>/* Calls the perception teams' MAP interface API */

#include <mex.h>

#include <Eigen/Dense>
#include <lcm/lcm.h>
//#include <lcm/lcm-cpp.hpp>
#include <boost/thread/thread.hpp>

#include <maps/ViewClient.hpp>
#include <maps/BotWrapper.hpp>
#include <maps/DepthImageView.hpp>

#include <lcmtypes/drc/data_request_t.hpp>

using namespace Eigen;
using namespace maps;

struct ViewWrapperData {
  //  boost::shared_ptr<lcm::LCM> lcm;
  lcm_t* lcm;
  ViewClient* view_client;
  boost::thread* lcm_thread;
  bool b_interrupt_lcm;
};

void lcmThreadMain(struct ViewWrapperData* pdata) {
  //  while(0 == pdata->lcm->handle()) {
  while (0 == lcm_handle(pdata->lcm)) {
    if (pdata->b_interrupt_lcm) {
      mexPrintf("interruption requested\n"); mexEvalString("drawnow");
      break;
    }
  }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  // usage:
  // ptr = mapAPIwrapper()  - initializes MAP interface
  // mapAPIwrapper(ptr)     - frees MAP interface
  // [closest_terrain_pos,normal] = mapAPIwrapper(ptr,pos)
  //     - pos is a 3xn matrix
  //     - closest_terrain_pos is a 3xn matrix of the closest points on the terrain
  //     - normal is a 3xn matrix of the surface normals of the terrain at closest_terrain_pos

  struct ViewWrapperData* pdata = NULL;

  if (nrhs<1) { // then it's ptr = mapAPIWrapper()
    pdata = new struct ViewWrapperData;
    mexPrintf("creating view client\n"); mexEvalString("drawnow");
    pdata->view_client = new ViewClient();

    // create lcm instance, and fork listener thread
    mexPrintf("creating lcm\n"); mexEvalString("drawnow");
    pdata->lcm = lcm_create(NULL);
    if (!pdata->lcm)
    //    pdata->lcm.reset(new lcm::LCM());  //lcm_create(NULL);
    //    if (!pdata->lcm->good())
      mexErrMsgIdAndTxt("DRC:mapAPIwrapper:LCMFailed","Failed to create LCM instance");
    mexPrintf("spawning LCM thread\n");
    pdata->b_interrupt_lcm = false;
    pdata->lcm_thread = new boost::thread(lcmThreadMain,pdata);

    BotWrapper::Ptr botWrapper(new BotWrapper(pdata->lcm, NULL, NULL));
    pdata->view_client->setBotWrapper(botWrapper);

    // start listening for view data
    pdata->view_client->start();

    // return a pointer to the model
    mxClassID cid;
    if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
    else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("DRC:mapAPIWrapper:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));

    return;
  }

  // load the data ptr
  if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
    mexErrMsgIdAndTxt("DRC:mapAPIWrapper:BadInputs","the first argument should be the ptr");
  memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));

  if (nrhs<2) { // then free the memory and exit
    mexPrintf("stopping view client\n"); mexEvalString("drawnow");
    pdata->view_client->stop();
    mexPrintf("deleting view client\n"); mexEvalString("drawnow");
    delete pdata->view_client;
    mexPrintf("interrupting thread\n"); mexEvalString("drawnow");
    //    pdata->lcm_thread->interrupt();
    pdata->b_interrupt_lcm = true;
    mexPrintf("joining thread\n"); mexEvalString("drawnow");
    pdata->lcm_thread->join();
    mexPrintf("destroying thread\n"); mexEvalString("drawnow");
    delete pdata->lcm_thread;
    mexPrintf("done.\n"); mexEvalString("drawnow");
    lcm_destroy(pdata->lcm);
    // delete lcm handled by shared_ptr and lcm destructor
    delete pdata;
    return;
  }

  ViewClient::ViewPtr vptr = pdata->view_client->getView(drc::data_request_t::HEIGHT_MAP_SCENE);
  if (!vptr) { // check if null
    mexErrMsgIdAndTxt("DRC:mapAPIwrapper:NullViewPtr","Have not received height map via LCM yet.  Perhaps you still need to request it in the viewer?\n");
  }

  int N = mxGetN(prhs[1]);
  if (mxGetM(prhs[1])!=3) mexErrMsgIdAndTxt("DRC:mapAPIwrapper:BadInputs","pos must be 3xn\n");

  plhs[0] = mxCreateDoubleMatrix(3,N,mxREAL);
  mxArray* pmxNormal = mxCreateDoubleMatrix(3,N,mxREAL);

  Vector3f pos, closest_terrain_pos, normal;
  double *ppos = mxGetPr(prhs[1]), *pclosest_terrain_pos = mxGetPr(plhs[0]), *pnormal = mxGetPr(pmxNormal);

  int j;
  for (int i=0; i<N; i++) {
    pos << (float) ppos[3*i], (float) ppos[3*i+1], (float) ppos[3*i+2];
    if (vptr->getClosest(pos,closest_terrain_pos,normal)) {
	for (j=0; j<3; j++) {
	  pclosest_terrain_pos[3*i+j] = (double) closest_terrain_pos(j);
	  pnormal[3*i+j] = (double) normal(j);
	}
    } else { // returns false if off the grid
	for (j=0; j<3; j++) {
	  pclosest_terrain_pos[3*i+j] = NAN;
	  pnormal[3*i+j] = NAN;
	}
    }
  }

  if (nlhs>1) {
    plhs[1] = pmxNormal;
  } else {
    mxDestroyArray(pmxNormal);
  }
}

<commit_msg>more printfs reveal that it's actually the connection to botwrapper that is hanging<commit_after>/* Calls the perception teams' MAP interface API */

#include <mex.h>
#include <ctime>

#include <Eigen/Dense>
#include <lcm/lcm.h>
//#include <lcm/lcm-cpp.hpp>
#include <boost/thread/thread.hpp>

#include <maps/ViewClient.hpp>
#include <maps/BotWrapper.hpp>
#include <maps/DepthImageView.hpp>

#include <lcmtypes/drc/data_request_t.hpp>

using namespace Eigen;
using namespace maps;

struct ViewWrapperData {
  //  boost::shared_ptr<lcm::LCM> lcm;
  lcm_t* lcm;
  ViewClient* view_client;
  boost::thread* lcm_thread;
  bool b_interrupt_lcm;
};

void lcmThreadMain(struct ViewWrapperData* pdata) {
  //  while(0 == pdata->lcm->handle()) {
  while (0 == lcm_handle(pdata->lcm)) {
    if (pdata->b_interrupt_lcm) {
      mexPrintf("interruption requested\n"); mexEvalString("drawnow");
      break;
    }
  }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  // usage:
  // ptr = mapAPIwrapper()  - initializes MAP interface
  // mapAPIwrapper(ptr)     - frees MAP interface
  // [closest_terrain_pos,normal] = mapAPIwrapper(ptr,pos)
  //     - pos is a 3xn matrix
  //     - closest_terrain_pos is a 3xn matrix of the closest points on the terrain
  //     - normal is a 3xn matrix of the surface normals of the terrain at closest_terrain_pos

  struct ViewWrapperData* pdata = NULL;

  if (nrhs<1) { // then it's ptr = mapAPIWrapper()
    pdata = new struct ViewWrapperData;
    mexPrintf("creating view client\n"); mexEvalString("drawnow");
    pdata->view_client = new ViewClient();

    // create lcm instance, and fork listener thread
    mexPrintf("creating lcm\n"); mexEvalString("drawnow");
    pdata->lcm = lcm_create(NULL);
    if (!pdata->lcm)
    //    pdata->lcm.reset(new lcm::LCM());  //lcm_create(NULL);
    //    if (!pdata->lcm->good())
      mexErrMsgIdAndTxt("DRC:mapAPIwrapper:LCMFailed","Failed to create LCM instance");
    mexPrintf("spawning LCM thread\n"); mexEvalString("drawnow");
    pdata->b_interrupt_lcm = false;
    pdata->lcm_thread = new boost::thread(lcmThreadMain,pdata);

    mexPrintf("creating botwrapper\n"); mexEvalString("drawnow");
    BotWrapper::Ptr botWrapper(new BotWrapper(pdata->lcm, NULL, NULL));
    mexPrintf("setting botwrapper in view client\n"); mexEvalString("drawnow");
    pdata->view_client->setBotWrapper(botWrapper);

    // start listening for view data
    mexPrintf("starting view client\n"); mexEvalString("drawnow");
    pdata->view_client->start();

    // return a pointer to the model
    mxClassID cid;
    if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
    else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("DRC:mapAPIWrapper:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));

    return;
  }

  // load the data ptr
  if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
    mexErrMsgIdAndTxt("DRC:mapAPIWrapper:BadInputs","the first argument should be the ptr");
  memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));

  if (nrhs<2) { // then free the memory and exit
    mexPrintf("stopping view client\n"); mexEvalString("drawnow");
    pdata->view_client->stop();
    mexPrintf("deleting view client\n"); mexEvalString("drawnow");
    delete pdata->view_client;
    mexPrintf("interrupting thread\n"); mexEvalString("drawnow");
    //    pdata->lcm_thread->interrupt();
    pdata->b_interrupt_lcm = true;
    mexPrintf("joining thread\n"); mexEvalString("drawnow");
    pdata->lcm_thread->join();
    mexPrintf("destroying thread\n"); mexEvalString("drawnow");
    delete pdata->lcm_thread;
    mexPrintf("done.\n"); mexEvalString("drawnow");
    lcm_destroy(pdata->lcm);
    // delete lcm handled by shared_ptr and lcm destructor
    delete pdata;
    return;
  }

  ViewClient::ViewPtr vptr = pdata->view_client->getView(drc::data_request_t::HEIGHT_MAP_SCENE);
  if (!vptr) { // check if null
    mexErrMsgIdAndTxt("DRC:mapAPIwrapper:NullViewPtr","Have not received height map via LCM yet.  Perhaps you still need to request it in the viewer?\n");
  }

  int N = mxGetN(prhs[1]);
  if (mxGetM(prhs[1])!=3) mexErrMsgIdAndTxt("DRC:mapAPIwrapper:BadInputs","pos must be 3xn\n");

  plhs[0] = mxCreateDoubleMatrix(3,N,mxREAL);
  mxArray* pmxNormal = mxCreateDoubleMatrix(3,N,mxREAL);

  Vector3f pos, closest_terrain_pos, normal;
  double *ppos = mxGetPr(prhs[1]), *pclosest_terrain_pos = mxGetPr(plhs[0]), *pnormal = mxGetPr(pmxNormal);

  int j;
  for (int i=0; i<N; i++) {
    pos << (float) ppos[3*i], (float) ppos[3*i+1], (float) ppos[3*i+2];
    if (vptr->getClosest(pos,closest_terrain_pos,normal)) {
	for (j=0; j<3; j++) {
	  pclosest_terrain_pos[3*i+j] = (double) closest_terrain_pos(j);
	  pnormal[3*i+j] = (double) normal(j);
	}
    } else { // returns false if off the grid
	for (j=0; j<3; j++) {
	  pclosest_terrain_pos[3*i+j] = NAN;
	  pnormal[3*i+j] = NAN;
	}
    }
  }

  if (nlhs>1) {
    plhs[1] = pmxNormal;
  } else {
    mxDestroyArray(pmxNormal);
  }
}

<|endoftext|>


<commit_before>/// \file
/// \ingroup tutorial_tmva
/// \notebook -nodraw
/// This macro provides an example of how to use TMVA for k-folds cross
/// evaluation.
///
/// As input data is used a toy-MC sample consisting of two guassian
/// distributions.
///
/// The output file "TMVA.root" can be analysed with the use of dedicated
/// macros (simply say: root -l <macro.C>), which can be conveniently
/// invoked through a GUI that will appear at the end of the run of this macro.
/// Launch the GUI via the command:
///
/// ```
/// root -l -e 'TMVA::TMVAGui("TMVA.root")'
/// ```
///
/// ## Cross Evaluation
/// Cross evaluation is a special case of k-folds cross validation where the
/// splitting into k folds is computed deterministically. This ensures that the
/// a given event will always end up in the same fold.
///
/// In addition all resulting classifiers are saved and can be applied to new
/// data using `MethodCrossValidation`. One requirement for this to work is a
/// splitting function that is evaluated for each event to determine into what
/// fold it goes (for training/evaluation) or to what classifier (for
/// application).
///
/// ## Split Expression
/// Cross evaluation uses a deterministic split to partition the data into
/// folds called the split expression. The expression can be any valid
/// `TFormula` as long as all parts used are defined.
///
/// For each event the split expression is evaluated to a number and the event
/// is put in the fold corresponding to that number.
///
/// It is recommended to always use `%int([NumFolds])` at the end of the
/// expression.
///
/// The split expression has access to all spectators and variables defined in
/// the dataloader. Additionally, the number of folds in the split can be
/// accessed with `NumFolds` (or `numFolds`).
///
/// ### Example
///  ```
///  "int(fabs([eventID]))%int([NumFolds])"
///  ```
///
/// - Project   : TMVA - a ROOT-integrated toolkit for multivariate data analysis
/// - Package   : TMVA
/// - Root Macro: TMVACrossEvaluation
///
/// \macro_output
/// \macro_code
/// \author Kim Albertsson (adapted from code originally by Andreas Hoecker)

#include <cstdlib>
#include <iostream>
#include <map>
#include <string>

#include "TChain.h"
#include "TFile.h"
#include "TTree.h"
#include "TString.h"
#include "TObjString.h"
#include "TSystem.h"
#include "TROOT.h"

#include "TMVA/Factory.h"
#include "TMVA/DataLoader.h"
#include "TMVA/Tools.h"
#include "TMVA/TMVAGui.h"

// Helper function to load data into TTrees.
TTree *genTree(Int_t nPoints, Double_t offset, Double_t scale, UInt_t seed = 100)
{
   TRandom3 rng(seed);
   Double_t x = 0;
   Double_t y = 0;
   UInt_t eventID = 0;

   TTree *data = new TTree();
   data->Branch("x", &x, "x/D");
   data->Branch("y", &y, "y/D");
   data->Branch("eventID", &eventID, "eventID/I");

   for (Int_t n = 0; n < nPoints; ++n) {
      x = rng.Gaus(offset, scale);
      y = rng.Gaus(offset, scale);

      // For our simple example it is enough that the id's are uniformly
      // distributed and independent of the data.
      ++eventID;

      data->Fill();
   }

   // Important: Disconnects the tree from the memory locations of x and y.
   data->ResetBranchAddresses();
   return data;
}

int TMVACrossEvaluation()
{
   // This loads the library
   TMVA::Tools::Instance();

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

   // Load the data into TTrees. If you load data from file you can use a
   // variant of
   // ```
   // TString filename = "/path/to/file";
   // TFile * input = TFile::Open( filename );
   // TTree * signalTree = (TTree*)input->Get("TreeName");
   // ```
   TTree *sigTree = genTree(1000, 1.0, 1.0, 100);
   TTree *bkgTree = genTree(1000, -1.0, 1.0, 101);

   // Create a ROOT output file where TMVA will store ntuples, histograms, etc.
   TString outfileName("TMVA.root");
   TFile *outputFile = TFile::Open(outfileName, "RECREATE");

   // DataLoader definitions; We declare variables in the tree so that TMVA can
   // find them. For more information see TMVAClassification tutorial.
   TMVA::DataLoader *dataloader = new TMVA::DataLoader("dataset");

   // Data variables
   dataloader->AddVariable("x", 'F');
   dataloader->AddVariable("y", 'F');

   // Spectator used for split
   dataloader->AddSpectator("eventID", 'I');

   // Attaches the trees so they can be read from
   dataloader->AddSignalTree(sigTree, 1.0);
   dataloader->AddBackgroundTree(bkgTree, 1.0);

   // Bypasses the normal splitting mechanism. Unfortunately we must set the
   // number of events in the training and test sets to 1, otherwise the non-CV
   // part of TMVA is unhappy.
   dataloader->PrepareTrainingAndTestTree("", "",
                                          "nTest_Signal=1"
                                          ":nTest_Background=1"
                                          ":SplitMode=Random"
                                          ":NormMode=NumEvents"
                                          ":!V");

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

   //
   // This sets up a CrossValidation class (which wraps a TMVA::Factory
   // internally) for 2-fold cross validation that splits the data on the
   // dataset spectator `eventID`.
   //
   // The idea here is that eventID should be an event number that is integral,
   // random and independent of the data, generated only once. This last
   // property ensures that if a calibration is changed the same event will
   // still be assigned the same fold.
   //
   UInt_t numFolds = 2;
   TString analysisType = "Classification";
   TString splitExpr = "int(fabs([eventID]))%int([NumFolds])";

   TString cvOptions = Form("!V"
                            ":!Silent"
                            ":ModelPersistence"
                            ":AnalysisType=%s"
                            ":NumFolds=%i"
                            ":SplitExpr=%s",
                            analysisType.Data(), numFolds, splitExpr.Data());

   TMVA::CrossValidation ce{"TMVACrossEvaluation", dataloader, outputFile, cvOptions};

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

   //
   // Books a method to use for evaluation
   //
   ce.BookMethod(TMVA::Types::kBDT, "BDTG",
                 "!H:!V:NTrees=100:MinNodeSize=2.5%:BoostType=Grad:"
                 "Shrinkage=0.10:nCuts=20:MaxDepth=2");

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

   //
   // Train, test and evaluate the booked methods.
   // Evaluates the booked methods once for each fold and aggregates the result
   // in the specified output file.
   //
   ce.Evaluate();

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

   //
   // Process some output programatically, printing the ROC score for each
   // booked method.
   //
   auto bdtg_result = ce.GetResults()[0];
   std::cout << "==> BDTG ROC: avg (std): " << bdtg_result.GetROCAverage() << " ("
             << bdtg_result.GetROCStandardDeviation() << ")"
             << "" << std::endl;

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

   //
   // Save the output
   //
   outputFile->Close();

   std::cout << "==> Wrote root file: " << outputFile->GetName() << std::endl;
   std::cout << "==> TMVACrossEvaluation is done!" << std::endl;

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

   //
   // Launch the GUI for the root macros
   //
   if (!gROOT->IsBatch()) {
      TMVA::TMVAGui(outfileName);
   }

   return 0;
}

//
// This is used if the macro is compiled. If run through ROOT with
// `root -l -b -q MACRO.C` or similar it is unused.
//
int main(int argc, char **argv)
{
   TMVACrossEvaluation();
}
<commit_msg>[TMVA] CV - add multiple methods to CV tutorial<commit_after>/// \file
/// \ingroup tutorial_tmva
/// \notebook -nodraw
/// This macro provides an example of how to use TMVA for k-folds cross
/// evaluation.
///
/// As input data is used a toy-MC sample consisting of two guassian
/// distributions.
///
/// The output file "TMVA.root" can be analysed with the use of dedicated
/// macros (simply say: root -l <macro.C>), which can be conveniently
/// invoked through a GUI that will appear at the end of the run of this macro.
/// Launch the GUI via the command:
///
/// ```
/// root -l -e 'TMVA::TMVAGui("TMVA.root")'
/// ```
///
/// ## Cross Evaluation
/// Cross evaluation is a special case of k-folds cross validation where the
/// splitting into k folds is computed deterministically. This ensures that the
/// a given event will always end up in the same fold.
///
/// In addition all resulting classifiers are saved and can be applied to new
/// data using `MethodCrossValidation`. One requirement for this to work is a
/// splitting function that is evaluated for each event to determine into what
/// fold it goes (for training/evaluation) or to what classifier (for
/// application).
///
/// ## Split Expression
/// Cross evaluation uses a deterministic split to partition the data into
/// folds called the split expression. The expression can be any valid
/// `TFormula` as long as all parts used are defined.
///
/// For each event the split expression is evaluated to a number and the event
/// is put in the fold corresponding to that number.
///
/// It is recommended to always use `%int([NumFolds])` at the end of the
/// expression.
///
/// The split expression has access to all spectators and variables defined in
/// the dataloader. Additionally, the number of folds in the split can be
/// accessed with `NumFolds` (or `numFolds`).
///
/// ### Example
///  ```
///  "int(fabs([eventID]))%int([NumFolds])"
///  ```
///
/// - Project   : TMVA - a ROOT-integrated toolkit for multivariate data analysis
/// - Package   : TMVA
/// - Root Macro: TMVACrossEvaluation
///
/// \macro_output
/// \macro_code
/// \author Kim Albertsson (adapted from code originally by Andreas Hoecker)

#include <cstdlib>
#include <iostream>
#include <map>
#include <string>

#include "TChain.h"
#include "TFile.h"
#include "TTree.h"
#include "TString.h"
#include "TObjString.h"
#include "TSystem.h"
#include "TROOT.h"

#include "TMVA/Factory.h"
#include "TMVA/DataLoader.h"
#include "TMVA/Tools.h"
#include "TMVA/TMVAGui.h"

// Helper function to load data into TTrees.
TTree *genTree(Int_t nPoints, Double_t offset, Double_t scale, UInt_t seed = 100)
{
   TRandom3 rng(seed);
   Double_t x = 0;
   Double_t y = 0;
   UInt_t eventID = 0;

   TTree *data = new TTree();
   data->Branch("x", &x, "x/D");
   data->Branch("y", &y, "y/D");
   data->Branch("eventID", &eventID, "eventID/I");

   for (Int_t n = 0; n < nPoints; ++n) {
      x = rng.Gaus(offset, scale);
      y = rng.Gaus(offset, scale);

      // For our simple example it is enough that the id's are uniformly
      // distributed and independent of the data.
      ++eventID;

      data->Fill();
   }

   // Important: Disconnects the tree from the memory locations of x and y.
   data->ResetBranchAddresses();
   return data;
}

int TMVACrossEvaluation()
{
   // This loads the library
   TMVA::Tools::Instance();

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

   // Load the data into TTrees. If you load data from file you can use a
   // variant of
   // ```
   // TString filename = "/path/to/file";
   // TFile * input = TFile::Open( filename );
   // TTree * signalTree = (TTree*)input->Get("TreeName");
   // ```
   TTree *sigTree = genTree(1000, 1.0, 1.0, 100);
   TTree *bkgTree = genTree(1000, -1.0, 1.0, 101);

   // Create a ROOT output file where TMVA will store ntuples, histograms, etc.
   TString outfileName("TMVA.root");
   TFile *outputFile = TFile::Open(outfileName, "RECREATE");

   // DataLoader definitions; We declare variables in the tree so that TMVA can
   // find them. For more information see TMVAClassification tutorial.
   TMVA::DataLoader *dataloader = new TMVA::DataLoader("dataset");

   // Data variables
   dataloader->AddVariable("x", 'F');
   dataloader->AddVariable("y", 'F');

   // Spectator used for split
   dataloader->AddSpectator("eventID", 'I');

   // Attaches the trees so they can be read from
   dataloader->AddSignalTree(sigTree, 1.0);
   dataloader->AddBackgroundTree(bkgTree, 1.0);

   // Bypasses the normal splitting mechanism. Unfortunately we must set the
   // number of events in the training and test sets to 1, otherwise the non-CV
   // part of TMVA is unhappy.
   dataloader->PrepareTrainingAndTestTree("", "",
                                          "nTest_Signal=1"
                                          ":nTest_Background=1"
                                          ":SplitMode=Random"
                                          ":NormMode=NumEvents"
                                          ":!V");

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

   //
   // This sets up a CrossValidation class (which wraps a TMVA::Factory
   // internally) for 2-fold cross validation that splits the data on the
   // dataset spectator `eventID`.
   //
   // The idea here is that eventID should be an event number that is integral,
   // random and independent of the data, generated only once. This last
   // property ensures that if a calibration is changed the same event will
   // still be assigned the same fold.
   //
   UInt_t numFolds = 2;
   TString analysisType = "Classification";
   TString splitExpr = "int(fabs([eventID]))%int([NumFolds])";

   TString cvOptions = Form("!V"
                            ":!Silent"
                            ":ModelPersistence"
                            ":AnalysisType=%s"
                            ":NumFolds=%i"
                            ":SplitExpr=%s",
                            analysisType.Data(), numFolds, splitExpr.Data());

   TMVA::CrossValidation ce{"TMVACrossEvaluation", dataloader, outputFile, cvOptions};

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

   //
   // Books a method to use for evaluation
   //
   ce.BookMethod(TMVA::Types::kBDT, "BDTG",
                 "!H:!V:NTrees=100:MinNodeSize=2.5%:BoostType=Grad:"
                 "Shrinkage=0.10:nCuts=20:MaxDepth=2");

   ce.BookMethod(TMVA::Types::kFisher, "Fisher",
                 "H:!V:Fisher:VarTransform=None:CreateMVAPdfs:"
                 "PDFInterpolMVAPdf=Spline2:NbinsMVAPdf=50:NsmoothMVAPdf=10");

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

   //
   // Train, test and evaluate the booked methods.
   // Evaluates the booked methods once for each fold and aggregates the result
   // in the specified output file.
   //
   ce.Evaluate();

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

   //
   // Process some output programatically, printing the ROC score for each
   // booked method.
   //
   auto bdtg_result = ce.GetResults()[0];
   std::cout << "==> BDTG ROC: avg (std): " << bdtg_result.GetROCAverage() << " ("
             << bdtg_result.GetROCStandardDeviation() << ")"
             << "" << std::endl;

   auto fisher_result = ce.GetResults()[1];
   std::cout << "==> Fisher ROC: avg (std): " << fisher_result.GetROCAverage() << " ("
             << fisher_result.GetROCStandardDeviation() << ")"
             << "" << std::endl;

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

   //
   // Save the output
   //
   outputFile->Close();

   std::cout << "==> Wrote root file: " << outputFile->GetName() << std::endl;
   std::cout << "==> TMVACrossEvaluation is done!" << std::endl;

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

   //
   // Launch the GUI for the root macros
   //
   if (!gROOT->IsBatch()) {
      TMVA::TMVAGui(outfileName);
   }

   return 0;
}

//
// This is used if the macro is compiled. If run through ROOT with
// `root -l -b -q MACRO.C` or similar it is unused.
//
int main(int argc, char **argv)
{
   TMVACrossEvaluation();
}
<|endoftext|>


<commit_before>/***************************************************************
 *
 * Copyright (C) 1990-2007, Condor Team, Computer Sciences Department,
 * University of Wisconsin-Madison, WI.
 *
 * 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.
 *
 ***************************************************************/

// This is a wrapper program that we use for things we launch via glexec,
// in order to get around glexec's inability to pass environment variables.
// The basic procedure is:
//   1) Condor sets up a UNIX domain socket pair
//   2) Condor invokes this wrapper via glexec, passing one of the sockets
//      to the wrapper as it's standard input stream.
//   3) A stream of environment variables is sent over the socket pair and
//      merged into the environment that will be used for the job.
//   4) The job's "real" standard input FD is sent over the socket pair. The
//      wrapper's socket end is dup'ed and FD 0 is then set to use the
//      received FD.
//   5) The wrapper's socket end is set close-on-exec, and the wrapper then
//      exec's the job.

#define _CONDOR_ALLOW_OPEN

#include "condor_common.h"
#include "MyString.h"
#include "env.h"
#include "condor_blkng_full_disk_io.h"
#include "fdpass.h"

static char* read_env(int);
static int read_fd(int);

int
main(int, char* argv[])
{
	MyString err;

	// dup FD 0 since well will later replace FD 0 with the job's stdin
	//
	int sock_fd = dup(0);
	if (sock_fd == -1) {
		err.sprintf("dup error on FD 0: %s", strerror(errno));
		write(0, err.Value(), err.Length() + 1);
		exit(1);
	}

	// set up an Env object that we'll use for the job. we'll initialize
	// it with the environment that Condor sends us then merge on top of
	// that the environment that glexec prepared for us
	//
	Env env;
	char* env_buf = read_env(sock_fd);
	MyString merge_err;
	if (!env.MergeFromV2Raw(env_buf, &merge_err)) {
		err.sprintf("Env::MergeFromV2Raw error: %s", merge_err.Value());
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	env.MergeFrom(environ);
	delete[] env_buf;

	// now receive an FD on our stdin (which is a UNIX domain socket)
	// that we'll use as the job's stdin
	//
	int job_fd = read_fd(sock_fd);
	if (dup2(job_fd, 0) == -1) {
		err.sprintf("dup2 to FD 0 error: %s", strerror(errno));
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	close(job_fd);
	if (fcntl(sock_fd, F_SETFD, FD_CLOEXEC) == -1) {
		err.sprintf("fcntl error setting close-on-exec: %s",
		            strerror(errno));
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}

	// now we can exec the job. for arguments, we shift this wrappers
	// arguments by one. similarly, the job's executable path is taken
	// as our argv[1]
	//
	char** envp = env.getStringArray();
	execve(argv[1], &argv[1], envp);
	err.sprintf("execve error: %s", strerror(errno));
	write(sock_fd, err.Value(), err.Length() + 1);
	exit(1);
}

static char*
read_env(int sock_fd)
{
	MyString err;
	int bytes;
	int env_len;
	bytes = full_read(0, &env_len, sizeof(env_len));
	if (bytes != sizeof(env_len)) {
		if (bytes == -1) {
			err.sprintf("read error getting env size: %s",
			            strerror(errno));
		}
		else {
			err.sprintf("short read of env size: %d of %d bytes",
			            bytes,
			            sizeof(env_len));
		}
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	if (env_len <= 0) {
		err.sprintf("invalid env size %d read from stdin", env_len);
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	char* env_buf = new char[env_len];
	if (env_buf == NULL) {
		err.sprintf("failure to allocate %d bytes", env_len);
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	bytes = full_read(0, env_buf, env_len);
	if (bytes != env_len) {
		if (bytes == -1) {
			err.sprintf("read error getting env: %s",
			            strerror(errno));
		}
		else {
			err.sprintf("short read of env: %d of %d bytes",
			            bytes,
			            env_len);
		}
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	return env_buf;
}

static int
read_fd(int sock_fd)
{
	MyString err;
	int bytes;
	int flag;
	bytes = full_read(0, &flag, sizeof(flag));
	if (bytes != sizeof(flag)) {
		if (bytes == -1) {
			err.sprintf("read error getting flag: %s",
			            strerror(errno));
		}
		else {
			err.sprintf("short read of flag: %d of %d bytes",
			            bytes,
			            sizeof(flag));
		}
		write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	int fd;
	if (flag) {
		fd = fdpass_recv(sock_fd);
		if (fd == -1) {
			err.sprintf("fdpass_recv failed\n");
			write(sock_fd, err.Value(), err.Length() + 1);
			exit(1);
		}
	}
	else {
		fd = open("/dev/null", O_RDONLY);
		if (fd == -1) {
			err.sprintf("error opening /dev/null: %s",
			            strerror(errno));
			write(sock_fd, err.Value(), err.Length() + 1);
			exit(1);
		}
	}
	return fd;
}
<commit_msg>Warnings fixes.<commit_after>/***************************************************************
 *
 * Copyright (C) 1990-2007, Condor Team, Computer Sciences Department,
 * University of Wisconsin-Madison, WI.
 *
 * 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.
 *
 ***************************************************************/

// This is a wrapper program that we use for things we launch via glexec,
// in order to get around glexec's inability to pass environment variables.
// The basic procedure is:
//   1) Condor sets up a UNIX domain socket pair
//   2) Condor invokes this wrapper via glexec, passing one of the sockets
//      to the wrapper as it's standard input stream.
//   3) A stream of environment variables is sent over the socket pair and
//      merged into the environment that will be used for the job.
//   4) The job's "real" standard input FD is sent over the socket pair. The
//      wrapper's socket end is dup'ed and FD 0 is then set to use the
//      received FD.
//   5) The wrapper's socket end is set close-on-exec, and the wrapper then
//      exec's the job.

#define _CONDOR_ALLOW_OPEN

#include "condor_common.h"
#include "MyString.h"
#include "env.h"
#include "condor_blkng_full_disk_io.h"
#include "fdpass.h"

static char* read_env(int);
static int read_fd(int);

int
main(int, char* argv[])
{
	MyString err;

	// dup FD 0 since well will later replace FD 0 with the job's stdin
	//
	int sock_fd = dup(0);
	if (sock_fd == -1) {
		err.sprintf("dup error on FD 0: %s", strerror(errno));
		full_write(0, err.Value(), err.Length() + 1);
		exit(1);
	}

	// set up an Env object that we'll use for the job. we'll initialize
	// it with the environment that Condor sends us then merge on top of
	// that the environment that glexec prepared for us
	//
	Env env;
	char* env_buf = read_env(sock_fd);
	MyString merge_err;
	if (!env.MergeFromV2Raw(env_buf, &merge_err)) {
		err.sprintf("Env::MergeFromV2Raw error: %s", merge_err.Value());
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	env.MergeFrom(environ);
	delete[] env_buf;

	// now receive an FD on our stdin (which is a UNIX domain socket)
	// that we'll use as the job's stdin
	//
	int job_fd = read_fd(sock_fd);
	if (dup2(job_fd, 0) == -1) {
		err.sprintf("dup2 to FD 0 error: %s", strerror(errno));
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	close(job_fd);
	if (fcntl(sock_fd, F_SETFD, FD_CLOEXEC) == -1) {
		err.sprintf("fcntl error setting close-on-exec: %s",
		            strerror(errno));
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}

	// now we can exec the job. for arguments, we shift this wrappers
	// arguments by one. similarly, the job's executable path is taken
	// as our argv[1]
	//
	char** envp = env.getStringArray();
	execve(argv[1], &argv[1], envp);
	err.sprintf("execve error: %s", strerror(errno));
	full_write(sock_fd, err.Value(), err.Length() + 1);
	exit(1);
}

static char*
read_env(int sock_fd)
{
	MyString err;
	int bytes;
	int env_len;
	bytes = full_read(0, &env_len, sizeof(env_len));
	if (bytes != sizeof(env_len)) {
		if (bytes == -1) {
			err.sprintf("read error getting env size: %s",
			            strerror(errno));
		}
		else {
			err.sprintf("short read of env size: %d of %lu bytes",
			            bytes,
			            sizeof(env_len));
		}
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	if (env_len <= 0) {
		err.sprintf("invalid env size %d read from stdin", env_len);
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	char* env_buf = new char[env_len];
	if (env_buf == NULL) {
		err.sprintf("failure to allocate %d bytes", env_len);
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	bytes = full_read(0, env_buf, env_len);
	if (bytes != env_len) {
		if (bytes == -1) {
			err.sprintf("read error getting env: %s",
			            strerror(errno));
		}
		else {
			err.sprintf("short read of env: %d of %d bytes",
			            bytes,
			            env_len);
		}
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	return env_buf;
}

static int
read_fd(int sock_fd)
{
	MyString err;
	int bytes;
	int flag;
	bytes = full_read(0, &flag, sizeof(flag));
	if (bytes != sizeof(flag)) {
		if (bytes == -1) {
			err.sprintf("read error getting flag: %s",
			            strerror(errno));
		}
		else {
			err.sprintf("short read of flag: %d of %lu bytes",
			            bytes,
			            sizeof(flag));
		}
		full_write(sock_fd, err.Value(), err.Length() + 1);
		exit(1);
	}
	int fd;
	if (flag) {
		fd = fdpass_recv(sock_fd);
		if (fd == -1) {
			err.sprintf("fdpass_recv failed\n");
			full_write(sock_fd, err.Value(), err.Length() + 1);
			exit(1);
		}
	}
	else {
		fd = open("/dev/null", O_RDONLY);
		if (fd == -1) {
			err.sprintf("error opening /dev/null: %s",
			            strerror(errno));
			full_write(sock_fd, err.Value(), err.Length() + 1);
			exit(1);
		}
	}
	return fd;
}
<|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/control/control_component.h"

#include <iomanip>
#include <string>

#include "cyber/common/log.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/time/time.h"
#include "modules/common/util/file.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/control/common/control_gflags.h"

namespace apollo {
namespace control {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleStateProvider;
using apollo::common::time::Clock;
using apollo::control::ControlCommand;
using apollo::control::PadMessage;
using apollo::localization::LocalizationEstimate;
using apollo::planning::ADCTrajectory;

ControlComponent::ControlComponent()
    : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {}

bool ControlComponent::Init() {
  // init_time_ = Clock::NowInSeconds();

  AINFO << "Control init, starting ...";

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_control_conf_file,
                                               &control_conf_))
      << "Unable to load control conf file: " + FLAGS_control_conf_file;

  AINFO << "Conf file: " << FLAGS_control_conf_file << " is loaded.";

  AINFO << "Conf file: " << ConfigFilePath() << " is loaded.";

  // set controller
  if (!controller_agent_.Init(&control_conf_).ok()) {
    monitor_logger_buffer_.ERROR("Control init controller failed! Stopping...");
    return false;
  }

  cyber::ReaderConfig chassis_reader_config;
  chassis_reader_config.channel_name = FLAGS_chassis_topic;
  chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size;

  chassis_reader_ = node_->CreateReader<Chassis>(
      chassis_reader_config, nullptr);
  CHECK(chassis_reader_ != nullptr);

  cyber::ReaderConfig planning_reader_config;
  planning_reader_config.channel_name = FLAGS_planning_trajectory_topic;
  planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size;

  trajectory_reader_ = node_->CreateReader<ADCTrajectory>(
      planning_reader_config, nullptr);
  CHECK(trajectory_reader_ != nullptr);

  cyber::ReaderConfig localization_reader_config;
  localization_reader_config.channel_name = FLAGS_localization_topic;
  localization_reader_config.pending_queue_size =
      FLAGS_localization_pending_queue_size;

  localization_reader_ = node_->CreateReader<LocalizationEstimate>(
      localization_reader_config, nullptr);
  CHECK(localization_reader_ != nullptr);

  cyber::ReaderConfig pad_msg_reader_config;
  pad_msg_reader_config.channel_name = FLAGS_pad_topic;
  pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size;

  pad_msg_reader_ = node_->CreateReader<PadMessage>(
      pad_msg_reader_config, nullptr);
  CHECK(pad_msg_reader_ != nullptr);

  control_cmd_writer_ =
      node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
  CHECK(control_cmd_writer_ != nullptr);

  // set initial vehicle state by cmd
  // need to sleep, because advertised channel is not ready immediately
  // simple test shows a short delay of 80 ms or so
  AINFO << "Control resetting vehicle state, sleeping for 1000 ms ...";
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // should init_vehicle first, let car enter work status, then use status msg
  // trigger control

  AINFO << "Control default driving action is "
        << DrivingAction_Name(control_conf_.action());
  pad_msg_.set_action(control_conf_.action());

  return true;
}

void ControlComponent::OnPad(const std::shared_ptr<PadMessage> &pad) {
  pad_msg_.CopyFrom(*pad);
  ADEBUG << "Received Pad Msg:" << pad_msg_.DebugString();
  AERROR_IF(!pad_msg_.has_action()) << "pad message check failed!";

  // do something according to pad message
  if (pad_msg_.action() == DrivingAction::RESET) {
    AINFO << "Control received RESET action!";
    estop_ = false;
    estop_reason_.clear();
  }
  pad_received_ = true;
}

void ControlComponent::OnChassis(const std::shared_ptr<Chassis> &chassis) {
  ADEBUG << "Received chassis data: run chassis callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  latest_chassis_.CopyFrom(*chassis);
}

void ControlComponent::OnPlanning(
    const std::shared_ptr<ADCTrajectory> &trajectory) {
  ADEBUG << "Received chassis data: run trajectory callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  latest_trajectory_.CopyFrom(*trajectory);
}

void ControlComponent::OnLocalization(
    const std::shared_ptr<LocalizationEstimate> &localization) {
  ADEBUG << "Received control data: run localization message callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  latest_localization_.CopyFrom(*localization);
}

void ControlComponent::OnMonitor(
    const common::monitor::MonitorMessage &monitor_message) {
  for (const auto &item : monitor_message.item()) {
    if (item.log_level() == common::monitor::MonitorMessageItem::FATAL) {
      estop_ = true;
      return;
    }
  }
}

Status ControlComponent::ProduceControlCommand(
    ControlCommand *control_command) {
  {
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.chassis = latest_chassis_;
    local_view_.trajectory = latest_trajectory_;
    local_view_.localization = latest_localization_;
  }

  Status status = CheckInput(&local_view_);
  // check data

  if (!status.ok()) {
    AERROR_EVERY(100) << "Control input data failed: "
                      << status.error_message();
    control_command->mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    control_command->mutable_engage_advice()->set_reason(
        status.error_message());
    estop_ = true;
    estop_reason_ = status.error_message();
  } else {
    Status status_ts = CheckTimestamp(local_view_);
    if (!status_ts.ok()) {
      AERROR << "Input messages timeout";
      // estop_ = true;
      status = status_ts;
      if (local_view_.chassis.driving_mode() !=
          apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
        control_command->mutable_engage_advice()->set_advice(
            apollo::common::EngageAdvice::DISALLOW_ENGAGE);
        control_command->mutable_engage_advice()->set_reason(
            status.error_message());
      }
    } else {
      control_command->mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::READY_TO_ENGAGE);
    }
  }

  // check estop
  estop_ = control_conf_.enable_persistent_estop()
               ? estop_ || local_view_.trajectory.estop().is_estop()
               : local_view_.trajectory.estop().is_estop();

  if (local_view_.trajectory.estop().is_estop()) {
    estop_reason_ = "estop from planning";
  }

  // if planning set estop, then no control process triggered
  if (!estop_) {
    if (local_view_.chassis.driving_mode() == Chassis::COMPLETE_MANUAL) {
      controller_agent_.Reset();
      AINFO_EVERY(100) << "Reset Controllers in Manual Mode";
    }

    auto debug = control_command->mutable_debug()->mutable_input_debug();
    debug->mutable_localization_header()->CopyFrom(
        local_view_.localization.header());
    debug->mutable_canbus_header()->CopyFrom(local_view_.chassis.header());
    debug->mutable_trajectory_header()->CopyFrom(
        local_view_.trajectory.header());

    Status status_compute = controller_agent_.ComputeControlCommand(
        &local_view_.localization, &local_view_.chassis,
        &local_view_.trajectory, control_command);

    if (!status_compute.ok()) {
      AERROR << "Control main function failed"
             << " with localization: "
             << local_view_.localization.ShortDebugString()
             << " with chassis: " << local_view_.chassis.ShortDebugString()
             << " with trajectory: "
             << local_view_.trajectory.ShortDebugString()
             << " with cmd: " << control_command->ShortDebugString()
             << " status:" << status_compute.error_message();
      estop_ = true;
      estop_reason_ = status_compute.error_message();
      status = status_compute;
    }
  }

  if (estop_) {
    AWARN_EVERY(100) << "Estop triggered! No control core method executed!";
    // set Estop command
    control_command->set_speed(0);
    control_command->set_throttle(0);
    control_command->set_brake(control_conf_.soft_estop_brake());
    control_command->set_gear_location(Chassis::GEAR_DRIVE);
  }
  // check signal
  if (local_view_.trajectory.decision().has_vehicle_signal()) {
    control_command->mutable_signal()->CopyFrom(
        local_view_.trajectory.decision().vehicle_signal());
  }
  return status;
}

bool ControlComponent::Proc() {
  double start_timestamp = Clock::NowInSeconds();

  chassis_reader_->Observe();
  const auto& chassis_msg = chassis_reader_->GetLatestObserved();
  if (chassis_msg == nullptr) {
    AERROR << "Chassis msg is not ready!";
    return false;
  }

  OnChassis(chassis_msg);

  trajectory_reader_->Observe();
  const auto& trajectory_msg = trajectory_reader_->GetLatestObserved();
  if (trajectory_msg == nullptr) {
    AERROR << "planning msg is not ready!";
    return false;
  }
  OnPlanning(trajectory_msg);

  localization_reader_->Observe();
  const auto& localization_msg = localization_reader_->GetLatestObserved();
  if (localization_msg == nullptr) {
    AERROR << "localization msg is not ready!";
    return false;
  }
  OnLocalization(localization_msg);

  pad_msg_reader_->Observe();
  const auto& pad_msg = pad_msg_reader_->GetLatestObserved();
  if (pad_msg != nullptr) {
    OnPad(pad_msg);
  }

  if (control_conf_.is_control_test_mode() &&
      control_conf_.control_test_duration() > 0 &&
      (start_timestamp - init_time_) > control_conf_.control_test_duration()) {
    AERROR << "Control finished testing. exit";
    return false;
  }

  ControlCommand control_command;

  Status status = ProduceControlCommand(&control_command);
  AERROR_IF(!status.ok()) << "Failed to produce control command:"
                          << status.error_message();

  double end_timestamp = Clock::NowInSeconds();

  if (pad_received_) {
    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
    pad_received_ = false;
  }

  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  control_command.mutable_latency_stats()->set_total_time_exceeded(
      time_diff_ms < control_conf_.control_period());
  ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
  status.Save(control_command.mutable_header()->mutable_status());

  // forward estop reason among following control frames.
  if (estop_) {
    control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
  }

  // set header
  control_command.mutable_header()->set_lidar_timestamp(
      local_view_.trajectory.header().lidar_timestamp());
  control_command.mutable_header()->set_camera_timestamp(
      local_view_.trajectory.header().camera_timestamp());
  control_command.mutable_header()->set_radar_timestamp(
      local_view_.trajectory.header().radar_timestamp());

  common::util::FillHeader(node_->Name(), &control_command);

  ADEBUG << control_command.ShortDebugString();
  if (control_conf_.is_control_test_mode()) {
    ADEBUG << "Skip publish control command in test mode";
    return true;
  }

  control_cmd_writer_->Write(std::make_shared<ControlCommand>(control_command));

  return true;
}

Status ControlComponent::CheckInput(LocalView *local_view) {
  ADEBUG << "Received localization:"
         << local_view->localization.ShortDebugString();
  ADEBUG << "Received chassis:" << local_view->chassis.ShortDebugString();

  if (!local_view->trajectory.estop().is_estop() &&
      local_view->trajectory.trajectory_point_size() == 0) {
    AWARN_EVERY(100) << "planning has no trajectory point. ";
    std::string msg("planning has no trajectory point. planning_seq_num:");
    msg += std::to_string(local_view->trajectory.header().sequence_num());
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, msg);
  }

  for (auto &trajectory_point :
       *local_view->trajectory.mutable_trajectory_point()) {
    if (trajectory_point.v() < control_conf_.minimum_speed_resolution()) {
      trajectory_point.set_v(0.0);
      trajectory_point.set_a(0.0);
    }
  }

  VehicleStateProvider::Instance()->Update(local_view->localization,
                                           local_view->chassis);

  return Status::OK();
}

Status ControlComponent::CheckTimestamp(const LocalView &local_view) {
  if (!control_conf_.enable_input_timestamp_check() ||
      control_conf_.is_control_test_mode()) {
    ADEBUG << "Skip input timestamp check by gflags.";
    return Status::OK();
  }
  double current_timestamp = Clock::NowInSeconds();
  double localization_diff =
      current_timestamp - local_view.localization.header().timestamp_sec();
  if (localization_diff > (control_conf_.max_localization_miss_num() *
                           control_conf_.localization_period())) {
    AERROR << "Localization msg lost for " << std::setprecision(6)
           << localization_diff << "s";
    monitor_logger_buffer_.ERROR("Localization msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Localization msg timeout");
  }

  double chassis_diff =
      current_timestamp - local_view.chassis.header().timestamp_sec();
  if (chassis_diff >
      (control_conf_.max_chassis_miss_num() * control_conf_.chassis_period())) {
    AERROR << "Chassis msg lost for " << std::setprecision(6) << chassis_diff
           << "s";
    monitor_logger_buffer_.ERROR("Chassis msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Chassis msg timeout");
  }

  double trajectory_diff =
      current_timestamp - local_view.trajectory.header().timestamp_sec();
  if (trajectory_diff > (control_conf_.max_planning_miss_num() *
                         control_conf_.trajectory_period())) {
    AERROR << "Trajectory msg lost for " << std::setprecision(6)
           << trajectory_diff << "s";
    monitor_logger_buffer_.ERROR("Trajectory msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Trajectory msg timeout");
  }
  return Status::OK();
}

}  // namespace control
}  // namespace apollo
<commit_msg>control: set estop for empty planning trajectory.<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/control/control_component.h"

#include <iomanip>
#include <string>

#include "cyber/common/log.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/time/time.h"
#include "modules/common/util/file.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/control/common/control_gflags.h"

namespace apollo {
namespace control {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::VehicleStateProvider;
using apollo::common::time::Clock;
using apollo::control::ControlCommand;
using apollo::control::PadMessage;
using apollo::localization::LocalizationEstimate;
using apollo::planning::ADCTrajectory;

ControlComponent::ControlComponent()
    : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {}

bool ControlComponent::Init() {
  // init_time_ = Clock::NowInSeconds();

  AINFO << "Control init, starting ...";

  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_control_conf_file,
                                               &control_conf_))
      << "Unable to load control conf file: " + FLAGS_control_conf_file;

  AINFO << "Conf file: " << FLAGS_control_conf_file << " is loaded.";

  AINFO << "Conf file: " << ConfigFilePath() << " is loaded.";

  // set controller
  if (!controller_agent_.Init(&control_conf_).ok()) {
    monitor_logger_buffer_.ERROR("Control init controller failed! Stopping...");
    return false;
  }

  cyber::ReaderConfig chassis_reader_config;
  chassis_reader_config.channel_name = FLAGS_chassis_topic;
  chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size;

  chassis_reader_ = node_->CreateReader<Chassis>(
      chassis_reader_config, nullptr);
  CHECK(chassis_reader_ != nullptr);

  cyber::ReaderConfig planning_reader_config;
  planning_reader_config.channel_name = FLAGS_planning_trajectory_topic;
  planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size;

  trajectory_reader_ = node_->CreateReader<ADCTrajectory>(
      planning_reader_config, nullptr);
  CHECK(trajectory_reader_ != nullptr);

  cyber::ReaderConfig localization_reader_config;
  localization_reader_config.channel_name = FLAGS_localization_topic;
  localization_reader_config.pending_queue_size =
      FLAGS_localization_pending_queue_size;

  localization_reader_ = node_->CreateReader<LocalizationEstimate>(
      localization_reader_config, nullptr);
  CHECK(localization_reader_ != nullptr);

  cyber::ReaderConfig pad_msg_reader_config;
  pad_msg_reader_config.channel_name = FLAGS_pad_topic;
  pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size;

  pad_msg_reader_ = node_->CreateReader<PadMessage>(
      pad_msg_reader_config, nullptr);
  CHECK(pad_msg_reader_ != nullptr);

  control_cmd_writer_ =
      node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
  CHECK(control_cmd_writer_ != nullptr);

  // set initial vehicle state by cmd
  // need to sleep, because advertised channel is not ready immediately
  // simple test shows a short delay of 80 ms or so
  AINFO << "Control resetting vehicle state, sleeping for 1000 ms ...";
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // should init_vehicle first, let car enter work status, then use status msg
  // trigger control

  AINFO << "Control default driving action is "
        << DrivingAction_Name(control_conf_.action());
  pad_msg_.set_action(control_conf_.action());

  return true;
}

void ControlComponent::OnPad(const std::shared_ptr<PadMessage> &pad) {
  pad_msg_.CopyFrom(*pad);
  ADEBUG << "Received Pad Msg:" << pad_msg_.DebugString();
  AERROR_IF(!pad_msg_.has_action()) << "pad message check failed!";

  // do something according to pad message
  if (pad_msg_.action() == DrivingAction::RESET) {
    AINFO << "Control received RESET action!";
    estop_ = false;
    estop_reason_.clear();
  }
  pad_received_ = true;
}

void ControlComponent::OnChassis(const std::shared_ptr<Chassis> &chassis) {
  ADEBUG << "Received chassis data: run chassis callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  latest_chassis_.CopyFrom(*chassis);
}

void ControlComponent::OnPlanning(
    const std::shared_ptr<ADCTrajectory> &trajectory) {
  ADEBUG << "Received chassis data: run trajectory callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  latest_trajectory_.CopyFrom(*trajectory);
}

void ControlComponent::OnLocalization(
    const std::shared_ptr<LocalizationEstimate> &localization) {
  ADEBUG << "Received control data: run localization message callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  latest_localization_.CopyFrom(*localization);
}

void ControlComponent::OnMonitor(
    const common::monitor::MonitorMessage &monitor_message) {
  for (const auto &item : monitor_message.item()) {
    if (item.log_level() == common::monitor::MonitorMessageItem::FATAL) {
      estop_ = true;
      return;
    }
  }
}

Status ControlComponent::ProduceControlCommand(
    ControlCommand *control_command) {
  {
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.chassis = latest_chassis_;
    local_view_.trajectory = latest_trajectory_;
    local_view_.localization = latest_localization_;
  }

  Status status = CheckInput(&local_view_);
  // check data

  if (!status.ok()) {
    AERROR_EVERY(100) << "Control input data failed: "
                      << status.error_message();
    control_command->mutable_engage_advice()->set_advice(
        apollo::common::EngageAdvice::DISALLOW_ENGAGE);
    control_command->mutable_engage_advice()->set_reason(
        status.error_message());
    estop_ = true;
    estop_reason_ = status.error_message();
  } else {
    Status status_ts = CheckTimestamp(local_view_);
    if (!status_ts.ok()) {
      AERROR << "Input messages timeout";
      // estop_ = true;
      status = status_ts;
      if (local_view_.chassis.driving_mode() !=
          apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
        control_command->mutable_engage_advice()->set_advice(
            apollo::common::EngageAdvice::DISALLOW_ENGAGE);
        control_command->mutable_engage_advice()->set_reason(
            status.error_message());
      }
    } else {
      control_command->mutable_engage_advice()->set_advice(
          apollo::common::EngageAdvice::READY_TO_ENGAGE);
    }
  }

  // check estop
  estop_ = control_conf_.enable_persistent_estop()
               ? estop_ || local_view_.trajectory.estop().is_estop()
               : local_view_.trajectory.estop().is_estop();

  if (local_view_.trajectory.estop().is_estop()) {
    estop_reason_ = "estop from planning";
  }

  if (local_view_.trajectory.trajectory_point_size() == 0) {
    AWARN_EVERY(100) << "planning has no trajectory point. ";
    estop_ = true;
    estop_reason_ = "estop for empty planning trajectory";
  }

  // if planning set estop, then no control process triggered
  if (!estop_) {
    if (local_view_.chassis.driving_mode() == Chassis::COMPLETE_MANUAL) {
      controller_agent_.Reset();
      AINFO_EVERY(100) << "Reset Controllers in Manual Mode";
    }

    auto debug = control_command->mutable_debug()->mutable_input_debug();
    debug->mutable_localization_header()->CopyFrom(
        local_view_.localization.header());
    debug->mutable_canbus_header()->CopyFrom(local_view_.chassis.header());
    debug->mutable_trajectory_header()->CopyFrom(
        local_view_.trajectory.header());

    Status status_compute = controller_agent_.ComputeControlCommand(
        &local_view_.localization, &local_view_.chassis,
        &local_view_.trajectory, control_command);

    if (!status_compute.ok()) {
      AERROR << "Control main function failed"
             << " with localization: "
             << local_view_.localization.ShortDebugString()
             << " with chassis: " << local_view_.chassis.ShortDebugString()
             << " with trajectory: "
             << local_view_.trajectory.ShortDebugString()
             << " with cmd: " << control_command->ShortDebugString()
             << " status:" << status_compute.error_message();
      estop_ = true;
      estop_reason_ = status_compute.error_message();
      status = status_compute;
    }
  }

  if (estop_) {
    AWARN_EVERY(100) << "Estop triggered! No control core method executed!";
    // set Estop command
    control_command->set_speed(0);
    control_command->set_throttle(0);
    control_command->set_brake(control_conf_.soft_estop_brake());
    control_command->set_gear_location(Chassis::GEAR_DRIVE);
  }
  // check signal
  if (local_view_.trajectory.decision().has_vehicle_signal()) {
    control_command->mutable_signal()->CopyFrom(
        local_view_.trajectory.decision().vehicle_signal());
  }
  return status;
}

bool ControlComponent::Proc() {
  double start_timestamp = Clock::NowInSeconds();

  chassis_reader_->Observe();
  const auto& chassis_msg = chassis_reader_->GetLatestObserved();
  if (chassis_msg == nullptr) {
    AERROR << "Chassis msg is not ready!";
    return false;
  }

  OnChassis(chassis_msg);

  trajectory_reader_->Observe();
  const auto& trajectory_msg = trajectory_reader_->GetLatestObserved();
  if (trajectory_msg == nullptr) {
    AERROR << "planning msg is not ready!";
    return false;
  }
  OnPlanning(trajectory_msg);

  localization_reader_->Observe();
  const auto& localization_msg = localization_reader_->GetLatestObserved();
  if (localization_msg == nullptr) {
    AERROR << "localization msg is not ready!";
    return false;
  }
  OnLocalization(localization_msg);

  pad_msg_reader_->Observe();
  const auto& pad_msg = pad_msg_reader_->GetLatestObserved();
  if (pad_msg != nullptr) {
    OnPad(pad_msg);
  }

  if (control_conf_.is_control_test_mode() &&
      control_conf_.control_test_duration() > 0 &&
      (start_timestamp - init_time_) > control_conf_.control_test_duration()) {
    AERROR << "Control finished testing. exit";
    return false;
  }

  ControlCommand control_command;

  Status status = ProduceControlCommand(&control_command);
  AERROR_IF(!status.ok()) << "Failed to produce control command:"
                          << status.error_message();

  double end_timestamp = Clock::NowInSeconds();

  if (pad_received_) {
    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
    pad_received_ = false;
  }

  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  control_command.mutable_latency_stats()->set_total_time_exceeded(
      time_diff_ms < control_conf_.control_period());
  ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
  status.Save(control_command.mutable_header()->mutable_status());

  // forward estop reason among following control frames.
  if (estop_) {
    control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
  }

  // set header
  control_command.mutable_header()->set_lidar_timestamp(
      local_view_.trajectory.header().lidar_timestamp());
  control_command.mutable_header()->set_camera_timestamp(
      local_view_.trajectory.header().camera_timestamp());
  control_command.mutable_header()->set_radar_timestamp(
      local_view_.trajectory.header().radar_timestamp());

  common::util::FillHeader(node_->Name(), &control_command);

  ADEBUG << control_command.ShortDebugString();
  if (control_conf_.is_control_test_mode()) {
    ADEBUG << "Skip publish control command in test mode";
    return true;
  }

  control_cmd_writer_->Write(std::make_shared<ControlCommand>(control_command));

  return true;
}

Status ControlComponent::CheckInput(LocalView *local_view) {
  ADEBUG << "Received localization:"
         << local_view->localization.ShortDebugString();
  ADEBUG << "Received chassis:" << local_view->chassis.ShortDebugString();

  if (!local_view->trajectory.estop().is_estop() &&
      local_view->trajectory.trajectory_point_size() == 0) {
    AWARN_EVERY(100) << "planning has no trajectory point. ";
    std::string msg("planning has no trajectory point. planning_seq_num:");
    msg += std::to_string(local_view->trajectory.header().sequence_num());
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, msg);
  }

  for (auto &trajectory_point :
       *local_view->trajectory.mutable_trajectory_point()) {
    if (trajectory_point.v() < control_conf_.minimum_speed_resolution()) {
      trajectory_point.set_v(0.0);
      trajectory_point.set_a(0.0);
    }
  }

  VehicleStateProvider::Instance()->Update(local_view->localization,
                                           local_view->chassis);

  return Status::OK();
}

Status ControlComponent::CheckTimestamp(const LocalView &local_view) {
  if (!control_conf_.enable_input_timestamp_check() ||
      control_conf_.is_control_test_mode()) {
    ADEBUG << "Skip input timestamp check by gflags.";
    return Status::OK();
  }
  double current_timestamp = Clock::NowInSeconds();
  double localization_diff =
      current_timestamp - local_view.localization.header().timestamp_sec();
  if (localization_diff > (control_conf_.max_localization_miss_num() *
                           control_conf_.localization_period())) {
    AERROR << "Localization msg lost for " << std::setprecision(6)
           << localization_diff << "s";
    monitor_logger_buffer_.ERROR("Localization msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Localization msg timeout");
  }

  double chassis_diff =
      current_timestamp - local_view.chassis.header().timestamp_sec();
  if (chassis_diff >
      (control_conf_.max_chassis_miss_num() * control_conf_.chassis_period())) {
    AERROR << "Chassis msg lost for " << std::setprecision(6) << chassis_diff
           << "s";
    monitor_logger_buffer_.ERROR("Chassis msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Chassis msg timeout");
  }

  double trajectory_diff =
      current_timestamp - local_view.trajectory.header().timestamp_sec();
  if (trajectory_diff > (control_conf_.max_planning_miss_num() *
                         control_conf_.trajectory_period())) {
    AERROR << "Trajectory msg lost for " << std::setprecision(6)
           << trajectory_diff << "s";
    monitor_logger_buffer_.ERROR("Trajectory msg lost");
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Trajectory msg timeout");
  }
  return Status::OK();
}

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


<commit_before>// Copyright (c) 2011 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <vector>

#include "base/base_paths.h"
#include "base/command_line.h"
#include "base/file_path.h"
#include "base/logging.h"
#include "base/native_library.h"
#include "base/path_service.h"
#include "ui/gfx/gl/gl_bindings.h"
#include "ui/gfx/gl/gl_implementation.h"

namespace gfx {
namespace {

// TODO(piman): it should be Desktop GL marshalling from double to float. Today
// on native GLES, we do float->double->float.
void GL_BINDING_CALL MarshalClearDepthToClearDepthf(GLclampd depth) {
  glClearDepthf(static_cast<GLclampf>(depth));
}

void GL_BINDING_CALL MarshalDepthRangeToDepthRangef(GLclampd z_near,
                                                    GLclampd z_far) {
  glDepthRangef(static_cast<GLclampf>(z_near), static_cast<GLclampf>(z_far));
}

// Load a library, printing an error message on failure.
base::NativeLibrary LoadLibrary(const FilePath& filename) {
  std::string error;
  base::NativeLibrary library = base::LoadNativeLibrary(filename,
                                                        &error);
  if (!library) {
    VLOG(1) << "Failed to load " << filename.MaybeAsASCII() << ": " << error;
    return NULL;
  }
  return library;
}

base::NativeLibrary LoadLibrary(const char* filename) {
  return LoadLibrary(FilePath(filename));
}

}  // namespace anonymous

bool InitializeGLBindings(GLImplementation implementation) {
  // Prevent reinitialization with a different implementation. Once the gpu
  // unit tests have initialized with kGLImplementationMock, we don't want to
  // later switch to another GL implementation.
  if (GetGLImplementation() != kGLImplementationNone)
    return true;

  switch (implementation) {
    case kGLImplementationOSMesaGL: {
      FilePath module_path;
      if (!PathService::Get(base::DIR_MODULE, &module_path)) {
        LOG(ERROR) << "PathService::Get failed.";
        return false;
      }

      base::NativeLibrary library = LoadLibrary(
          module_path.Append("libosmesa.so"));
      if (!library)
        return false;

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "OSMesaGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "OSMesaGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationOSMesaGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsOSMESA();
      break;
    }
    case kGLImplementationDesktopGL: {
      base::NativeLibrary library = LoadLibrary("libGL.so.1");
      if (!library)
        return false;

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "glXGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "glxGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationDesktopGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsGLX();
      break;
    }
    case kGLImplementationEGLGLES2: {
      base::NativeLibrary gles_library = LoadLibrary("libGLESv2.so");
      if (!gles_library)
        return false;
      base::NativeLibrary egl_library = LoadLibrary("libEGL.so");
      if (!egl_library)
        return false;

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  egl_library, "eglGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "eglGetProcAddress not found.";
        base::UnloadNativeLibrary(egl_library);
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(egl_library);
      AddGLNativeLibrary(gles_library);
      SetGLImplementation(kGLImplementationEGLGLES2);

      InitializeGLBindingsGL();
      InitializeGLBindingsEGL();

      // These two functions take single precision float rather than double
      // precision float parameters in GLES.
      ::gfx::g_glClearDepth = MarshalClearDepthToClearDepthf;
      ::gfx::g_glDepthRange = MarshalDepthRangeToDepthRangef;
      break;
    }
    case kGLImplementationMockGL: {
      SetGLGetProcAddressProc(GetMockGLProcAddress);
      SetGLImplementation(kGLImplementationMockGL);
      InitializeGLBindingsGL();
      break;
    }
    default:
      return false;
  }

  return true;
}

void InitializeDebugGLBindings() {
  InitializeDebugGLBindingsEGL();
  InitializeDebugGLBindingsGL();
  InitializeDebugGLBindingsGLX();
  InitializeDebugGLBindingsOSMESA();
}

}  // namespace gfx
<commit_msg>RESOURCE_LEAK coverity fix.<commit_after>// Copyright (c) 2011 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <vector>

#include "base/base_paths.h"
#include "base/command_line.h"
#include "base/file_path.h"
#include "base/logging.h"
#include "base/native_library.h"
#include "base/path_service.h"
#include "ui/gfx/gl/gl_bindings.h"
#include "ui/gfx/gl/gl_implementation.h"

namespace gfx {
namespace {

// TODO(piman): it should be Desktop GL marshalling from double to float. Today
// on native GLES, we do float->double->float.
void GL_BINDING_CALL MarshalClearDepthToClearDepthf(GLclampd depth) {
  glClearDepthf(static_cast<GLclampf>(depth));
}

void GL_BINDING_CALL MarshalDepthRangeToDepthRangef(GLclampd z_near,
                                                    GLclampd z_far) {
  glDepthRangef(static_cast<GLclampf>(z_near), static_cast<GLclampf>(z_far));
}

// Load a library, printing an error message on failure.
base::NativeLibrary LoadLibrary(const FilePath& filename) {
  std::string error;
  base::NativeLibrary library = base::LoadNativeLibrary(filename,
                                                        &error);
  if (!library) {
    VLOG(1) << "Failed to load " << filename.MaybeAsASCII() << ": " << error;
    return NULL;
  }
  return library;
}

base::NativeLibrary LoadLibrary(const char* filename) {
  return LoadLibrary(FilePath(filename));
}

}  // namespace anonymous

bool InitializeGLBindings(GLImplementation implementation) {
  // Prevent reinitialization with a different implementation. Once the gpu
  // unit tests have initialized with kGLImplementationMock, we don't want to
  // later switch to another GL implementation.
  if (GetGLImplementation() != kGLImplementationNone)
    return true;

  switch (implementation) {
    case kGLImplementationOSMesaGL: {
      FilePath module_path;
      if (!PathService::Get(base::DIR_MODULE, &module_path)) {
        LOG(ERROR) << "PathService::Get failed.";
        return false;
      }

      base::NativeLibrary library = LoadLibrary(
          module_path.Append("libosmesa.so"));
      if (!library)
        return false;

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "OSMesaGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "OSMesaGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationOSMesaGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsOSMESA();
      break;
    }
    case kGLImplementationDesktopGL: {
      base::NativeLibrary library = LoadLibrary("libGL.so.1");
      if (!library)
        return false;

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "glXGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "glxGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationDesktopGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsGLX();
      break;
    }
    case kGLImplementationEGLGLES2: {
      base::NativeLibrary gles_library = LoadLibrary("libGLESv2.so");
      if (!gles_library)
        return false;
      base::NativeLibrary egl_library = LoadLibrary("libEGL.so");
      if (!egl_library) {
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  egl_library, "eglGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "eglGetProcAddress not found.";
        base::UnloadNativeLibrary(egl_library);
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(egl_library);
      AddGLNativeLibrary(gles_library);
      SetGLImplementation(kGLImplementationEGLGLES2);

      InitializeGLBindingsGL();
      InitializeGLBindingsEGL();

      // These two functions take single precision float rather than double
      // precision float parameters in GLES.
      ::gfx::g_glClearDepth = MarshalClearDepthToClearDepthf;
      ::gfx::g_glDepthRange = MarshalDepthRangeToDepthRangef;
      break;
    }
    case kGLImplementationMockGL: {
      SetGLGetProcAddressProc(GetMockGLProcAddress);
      SetGLImplementation(kGLImplementationMockGL);
      InitializeGLBindingsGL();
      break;
    }
    default:
      return false;
  }

  return true;
}

void InitializeDebugGLBindings() {
  InitializeDebugGLBindingsEGL();
  InitializeDebugGLBindingsGL();
  InitializeDebugGLBindingsGLX();
  InitializeDebugGLBindingsOSMESA();
}

}  // namespace gfx
<|endoftext|>