<commit_before>// This program test streaming through ports.

#include "vtkImageReader.h"
#include "vtkSynchronizedTemplates3D.h"
#include "vtkPolyDataCollector.h"
#include "vtkOutputPort.h"
#include "vtkInputPort.h"
#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkConeSource.h"
#include "vtkPolyDataMapper.h"
#include "vtkActor.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkMath.h"
#include "vtkDataInformation.h"

// callback to test streaming / ports by seeing what extents are being read in.
void reader_start_callback(void *arg)
{
  vtkImageReader *reader = (vtkImageReader*)(arg);
  int *e;

  e = reader->GetOutput()->GetUpdateExtent();

  cerr << "Reading: " << e[0] << ", " << e[1] << ", " << e[2] << ", "
       << e[3] << ", " << e[4] << ", " << e[5] << endl;
}

// callback to see if iso has data
void iso_end_callback(void *arg)
{
  vtkPolyData *out = (vtkPolyData*)(arg);

  cerr << "iso out: " << *out << endl;
}

// call back to exit program
// This should really be embedded in the controller.
void exit_callback(void *arg, int id)
{
  // clean up controller ?
  exit(0);
}

VTK_THREAD_RETURN_TYPE process_a( void *vtkNotUsed(arg) )
{
  vtkMultiProcessController *controller;
  vtkImageReader *reader;
  vtkSynchronizedTemplates3D *iso;

  controller = vtkMultiProcessController::RegisterAndGetGlobalController(NULL);

  reader = vtkImageReader::New();
  reader->SetDataByteOrderToLittleEndian();
  reader->SetDataExtent(0, 127, 0, 127, 1, 93);
  reader->SetFilePrefix("../../../vtkdata/headsq/half");
  reader->SetDataSpacing(1.6, 1.6, 1.5);
  reader->SetStartMethod(reader_start_callback, (void*)(reader));

  iso = vtkSynchronizedTemplates3D::New();
  iso->SetInput(reader->GetOutput());
  iso->SetValue(0, 500);
  iso->ComputeScalarsOff();
  iso->ComputeGradientsOff();
  //iso->SetEndMethod(iso_end_callback, (void*)(iso->GetOutput()));
  // This should be automatically determined by controller.
  iso->SetNumberOfThreads(1);

  // Send data throug port.
  vtkOutputPort *upPort = vtkOutputPort::New();
  upPort->SetInput(iso->GetOutput());
  upPort->SetTag(999);

  // last, set up a RMI call backs.
  controller->AddRMI(exit_callback, (void *)iso, 666);

  // wait for the call back to execute.
  upPort->WaitForUpdate();

  // last call never returns, but ...
  upPort->Delete();

  return VTK_THREAD_RETURN_VALUE;
}

VTK_THREAD_RETURN_TYPE process_b( void *vtkNotUsed(arg) )
{
  vtkMultiProcessController *controller;
  vtkInputPort *downPort;
  vtkPolyDataCollector *collector = vtkPolyDataCollector::New();
  vtkPolyDataMapper *mapper = vtkPolyDataMapper::New();
  vtkActor *actor = vtkActor::New();
  vtkRenderer *ren = vtkRenderer::New();
  vtkRenderWindow *renWindow = vtkRenderWindow::New();
  vtkRenderWindowInteractor *iren = vtkRenderWindowInteractor::New();
  vtkCamera *cam = vtkCamera::New();
  int myid, otherid;

  controller = vtkMultiProcessController::RegisterAndGetGlobalController(NULL);
  myid = controller->GetLocalProcessId();
  otherid = ( ! myid);

  downPort = vtkInputPort::New();
  downPort->SetRemoteProcessId(otherid);
  downPort->SetTag(999);

  collector->SetInput(downPort->GetPolyDataOutput());
  collector->SetInputMemoryLimit(1000);

  mapper->SetInput(collector->GetOutput());
  actor->SetMapper(mapper);

  putenv("DISPLAY=:0.0");

  renWindow->AddRenderer(ren);
  iren->SetRenderWindow(renWindow);
  ren->SetBackground(0.9, 0.9, 0.9);
  renWindow->SetSize( 400, 400);

  // assign our actor to the renderer
  ren->AddActor(actor);

  cam->SetFocalPoint(100, 100, 65);
  cam->SetPosition(100, 450, 65);
  cam->SetViewUp(0, 0, -1);
  cam->SetViewAngle(30);
  // this was causing an update.
  //ren->ResetCameraClippingRange();
  //{
  //double *range = ren->GetActiveCamera()->GetClippingRange();
  //cerr << range[0] << ", " << range[1] << endl;
  //}
  cam->SetClippingRange(177.0, 536.0);
  ren->SetActiveCamera(cam);

  collector->Update();
  collector->Update();
  cerr << "WholeMemorySize: "
       << collector->GetOutput()->GetDataInformation()->GetEstimatedWholeMemorySize()
       << endl;

  renWindow->Render();

  // just exit
  //controller->TriggerRMI(otherid, 666);
  //exit(0);

  //  Begin mouse interaction
  iren->Start();

  // Clean up
  collector->Delete();
  ren->Delete();
  renWindow->Delete();
  iren->Delete();
  mapper->Delete();
  actor->Delete();

  // clean up objects in all processes.
  controller->UnRegister(NULL);

  return VTK_THREAD_RETURN_VALUE;
}

void main( int argc, char *argv[] )
{
  vtkMultiProcessController *controller;

  controller = vtkMultiProcessController::RegisterAndGetGlobalController(NULL);

  controller->Initialize(argc, argv);
  controller->SetNumberOfProcesses(2);
  controller->SetMultipleMethod(1, process_a, NULL);
  controller->SetMultipleMethod(0, process_b, NULL);
  controller->MultipleMethodExecute();

  controller->UnRegister(NULL);
}

<commit_msg>Converted to VTK3.1 (still not working)<commit_after>// This program test streaming through ports.

#include "vtkImageReader.h"
#include "vtkSynchronizedTemplates3D.h"
#include "vtkPolyDataCollector.h"
#include "vtkOutputPort.h"
#include "vtkInputPort.h"
#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkConeSource.h"
#include "vtkPolyDataMapper.h"
#include "vtkActor.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkMath.h"

// callback to test streaming / ports by seeing what extents are being read in.
void reader_start_callback(void *arg)
{
  vtkImageReader *reader = (vtkImageReader*)(arg);
  int *e;

  e = reader->GetOutput()->GetUpdateExtent();

  cerr << "Reading: " << e[0] << ", " << e[1] << ", " << e[2] << ", "
       << e[3] << ", " << e[4] << ", " << e[5] << endl;
}

// callback to see if iso has data
void iso_end_callback(void *arg)
{
  vtkPolyData *out = (vtkPolyData*)(arg);

  cerr << "iso out: " << *out << endl;
}

void process_a(vtkMultiProcessController *controller, void *vtkNotUsed(arg) )
{
  vtkImageReader *reader;
  vtkSynchronizedTemplates3D *iso;

  reader = vtkImageReader::New();
  reader->SetDataByteOrderToLittleEndian();
  reader->SetDataExtent(0, 127, 0, 127, 1, 93);
  reader->SetFilePrefix("../../../vtkdata/headsq/half");
  reader->SetDataSpacing(1.6, 1.6, 1.5);
  reader->SetStartMethod(reader_start_callback, (void*)(reader));

  iso = vtkSynchronizedTemplates3D::New();
  iso->SetInput(reader->GetOutput());
  iso->SetValue(0, 500);
  iso->ComputeScalarsOff();
  iso->ComputeGradientsOff();
  //iso->SetEndMethod(iso_end_callback, (void*)(iso->GetOutput()));
  // This should be automatically determined by controller.
  iso->SetNumberOfThreads(1);

  // Send data throug port.
  vtkOutputPort *upPort = vtkOutputPort::New();
  upPort->SetInput(iso->GetOutput());
  upPort->SetTag(999);

  // wait for the call back to execute.
  upPort->WaitForUpdate();

  // last call never returns, but ...
  upPort->Delete();
}

void process_b(vtkMultiProcessController *controller, void *vtkNotUsed(arg) )
{
  vtkInputPort *downPort;
  vtkPolyDataCollector *collector = vtkPolyDataCollector::New();
  vtkPolyDataMapper *mapper = vtkPolyDataMapper::New();
  vtkActor *actor = vtkActor::New();
  vtkRenderer *ren = vtkRenderer::New();
  vtkRenderWindow *renWindow = vtkRenderWindow::New();
  vtkRenderWindowInteractor *iren = vtkRenderWindowInteractor::New();
  vtkCamera *cam = vtkCamera::New();
  int myid, otherid;

  myid = controller->GetLocalProcessId();
  otherid = ( ! myid);

  downPort = vtkInputPort::New();
  downPort->SetRemoteProcessId(otherid);
  downPort->SetTag(999);

  collector->SetInput(downPort->GetPolyDataOutput());
  collector->SetInputMemoryLimit(1000);

  mapper->SetInput(collector->GetOutput());
  actor->SetMapper(mapper);

  putenv("DISPLAY=:0.0");

  renWindow->AddRenderer(ren);
  iren->SetRenderWindow(renWindow);
  ren->SetBackground(0.9, 0.9, 0.9);
  renWindow->SetSize( 400, 400);

  // assign our actor to the renderer
  ren->AddActor(actor);

  cam->SetFocalPoint(100, 100, 65);
  cam->SetPosition(100, 450, 65);
  cam->SetViewUp(0, 0, -1);
  cam->SetViewAngle(30);
  // this was causing an update.
  //ren->ResetCameraClippingRange();
  //{
  //double *range = ren->GetActiveCamera()->GetClippingRange();
  //cerr << range[0] << ", " << range[1] << endl;
  //}
  cam->SetClippingRange(177.0, 536.0);
  ren->SetActiveCamera(cam);

  collector->Update();
  collector->Update();
  renWindow->Render();

  // just exit
  //controller->TriggerRMI(otherid, VTK_BREAK_RMI_TAG);
  //exit(0);

  //  Begin mouse interaction
  iren->Start();

  // Clean up
  collector->Delete();
  ren->Delete();
  renWindow->Delete();
  iren->Delete();
  mapper->Delete();
  actor->Delete();

}

void main( int argc, char *argv[] )
{
  vtkMultiProcessController *controller;

  controller = vtkMultiProcessController::New();

  controller->Initialize(argc, argv);
  controller->SetNumberOfProcesses(2);
  controller->SetMultipleMethod(1, process_a, NULL);
  controller->SetMultipleMethod(0, process_b, NULL);
  controller->MultipleMethodExecute();

  controller->Delete();
}

<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Sergey Lisitsyn
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */

#include <shogun/evaluation/ROCEvaluation.h>
#include <shogun/mathematics/Math.h>

using namespace shogun;

CROCEvaluation::~CROCEvaluation()
{
}

float64_t CROCEvaluation::evaluate(CLabels* predicted, CLabels* ground_truth)
{
	REQUIRE(predicted->get_label_type()==LT_BINARY, "ROC evalution requires binary labels.");
	REQUIRE(ground_truth->get_label_type()==LT_BINARY, "ROC evalution requires binary labels.");

	return evaluate_roc((CBinaryLabels*)predicted,(CBinaryLabels*)ground_truth);
}

float64_t CROCEvaluation::evaluate_roc(CBinaryLabels* predicted, CBinaryLabels* ground_truth)
{
	ASSERT(predicted && ground_truth)
	ASSERT(predicted->get_num_labels()==ground_truth->get_num_labels())
	ground_truth->ensure_valid();

	// assume threshold as negative infinity
	float64_t threshold = CMath::ALMOST_NEG_INFTY;
	// false positive rate
	float64_t fp = 0.0;
	// true positive rate
	float64_t tp=0.0;

	int32_t i;
	// total number of positive labels in predicted
	int32_t pos_count=0;
	int32_t neg_count=0;

	// initialize number of labels and labels
	SGVector<float64_t> orig_labels(predicted->get_num_labels());
	int32_t length = orig_labels.vlen;
	for (i=0; i<length; i++)
		orig_labels[i] = predicted->get_value(i);
	float64_t* labels = SGVector<float64_t>::clone_vector(orig_labels.vector, length);

	// get sorted indexes
	SGVector<int32_t> idxs(length);
	for(i=0; i<length; i++)
		idxs[i] = i;

	CMath::qsort_backward_index(labels,idxs.vector,idxs.vlen);

	// number of different predicted labels
	int32_t diff_count=1;

	// get number of different labels
	for (i=0; i<length-1; i++)
	{
		if (labels[i] != labels[i+1])
			diff_count++;
	}

	SG_FREE(labels);

	// initialize graph and auROC
	m_ROC_graph = SGMatrix<float64_t>(2,diff_count+1);
	m_thresholds = SGVector<float64_t>(length);
	m_auROC = 0.0;

	// get total numbers of positive and negative labels
	for(i=0; i<length; i++)
	{
		if (ground_truth->get_label(i) >= 0)
			pos_count++;
		else
			neg_count++;
	}

	// assure both number of positive and negative examples is >0
	REQUIRE(pos_count>0, "%s::evaluate_roc(): Number of positive labels is "
			"zero, ROC fails!\n", get_name());
	REQUIRE(neg_count>0, "%s::evaluate_roc(): Number of negative labels is "
			"zero, ROC fails!\n", get_name());

	int32_t j = 0;
	float64_t label;

	// create ROC curve and calculate auROC
	for(i=0; i<length; i++)
	{
		label = predicted->get_value(idxs[i]);

		if (label != threshold)
		{
			threshold = label;
			m_ROC_graph[2*j] = fp/neg_count;
			m_ROC_graph[2*j+1] = tp/pos_count;
			j++;
		}

		m_thresholds[i]=threshold;

		if (ground_truth->get_label(idxs[i]) > 0)
			tp+=1.0;
		else
			fp+=1.0;
	}

	// add (1,1) to ROC curve
	m_ROC_graph[2*diff_count] = 1.0;
	m_ROC_graph[2*diff_count+1] = 1.0;

	// calc auROC using area under curve
	m_auROC = CMath::area_under_curve(m_ROC_graph.matrix,diff_count+1,false);

	m_computed = true;

	return m_auROC;
}

SGMatrix<float64_t> CROCEvaluation::get_ROC()
{
	if (!m_computed)
		SG_ERROR("Uninitialized, please call evaluate first")

	return m_ROC_graph;
}

SGVector<float64_t> CROCEvaluation::get_thresholds()
{
	if (!m_computed)
		SG_ERROR("Uninitialized, please call evaluate first")

	return m_thresholds;
}

float64_t CROCEvaluation::get_auROC()
{
	if (!m_computed)
			SG_ERROR("Uninitialized, please call evaluate first")

	return m_auROC;
}
<commit_msg>Fix ROC error messages (#4042)<commit_after>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Sergey Lisitsyn
 * Copyright (C) 2011 Berlin Institute of Technology and Max-Planck-Society
 */

#include <shogun/evaluation/ROCEvaluation.h>
#include <shogun/mathematics/Math.h>

using namespace shogun;

CROCEvaluation::~CROCEvaluation()
{
}

float64_t CROCEvaluation::evaluate(CLabels* predicted, CLabels* ground_truth)
{
	REQUIRE(predicted, "No predicted labels provided.\n");
	REQUIRE(ground_truth, "No ground truth labels provided.\n");
	REQUIRE(
	    predicted->get_label_type() == LT_BINARY,
	    "Given predicted labels (%d) must be binary (%d).\n",
	    predicted->get_label_type(), LT_BINARY);
	REQUIRE(
	    ground_truth->get_label_type() == LT_BINARY,
	    "Given ground truth labels (%d) must be binary (%d).\n",
	    ground_truth->get_label_type(), LT_BINARY);

	return evaluate_roc((CBinaryLabels*)predicted,(CBinaryLabels*)ground_truth);
}

float64_t CROCEvaluation::evaluate_roc(CBinaryLabels* predicted, CBinaryLabels* ground_truth)
{
	ASSERT(predicted && ground_truth)
	ASSERT(predicted->get_num_labels()==ground_truth->get_num_labels())
	ground_truth->ensure_valid();

	// assume threshold as negative infinity
	float64_t threshold = CMath::ALMOST_NEG_INFTY;
	// false positive rate
	float64_t fp = 0.0;
	// true positive rate
	float64_t tp=0.0;

	int32_t i;
	// total number of positive labels in predicted
	int32_t pos_count=0;
	int32_t neg_count=0;

	// initialize number of labels and labels
	SGVector<float64_t> orig_labels(predicted->get_num_labels());
	int32_t length = orig_labels.vlen;
	for (i=0; i<length; i++)
		orig_labels[i] = predicted->get_value(i);
	float64_t* labels = SGVector<float64_t>::clone_vector(orig_labels.vector, length);

	// get sorted indexes
	SGVector<int32_t> idxs(length);
	for(i=0; i<length; i++)
		idxs[i] = i;

	CMath::qsort_backward_index(labels,idxs.vector,idxs.vlen);

	// number of different predicted labels
	int32_t diff_count=1;

	// get number of different labels
	for (i=0; i<length-1; i++)
	{
		if (labels[i] != labels[i+1])
			diff_count++;
	}

	SG_FREE(labels);

	// initialize graph and auROC
	m_ROC_graph = SGMatrix<float64_t>(2,diff_count+1);
	m_thresholds = SGVector<float64_t>(length);
	m_auROC = 0.0;

	// get total numbers of positive and negative labels
	for(i=0; i<length; i++)
	{
		if (ground_truth->get_label(i) >= 0)
			pos_count++;
		else
			neg_count++;
	}

	// assure both number of positive and negative examples is >0
	REQUIRE(pos_count>0, "%s::evaluate_roc(): Number of positive labels is "
			"zero, ROC fails!\n", get_name());
	REQUIRE(neg_count>0, "%s::evaluate_roc(): Number of negative labels is "
			"zero, ROC fails!\n", get_name());

	int32_t j = 0;
	float64_t label;

	// create ROC curve and calculate auROC
	for(i=0; i<length; i++)
	{
		label = predicted->get_value(idxs[i]);

		if (label != threshold)
		{
			threshold = label;
			m_ROC_graph[2*j] = fp/neg_count;
			m_ROC_graph[2*j+1] = tp/pos_count;
			j++;
		}

		m_thresholds[i]=threshold;

		if (ground_truth->get_label(idxs[i]) > 0)
			tp+=1.0;
		else
			fp+=1.0;
	}

	// add (1,1) to ROC curve
	m_ROC_graph[2*diff_count] = 1.0;
	m_ROC_graph[2*diff_count+1] = 1.0;

	// calc auROC using area under curve
	m_auROC = CMath::area_under_curve(m_ROC_graph.matrix,diff_count+1,false);

	m_computed = true;

	return m_auROC;
}

SGMatrix<float64_t> CROCEvaluation::get_ROC()
{
	if (!m_computed)
		SG_ERROR("Uninitialized, please call evaluate first")

	return m_ROC_graph;
}

SGVector<float64_t> CROCEvaluation::get_thresholds()
{
	if (!m_computed)
		SG_ERROR("Uninitialized, please call evaluate first")

	return m_thresholds;
}

float64_t CROCEvaluation::get_auROC()
{
	if (!m_computed)
			SG_ERROR("Uninitialized, please call evaluate first")

	return m_auROC;
}
<|endoftext|>


<commit_before>#pragma once

#include "basic_Error.hpp"

namespace serialkeymanager_com {

class Error : public basic_Error {
  // TODO: Why is it neccessary to prefix basic_Error here for the compiler to
  //       find the methods?
  int get_subsystem() const noexcept { return basic_Error::get_subsystem(api::main()); }
  int get_reason() const noexcept { return basic_Error::get_reason(api::main()); }
  size_t get_extra() const noexcept { return basic_Error::get_extra(api::main()); }
};

} // namespace serialkeymanager_com
<commit_msg>Make members public and unhide inherited methods for Error<commit_after>#pragma once

#include "basic_Error.hpp"

namespace serialkeymanager_com {

class Error : public basic_Error {
public:
  // Declarations below hide these methods inherited from the basic_Error class
  using basic_Error::get_subsystem;
  using basic_Error::get_reason;
  using basic_Error::get_extra;

  int get_subsystem() const noexcept { return get_subsystem(api::main()); }
  int get_reason() const noexcept { return get_reason(api::main()); }
  size_t get_extra() const noexcept { return get_extra(api::main()); }
};

} // namespace serialkeymanager_com
<|endoftext|>


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

/*
 * This file contains tables for the electrical layout and setup of the
 * electrical components.  Keep the formatting in this file very easy to read
 * such that it is easy to maintain and quickly check if it is in sync with
 * the actual state of affairs on the electrical board!
 */

#include <RobotDrive.h>
#include <Jaguar.h>
#include <Joystick.h>
#include <Encoder.h>
#include <Ultrasonic.h>
#include <DigitalInput.h>
#include <Relay.h>
#include <Servo.h>
#include <AnalogChannel.h>
#include <Vision/AxisCamera.h>
#include "612.h"
#include "ports.h"
#include "joysmooth.h"
#include "roller.h"
#include "shifter.h"

//just define & initialize all of the consts in ports.h

//slots table - in order to make sync with electrical easier.
const module_t slot1 = 1; //1st analog
const module_t slot2 = 1; //1st digital
const module_t slot3 = 1; //1st solenoid
const module_t slot4 = 0; //EMPTY
const module_t slot5 = 2; //2nd analog
const module_t slot6 = 2; //2nd digital
const module_t slot7 = 2; //2nd solenoid
const module_t slot8 = 0; //EMPTY

//camera IP address:
const char * cameraIP = "10.6.12.11"; //static IP, camera configured for connection to bridge
//const char * cameraIP = "192.168.0.90"; //static IP, camera configured for connection to cRIO

//PORTS TABLE

//PWMs                                    SLOT     PORT
Jaguar left_launcher_jag                ( slot2,     1 );
Jaguar right_launcher_jag               ( slot2,     2 );
Jaguar right_front_jag                  ( slot2,     3 );
Jaguar right_rear_jag                   ( slot2,     4 );
Jaguar turret_rotation_jag              ( slot2,     5 );
Servo right_servo_shift                 ( slot2,     6 );

Jaguar turret_winch_jag                 ( slot6,     1 );
Jaguar left_front_jag                   ( slot6,     2 );
Jaguar left_rear_jag                    ( slot6,     3 );
Servo left_servo_shift                  ( slot6,     4 );

PWM camera_led                          ( slot6,     5 );

//NOTE: Sica wants to use pots (potentiometers) for the angles, as those are
//absolute and don't need a zero switch.  If we do that they're seen as
//AnalogChannel s.  We'll want to write a quick wrapper class so we can get
//an angular offset, but we'll cross that bridge when we come to it.

//note: for two channel encoders, we need to specify a slot for both ports
//DIOs                                    SLOT     PORT
Encoder right_drive                     ( slot2,     1,
                                          slot2,     2 );

DigitalInput turret_limit_mid           ( slot2,     5 );
DigitalInput turret_limit_right         ( slot2,     6 );
DigitalInput turret_limit_left          ( slot2,     7 );

Counter launcher_wheel                  ( slot2,    10 );

Ultrasonic front_ultrasonic             ( slot6,     2,
                                          slot6,     3 );
Encoder left_drive                      ( slot6,     4,
                                          slot6,     5 );
DigitalInput bridge_arm_switch          ( slot6,     6 );

//note: since we rely on the default value of kInches for the 5th arg
//we should use Ultrasonic::GetRangeInches().

//AIOs                                    SLOT     PORT
AnalogChannel launch_angle_pot          ( slot1,     1 );

//Relays                                  SLOT     PORT
Relay roller_spike_1                    ( slot6,     1 );
Relay roller_spike_2                    ( slot6,     2 );
Relay bridge_arm_spike                  ( slot6,    10 );

//USBs (on driver station)                         PORT
Joystick left_joystick_raw                         ( 1 );
Joystick right_joystick_raw                        ( 2 );
Joystick gunner_joystick_raw                       ( 3 );

joysmooth left_joystick        (  left_joystick_raw    );
joysmooth right_joystick       (  right_joystick_raw   );
joysmooth gunner_joystick      (  gunner_joystick_raw  );

//initialization of virtual devices:

//RobotDrive
RobotDrive drive (
    left_front_motor.jag,  //KEEP THIS ORDER (HERE)!  IT'S IMPORTANT!
    left_rear_motor.jag,   //LF, LR, RF, RR -- see documentation
    right_front_motor.jag, //for more details
    right_rear_motor.jag
);

//roller_t
roller_t rollers(roller_spike_1, roller_spike_2);

//shifter
shifter servo_shifter(left_servo_shift, right_servo_shift);

/*
//turret
turret shooter_turret(
    turret_rotation_jag,
    turret_winch_jag,
    left_launcher_jag,
    right_launcher_jag,
    launcher_wheel,
    launch_angle_pot,
    launch_angle_switch,

);
*/

//drive_jaguar                           JAGUAR&                 TYPE               REVERSE
drive_jaguar left_front_motor  =    { left_front_jag,  RobotDrive::kFrontLeftMotor,  false };
drive_jaguar left_rear_motor   =    { left_rear_jag,   RobotDrive::kRearLeftMotor,   false };
drive_jaguar right_front_motor =    { right_front_jag, RobotDrive::kFrontRightMotor, false };
drive_jaguar right_rear_motor  =    { right_rear_jag,  RobotDrive::kRearRightMotor,  false };
<commit_msg>Revert "Fixed compile errors"<commit_after>/* ports.cpp
 *
 * Copyright (c) 2011, 2012 Chantilly Robotics <chantilly612@gmail.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/*
 * This file contains tables for the electrical layout and setup of the
 * electrical components.  Keep the formatting in this file very easy to read
 * such that it is easy to maintain and quickly check if it is in sync with
 * the actual state of affairs on the electrical board!
 */

#include <RobotDrive.h>
#include <Jaguar.h>
#include <Joystick.h>
#include <Encoder.h>
#include <Ultrasonic.h>
#include <DigitalInput.h>
#include <Relay.h>
#include <Servo.h>
#include <AnalogChannel.h>
#include <Vision/AxisCamera.h>
#include "612.h"
#include "ports.h"
#include "joysmooth.h"
#include "roller.h"
#include "shifter.h"

//just define & initialize all of the consts in ports.h

//slots table - in order to make sync with electrical easier.
const module_t slot1 = 1; //1st analog
const module_t slot2 = 1; //1st digital
const module_t slot3 = 1; //1st solenoid
const module_t slot4 = 0; //EMPTY
const module_t slot5 = 2; //2nd analog
const module_t slot6 = 2; //2nd digital
const module_t slot7 = 2; //2nd solenoid
const module_t slot8 = 0; //EMPTY

//camera IP address:
const char * cameraIP = "10.6.12.11"; //static IP, camera configured for connection to bridge
//const char * cameraIP = "192.168.0.90"; //static IP, camera configured for connection to cRIO

//PORTS TABLE

//PWMs                                    SLOT     PORT
Jaguar left_launcher_jag                ( slot2,     1 );
Jaguar right_launcher_jag               ( slot2,     2 );
Jaguar right_front_jag                  ( slot2,     3 );
Jaguar right_rear_jag                   ( slot2,     4 );
Jaguar turret_rotation_jag              ( slot2,     5 );
Servo right_servo_shift                 ( slot2,     6 );

Jaguar turret_winch_jag                 ( slot6,     1 );
Jaguar left_front_jag                   ( slot6,     2 );
Jaguar left_rear_jag                    ( slot6,     3 );
Servo left_servo_shift                  ( slot6,     4 );

PWM camera_led                          ( slot6,     5 );

//NOTE: Sica wants to use pots (potentiometers) for the angles, as those are
//absolute and don't need a zero switch.  If we do that they're seen as
//AnalogChannel s.  We'll want to write a quick wrapper class so we can get
//an angular offset, but we'll cross that bridge when we come to it.

//note: for two channel encoders, we need to specify a slot for both ports
//DIOs                                    SLOT     PORT
Encoder right_drive                     ( slot2,     1,
                                          slot2,     2 );

DigitalInput turret_limit_mid           ( slot2,     5 );
DigitalInput turret_limit_right         ( slot2,     6 );
DigitalInput turret_limit_left          ( slot2,     7 );

Counter launcher_wheel                  ( slot2,    10 );

Ultrasonic front_ultrasonic             ( slot6,     2,
                                          slot6,     3 );
Encoder left_drive                      ( slot6,     4,
                                          slot6,     5 );
DigitalInput bridge_arm_switch          ( slot6,     6 );

//note: since we rely on the default value of kInches for the 5th arg
//we should use Ultrasonic::GetRangeInches().

//AIOs                                    SLOT     PORT
AnalogChannel launch_angle_pot          ( slot1,     1 );

//Relays                                  SLOT     PORT
Relay roller_spike_1                    ( slot6,     1 );
Relay roller_spike_2                    ( slot6,     2 );
Relay bridge_arm_spike                  ( slot6,     8 );

//USBs (on driver station)                         PORT
Joystick left_joystick_raw                         ( 1 );
Joystick right_joystick_raw                        ( 2 );
Joystick gunner_joystick_raw                       ( 3 );

joysmooth left_joystick        (  left_joystick_raw    );
joysmooth right_joystick       (  right_joystick_raw   );
joysmooth gunner_joystick      (  gunner_joystick_raw  );

//initialization of virtual devices:

//RobotDrive
RobotDrive drive (
    left_front_motor.jag,  //KEEP THIS ORDER (HERE)!  IT'S IMPORTANT!
    left_rear_motor.jag,   //LF, LR, RF, RR -- see documentation
    right_front_motor.jag, //for more details
    right_rear_motor.jag
);

//roller_t
roller_t rollers(roller_spike_1, roller_spike_2);

//shifter
shifter servo_shifter(left_servo_shift, right_servo_shift);

/*
//turret
turret shooter_turret(
    turret_rotation_jag,
    turret_winch_jag,
    left_launcher_jag,
    right_launcher_jag,
    launcher_wheel,
    launch_angle_pot,
    launch_angle_switch,

);
*/

//drive_jaguar                           JAGUAR&                 TYPE               REVERSE
drive_jaguar left_front_motor  =    { left_front_jag,  RobotDrive::kFrontLeftMotor,  false };
drive_jaguar left_rear_motor   =    { left_rear_jag,   RobotDrive::kRearLeftMotor,   false };
drive_jaguar right_front_motor =    { right_front_jag, RobotDrive::kFrontRightMotor, false };
drive_jaguar right_rear_motor  =    { right_rear_jag,  RobotDrive::kRearRightMotor,  false };
<|endoftext|>


<commit_before>#include "gps.h"

#include "constants.h"

inline constexpr float my_abs(float arg) { return arg < 0 ? -arg : arg; }
inline constexpr int my_abs(int arg) { return arg < 0 ? -arg : arg; }

namespace rcr {
namespace level1payload {

GpsReceiver::GpsReceiver() : Sensor(KALMAN_PROCESS_NOISE, KALMAN_MEASUREMENT_NOISE, KALMAN_ERROR) {
  latitude_ = 0.;
  longitude_ = 0.;

  speed_ = kalmanInit(0.);
  altitude_ = kalmanInit(0.);
}

bool GpsReceiver::Init() {
  Serial1.setRX(GPS_RX_PIN);
  Serial1.setTX(GPS_TX_PIN);
  Serial1.begin(GPS_BAUD);

  smartDelay(1000ul);
  return true;
}

double GpsReceiver::getLatitude() {
  smartDelay();
  if (gps_.location.isValid())
    latitude_ = gps_.location.lat();
  return latitude_;
}

double GpsReceiver::getLongitude() {
  smartDelay();
  if (gps_.location.isValid())
    longitude_ = gps_.location.lng();
  return longitude_;
}

float GpsReceiver::getSpeed() {
  smartDelay();
  if (gps_.speed.isValid()) {
    kalmanUpdate(&speed_, gps_.speed.knots());
  }
  return speed_.value;
}

float GpsReceiver::getAltitude() {
  smartDelay();
  if (gps_.altitude.isValid()) {
    kalmanUpdate(&altitude_, gps_.altitude.meters());
  }
  return altitude_.value;
}

void GpsReceiver::smartDelay() {
  while (Serial1.available())
    gps_.encode(Serial1.read());
}

void GpsReceiver::smartDelay(unsigned long ms) {
  unsigned long start = millis();
  do {
    while (Serial1.available())
      gps_.encode(Serial1.read());
  } while (millis() - start < ms);
}

String GpsReceiver::getDateString(TinyGPSDate &d) {
  smartDelay();

  if (d.isValid()) {
    char sz[11];
    sprintf(sz, "%02d/%02d/%02d", d.month(), d.day(), d.year());
    return String{ sz };
  }
  return String{ "**/**/****" }; // same length
}
String GpsReceiver::getTimeString(TinyGPSTime &t) {
  smartDelay();

  if (t.isValid()) {
    char sz[9];
    sprintf(sz, "%02d:%02d:%02d", t.hour(), t.minute(), t.second());
    return String{ sz };
  }
  return String{ "**:**:**" }; // same length
}
String GpsReceiver::getDateAgeString(TinyGPSDate &d) {
  smartDelay();
  return getIntString(d.age(), d.isValid(), 5);
}
String GpsReceiver::getFloatString(float val, bool valid, int len, int prec) {
  String result = "";

  if (!valid) {
    while (len-- > 1)
      result += "*";
    result += " ";
  }
  else {
    result += String(val, prec);
    int vi = my_abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // for "." and "-"
    flen += vi >= 1000 ? 4
      : vi >= 100 ? 3
      : vi >= 10 ? 2 : 1;
    for (int i = flen; i < len; ++i)
      result += " ";
  }

  smartDelay();
  return result;
}
String GpsReceiver::getIntString(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i = strlen(sz); i<len; ++i)
    sz[i] = ' ';
  if (len > 0)
    sz[len - 1] = ' ';

  smartDelay();
  return String{ sz };
}
String GpsReceiver::getCstringString(const char *str, int len)
{
  String result = "";
  int slen = strlen(str);
  for (auto i = 0; i<len; ++i)
    result += (i < slen ? str[i] : ' ');
  smartDelay();
  return result;
}

String GpsReceiver::GetCsvLine() {
  smartDelay();
  String line = "";

  line += getIntString(gps_.satellites.value(), gps_.satellites.isValid(), 5);
  line += ",";
  line += getIntString(gps_.hdop.value(), gps_.hdop.isValid(), 5);
  line += ",";
  line += getFloatString(gps_.location.lat(), gps_.location.isValid(), 11, 6);
  line += ",";
  line += getFloatString(gps_.location.lng(), gps_.location.isValid(), 12, 6);
  line += ",";
  line += getIntString(gps_.location.age(), gps_.location.isValid(), 5);
  line += ",";
  line += getDateString(gps_.date);
  line += ",";
  line += getTimeString(gps_.time);
  line += ",";
  line += getDateAgeString(gps_.date);
  line += ",";
  line += getFloatString(gps_.altitude.meters(), gps_.altitude.isValid(), 7, 2);
  line += ",";
  line += getFloatString(gps_.course.deg(), gps_.course.isValid(), 7, 2);
  line += ",";
  line += getFloatString(gps_.speed.kmph(), gps_.speed.isValid(), 6, 2);
  line += ",";
  line += getCstringString(gps_.course.isValid() ? TinyGPSPlus::cardinal(gps_.course.deg()) : "*** ", 6);
  line += ",";

  unsigned long distance_to_pad =
  (unsigned long)TinyGPSPlus::distanceBetween(
    gps_.location.lat(),
    gps_.location.lng(),
    PAD_LAT,
    PAD_LON) / 1000;

  line += getIntString(distance_to_pad, gps_.location.isValid(), 9);
  line += ",";

  double course_to_pad = TinyGPSPlus::courseTo(
    gps_.location.lat(),
    gps_.location.lng(),
    PAD_LAT,
    PAD_LON);
  line += getFloatString(course_to_pad, gps_.location.isValid(), 7, 2);
  line += ",";

  const char *cardinalToLondon = TinyGPSPlus::cardinal(course_to_pad);
  line += getCstringString(gps_.location.isValid() ? cardinalToLondon : "*** ", 6);
  line += ",";

  line += getIntString(gps_.charsProcessed(), true, 6);
  line += ",";
  line += getIntString(gps_.sentencesWithFix(), true, 10);
  line += ",";
  line += getIntString(gps_.failedChecksum(), true, 9);
  line += ",";

  if (millis() > 5000 && gps_.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));

  smartDelay();

  return line;
}

} // namespace level1_payload
} // namespace rcr
<commit_msg>remove alll internal smartDelay()s<commit_after>#include "gps.h"

#include "constants.h"

inline constexpr float my_abs(float arg) { return arg < 0 ? -arg : arg; }
inline constexpr int my_abs(int arg) { return arg < 0 ? -arg : arg; }

namespace rcr {
namespace level1payload {

GpsReceiver::GpsReceiver() : Sensor(KALMAN_PROCESS_NOISE, KALMAN_MEASUREMENT_NOISE, KALMAN_ERROR) {
  latitude_ = 0.;
  longitude_ = 0.;

  speed_ = kalmanInit(0.);
  altitude_ = kalmanInit(0.);
}

bool GpsReceiver::Init() {
  Serial1.setRX(GPS_RX_PIN);
  Serial1.setTX(GPS_TX_PIN);
  Serial1.begin(GPS_BAUD);

  delay(1000u);
  return true;
}

double GpsReceiver::getLatitude() {
  if (gps_.location.isValid())
    latitude_ = gps_.location.lat();
  return latitude_;
}

double GpsReceiver::getLongitude() {
  if (gps_.location.isValid())
    longitude_ = gps_.location.lng();
  return longitude_;
}

float GpsReceiver::getSpeed() {
  if (gps_.speed.isValid()) {
    kalmanUpdate(&speed_, gps_.speed.knots());
  }
  return speed_.value;
}

float GpsReceiver::getAltitude() {
  if (gps_.altitude.isValid()) {
    kalmanUpdate(&altitude_, gps_.altitude.meters());
  }
  return altitude_.value;
}

void GpsReceiver::smartDelay() {
  while (Serial1.available())
    gps_.encode(Serial1.read());
}

void GpsReceiver::smartDelay(unsigned long ms) {
  unsigned long start = millis();
  do {
    while (Serial1.available())
      gps_.encode(Serial1.read());
  } while (millis() - start < ms);
}

String GpsReceiver::getDateString(TinyGPSDate &d) {
  if (d.isValid()) {
    char sz[11];
    sprintf(sz, "%02d/%02d/%02d", d.month(), d.day(), d.year());
    return String{ sz };
  }
  return String{ "**/**/****" }; // same length
}
String GpsReceiver::getTimeString(TinyGPSTime &t) {
  if (t.isValid()) {
    char sz[9];
    sprintf(sz, "%02d:%02d:%02d", t.hour(), t.minute(), t.second());
    return String{ sz };
  }
  return String{ "**:**:**" }; // same length
}
String GpsReceiver::getDateAgeString(TinyGPSDate &d) {
  return getIntString(d.age(), d.isValid(), 5);
}
String GpsReceiver::getFloatString(float val, bool valid, int len, int prec) {
  String result = "";

  if (!valid) {
    while (len-- > 1)
      result += "*";
    result += " ";
  }
  else {
    result += String(val, prec);
    int vi = my_abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // for "." and "-"
    flen += vi >= 1000 ? 4
      : vi >= 100 ? 3
      : vi >= 10 ? 2 : 1;
    for (int i = flen; i < len; ++i)
      result += " ";
  }

  return result;
}
String GpsReceiver::getIntString(unsigned long val, bool valid, int len) {
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i = strlen(sz); i<len; ++i)
    sz[i] = ' ';
  if (len > 0)
    sz[len - 1] = ' ';

  return String{ sz };
}
String GpsReceiver::getCstringString(const char *str, int len) {
  String result = "";
  int slen = strlen(str);
  for (auto i = 0; i<len; ++i)
    result += (i < slen ? str[i] : ' ');

  return result;
}

String GpsReceiver::GetCsvLine() {
  String line = "";

  line += getIntString(gps_.satellites.value(), gps_.satellites.isValid(), 5);
  line += ",";
  line += getIntString(gps_.hdop.value(), gps_.hdop.isValid(), 5);
  line += ",";
  line += getFloatString(gps_.location.lat(), gps_.location.isValid(), 11, 6);
  line += ",";
  line += getFloatString(gps_.location.lng(), gps_.location.isValid(), 12, 6);
  line += ",";
  line += getIntString(gps_.location.age(), gps_.location.isValid(), 5);
  line += ",";
  line += getDateString(gps_.date);
  line += ",";
  line += getTimeString(gps_.time);
  line += ",";
  line += getDateAgeString(gps_.date);
  line += ",";
  line += getFloatString(gps_.altitude.meters(), gps_.altitude.isValid(), 7, 2);
  line += ",";
  line += getFloatString(gps_.course.deg(), gps_.course.isValid(), 7, 2);
  line += ",";
  line += getFloatString(gps_.speed.kmph(), gps_.speed.isValid(), 6, 2);
  line += ",";
  line += getCstringString(gps_.course.isValid() ? TinyGPSPlus::cardinal(gps_.course.deg()) : "*** ", 6);
  line += ",";

  unsigned long distance_to_pad =
  (unsigned long)TinyGPSPlus::distanceBetween(
    gps_.location.lat(),
    gps_.location.lng(),
    PAD_LAT,
    PAD_LON) / 1000;

  line += getIntString(distance_to_pad, gps_.location.isValid(), 9);
  line += ",";

  double course_to_pad = TinyGPSPlus::courseTo(
    gps_.location.lat(),
    gps_.location.lng(),
    PAD_LAT,
    PAD_LON);
  line += getFloatString(course_to_pad, gps_.location.isValid(), 7, 2);
  line += ",";

  const char *cardinalToLondon = TinyGPSPlus::cardinal(course_to_pad);
  line += getCstringString(gps_.location.isValid() ? cardinalToLondon : "*** ", 6);
  line += ",";

  line += getIntString(gps_.charsProcessed(), true, 6);
  line += ",";
  line += getIntString(gps_.sentencesWithFix(), true, 10);
  line += ",";
  line += getIntString(gps_.failedChecksum(), true, 9);
  line += ",";

  if (millis() > 5000 && gps_.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));

  return line;
}

} // namespace level1_payload
} // namespace rcr
<|endoftext|>


<commit_before>#include "errors.hpp"
#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>

#include "perfmon/core.hpp"
#include "arch/runtime/coroutines.hpp"

/* Constructor and destructor register and deregister the perfmon. */
perfmon_t::perfmon_t(perfmon_collection_t *_parent, bool _insert)
    : parent(_parent), insert(_insert)
{
    if (insert) {
        guarantee(_parent != NULL, "Global perfmon collection must be specified explicitly instead of using NULL");
        if (coroutines_have_been_initialized()) {
            // We spawn a coroutine which will add the current perfmon into the parent perfmon collection. We do
            // it in order to avoid adding an incompletely constructed object into the perfmon collection which
            // could be queried concurrently. It is assumed here that the constructor of the deriving class
            // cannot block. If it can, it's a bug!
            coro_t::spawn_later_ordered(boost::bind(&perfmon_collection_t::add, parent, this));
        } else {
            // There are no other coroutines and, we assume, no other threads that can access the parent perfmon
            // collection, so we add `this` into `parent` right now, even though the current object hasn't been
            // completely constructed.
            parent->add(this);
        }
    }
}

perfmon_t::~perfmon_t() {
    if (insert) {
        parent->remove(this);
    }
}

struct stats_collection_context_t : public home_thread_mixin_t {
    stats_collection_context_t(rwi_lock_t *constituents_lock, size_t size)
        : contexts(new void *[size]), lock_sentry(constituents_lock) { }

    ~stats_collection_context_t() {
        delete[] contexts;
    }

    void **contexts;
private:
    rwi_lock_t::read_acq_t lock_sentry;
};

void *perfmon_collection_t::begin_stats() {
    stats_collection_context_t *ctx;
    {
        on_thread_t thread_switcher(home_thread());
        ctx = new stats_collection_context_t(&constituents_access, constituents.size());
    }

    size_t i = 0;
    for (perfmon_t *p = constituents.head(); p; p = constituents.next(p), ++i) {
        ctx->contexts[i] = p->begin_stats();
    }
    return ctx;
}

void perfmon_collection_t::visit_stats(void *_context) {
    stats_collection_context_t *ctx = reinterpret_cast<stats_collection_context_t*>(_context);
    size_t i = 0;
    for (perfmon_t *p = constituents.head(); p; p = constituents.next(p), ++i) {
        p->visit_stats(ctx->contexts[i]);
    }
}

void perfmon_collection_t::end_stats(void *_context, perfmon_result_t *result) {
    stats_collection_context_t *ctx = reinterpret_cast<stats_collection_context_t*>(_context);

    // TODO: This is completely fucked up shitty code.
    perfmon_result_t *map;
    if (create_submap) {
        perfmon_result_t::alloc_map_result(&map);
        rassert(result->get_map()->count(name) == 0);
        result->get_map()->insert(std::pair<std::string, perfmon_result_t *>(name, map));
    } else {
        map = result;
    }

    size_t i = 0;
    for (perfmon_t *p = constituents.head(); p; p = constituents.next(p), ++i) {
        assert_thread();
        p->end_stats(ctx->contexts[i], map);
    }

    {
        on_thread_t thread_switcher(home_thread());
        delete ctx; // cleans up, unlocks
    }
}

void perfmon_collection_t::add(perfmon_t *perfmon) {
    boost::scoped_ptr<on_thread_t> thread_switcher;
    if (coroutines_have_been_initialized()) {
        thread_switcher.reset(new on_thread_t(home_thread()));
    }

    rwi_lock_t::write_acq_t write_acq(&constituents_access);
    constituents.push_back(perfmon);
}

void perfmon_collection_t::remove(perfmon_t *perfmon) {
    boost::scoped_ptr<on_thread_t> thread_switcher;
    if (coroutines_have_been_initialized()) {
        thread_switcher.reset(new on_thread_t(home_thread()));
    }

    rwi_lock_t::write_acq_t write_acq(&constituents_access);
    constituents.remove(perfmon);
}

perfmon_result_t::perfmon_result_t() {
    type = type_value;
}

perfmon_result_t::perfmon_result_t(const perfmon_result_t &copyee)
    : type(copyee.type), value_(copyee.value_), map_() {
    for (perfmon_result_t::internal_map_t::const_iterator it = copyee.map_.begin(); it != copyee.map_.end(); ++it) {
        perfmon_result_t *subcopy = new perfmon_result_t(*it->second);
        map_.insert(std::pair<std::string, perfmon_result_t *>(it->first, subcopy));
    }
}

perfmon_result_t::perfmon_result_t(const std::string &s) {
    type = type_value;
    value_ = s;
}

perfmon_result_t::perfmon_result_t(const std::map<std::string, perfmon_result_t *> &m) {
    type = type_map;
    map_ = m;
}

perfmon_result_t::~perfmon_result_t() {
    if (type == type_map) {
        clear_map();
    }
    rassert(map_.empty());
}

void perfmon_result_t::clear_map() {
    for (perfmon_result_t::internal_map_t::iterator it = map_.begin(); it != map_.end(); ++it) {
        delete it->second;
    }
    map_.clear();
}

perfmon_result_t perfmon_result_t::make_string() {
    return perfmon_result_t(std::string());
}

void perfmon_result_t::alloc_string_result(perfmon_result_t **out) {
    *out = new perfmon_result_t(std::string());
}

perfmon_result_t perfmon_result_t::make_map() {
    return perfmon_result_t(perfmon_result_t::internal_map_t());
}

void perfmon_result_t::alloc_map_result(perfmon_result_t **out) {
    *out = new perfmon_result_t(perfmon_result_t::internal_map_t());
}

std::string *perfmon_result_t::get_string() {
    rassert(type == type_value);
    return &value_;
}

const std::string *perfmon_result_t::get_string() const {
    rassert(type == type_value);
    return &value_;
}

perfmon_result_t::internal_map_t *perfmon_result_t::get_map() {
    rassert(type == type_map);
    return &map_;
}

const perfmon_result_t::internal_map_t *perfmon_result_t::get_map() const {
    rassert(type == type_map);
    return &map_;
}

size_t perfmon_result_t::get_map_size() const {
    rassert(type == type_map);
    return map_.size();
}

bool perfmon_result_t::is_string() const {
    return type == type_value;
}

bool perfmon_result_t::is_map() const {
    return type == type_map;
}

perfmon_result_t::perfmon_result_type_t perfmon_result_t::get_type() const {
    return type;
}

void perfmon_result_t::reset_type(perfmon_result_type_t new_type) {
    value_.clear();
    clear_map();
    type = new_type;
}

std::pair<perfmon_result_t::iterator, bool> perfmon_result_t::insert(const std::string &name, perfmon_result_t *val) {
    std::string s(name);
    perfmon_result_t::internal_map_t *map = get_map();
    rassert(map->count(name) == 0);
    return map->insert(std::pair<std::string, perfmon_result_t *>(s, val));
}

perfmon_result_t::iterator perfmon_result_t::begin() {
    return map_.begin();
}

perfmon_result_t::iterator perfmon_result_t::end() {
    return map_.end();
}

perfmon_result_t::const_iterator perfmon_result_t::begin() const {
    return map_.begin();
}

perfmon_result_t::const_iterator perfmon_result_t::end() const {
    return map_.end();
}

perfmon_collection_t &get_global_perfmon_collection() {
    /* Getter function so that we can be sure that `collection` is initialized
    before it is needed, as advised by the C++ FAQ. Otherwise, a `perfmon_t`
    might be initialized before `collection` was initialized. */

    // FIXME: probably use "new" to create the perfmon_collection_t. For more info check out C++ FAQ Lite answer 10.16.
    static perfmon_collection_t collection("Global", NULL, false, false);
    return collection;
}

<commit_msg>Change spawn_later_ordered to spawn_sometime in perfmon adding to a collection<commit_after>#include "errors.hpp"
#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>

#include "perfmon/core.hpp"
#include "arch/runtime/coroutines.hpp"

/* Constructor and destructor register and deregister the perfmon. */
perfmon_t::perfmon_t(perfmon_collection_t *_parent, bool _insert)
    : parent(_parent), insert(_insert)
{
    if (insert) {
        guarantee(_parent != NULL, "Global perfmon collection must be specified explicitly instead of using NULL");
        if (coroutines_have_been_initialized()) {
            // We spawn a coroutine which will add the current perfmon into the parent perfmon collection. We do
            // it in order to avoid adding an incompletely constructed object into the perfmon collection which
            // could be queried concurrently. It is assumed here that the constructor of the deriving class
            // cannot block. If it can, it's a bug!
            coro_t::spawn_sometime(boost::bind(&perfmon_collection_t::add, parent, this));
        } else {
            // There are no other coroutines and, we assume, no other threads that can access the parent perfmon
            // collection, so we add `this` into `parent` right now, even though the current object hasn't been
            // completely constructed.
            parent->add(this);
        }
    }
}

perfmon_t::~perfmon_t() {
    if (insert) {
        parent->remove(this);
    }
}

struct stats_collection_context_t : public home_thread_mixin_t {
    stats_collection_context_t(rwi_lock_t *constituents_lock, size_t size)
        : contexts(new void *[size]), lock_sentry(constituents_lock) { }

    ~stats_collection_context_t() {
        delete[] contexts;
    }

    void **contexts;
private:
    rwi_lock_t::read_acq_t lock_sentry;
};

void *perfmon_collection_t::begin_stats() {
    stats_collection_context_t *ctx;
    {
        on_thread_t thread_switcher(home_thread());
        ctx = new stats_collection_context_t(&constituents_access, constituents.size());
    }

    size_t i = 0;
    for (perfmon_t *p = constituents.head(); p; p = constituents.next(p), ++i) {
        ctx->contexts[i] = p->begin_stats();
    }
    return ctx;
}

void perfmon_collection_t::visit_stats(void *_context) {
    stats_collection_context_t *ctx = reinterpret_cast<stats_collection_context_t*>(_context);
    size_t i = 0;
    for (perfmon_t *p = constituents.head(); p; p = constituents.next(p), ++i) {
        p->visit_stats(ctx->contexts[i]);
    }
}

void perfmon_collection_t::end_stats(void *_context, perfmon_result_t *result) {
    stats_collection_context_t *ctx = reinterpret_cast<stats_collection_context_t*>(_context);

    // TODO: This is completely fucked up shitty code.
    perfmon_result_t *map;
    if (create_submap) {
        perfmon_result_t::alloc_map_result(&map);
        rassert(result->get_map()->count(name) == 0);
        result->get_map()->insert(std::pair<std::string, perfmon_result_t *>(name, map));
    } else {
        map = result;
    }

    size_t i = 0;
    for (perfmon_t *p = constituents.head(); p; p = constituents.next(p), ++i) {
        assert_thread();
        p->end_stats(ctx->contexts[i], map);
    }

    {
        on_thread_t thread_switcher(home_thread());
        delete ctx; // cleans up, unlocks
    }
}

void perfmon_collection_t::add(perfmon_t *perfmon) {
    boost::scoped_ptr<on_thread_t> thread_switcher;
    if (coroutines_have_been_initialized()) {
        thread_switcher.reset(new on_thread_t(home_thread()));
    }

    rwi_lock_t::write_acq_t write_acq(&constituents_access);
    constituents.push_back(perfmon);
}

void perfmon_collection_t::remove(perfmon_t *perfmon) {
    boost::scoped_ptr<on_thread_t> thread_switcher;
    if (coroutines_have_been_initialized()) {
        thread_switcher.reset(new on_thread_t(home_thread()));
    }

    rwi_lock_t::write_acq_t write_acq(&constituents_access);
    constituents.remove(perfmon);
}

perfmon_result_t::perfmon_result_t() {
    type = type_value;
}

perfmon_result_t::perfmon_result_t(const perfmon_result_t &copyee)
    : type(copyee.type), value_(copyee.value_), map_() {
    for (perfmon_result_t::internal_map_t::const_iterator it = copyee.map_.begin(); it != copyee.map_.end(); ++it) {
        perfmon_result_t *subcopy = new perfmon_result_t(*it->second);
        map_.insert(std::pair<std::string, perfmon_result_t *>(it->first, subcopy));
    }
}

perfmon_result_t::perfmon_result_t(const std::string &s) {
    type = type_value;
    value_ = s;
}

perfmon_result_t::perfmon_result_t(const std::map<std::string, perfmon_result_t *> &m) {
    type = type_map;
    map_ = m;
}

perfmon_result_t::~perfmon_result_t() {
    if (type == type_map) {
        clear_map();
    }
    rassert(map_.empty());
}

void perfmon_result_t::clear_map() {
    for (perfmon_result_t::internal_map_t::iterator it = map_.begin(); it != map_.end(); ++it) {
        delete it->second;
    }
    map_.clear();
}

perfmon_result_t perfmon_result_t::make_string() {
    return perfmon_result_t(std::string());
}

void perfmon_result_t::alloc_string_result(perfmon_result_t **out) {
    *out = new perfmon_result_t(std::string());
}

perfmon_result_t perfmon_result_t::make_map() {
    return perfmon_result_t(perfmon_result_t::internal_map_t());
}

void perfmon_result_t::alloc_map_result(perfmon_result_t **out) {
    *out = new perfmon_result_t(perfmon_result_t::internal_map_t());
}

std::string *perfmon_result_t::get_string() {
    rassert(type == type_value);
    return &value_;
}

const std::string *perfmon_result_t::get_string() const {
    rassert(type == type_value);
    return &value_;
}

perfmon_result_t::internal_map_t *perfmon_result_t::get_map() {
    rassert(type == type_map);
    return &map_;
}

const perfmon_result_t::internal_map_t *perfmon_result_t::get_map() const {
    rassert(type == type_map);
    return &map_;
}

size_t perfmon_result_t::get_map_size() const {
    rassert(type == type_map);
    return map_.size();
}

bool perfmon_result_t::is_string() const {
    return type == type_value;
}

bool perfmon_result_t::is_map() const {
    return type == type_map;
}

perfmon_result_t::perfmon_result_type_t perfmon_result_t::get_type() const {
    return type;
}

void perfmon_result_t::reset_type(perfmon_result_type_t new_type) {
    value_.clear();
    clear_map();
    type = new_type;
}

std::pair<perfmon_result_t::iterator, bool> perfmon_result_t::insert(const std::string &name, perfmon_result_t *val) {
    std::string s(name);
    perfmon_result_t::internal_map_t *map = get_map();
    rassert(map->count(name) == 0);
    return map->insert(std::pair<std::string, perfmon_result_t *>(s, val));
}

perfmon_result_t::iterator perfmon_result_t::begin() {
    return map_.begin();
}

perfmon_result_t::iterator perfmon_result_t::end() {
    return map_.end();
}

perfmon_result_t::const_iterator perfmon_result_t::begin() const {
    return map_.begin();
}

perfmon_result_t::const_iterator perfmon_result_t::end() const {
    return map_.end();
}

perfmon_collection_t &get_global_perfmon_collection() {
    /* Getter function so that we can be sure that `collection` is initialized
    before it is needed, as advised by the C++ FAQ. Otherwise, a `perfmon_t`
    might be initialized before `collection` was initialized. */

    // FIXME: probably use "new" to create the perfmon_collection_t. For more info check out C++ FAQ Lite answer 10.16.
    static perfmon_collection_t collection("Global", NULL, false, false);
    return collection;
}

<|endoftext|>


<commit_before>/****************************************************************************
 *   Copyright (C) 2013-2014 by Savoir-Faire Linux                          *
 *   Author : Emmanuel Lepage Vallee <emmanuel.lepage@savoirfairelinux.com> *
 *                                                                          *
 *   This library is free software; you can redistribute it and/or          *
 *   modify it under the terms of 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.     *
 *                                                                          *
 *   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 General Public License      *
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.  *
 ***************************************************************************/
#include "phonenumber.h"
#include "phonedirectorymodel.h"
#include "contact.h"
#include "account.h"
#include "call.h"
#include "dbus/presencemanager.h"
#include "numbercategorymodel.h"
#include "numbercategory.h"

QHash<int,Call*> PhoneNumber::m_shMostUsed = QHash<int,Call*>();

const PhoneNumber* PhoneNumber::m_spBlank = nullptr;
const PhoneNumber* PhoneNumber::BLANK()
{
   if (!m_spBlank) {
      m_spBlank = new PhoneNumber("",NumberCategoryModel::other());
      const_cast<PhoneNumber*>(m_spBlank)->m_Type = PhoneNumber::Type::BLANK;
   }
   return m_spBlank;
}

///Constructor
PhoneNumber::PhoneNumber(const QString& number, NumberCategory* cat, Type st) : QObject(PhoneDirectoryModel::instance()),
   m_Uri(stripUri(number)),m_pCategory(cat),m_Tracked(false),m_Present(false),m_LastUsed(0),
   m_Type(st),m_PopularityIndex(-1),m_pContact(nullptr),m_pAccount(nullptr),
   m_LastWeekCount(0),m_LastTrimCount(0),m_HaveCalled(false),m_IsBookmark(false),m_TotalSeconds(0)
{
   setObjectName(m_Uri);
   m_hasType = cat != NumberCategoryModel::other();
   if (m_hasType) {
      NumberCategoryModel::instance()->registerNumber(this);
   }
}

PhoneNumber::~PhoneNumber()
{
}

///Return if this number presence is being tracked
bool PhoneNumber::isTracked() const
{
   //If the number doesn't support it, ignore the flag
   return supportPresence() && m_Tracked;
}

///Is this number present
bool PhoneNumber::isPresent() const
{
   return m_Tracked && m_Present;
}

///This number presence status string
QString PhoneNumber::presenceMessage() const
{
   return m_PresentMessage;
}

///Return the number
QString PhoneNumber::uri() const {
   return m_Uri ;
}

///Return the phone number type
NumberCategory* PhoneNumber::category() const {
   return m_pCategory ;
}

///Return this number associated account, if any
Account* PhoneNumber::account() const
{
   return m_pAccount;
}

///Return this number associated contact, if any
Contact* PhoneNumber::contact() const
{
   return m_pContact;
}

///Return when this number was last used
time_t PhoneNumber::lastUsed() const
{
   return m_LastUsed;
}

///Set this number default account
void PhoneNumber::setAccount(Account* account)
{
   m_pAccount = account;
   if (m_pAccount)
      connect (m_pAccount,SIGNAL(destroyed(QObject*)),this,SLOT(accountDestroyed(QObject*)));
   emit changed();
}

///Set this number contact
void PhoneNumber::setContact(Contact* contact)
{
   m_pContact = contact;
   if (contact && m_Type != PhoneNumber::Type::TEMPORARY)
      PhoneDirectoryModel::instance()->indexNumber(this,m_hNames.keys()+QStringList(contact->formattedName()));
   emit changed();
}

void PhoneNumber::setCategory(NumberCategory* cat)
{
   if (cat == m_pCategory) return;
   if (m_hasType)
      NumberCategoryModel::instance()->unregisterNumber(this);
   m_hasType = cat != NumberCategoryModel::other();
   m_pCategory = cat;
   if (m_hasType)
      NumberCategoryModel::instance()->registerNumber(this);
   emit changed();
}

void PhoneNumber::setBookmarked(bool bookmarked )
{
   m_IsBookmark = bookmarked;
}

///Force an Uid on this number (instead of hash)
void PhoneNumber::setUid(const QString& uri)
{
   m_Uid = uri;
}

///Attempt to change the number type
bool PhoneNumber::setType(PhoneNumber::Type t)
{
   if (m_Type == PhoneNumber::Type::BLANK)
      return false;
   if (account() && t == PhoneNumber::Type::ACCOUNT) {
      if (account()->supportPresenceSubscribe()) {
         m_Tracked = true; //The daemon will init the tracker itself
         emit trackedChanged(true);
      }
      m_Type = t;
      return true;
   }
   return false;
}

///Set if this number is tracking presence information
void PhoneNumber::setTracked(bool track)
{
   if (track != m_Tracked) { //Subscribe only once
      //You can't subscribe without account
      if (track && !m_pAccount) return;
      m_Tracked = track;
      DBus::PresenceManager::instance().subscribeBuddy(m_pAccount->id(),fullUri(),track);
      emit changed();
      emit trackedChanged(track);
   }
}

///Allow phonedirectorymodel to change presence status
void PhoneNumber::setPresent(bool present)
{
   if (m_Present != present) {
      m_Present = present;
      emit presentChanged(present);
   }
}

void PhoneNumber::setPresenceMessage(const QString& message)
{
   if (m_PresentMessage != message) {
      m_PresentMessage = message;
      emit presenceMessageChanged(message);
   }
}

///Return the current type of the number
PhoneNumber::Type PhoneNumber::type() const
{
   return m_Type;
}

///Return the number of calls from this number
int PhoneNumber::callCount() const
{
   return m_lCalls.size();
}

uint PhoneNumber::weekCount() const
{
   return m_LastWeekCount;
}

uint PhoneNumber::trimCount() const
{
   return m_LastTrimCount;
}

bool PhoneNumber::haveCalled() const
{
   return m_HaveCalled;
}

///Best bet for this person real name
QString PhoneNumber::primaryName() const
{
   if (m_pContact)
      return m_pContact->formattedName();
   else if (m_hNames.size() == 1)
      return m_hNames.constBegin().key();
   else {
      QString toReturn = tr("Unknown");
      QHash<QString,int>::const_iterator i = m_hNames.constBegin();
      int max = 0;
      while (i != m_hNames.end()) {
         if (i.value() > max) {
            max      = i.value();
            toReturn = i.key  ();
         }
      }
      return toReturn;
   }
}

///Is this number bookmarked
bool PhoneNumber::isBookmarked() const
{
   return m_IsBookmark;
}

///If this number could (theoretically) support presence status
bool PhoneNumber::supportPresence() const
{
   //Without an account, presence is impossible
   if (!m_pAccount)
      return false;
   //The account also have to support it
   if (!m_pAccount->supportPresenceSubscribe())
       return false;

   //In the end, it all come down to this, is the number tracked
   return true;
}

///Proxy accessor to the category icon
QVariant PhoneNumber::icon() const
{
   return category()->icon(isTracked(),isPresent());
}

///The number of seconds spent with the URI (from history)
int PhoneNumber::totalSpentTime() const
{
   return m_TotalSeconds;
}

///Return this number unique identifier (hash)
QString PhoneNumber::uid() const
{
   return m_Uid.isEmpty()?toHash():m_Uid;
}

///Return all calls from this number
QList<Call*> PhoneNumber::calls() const
{
   return m_lCalls;
}

///Return the phonenumber position in the popularity index
int PhoneNumber::popularityIndex() const
{
   return m_PopularityIndex;
}

QHash<QString,int> PhoneNumber::alternativeNames() const
{
   return m_hNames;
}

///Add a call to the call list, notify listener
void PhoneNumber::addCall(Call* call)
{
   if (!call) return;
   m_Type = PhoneNumber::Type::USED;
   m_lCalls << call;
   m_TotalSeconds += call->stopTimeStamp() - call->startTimeStamp();
   time_t now;
   ::time ( &now );
   if (now - 3600*24*7 < call->stopTimeStamp())
      m_LastWeekCount++;
   if (now - 3600*24*7*15 < call->stopTimeStamp())
      m_LastTrimCount++;

   if (call->historyState() == Call::LegacyHistoryState::OUTGOING || call->direction() == Call::Direction::OUTGOING)
      m_HaveCalled = true;

   emit callAdded(call);
   if (call->startTimeStamp() > m_LastUsed)
      m_LastUsed = call->startTimeStamp();
   emit changed();
}

///Generate an unique representation of this number
QString PhoneNumber::toHash() const
{
   return QString("%1///%2///%3").arg(uri()).arg(account()?account()->id():QString()).arg(contact()?contact()->uid():QString());
}

///Return the domaine of an URI (<sip:12345@exemple.com>)
QString PhoneNumber::hostname() const
{
   if (m_Uri.indexOf('@') != -1) {
      return m_Uri.split('@')[1].left(m_Uri.split('@')[1].size());
   }
   return QString();
}

QString PhoneNumber::fullUri() const
{
   return QString("<sip:%1>").arg(m_Uri);
}

///Strip out <sip:****> from the URI
QString PhoneNumber::stripUri(const QString& uri)
{
   int start(0),end(uri.size()-1); //Other type of comparaisons were too slow
   if (uri.size() > 0 && uri[0] == '<' && uri[4] == ':')
      start = 5;
   if (end && uri[end] == '>')
      end--;
   return uri.mid(start,end-start+1);
}

///Increment name counter and update indexes
void PhoneNumber::incrementAlternativeName(const QString& name)
{
   const bool needReIndexing = !m_hNames[name];
   m_hNames[name]++;
   if (needReIndexing && m_Type != PhoneNumber::Type::TEMPORARY)
      PhoneDirectoryModel::instance()->indexNumber(this,m_hNames.keys()+(m_pContact?(QStringList(m_pContact->formattedName())):QStringList()));
}

void PhoneNumber::accountDestroyed(QObject* o)
{
   if (o == m_pAccount)
      m_pAccount = nullptr;
}

/************************************************************************************
 *                                                                                *
 *                             Temporary phone number                               *
 *                                                                                *
 ***********************************************************************************/

void TemporaryPhoneNumber::setUri(const QString& uri)
{
   m_Uri = uri;
   emit changed();
}

///Constructor
TemporaryPhoneNumber::TemporaryPhoneNumber(const PhoneNumber* number) :
   PhoneNumber(QString(),NumberCategoryModel::other(),PhoneNumber::Type::TEMPORARY)
{
   if (number) {
      setContact(number->contact());
      setAccount(number->account());
   }
}
<commit_msg>[ #39354 ] Prevent an infinite loop<commit_after>/****************************************************************************
 *   Copyright (C) 2013-2014 by Savoir-Faire Linux                          *
 *   Author : Emmanuel Lepage Vallee <emmanuel.lepage@savoirfairelinux.com> *
 *                                                                          *
 *   This library is free software; you can redistribute it and/or          *
 *   modify it under the terms of 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.     *
 *                                                                          *
 *   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 General Public License      *
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.  *
 ***************************************************************************/
#include "phonenumber.h"
#include "phonedirectorymodel.h"
#include "contact.h"
#include "account.h"
#include "call.h"
#include "dbus/presencemanager.h"
#include "numbercategorymodel.h"
#include "numbercategory.h"

QHash<int,Call*> PhoneNumber::m_shMostUsed = QHash<int,Call*>();

const PhoneNumber* PhoneNumber::m_spBlank = nullptr;
const PhoneNumber* PhoneNumber::BLANK()
{
   if (!m_spBlank) {
      m_spBlank = new PhoneNumber("",NumberCategoryModel::other());
      const_cast<PhoneNumber*>(m_spBlank)->m_Type = PhoneNumber::Type::BLANK;
   }
   return m_spBlank;
}

///Constructor
PhoneNumber::PhoneNumber(const QString& number, NumberCategory* cat, Type st) : QObject(PhoneDirectoryModel::instance()),
   m_Uri(stripUri(number)),m_pCategory(cat),m_Tracked(false),m_Present(false),m_LastUsed(0),
   m_Type(st),m_PopularityIndex(-1),m_pContact(nullptr),m_pAccount(nullptr),
   m_LastWeekCount(0),m_LastTrimCount(0),m_HaveCalled(false),m_IsBookmark(false),m_TotalSeconds(0)
{
   setObjectName(m_Uri);
   m_hasType = cat != NumberCategoryModel::other();
   if (m_hasType) {
      NumberCategoryModel::instance()->registerNumber(this);
   }
}

PhoneNumber::~PhoneNumber()
{
}

///Return if this number presence is being tracked
bool PhoneNumber::isTracked() const
{
   //If the number doesn't support it, ignore the flag
   return supportPresence() && m_Tracked;
}

///Is this number present
bool PhoneNumber::isPresent() const
{
   return m_Tracked && m_Present;
}

///This number presence status string
QString PhoneNumber::presenceMessage() const
{
   return m_PresentMessage;
}

///Return the number
QString PhoneNumber::uri() const {
   return m_Uri ;
}

///Return the phone number type
NumberCategory* PhoneNumber::category() const {
   return m_pCategory ;
}

///Return this number associated account, if any
Account* PhoneNumber::account() const
{
   return m_pAccount;
}

///Return this number associated contact, if any
Contact* PhoneNumber::contact() const
{
   return m_pContact;
}

///Return when this number was last used
time_t PhoneNumber::lastUsed() const
{
   return m_LastUsed;
}

///Set this number default account
void PhoneNumber::setAccount(Account* account)
{
   m_pAccount = account;
   if (m_pAccount)
      connect (m_pAccount,SIGNAL(destroyed(QObject*)),this,SLOT(accountDestroyed(QObject*)));
   emit changed();
}

///Set this number contact
void PhoneNumber::setContact(Contact* contact)
{
   m_pContact = contact;
   if (contact && m_Type != PhoneNumber::Type::TEMPORARY)
      PhoneDirectoryModel::instance()->indexNumber(this,m_hNames.keys()+QStringList(contact->formattedName()));
   emit changed();
}

void PhoneNumber::setCategory(NumberCategory* cat)
{
   if (cat == m_pCategory) return;
   if (m_hasType)
      NumberCategoryModel::instance()->unregisterNumber(this);
   m_hasType = cat != NumberCategoryModel::other();
   m_pCategory = cat;
   if (m_hasType)
      NumberCategoryModel::instance()->registerNumber(this);
   emit changed();
}

void PhoneNumber::setBookmarked(bool bookmarked )
{
   m_IsBookmark = bookmarked;
}

///Force an Uid on this number (instead of hash)
void PhoneNumber::setUid(const QString& uri)
{
   m_Uid = uri;
}

///Attempt to change the number type
bool PhoneNumber::setType(PhoneNumber::Type t)
{
   if (m_Type == PhoneNumber::Type::BLANK)
      return false;
   if (account() && t == PhoneNumber::Type::ACCOUNT) {
      if (account()->supportPresenceSubscribe()) {
         m_Tracked = true; //The daemon will init the tracker itself
         emit trackedChanged(true);
      }
      m_Type = t;
      return true;
   }
   return false;
}

///Set if this number is tracking presence information
void PhoneNumber::setTracked(bool track)
{
   if (track != m_Tracked) { //Subscribe only once
      //You can't subscribe without account
      if (track && !m_pAccount) return;
      m_Tracked = track;
      DBus::PresenceManager::instance().subscribeBuddy(m_pAccount->id(),fullUri(),track);
      emit changed();
      emit trackedChanged(track);
   }
}

///Allow phonedirectorymodel to change presence status
void PhoneNumber::setPresent(bool present)
{
   if (m_Present != present) {
      m_Present = present;
      emit presentChanged(present);
   }
}

void PhoneNumber::setPresenceMessage(const QString& message)
{
   if (m_PresentMessage != message) {
      m_PresentMessage = message;
      emit presenceMessageChanged(message);
   }
}

///Return the current type of the number
PhoneNumber::Type PhoneNumber::type() const
{
   return m_Type;
}

///Return the number of calls from this number
int PhoneNumber::callCount() const
{
   return m_lCalls.size();
}

uint PhoneNumber::weekCount() const
{
   return m_LastWeekCount;
}

uint PhoneNumber::trimCount() const
{
   return m_LastTrimCount;
}

bool PhoneNumber::haveCalled() const
{
   return m_HaveCalled;
}

///Best bet for this person real name
QString PhoneNumber::primaryName() const
{
   if (m_pContact)
      return m_pContact->formattedName();
   else if (m_hNames.size() == 1)
      return m_hNames.constBegin().key();
   else {
      QString toReturn = tr("Unknown");
      QHash<QString,int>::const_iterator i = m_hNames.constBegin();
      int max = 0;
      while (i != m_hNames.end()) {
         if (i.value() > max) {
            max      = i.value();
            toReturn = i.key  ();
         }
         i++;
      }
      return toReturn;
   }
}

///Is this number bookmarked
bool PhoneNumber::isBookmarked() const
{
   return m_IsBookmark;
}

///If this number could (theoretically) support presence status
bool PhoneNumber::supportPresence() const
{
   //Without an account, presence is impossible
   if (!m_pAccount)
      return false;
   //The account also have to support it
   if (!m_pAccount->supportPresenceSubscribe())
       return false;

   //In the end, it all come down to this, is the number tracked
   return true;
}

///Proxy accessor to the category icon
QVariant PhoneNumber::icon() const
{
   return category()->icon(isTracked(),isPresent());
}

///The number of seconds spent with the URI (from history)
int PhoneNumber::totalSpentTime() const
{
   return m_TotalSeconds;
}

///Return this number unique identifier (hash)
QString PhoneNumber::uid() const
{
   return m_Uid.isEmpty()?toHash():m_Uid;
}

///Return all calls from this number
QList<Call*> PhoneNumber::calls() const
{
   return m_lCalls;
}

///Return the phonenumber position in the popularity index
int PhoneNumber::popularityIndex() const
{
   return m_PopularityIndex;
}

QHash<QString,int> PhoneNumber::alternativeNames() const
{
   return m_hNames;
}

///Add a call to the call list, notify listener
void PhoneNumber::addCall(Call* call)
{
   if (!call) return;
   m_Type = PhoneNumber::Type::USED;
   m_lCalls << call;
   m_TotalSeconds += call->stopTimeStamp() - call->startTimeStamp();
   time_t now;
   ::time ( &now );
   if (now - 3600*24*7 < call->stopTimeStamp())
      m_LastWeekCount++;
   if (now - 3600*24*7*15 < call->stopTimeStamp())
      m_LastTrimCount++;

   if (call->historyState() == Call::LegacyHistoryState::OUTGOING || call->direction() == Call::Direction::OUTGOING)
      m_HaveCalled = true;

   emit callAdded(call);
   if (call->startTimeStamp() > m_LastUsed)
      m_LastUsed = call->startTimeStamp();
   emit changed();
}

///Generate an unique representation of this number
QString PhoneNumber::toHash() const
{
   return QString("%1///%2///%3").arg(uri()).arg(account()?account()->id():QString()).arg(contact()?contact()->uid():QString());
}

///Return the domaine of an URI (<sip:12345@exemple.com>)
QString PhoneNumber::hostname() const
{
   if (m_Uri.indexOf('@') != -1) {
      return m_Uri.split('@')[1].left(m_Uri.split('@')[1].size());
   }
   return QString();
}

QString PhoneNumber::fullUri() const
{
   return QString("<sip:%1>").arg(m_Uri);
}

///Strip out <sip:****> from the URI
QString PhoneNumber::stripUri(const QString& uri)
{
   int start(0),end(uri.size()-1); //Other type of comparaisons were too slow
   if (uri.size() > 0 && uri[0] == '<' && uri[4] == ':')
      start = 5;
   if (end && uri[end] == '>')
      end--;
   return uri.mid(start,end-start+1);
}

///Increment name counter and update indexes
void PhoneNumber::incrementAlternativeName(const QString& name)
{
   const bool needReIndexing = !m_hNames[name];
   m_hNames[name]++;
   if (needReIndexing && m_Type != PhoneNumber::Type::TEMPORARY)
      PhoneDirectoryModel::instance()->indexNumber(this,m_hNames.keys()+(m_pContact?(QStringList(m_pContact->formattedName())):QStringList()));
}

void PhoneNumber::accountDestroyed(QObject* o)
{
   if (o == m_pAccount)
      m_pAccount = nullptr;
}

/************************************************************************************
 *                                                                                *
 *                             Temporary phone number                               *
 *                                                                                *
 ***********************************************************************************/

void TemporaryPhoneNumber::setUri(const QString& uri)
{
   m_Uri = uri;
   emit changed();
}

///Constructor
TemporaryPhoneNumber::TemporaryPhoneNumber(const PhoneNumber* number) :
   PhoneNumber(QString(),NumberCategoryModel::other(),PhoneNumber::Type::TEMPORARY)
{
   if (number) {
      setContact(number->contact());
      setAccount(number->account());
   }
}
<|endoftext|>


<commit_before>//=======================================================================
// Copyright Baptiste Wicht 2013.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef MONEY_H
#define MONEY_H

#include <string>
#include <ostream>

#include "utils.hpp"

namespace budget {

struct money {
    int dollars = 0;
    int cents = 0;

    money operator+(const money& rhs) const {
        money new_money = *this;
        new_money += rhs;
        return new_money;
    }

    money& operator+=(const money& rhs){
        dollars += rhs.dollars;
        cents += rhs.cents;

        if(cents > 100){
            dollars += cents / 100;
            cents %= 100;
        }

        return *this;
    }

    money operator-(const money& rhs) const {
        money new_money = *this;
        new_money -= rhs;
        return new_money;
    }

    money& operator-=(const money& rhs){
        dollars = dollars - rhs.dollars;
        cents = cents - rhs.cents;

        if(cents < 0){
            dollars -= cents / 100;
            cents %= 100;
            cents *= -1;
        }

        return *this;
    }

    money operator*(int factor) const {
        money new_money = *this;
        new_money *= factor;
        return new_money;
    }

    money& operator*=(int factor){
        dollars = dollars * factor;
        cents = cents * factor;

        if(cents > 100){
            dollars += cents / 100;
            cents %= 100;
        }

        if(cents < 0){
            dollars -= cents / 100;
            cents %= 100;
            cents *= -1;
        }

        return *this;
    }

    money operator/(int factor) const {
        money new_money = *this;
        new_money /= factor;
        return new_money;
    }

    money& operator/=(int factor){
        dollars = dollars / factor;
        cents = cents / factor;

        //TODO The cents of the dollars should be reported to cents

        return *this;
    }
};

std::ostream& operator<<(std::ostream& stream, const money& amount);

money parse_money(const std::string& money_string);
void not_negative(const money& money);

template<>
inline std::string to_string(const money& amount){
    std::stringstream stream;
    stream << amount;
    return stream.str();
}

} //end of namespace budget

#endif
<commit_msg>Fix subtraction of money<commit_after>//=======================================================================
// Copyright Baptiste Wicht 2013.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef MONEY_H
#define MONEY_H

#include <string>
#include <ostream>

#include "utils.hpp"

namespace budget {

struct money {
    int dollars;
    int cents;

    money() : dollars(0), cents(0) {
        //Nothing to init
    }

    money(int dollars, int cents) : dollars(dollars), cents(cents) {
        //Nothing to init
    }

    money operator+(const money& rhs) const {
        money new_money = *this;
        new_money += rhs;
        return new_money;
    }

    money& operator+=(const money& rhs){
        dollars += rhs.dollars;
        cents += rhs.cents;

        if(cents > 100){
            dollars += cents / 100;
            cents %= 100;
        }

        return *this;
    }

    money operator-(const money& rhs) const {
        money new_money = *this;
        new_money -= rhs;
        return new_money;
    }

    money& operator-=(const money& rhs){
        dollars = dollars - rhs.dollars;
        cents = cents - rhs.cents;

        if(cents < 0){
            dollars -= (-cents / 100) + 1;
            cents = 100 - (-cents % 100);
        }

        return *this;
    }

    money operator*(int factor) const {
        money new_money = *this;
        new_money *= factor;
        return new_money;
    }

    money& operator*=(int factor){
        dollars = dollars * factor;
        cents = cents * factor;

        if(cents > 100){
            dollars += cents / 100;
            cents %= 100;
        }

        if(cents < 0){
            dollars -= cents / 100;
            cents %= 100;
            cents *= -1;
        }

        return *this;
    }

    money operator/(int factor) const {
        money new_money = *this;
        new_money /= factor;
        return new_money;
    }

    money& operator/=(int factor){
        dollars = dollars / factor;
        cents = cents / factor;

        //TODO The cents of the dollars should be reported to cents

        return *this;
    }
};

std::ostream& operator<<(std::ostream& stream, const money& amount);

money parse_money(const std::string& money_string);
void not_negative(const money& money);

template<>
inline std::string to_string(const money& amount){
    std::stringstream stream;
    stream << amount;
    return stream.str();
}

} //end of namespace budget

#endif
<|endoftext|>


<commit_before>#include <ecn_sensorbased/pioneer_cam.h>
#include <visp/vpCameraParameters.h>
#include <visp/vpPixelMeterConversion.h>
#include <algorithm>
#include <visp/vpSubMatrix.h>
using namespace std;

PioneerCam::PioneerCam(ros::NodeHandle &_nh) : it_(_nh)
{
    // joint setpoint publisher
    joint_pub_  =_nh.advertise<sensor_msgs::JointState>("/joint_setpoint", 1);
    joint_setpoint_.name = {"Pioneer_p3dx_leftMotor", "Pioneer_p3dx_rightMotor", "camera_pan", "camera_tilt"};
    joint_setpoint_.velocity.resize(4);

    // wheels
    radius_ = .0975;
    base_ = .331;
    w_max_ = 4;

    // camera calibration
    cam_.initFromFov(640,480,vpMath::rad(60), vpMath::rad(60.*480/640));

    // joints subscriber
    joint_ok_ = false;
    joint_sub_ = _nh.subscribe("/joint_states", 1, &PioneerCam::readJointState, this);
    joint_names_ = {"camera_pan", "camera_tilt"};
    q_.resize(joint_names_.size());

    // image transport
    im_ok_ = false;
    s_im_.resize(2);
    //cv::startWindowThread();
    im_sub_ = it_.subscribe("/image", 1, &PioneerCam::readImage, this);
    std::cout << "Pioneer init ok" << std::endl;
    sphere_sub_ = _nh.subscribe("/sphere", 1, &PioneerCam::readSpherePose, this);

    // pose
    target_ok_ = false;
    target_sub_ = _nh.subscribe("/target", 1, &PioneerCam::readTargetPose, this);

}

void PioneerCam::setVelocity(const vpColVector &v)
{
    if(v.size() != 4)
    {
        std::cout << "PioneerCam::setVelocity: v should be length 4, is " << v.size() << std::endl;
        return;
    }

    // change (v,w) to left and right wheel velocities
    joint_setpoint_.velocity[0] = (v[0]-base_*v[1])/radius_;
    joint_setpoint_.velocity[1] = (v[0]+base_*v[1])/radius_;
    double a = std::max(vpMath::abs(joint_setpoint_.velocity[0])/w_max_, vpMath::abs(joint_setpoint_.velocity[1])/w_max_);
    if(a<1)
        a = 1;

    // scale wheel velocities if activated
    if(vpMath::abs(joint_setpoint_.velocity[0])/w_max_ > 1)
        cout << "*** Left wheel velocity above limit (" << joint_setpoint_.velocity[0] << ") ***" << endl;
    if(vpMath::abs(joint_setpoint_.velocity[1])/w_max_ > 1)
        cout << "*** Right wheel velocity above limit (" << joint_setpoint_.velocity[1] << ") ***" << endl;

    joint_setpoint_.velocity[0] *= 1./a;
    joint_setpoint_.velocity[1] *= 1./a;

    // copy camera joints velocity
    joint_setpoint_.velocity[2] = v[2];
    joint_setpoint_.velocity[3] = v[3];

    joint_pub_.publish(joint_setpoint_);
}

vpMatrix PioneerCam::getCamJacobian(const vpColVector &_q)
{
    // autogenerated from pioneer.urdf
    vpMatrix J(6,4);
    const double c1 = cos(_q[0]);
    const double c2 = cos(_q[1]);
    const double s1 = sin(_q[0]);
    const double s2 = sin(_q[1]);

    J[0][0] = s1;
    J[0][1] = -0.155485*c1 - 0.035*c2;
    J[0][2] = -0.035*c2;
    //J[0][3] = 0;
    J[1][0] = s2*c1;
    //J[1][1] = 0;
    //J[1][2] = 0;
    J[1][3] = -0.035;
    J[2][0] = c1*c2;
    //J[2][1] = 0;
    //J[2][2] = 0;
    //J[2][3] = 0;
    //J[3][0] = 0;
    //J[3][1] = 0;
    //J[3][2] = 0;
    J[3][3] = 1.;
    //J[4][0] = 0;
    J[4][1] = -c2;
    J[4][2] = -c2;
    //J[4][3] = 0;
    //J[5][0] = 0;
    J[5][1] = s2;
    J[5][2] = s2;
    //J[5][3] = 0;

    return J;
}

void PioneerCam::readJointState(const sensor_msgs::JointStateConstPtr &_msg)
{
    joint_ok_ = true;
    for(unsigned int j=0;j<joint_names_.size();++j)
    {
        for(unsigned int i=0;i<_msg->name.size();++i)
        {
            if(_msg->name[i] == joint_names_[j])
            {
                q_[j] = _msg->position[i];
                break;
            }
        }
    }
}

void PioneerCam::readImage(const sensor_msgs::ImageConstPtr& msg)
{

    cv::Mat im = cv_bridge::toCvShare(msg, "bgr8")->image;

    try
    {
        cv::Mat img;
        cv::cvtColor(im, img, cv::COLOR_BGR2HSV);

        // segment for green detection
        cv::inRange(img, cv::Scalar(55,0,0), cv::Scalar(65,255,255), img);

        // edge detection
        cv::Canny(img, img, 20, 150);

        vector<vector<cv::Point> > contours;
        vector<cv::Vec4i> hierarchy;
        cv::findContours( img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);

        // sort contours from largest to smallest
        std::sort(contours.begin(), contours.end(),
                  [](const vector<cv::Point> &c1, const vector<cv::Point> &c2)
        {return cv::contourArea(c1) > cv::contourArea(c2);});

        // show largest contour in red
        if(contours.size() > 0)
        {
            cv::Moments m = cv::moments(contours[0], false);
            cv::drawContours(im, contours, 0, cv::Scalar(0,0,255), 2);
        }
    }
    catch (...)
    {
        ROS_WARN("Could not detect sphere");
    }
    cv::imshow("view", im);
    cv::waitKey(1);
}

void PioneerCam::readTargetPose(const geometry_msgs::PoseConstPtr &msg)
{
    target_ok_ = true;
    target_pose_.x = msg->position.x;
    target_pose_.y = msg->position.y;
    target_pose_.theta = 2*atan2(msg->orientation.z, msg->orientation.w);
}

void PioneerCam::readSpherePose(const geometry_msgs::PoseConstPtr &msg)
{
    im_ok_ = true;
    s_im_[0] = msg->position.x/msg->position.z;
    s_im_[1] = msg->position.y/msg->position.z;
}
<commit_msg>sterne<commit_after>#include <ecn_sensorbased/pioneer_cam.h>
#include <visp/vpCameraParameters.h>
#include <visp/vpPixelMeterConversion.h>
#include <algorithm>
#include <visp/vpSubMatrix.h>
using namespace std;

PioneerCam::PioneerCam(ros::NodeHandle &_nh) : it_(_nh)
{
    // joint setpoint publisher
    joint_pub_  =_nh.advertise<sensor_msgs::JointState>("/joint_setpoint", 1);
    joint_setpoint_.name = {"Pioneer_p3dx_leftMotor", "Pioneer_p3dx_rightMotor", "camera_pan", "camera_tilt"};
    joint_setpoint_.velocity.resize(4);

    // wheels
    radius_ = .0975;
    base_ = .331;
    w_max_ = 4;

    // camera calibration
    cam_.initFromFov(640,480,vpMath::rad(60), vpMath::rad(60.*480/640));

    // joints subscriber
    joint_ok_ = false;
    joint_sub_ = _nh.subscribe("/joint_states", 1, &PioneerCam::readJointState, this);
    joint_names_ = {"camera_pan", "camera_tilt"};
    q_.resize(joint_names_.size());

    // image transport
    im_ok_ = false;
    s_im_.resize(2);
    //cv::startWindowThread();
    im_sub_ = it_.subscribe("/image", 1, &PioneerCam::readImage, this);
    std::cout << "Pioneer init ok" << std::endl;
    sphere_sub_ = _nh.subscribe("/sphere", 1, &PioneerCam::readSpherePose, this);

    // pose
    target_ok_ = false;
    target_sub_ = _nh.subscribe("/target", 1, &PioneerCam::readTargetPose, this);

}

void PioneerCam::setVelocity(const vpColVector &v)
{
    if(v.size() != 4)
    {
        std::cout << "PioneerCam::setVelocity: v should be length 4, is " << v.size() << std::endl;
        return;
    }

    // change (v,w) to left and right wheel velocities
    joint_setpoint_.velocity[0] = (v[0]-base_*v[1])/radius_;
    joint_setpoint_.velocity[1] = (v[0]+base_*v[1])/radius_;
    double a = std::max(vpMath::abs(joint_setpoint_.velocity[0])/w_max_, vpMath::abs(joint_setpoint_.velocity[1])/w_max_);
    if(a<1)
        a = 1;

    // scale wheel velocities if activated
    if(vpMath::abs(joint_setpoint_.velocity[0])/w_max_ > 1)
        cout << "*** Left wheel velocity above limit (" << joint_setpoint_.velocity[0] << ") ***" << endl;
    if(vpMath::abs(joint_setpoint_.velocity[1])/w_max_ > 1)
        cout << "*** Right wheel velocity above limit (" << joint_setpoint_.velocity[1] << ") ***" << endl;

    joint_setpoint_.velocity[0] *= 1./a;
    joint_setpoint_.velocity[1] *= 1./a;

    // copy camera joints velocity
    joint_setpoint_.velocity[2] = v[2];
    joint_setpoint_.velocity[3] = v[3];

    joint_pub_.publish(joint_setpoint_);
}

vpMatrix PioneerCam::getCamJacobian(const vpColVector &_q)
{
    // autogenerated from pioneer.urdf
    vpMatrix J(6,4);
    const double c1 = cos(_q[0]);
    const double c2 = cos(_q[1]);
    const double s1 = sin(_q[0]);
    const double s2 = sin(_q[1]);

    J[0][0] = s1;
    J[0][1] = -0.155485*c1 - 0.035*c2;
    J[0][2] = -0.035*c2;
    //J[0][3] = 0;
    J[1][0] = s2*c1;
    //J[1][1] = 0;
    //J[1][2] = 0;
    J[1][3] = -0.035;
    J[2][0] = c1*c2;
    //J[2][1] = 0;
    //J[2][2] = 0;
    //J[2][3] = 0;
    //J[3][0] = 0;
    //J[3][1] = 0;
    //J[3][2] = 0;
    J[3][3] = 1.;
    //J[4][0] = 0;
    J[4][1] = -c2;
    J[4][2] = -c2;
    //J[4][3] = 0;
    //J[5][0] = 0;
    J[5][1] = s2;
    J[5][2] = s2;
    //J[5][3] = 0;

    return J;
}

void PioneerCam::readJointState(const sensor_msgs::JointStateConstPtr &_msg)
{
    joint_ok_ = true;
    for(unsigned int j=0;j<joint_names_.size();++j)
    {
        for(unsigned int i=0;i<_msg->name.size();++i)
        {
            if(_msg->name[i] == joint_names_[j])
            {
                q_[j] = _msg->position[i];
                break;
            }
        }
    }
}

void PioneerCam::readImage(const sensor_msgs::ImageConstPtr& msg)
{

    cv::Mat im = cv_bridge::toCvShare(msg, "bgr8")->image;

    try
    {
        cv::Mat img;
        cv::cvtColor(im, img, cv::COLOR_BGR2HSV);

        // segment for green detection
        cv::inRange(img, cv::Scalar(55,0,0), cv::Scalar(65,255,255), img);

        vector<vector<cv::Point> > contours;
        vector<cv::Vec4i> hierarchy;
        cv::findContours( img, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);

        if(contours.size())
        {
            // get largest contour
            auto largest = std::max_element(
                           contours.begin(), contours.end(),
                           [](const vector<cv::Point> &c1, const vector<cv::Point> &c2)
                           {return cv::contourArea(c1) < cv::contourArea(c2);});
            int idx = std::distance(contours.begin(), largest);
            // show it in red
            cv::Moments m = cv::moments(contours[idx], false);
            cv::drawContours(im, contours, idx, cv::Scalar(0,0,255), 2);
        }
    }
    catch (...)
    {
        ROS_WARN("Could not detect sphere");
    }
    cv::imshow("view", im);
    cv::waitKey(1);
}

void PioneerCam::readTargetPose(const geometry_msgs::PoseConstPtr &msg)
{
    target_ok_ = true;
    target_pose_.x = msg->position.x;
    target_pose_.y = msg->position.y;
    target_pose_.theta = 2*atan2(msg->orientation.z, msg->orientation.w);
}

void PioneerCam::readSpherePose(const geometry_msgs::PoseConstPtr &msg)
{
    im_ok_ = true;
    s_im_[0] = msg->position.x/msg->position.z;
    s_im_[1] = msg->position.y/msg->position.z;
}
<|endoftext|>


<commit_before><commit_msg>Fix  moveBelowPositionRecursively and  moveAbovePositionRecursively<commit_after><|endoftext|>


<commit_before>//=======================================================================
// Copyright (c) 2013-2014 Baptiste Wicht.
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#ifndef UTILS_H
#define UTILS_H

#include <string>
#include <sstream>
#include <vector>
#include <algorithm>
#include <functional>
#include <cctype>
#include <locale>

namespace budget {

/*!
 * \brief Convert a string to a number of an arbitrary type.
 * \param text The string to convert.
 * \return The converted text in the good type.
 */
template <typename T>
inline T to_number (const std::string& text) {
    std::stringstream ss(text);
    T result;
    ss >> result;
    return result;
}

/*!
 * \brief Convert a string to a number of an int. This function is optimized for speed when converting int.
 * \param text The string to convert.
 * \return The text converted to an int.
 */
template<>
inline int to_number (const std::string& text) {
    return std::stoi(text);
}

template<>
inline unsigned short to_number (const std::string& text) {
    return static_cast<unsigned short>(std::stoi(text));
}

template<typename T>
inline std::string to_string(T value){
    return std::to_string(value);
}

template<>
inline std::string to_string(std::string value){
    return value;
}

template<>
inline std::string to_string(const char* value){
    return value;
}

void one_of(const std::string& value, const std::string& message, std::vector<std::string> values);

unsigned short terminal_width();
unsigned short terminal_height();

inline std::string& ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
    return s;
}

inline std::string& rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
    return s;
}

inline std::string &trim(std::string &s) {
    return ltrim(rtrim(s));
}

} //end of namespace budget

#endif
<commit_msg>Fix integer parsing<commit_after>//=======================================================================
// Copyright (c) 2013-2014 Baptiste Wicht.
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#ifndef UTILS_H
#define UTILS_H

#include <string>
#include <sstream>
#include <vector>
#include <algorithm>
#include <functional>
#include <cctype>
#include <locale>

namespace budget {

/*!
 * \brief Convert a string to a number of an arbitrary type.
 * \param text The string to convert.
 * \return The converted text in the good type.
 */
template <typename T>
inline T to_number (const std::string& text) {
    std::stringstream ss(text);
    T result;
    ss >> result;
    return result;
}

template<typename T>
inline std::string to_string(T value){
    return std::to_string(value);
}

template<>
inline std::string to_string(std::string value){
    return value;
}

template<>
inline std::string to_string(const char* value){
    return value;
}

void one_of(const std::string& value, const std::string& message, std::vector<std::string> values);

unsigned short terminal_width();
unsigned short terminal_height();

inline std::string& ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
    return s;
}

inline std::string& rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
    return s;
}

inline std::string &trim(std::string &s) {
    return ltrim(rtrim(s));
}

} //end of namespace budget

#endif
<|endoftext|>


<commit_before>//
//  Copyright Silvin Lubecki 2010
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#include "test.h"
#include "openclam/cl.hpp"
#include <memory>

BOOST_AUTO_TEST_CASE( simple_kernel_instanciation )
{
    const std::string expected =
        "__kernel void VectorAdd( __global const float* a, __global const float* b, __global float* c, unsigned int size ) "
        "{ "
        "const unsigned int iGID = get_global_id( 0 ); "
        "if( iGID >= size ) "
        "return; "
        "c[ iGID ] = a[ iGID ] + b[ iGID ]; "
        "}";
    const std::string actual = KERNEL( VectorAdd,
        __kernel void VectorAdd( __global const float* a, __global const float* b, __global float* c, unsigned int size )
        {
            const unsigned int iGID = get_global_id( 0 );
            if( iGID >= size )
                return;
            c[ iGID ] = a[ iGID ] + b[ iGID ];
        } );
    BOOST_CHECK_EQUAL( actual, expected );
}
<commit_msg>simplify kernel test<commit_after>//
//  Copyright Silvin Lubecki 2010
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#include "test.h"
#include "openclam/cl.hpp"
#include <memory>

BOOST_AUTO_TEST_CASE( simple_kernel_instanciation )
{
    const std::string expected =
        "__kernel void MyKernel( __global const float* a, global unsigned int size ) "
        "{ "
        "const unsigned int id = get_global_id( 0 ); "
        "a[ id ]; "
        "size; "
        "}";
    const std::string actual = KERNEL( MyKernel,
        __kernel void MyKernel( __global const float* a, global unsigned int size )
        {
            const unsigned int id = get_global_id( 0 );
            a[ id ];
            size;
        } );
    BOOST_CHECK_EQUAL( actual, expected );
}
<|endoftext|>