<commit_before>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: SteepestDescentMinimizer_test.C,v 1.5.30.1 2007/03/25 21:49:02 oliver Exp $
//

#include <BALL/CONCEPT/classTest.h>

///////////////////////////
#include <BALL/MOLMEC/MINIMIZATION/steepestDescent.h>
#include <BALL/MOLMEC/AMBER/amber.h>
#include <BALL/DATATYPE/options.h>
#include <BALL/KERNEL/PTE.h>
#include <BALL/FORMAT/HINFile.h>
#include <BALL/MATHS/analyticalGeometry.h>

///////////////////////////

START_TEST(ConjugateGradienMinimizer, "$Id: SteepestDescentMinimizer_test.C,v 1.5.30.1 2007/03/25 21:49:02 oliver Exp $")

using namespace BALL;

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

System S;
AmberFF FF(S);

SteepestDescentMinimizer*	em;
CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer())
	em = new SteepestDescentMinimizer;
	TEST_NOT_EQUAL(em, 0)
RESULT

CHECK(SteepestDescentMinimizer::~SteepestDescentMinimizer())
	delete em;
RESULT

CHECK(SteepestDescentMinimizer::getForceField())
	SteepestDescentMinimizer em;
	TEST_EQUAL(em.getForceField(), 0)
RESULT

CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer(ForceField&))
	TEST_EQUAL(FF.isValid(), true)
	SteepestDescentMinimizer em(FF);
	TEST_EQUAL(em.getForceField(), &FF)
RESULT

CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer(ForceField&, const Options&))
	Options options;
	options.set("ABC", "DEF");
	em = new SteepestDescentMinimizer(FF, options);
	TEST_EQUAL(em->getForceField(), &FF)
	TEST_EQUAL(em->options["ABC"], "DEF")
	delete em;
RESULT

CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer(const SteepestDescentMinimizer&, bool))
	SteepestDescentMinimizer em1;
	SteepestDescentMinimizer em2(em1);
	bool test = (em1 == em2);
	TEST_EQUAL(test, true)
	em1.setup(FF);
	SteepestDescentMinimizer em3(em1);
	test = (em1 == em3);
	TEST_EQUAL(test, true)
	em1.setup(FF);
RESULT

CHECK(SteepestDescentMinimizer::operator = (const SteepestDescentMinimizer&))
	SteepestDescentMinimizer em1;
	SteepestDescentMinimizer em2 = em1;
	bool test = (em1 == em2);
	TEST_EQUAL(test, true)
	em1.setup(FF);
	SteepestDescentMinimizer em3 = em1;
	test = (em1 == em3);
	TEST_EQUAL(test, true)
RESULT

CHECK(SteepestDescentMinimizer::isValid() const)
	em = new SteepestDescentMinimizer;
	TEST_EQUAL(em->isValid(), false)
	delete em;
	em = new SteepestDescentMinimizer(FF);
	TEST_EQUAL(em->isValid(), true)
	delete em;
RESULT

CHECK(SteepestDescentMinimizer::setup(ForceField&))
	SteepestDescentMinimizer e_min(FF);
	TEST_EQUAL(e_min.getForceUpdateCounter(), 0)
	TEST_EQUAL(e_min.getEnergyUpdateCounter(), 0)
	bool test = (e_min.getForceField() == &FF);
	TEST_EQUAL(test, true)
RESULT

CHECK(SteepestDescentMinimizer::setup(ForceField&, const Options&))
	Options options;
	options.setInteger(EnergyMinimizer::Option::ENERGY_OUTPUT_FREQUENCY, 3456);
	SteepestDescentMinimizer e_min(FF, options);
	TEST_EQUAL(e_min.getForceUpdateCounter(), 0)
	TEST_EQUAL(e_min.getEnergyUpdateCounter(), 0)
	bool test = (e_min.getForceField() == &FF);
	TEST_EQUAL(test, true)
	TEST_EQUAL(e_min.getEnergyOutputFrequency(), 3456)
RESULT

CHECK(SteepestDescentMinimizer::specificSetup())
	SteepestDescentMinimizer em(FF);
	TEST_EQUAL(em.specificSetup(), true)
	// specificSetup() shouldn't do anything except returning true
RESULT

CHECK(SteepestDescentMinimizer::getNumberOfIteration() const)
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getNumberOfIterations(), 0)
RESULT

CHECK(SteepestDescentMinimizer::setNumberOfIteration(Size))
	SteepestDescentMinimizer e_min;
	e_min.setNumberOfIterations(4);
	TEST_EQUAL(e_min.getNumberOfIterations(), 4)
RESULT

CHECK(SteepestDescentMinimizer::getMaxNumberOfIterations())
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getMaxNumberOfIterations(), 0)
RESULT

CHECK(SteepestDescentMinimizer::setMaxNumberOfIterations(Size))
	SteepestDescentMinimizer e_min;
	e_min.setMaxNumberOfIterations(2000);
	TEST_EQUAL(e_min.getMaxNumberOfIterations(), 2000)
RESULT

CHECK(SteepestDescentMinimizer::getEnergyOutputFrequency() const)
	SteepestDescentMinimizer e_min2;
	TEST_EQUAL(e_min2.getEnergyOutputFrequency(), 0)
RESULT

CHECK(SteepestDescentMinimizer::setEnergyOutputFrequency(Size))
	SteepestDescentMinimizer e_min;
	e_min.setEnergyOutputFrequency(8);
	TEST_EQUAL(e_min.getEnergyOutputFrequency(), 8)
RESULT

CHECK(SteepestDescentMinimizer::getEnergyDifferenceBound() const)
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getEnergyDifferenceBound(), 0.0)
RESULT

CHECK(SteepestDescentMinimizer::setEnergyDifferenceBound(float))
	SteepestDescentMinimizer e_min;
	e_min.setEnergyDifferenceBound(9.0);
	TEST_EQUAL(e_min.getEnergyDifferenceBound(), 9.0)
RESULT

CHECK(SteepestDescentMinimizer::getMaximumDisplacement() const)
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getMaximumDisplacement(), 0.0)
RESULT

CHECK(SteepestDescentMinimizer::setMaximumDisplacement(float))
	SteepestDescentMinimizer e_min;
	e_min.setMaximumDisplacement(56.0);
	TEST_EQUAL(e_min.getMaximumDisplacement(), 56.0)
RESULT

CHECK(SteepestDescentMinimizer::minimize(Size, bool))
	System S;
	Molecule* m = new Molecule;
	Atom* a1 = new Atom;
	Atom* a2 = new Atom;
	S.insert(*m);
	m->insert(*a1);
	m->insert(*a2);
	a1->setPosition(Vector3(-0.5, 0, 0));
	a2->setPosition(Vector3(0.5, 0, 0));
	a1->setElement(PTE[Element::C]);
	a2->setElement(PTE[Element::C]);
	a1->setTypeName("C");
	a2->setTypeName("C");
	a1->setCharge(0.0);
	a2->setCharge(0.0);
	FF.options[AmberFF::Option::ASSIGN_CHARGES] = "false";
	FF.setup(S);
	TEST_EQUAL(FF.isValid(), true)
	PRECISION(1e-4)
	FF.updateEnergy();
	FF.updateForces();
	PRECISION(10)
	TEST_REAL_EQUAL(FF.getEnergy(), 3.42854e+06)
	SteepestDescentMinimizer cgm(FF);
	cgm.setEnergyOutputFrequency(1);
	cgm.setMaxGradient(0.01);
	TEST_EQUAL(cgm.isValid(), true)
	bool converged = cgm.minimize(20);
	TEST_EQUAL(converged, true)

	FF.updateEnergy();
	FF.updateForces();

	PRECISION(1e-3)
  TEST_REAL_EQUAL(FF.getEnergy(), -0.359813)
	PRECISION(7e-3)
  TEST_REAL_EQUAL(a1->getPosition().getDistance(a2->getPosition()), 3.81244)
RESULT

CHECK(SteepestDescentMinimizer::minimize(Size, bool) ethan)
  System S;
  HINFile f("data/ethan.hin");
  f >> S;
  FF.options[AmberFF::Option::ASSIGN_CHARGES] = "false";
  FF.setup(S);

  TEST_EQUAL(FF.isValid(), true)
  FF.updateEnergy();
  FF.updateForces();
  PRECISION(1E-4)
  TEST_REAL_EQUAL(FF.getEnergy(), 18.5605)

  SteepestDescentMinimizer sd(FF);

  sd.setEnergyOutputFrequency(5);
  sd.setMaxGradient(0.418);
	sd.setMaxNumberOfIterations(10000);
  sd.setEnergyDifferenceBound(0.00000001);

  TEST_EQUAL(sd.isValid(), true)
  FF.updateEnergy();
  FF.updateForces();
  bool result = sd.minimize(500);

  TEST_EQUAL(result, true)
  float energy = FF.updateEnergy();
  FF.updateForces();

  PRECISION(6E-2)
  TEST_REAL_EQUAL(energy, 5.906)

  AtomIterator atit;
  Vector3 pos[8];
  int i=0;

  for (atit = S.beginAtom(); +atit; ++atit)
  {
    pos[i] = atit->getPosition();
    ++i;
	}

  PRECISION(5E-2)
  Angle torsion;
  torsion = getTorsionAngle(pos[2].x, pos[2].y, pos[2].z, pos[0].x, pos[0].y, pos[0].z, pos[1].x, pos[1].y, pos[1].z, pos[6].x, pos[6].y, pos[6].z);
  TEST_REAL_EQUAL(torsion.toRadian(), 1.047)
  torsion = getTorsionAngle(pos[3].x, pos[3].y, pos[3].z, pos[0].x, pos[0].y, pos[0].z, pos[1].x, pos[1].y, pos[1].z, pos[5].x, pos[5].y, pos[5].z);
  TEST_REAL_EQUAL(torsion.toRadian(), 1.047)
  torsion = getTorsionAngle(pos[4].x, pos[4].y, pos[4].z, pos[0].x, pos[0].y, pos[0].z, pos[1].x, pos[1].y, pos[1].z, pos[7].x, pos[7].y, pos[7].z);
  TEST_REAL_EQUAL(torsion.toRadian(), 1.047)

  PRECISION(1E-2)
  Angle tet;
  tet = (pos[2] - pos[0]).getAngle(pos[3] - pos[0]);
  TEST_REAL_EQUAL(tet.toRadian(), 1.902)
  tet = (pos[2] - pos[0]).getAngle(pos[4] - pos[0]);
  TEST_REAL_EQUAL(tet.toRadian(), 1.902)
  tet = (pos[4] - pos[0]).getAngle(pos[3] - pos[0]);
  TEST_REAL_EQUAL(tet.toRadian(), 1.902)
  tet = (pos[5] - pos[1]).getAngle(pos[6] - pos[1]);
  TEST_REAL_EQUAL(tet.toRadian(), 1.902)
  tet = (pos[5] - pos[1]).getAngle(pos[7] - pos[1]);
  TEST_REAL_EQUAL(tet.toRadian(), 1.902)
  tet = (pos[7] - pos[1]).getAngle(pos[6] - pos[1]);
  TEST_REAL_EQUAL(tet.toRadian(), 1.902)

RESULT

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

END_TEST
<commit_msg>New steepest descent test<commit_after>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: SteepestDescentMinimizer_test.C,v 1.5.30.2 2007/05/07 11:50:49 aleru Exp $
//

#include <BALL/CONCEPT/classTest.h>

///////////////////////////
#include <BALL/MOLMEC/MINIMIZATION/steepestDescent.h>
#include <BALL/MOLMEC/AMBER/amber.h>
#include <BALL/DATATYPE/options.h>
#include <BALL/KERNEL/PTE.h>
#include <BALL/FORMAT/HINFile.h>
#include <BALL/MATHS/analyticalGeometry.h>

///////////////////////////

START_TEST(ConjugateGradienMinimizer, "$Id: SteepestDescentMinimizer_test.C,v 1.5.30.2 2007/05/07 11:50:49 aleru Exp $")

using namespace BALL;

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

System S;
AmberFF FF(S);

SteepestDescentMinimizer*	em;
CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer())
	em = new SteepestDescentMinimizer;
	TEST_NOT_EQUAL(em, 0)
RESULT

CHECK(SteepestDescentMinimizer::~SteepestDescentMinimizer())
	delete em;
RESULT

CHECK(SteepestDescentMinimizer::getForceField())
	SteepestDescentMinimizer em;
	TEST_EQUAL(em.getForceField(), 0)
RESULT

CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer(ForceField&))
	TEST_EQUAL(FF.isValid(), true)
	SteepestDescentMinimizer em(FF);
	TEST_EQUAL(em.getForceField(), &FF)
RESULT

CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer(ForceField&, const Options&))
	Options options;
	options.set("ABC", "DEF");
	em = new SteepestDescentMinimizer(FF, options);
	TEST_EQUAL(em->getForceField(), &FF)
	TEST_EQUAL(em->options["ABC"], "DEF")
	delete em;
RESULT

CHECK(SteepestDescentMinimizer::SteepestDescentMinimizer(const SteepestDescentMinimizer&, bool))
	SteepestDescentMinimizer em1;
	SteepestDescentMinimizer em2(em1);
	bool test = (em1 == em2);
	TEST_EQUAL(test, true)
	em1.setup(FF);
	SteepestDescentMinimizer em3(em1);
	test = (em1 == em3);
	TEST_EQUAL(test, true)
	em1.setup(FF);
RESULT

CHECK(SteepestDescentMinimizer::operator = (const SteepestDescentMinimizer&))
	SteepestDescentMinimizer em1;
	SteepestDescentMinimizer em2 = em1;
	bool test = (em1 == em2);
	TEST_EQUAL(test, true)
	em1.setup(FF);
	SteepestDescentMinimizer em3 = em1;
	test = (em1 == em3);
	TEST_EQUAL(test, true)
RESULT

CHECK(SteepestDescentMinimizer::isValid() const)
	em = new SteepestDescentMinimizer;
	TEST_EQUAL(em->isValid(), false)
	delete em;
	em = new SteepestDescentMinimizer(FF);
	TEST_EQUAL(em->isValid(), true)
	delete em;
RESULT

CHECK(SteepestDescentMinimizer::setup(ForceField&))
	SteepestDescentMinimizer e_min(FF);
	TEST_EQUAL(e_min.getForceUpdateCounter(), 0)
	TEST_EQUAL(e_min.getEnergyUpdateCounter(), 0)
	bool test = (e_min.getForceField() == &FF);
	TEST_EQUAL(test, true)
RESULT

CHECK(SteepestDescentMinimizer::setup(ForceField&, const Options&))
	Options options;
	options.setInteger(EnergyMinimizer::Option::ENERGY_OUTPUT_FREQUENCY, 3456);
	SteepestDescentMinimizer e_min(FF, options);
	TEST_EQUAL(e_min.getForceUpdateCounter(), 0)
	TEST_EQUAL(e_min.getEnergyUpdateCounter(), 0)
	bool test = (e_min.getForceField() == &FF);
	TEST_EQUAL(test, true)
	TEST_EQUAL(e_min.getEnergyOutputFrequency(), 3456)
RESULT

CHECK(SteepestDescentMinimizer::specificSetup())
	SteepestDescentMinimizer em(FF);
	TEST_EQUAL(em.specificSetup(), true)
	// specificSetup() shouldn't do anything except returning true
RESULT

CHECK(SteepestDescentMinimizer::getNumberOfIteration() const)
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getNumberOfIterations(), 0)
RESULT

CHECK(SteepestDescentMinimizer::setNumberOfIteration(Size))
	SteepestDescentMinimizer e_min;
	e_min.setNumberOfIterations(4);
	TEST_EQUAL(e_min.getNumberOfIterations(), 4)
RESULT

CHECK(SteepestDescentMinimizer::getMaxNumberOfIterations())
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getMaxNumberOfIterations(), 0)
RESULT

CHECK(SteepestDescentMinimizer::setMaxNumberOfIterations(Size))
	SteepestDescentMinimizer e_min;
	e_min.setMaxNumberOfIterations(2000);
	TEST_EQUAL(e_min.getMaxNumberOfIterations(), 2000)
RESULT

CHECK(SteepestDescentMinimizer::getEnergyOutputFrequency() const)
	SteepestDescentMinimizer e_min2;
	TEST_EQUAL(e_min2.getEnergyOutputFrequency(), 0)
RESULT

CHECK(SteepestDescentMinimizer::setEnergyOutputFrequency(Size))
	SteepestDescentMinimizer e_min;
	e_min.setEnergyOutputFrequency(8);
	TEST_EQUAL(e_min.getEnergyOutputFrequency(), 8)
RESULT

CHECK(SteepestDescentMinimizer::getEnergyDifferenceBound() const)
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getEnergyDifferenceBound(), 0.0)
RESULT

CHECK(SteepestDescentMinimizer::setEnergyDifferenceBound(float))
	SteepestDescentMinimizer e_min;
	e_min.setEnergyDifferenceBound(9.0);
	TEST_EQUAL(e_min.getEnergyDifferenceBound(), 9.0)
RESULT

CHECK(SteepestDescentMinimizer::getMaximumDisplacement() const)
	SteepestDescentMinimizer e_min;
	TEST_EQUAL(e_min.getMaximumDisplacement(), 0.0)
RESULT

CHECK(SteepestDescentMinimizer::setMaximumDisplacement(float))
	SteepestDescentMinimizer e_min;
	e_min.setMaximumDisplacement(56.0);
	TEST_EQUAL(e_min.getMaximumDisplacement(), 56.0)
RESULT

CHECK(SteepestDescentMinimizer::minimize(Size, bool))
	System S;
	Molecule* m = new Molecule;
	Atom* a1 = new Atom;
	Atom* a2 = new Atom;
	S.insert(*m);
	m->insert(*a1);
	m->insert(*a2);
	a1->setPosition(Vector3(-0.5, 0, 0));
	a2->setPosition(Vector3(0.5, 0, 0));
	a1->setElement(PTE[Element::C]);
	a2->setElement(PTE[Element::C]);
	a1->setTypeName("C");
	a2->setTypeName("C");
	a1->setCharge(0.0);
	a2->setCharge(0.0);
	FF.options[AmberFF::Option::ASSIGN_CHARGES] = "false";
	FF.setup(S);
	TEST_EQUAL(FF.isValid(), true)
	PRECISION(1e-4)
	FF.updateEnergy();
	FF.updateForces();
	PRECISION(10)
	TEST_REAL_EQUAL(FF.getEnergy(), 3.42854e+06)
	SteepestDescentMinimizer cgm(FF);
	cgm.setEnergyOutputFrequency(1);
	cgm.setMaxGradient(0.01);
	TEST_EQUAL(cgm.isValid(), true)
	bool converged = cgm.minimize(20);
	TEST_EQUAL(converged, true)

	FF.updateEnergy();
	FF.updateForces();

	PRECISION(1e-3)
	TEST_REAL_EQUAL(FF.getEnergy(), -0.359813)
	PRECISION(7e-3)
	TEST_REAL_EQUAL(a1->getPosition().getDistance(a2->getPosition()), 3.81244)
RESULT

CHECK(SteepestDescentMinimizer::minimize(Size, bool) ethan)
	System S;
	HINFile f("data/ethan.hin");
	f >> S;
	FF.options[AmberFF::Option::ASSIGN_CHARGES] = "false";
	FF.setup(S);

	TEST_EQUAL(FF.isValid(), true)
	FF.updateEnergy();
	FF.updateForces();
	PRECISION(1E-4)
	TEST_REAL_EQUAL(FF.getEnergy(), 18.5605)

	SteepestDescentMinimizer sd(FF);
	sd.setEnergyOutputFrequency(5);
	sd.setMaxGradient(0.418);
	sd.setMaxNumberOfIterations(10000);
	sd.setEnergyDifferenceBound(0.00000001);

	TEST_EQUAL(sd.isValid(), true)
	FF.updateEnergy();
	FF.updateForces();
	bool result = sd.minimize(500);

	TEST_EQUAL(result, true)
	float energy = FF.updateEnergy();
	FF.updateForces();

	PRECISION(6E-2)
	TEST_REAL_EQUAL(energy, 5.906)

	AtomIterator atit;
	Vector3 pos[8];
	int i=0;

	for (atit = S.beginAtom(); +atit; ++atit)
	{
		pos[i] = atit->getPosition();
		++i;
	}

	PRECISION(5E-2)
	Angle torsion;
	torsion = getTorsionAngle(pos[2].x, pos[2].y, pos[2].z, pos[0].x, pos[0].y, pos[0].z, pos[1].x, pos[1].y, pos[1].z, pos[6].x, pos[6].y, pos[6].z);
	TEST_REAL_EQUAL(torsion.toRadian(), 1.047)
	torsion = getTorsionAngle(pos[3].x, pos[3].y, pos[3].z, pos[0].x, pos[0].y, pos[0].z, pos[1].x, pos[1].y, pos[1].z, pos[5].x, pos[5].y, pos[5].z);
	TEST_REAL_EQUAL(torsion.toRadian(), 1.047)
	torsion = getTorsionAngle(pos[4].x, pos[4].y, pos[4].z, pos[0].x, pos[0].y, pos[0].z, pos[1].x, pos[1].y, pos[1].z, pos[7].x, pos[7].y, pos[7].z);
	TEST_REAL_EQUAL(torsion.toRadian(), 1.047)

	PRECISION(1E-2)
	Angle tet;
	tet = (pos[2] - pos[0]).getAngle(pos[3] - pos[0]);
	TEST_REAL_EQUAL(tet.toRadian(), 1.902)
	tet = (pos[2] - pos[0]).getAngle(pos[4] - pos[0]);
	TEST_REAL_EQUAL(tet.toRadian(), 1.902)
	tet = (pos[4] - pos[0]).getAngle(pos[3] - pos[0]);
	TEST_REAL_EQUAL(tet.toRadian(), 1.902)
	tet = (pos[5] - pos[1]).getAngle(pos[6] - pos[1]);
	TEST_REAL_EQUAL(tet.toRadian(), 1.902)
	tet = (pos[5] - pos[1]).getAngle(pos[7] - pos[1]);
	TEST_REAL_EQUAL(tet.toRadian(), 1.902)
	tet = (pos[7] - pos[1]).getAngle(pos[6] - pos[1]);
	TEST_REAL_EQUAL(tet.toRadian(), 1.902)

RESULT

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

END_TEST
<|endoftext|>


<commit_before>
#include <aidu_mobile_base/base.h>
#include <aidu_mobile_base/Pos.h>
#include <geometry_msgs/Twist.h>
#include <aidu_mobile_base/State.h>
#include <threemxl/dxlassert.h>
#include <math.h>
#include <algorithm>

#define BOUND(x,y) std::max(std::min(x, y), -y)

using namespace aidu;

mobile_base::Base::Base() : core::Node::Node() {

    // Read parameters from config file
    std::string motor_port_name, motor_config_name;
    nh->getParam("motor_port", motor_port_name);
    nh->getParam("motor_config", motor_config_name);

    //creating left and right motor
    leftWheelMotor = new mobile_base::Motor("left", motor_port_name, motor_config_name);
    rightWheelMotor = new mobile_base::Motor("right", motor_port_name, motor_config_name);
    initialLeftPos = 0.0;
    initialRightPos = 0.0;

    // Subscribing
    positionSubscriber = nh->subscribe("pos", 1, &mobile_base::Base::position, this);
    speedSubscriber = nh->subscribe("speed", 1, &mobile_base::Base::speed, this);
    positionSpeedSubscriber = nh->subscribe("posspeed", 1, &mobile_base::Base::positionSpeed, this);
    configSubscriber = nh->subscribe("config", 1, &mobile_base::Base::setConfig, this);
    leftWheelSubscriber = nh->subscribe("/lwheel_vtarget", 1, &mobile_base::Base::leftWheelSpeed, this);
    rightWheelSubscriber = nh->subscribe("/rwheel_vtarget", 1, &mobile_base::Base::rightWheelSpeed, this);

    // Publishing
    speedPublisher = nh->advertise<geometry_msgs::Twist>("speed", 1);
    statePublisher = nh->advertise<aidu_mobile_base::State>("state",1);
    maximumLinearVelocity = 1;
    maximumAngularVelocity = 2;

    // Odometry information
    currentX = 0.0;
    currentY = 0.0;
    currentTheta = 0.0;

    // Get sizes from launch parameters
    nh->getParam("wheel_base", radiusBase);
    radiusBase *= 0.5;
    nh->getParam("wheel_diameter", radiusWheel);
    radiusWheel *= 0.5;

}

void mobile_base::Base::leftWheelSpeed(const std_msgs::Float32::ConstPtr& msg) {
    leftWheelMotor->setVelocity(msg->data / radiusWheel);
}

void mobile_base::Base::rightWheelSpeed(const std_msgs::Float32::ConstPtr& msg) {
    rightWheelMotor->setVelocity(msg->data / radiusWheel);
}

void mobile_base::Base::position(const geometry_msgs::Twist::ConstPtr& msg) {

    // Reading position from topic
    float position=msg->linear.x;
    float angle=msg->angular.z;

    // Compute position-based wheel position
    position = position / radiusWheel;

    // Compute angle-based wheel position
    float wheelDifference = angle * (radiusBase / radiusWheel);

    // Sending position to 3Mxl
    leftWheelMotor->setPosition(position + wheelDifference); // setting left wheel postion
    rightWheelMotor->setPosition(position - wheelDifference); //setting right wheel position
}

void mobile_base::Base::speed(const geometry_msgs::Twist::ConstPtr& msg){

    // Reading velocity from topic
    float velocity = BOUND(msg->linear.x, maximumLinearVelocity);
    float angle = BOUND(msg->angular.z, maximumAngularVelocity);

    // Calculating velocity for each motor
    float linearLeftSpeed, linearRightSpeed, angularLeftSpeed, angularRightSpeed;
    linearLeftSpeed = linearRightSpeed = velocity / radiusWheel;
    angularLeftSpeed = angularRightSpeed = angle * (radiusBase / radiusWheel);

    // Sending speeds to 3mxl
    leftWheelMotor->setVelocity(linearLeftSpeed + angularLeftSpeed); // setting left wheel postion
    rightWheelMotor->setVelocity(linearRightSpeed - angularRightSpeed);//setting right wheel position

}

void mobile_base::Base::positionSpeed(const geometry_msgs::Twist::ConstPtr& msg) {

    // Base is nonholonomic, warn if sent a command we can't execute
    if (msg->linear.y || msg->linear.z || msg->angular.x || msg->angular.y) {
        ROS_WARN("Cannot use given position: linear:[%f, %f, %f], angular:[%f, %f, %f]", msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);
        return;
    }

    // Resets the positions so we count from 0
    this->resetPos();

    // Get positions, targets and errors as polar coordinates from base origin
    double actualPos = (getLeftPos() + getRightPos()) / 2.0;
    double actualAngle = getAngle();
    double targetPos = msg->linear.x;
    double targetAngle = msg->angular.z;
    double errorPos = targetPos - actualPos;
    double errorAngle = targetAngle - actualAngle;
    double prevErrorAngle = errorAngle;

    // Initialize values for our custom PD controller
    double epsilon = 0.01;
    double KpL = 1.0;
    double KpA = 1.8;
    double KdA = 40.0;
    double maxAngularSpeed = 1.57;
    double maxLinearSpeed = 0.3;

    // The message that will be sent
    geometry_msgs::Twist twist;
    aidu_mobile_base::State state;

    // PD controller loop. This will exit when the error is smaller than epsilon
    ros::Rate loop(10);
    while(ros::ok() && (fabs(errorPos) > epsilon || fabs(errorAngle) > epsilon)) {

        // Get current positions
        actualPos = (getLeftPos() + getRightPos()) / 2.0;
        actualAngle = getAngle();

        // Calculate errors
        errorPos = targetPos - actualPos;
        errorAngle = targetAngle - actualAngle;

        // Compute derivative of angle error
        double derivative = (errorAngle - prevErrorAngle) / 10;
        prevErrorAngle = errorAngle;

        // Set twist message values, bounded by maximum speeds
        twist.linear.x = std::max(-maxLinearSpeed, std::min(maxLinearSpeed, errorPos * KpL));
        twist.angular.z = std::max(-maxAngularSpeed, std::min(maxAngularSpeed, errorAngle * KpA + derivative * KdA ));

        // Getting position, speed and angle
        leftWheelMotor->motor->getPosAndSpeed();
        state.leftpos = getLeftPos();
        state.leftspeed = leftWheelMotor->motor->presentSpeed();
        rightWheelMotor->motor->getPosAndSpeed();
        state.rightpos = getRightPos();
        state.rightspeed = rightWheelMotor->motor->presentSpeed();
        state.angle = getAngle();

        // Publish twist and state messages
        speedPublisher.publish(twist);
        statePublisher.publish(state);

        // Loop
        ros::spinOnce();
        loop.sleep();

    }

    // Ensure 0 speed at the end
    twist.linear.x = 0;
    twist.angular.z = 0;
    speedPublisher.publish(twist);
    ros::spinOnce();

}

void mobile_base::Base::setConfig(const aidu_mobile_base::Config::ConstPtr& msg) {
    this->maximumLinearVelocity = msg->maximumLinearSpeed;
    this->maximumAngularVelocity = msg->maximumAngularSpeed;
    if (this->getLinearVelocity() > maximumLinearVelocity || this->getAngularVelocity() > maximumAngularVelocity) {
        geometry_msgs::Twist twist;
        twist.linear.x = BOUND(getLinearVelocity(), maximumLinearVelocity);
        twist.angular.z = BOUND(getAngularVelocity(), maximumAngularVelocity);
        speedPublisher.publish(twist);
    }
}

double mobile_base::Base::getAngle() {
    return ((getLeftPos() / radiusBase)-(getRightPos() / radiusBase)) / 2.0;
}

double mobile_base::Base::getLeftPos() {
    leftWheelMotor->motor->getLinearPos();
    return leftWheelMotor->motor->presentLinearPos() - initialLeftPos;
}

double mobile_base::Base::getRightPos() {
    rightWheelMotor->motor->getLinearPos();
    return rightWheelMotor->motor->presentLinearPos() - initialRightPos;
}

double mobile_base::Base::getLinearVelocity() {
    return ((leftWheelMotor->currentVelocity + rightWheelMotor->currentVelocity) / 2) * radiusWheel;
}

double mobile_base::Base::getAngularVelocity() {
    return ((leftWheelMotor->currentVelocity - rightWheelMotor->currentVelocity) / 2) * (radiusWheel / radiusBase);
}

void mobile_base::Base::resetPos() {
    leftWheelMotor->motor->getLinearPos();
    rightWheelMotor->motor->getLinearPos();
    initialLeftPos = leftWheelMotor->motor->presentLinearPos();
    initialRightPos = rightWheelMotor->motor->presentLinearPos();
}

void mobile_base::Base::publishState() {

    // Initialize
    aidu_mobile_base::State state;
    int countdown = 15;

    // Getting position, speed and angle
    leftWheelMotor->motor->getPosAndSpeed();
    state.leftpos = getLeftPos();
    state.leftspeed = leftWheelMotor->motor->presentSpeed();
    rightWheelMotor->motor->getPosAndSpeed();
    state.rightpos = getRightPos();
    state.rightspeed = rightWheelMotor->motor->presentSpeed();
    state.angle = getAngle();

    // Only publish state when the motors have been recently active
    if(state.rightspeed == 0 && state.leftspeed == 0) {
        if(countdown > 0) {
        countdown--;
        }
    } else {
        countdown = 15;
    }
    if(countdown > 0) {
        statePublisher.publish(state);
    }

}

void mobile_base::Base::spin(){

    ros::Rate rate(30); // rate at which position published (hertz)
    resetPos(); // Reset the position initially

    while(ros::ok()) {

        // Publish information
        publishState();

        // Spin ros
        ros::spinOnce();
        rate.sleep();

    }
}

mobile_base::Base::~Base() {
    delete leftWheelMotor;
    delete rightWheelMotor;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "base");
    mobile_base::Base base;
    base.spin();
    return 0;
}
<commit_msg>Fixed inverted angle in base calculations<commit_after>
#include <aidu_mobile_base/base.h>
#include <aidu_mobile_base/Pos.h>
#include <geometry_msgs/Twist.h>
#include <aidu_mobile_base/State.h>
#include <threemxl/dxlassert.h>
#include <math.h>
#include <algorithm>

#define BOUND(x,y) std::max(std::min(x, y), -y)

using namespace aidu;

mobile_base::Base::Base() : core::Node::Node() {

    // Read parameters from config file
    std::string motor_port_name, motor_config_name;
    nh->getParam("motor_port", motor_port_name);
    nh->getParam("motor_config", motor_config_name);

    //creating left and right motor
    leftWheelMotor = new mobile_base::Motor("left", motor_port_name, motor_config_name);
    rightWheelMotor = new mobile_base::Motor("right", motor_port_name, motor_config_name);
    initialLeftPos = 0.0;
    initialRightPos = 0.0;

    // Subscribing
    positionSubscriber = nh->subscribe("pos", 1, &mobile_base::Base::position, this);
    speedSubscriber = nh->subscribe("speed", 1, &mobile_base::Base::speed, this);
    positionSpeedSubscriber = nh->subscribe("posspeed", 1, &mobile_base::Base::positionSpeed, this);
    configSubscriber = nh->subscribe("config", 1, &mobile_base::Base::setConfig, this);
    leftWheelSubscriber = nh->subscribe("/lwheel_vtarget", 1, &mobile_base::Base::leftWheelSpeed, this);
    rightWheelSubscriber = nh->subscribe("/rwheel_vtarget", 1, &mobile_base::Base::rightWheelSpeed, this);

    // Publishing
    speedPublisher = nh->advertise<geometry_msgs::Twist>("speed", 1);
    statePublisher = nh->advertise<aidu_mobile_base::State>("state",1);
    maximumLinearVelocity = 1;
    maximumAngularVelocity = 2;

    // Odometry information
    currentX = 0.0;
    currentY = 0.0;
    currentTheta = 0.0;

    // Get sizes from launch parameters
    nh->getParam("wheel_base", radiusBase);
    radiusBase *= 0.5;
    nh->getParam("wheel_diameter", radiusWheel);
    radiusWheel *= 0.5;

}

void mobile_base::Base::leftWheelSpeed(const std_msgs::Float32::ConstPtr& msg) {
    leftWheelMotor->setVelocity(msg->data / radiusWheel);
}

void mobile_base::Base::rightWheelSpeed(const std_msgs::Float32::ConstPtr& msg) {
    rightWheelMotor->setVelocity(msg->data / radiusWheel);
}

void mobile_base::Base::position(const geometry_msgs::Twist::ConstPtr& msg) {

    // Reading position from topic
    float position=msg->linear.x;
    float angle=msg->angular.z;

    // Compute position-based wheel position
    position = position / radiusWheel;

    // Compute angle-based wheel position
    float wheelDifference = angle * (radiusBase / radiusWheel);

    // Sending position to 3Mxl
    leftWheelMotor->setPosition(position + wheelDifference); // setting left wheel postion
    rightWheelMotor->setPosition(position - wheelDifference); //setting right wheel position
}

void mobile_base::Base::speed(const geometry_msgs::Twist::ConstPtr& msg){

    // Reading velocity from topic
    float velocity = BOUND(msg->linear.x, maximumLinearVelocity);
    float angle = BOUND(msg->angular.z, maximumAngularVelocity);

    // Calculating velocity for each motor
    float linearLeftSpeed, linearRightSpeed, angularLeftSpeed, angularRightSpeed;
    linearLeftSpeed = linearRightSpeed = velocity / radiusWheel;
    angularLeftSpeed = angularRightSpeed = angle * (radiusBase / radiusWheel);

    // Sending speeds to 3mxl
    leftWheelMotor->setVelocity(linearLeftSpeed + angularLeftSpeed); // setting left wheel postion
    rightWheelMotor->setVelocity(linearRightSpeed - angularRightSpeed);//setting right wheel position

}

void mobile_base::Base::positionSpeed(const geometry_msgs::Twist::ConstPtr& msg) {

    // Base is nonholonomic, warn if sent a command we can't execute
    if (msg->linear.y || msg->linear.z || msg->angular.x || msg->angular.y) {
        ROS_WARN("Cannot use given position: linear:[%f, %f, %f], angular:[%f, %f, %f]", msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);
        return;
    }

    // Resets the positions so we count from 0
    this->resetPos();

    // Get positions, targets and errors as polar coordinates from base origin
    double actualPos = (getLeftPos() + getRightPos()) / 2.0;
    double actualAngle = getAngle();
    double targetPos = msg->linear.x;
    double targetAngle = msg->angular.z;
    double errorPos = targetPos - actualPos;
    double errorAngle = targetAngle - actualAngle;
    double prevErrorAngle = errorAngle;

    // Initialize values for our custom PD controller
    double epsilon = 0.01;
    double KpL = 1.0;
    double KpA = 1.8;
    double KdA = 40.0;
    double maxAngularSpeed = 1.57;
    double maxLinearSpeed = 0.3;

    // The message that will be sent
    geometry_msgs::Twist twist;
    aidu_mobile_base::State state;

    // PD controller loop. This will exit when the error is smaller than epsilon
    ros::Rate loop(10);
    while(ros::ok() && (fabs(errorPos) > epsilon || fabs(errorAngle) > epsilon)) {

        // Get current positions
        actualPos = (getLeftPos() + getRightPos()) / 2.0;
        actualAngle = getAngle();

        // Calculate errors
        errorPos = targetPos - actualPos;
        errorAngle = targetAngle - actualAngle;

        // Compute derivative of angle error
        double derivative = (errorAngle - prevErrorAngle) / 10;
        prevErrorAngle = errorAngle;

        // Set twist message values, bounded by maximum speeds
        twist.linear.x = std::max(-maxLinearSpeed, std::min(maxLinearSpeed, errorPos * KpL));
        twist.angular.z = std::max(-maxAngularSpeed, std::min(maxAngularSpeed, errorAngle * KpA + derivative * KdA ));

        // Getting position, speed and angle
        leftWheelMotor->motor->getPosAndSpeed();
        state.leftpos = getLeftPos();
        state.leftspeed = leftWheelMotor->motor->presentSpeed();
        rightWheelMotor->motor->getPosAndSpeed();
        state.rightpos = getRightPos();
        state.rightspeed = rightWheelMotor->motor->presentSpeed();
        state.angle = getAngle();

        // Publish twist and state messages
        speedPublisher.publish(twist);
        statePublisher.publish(state);

        // Loop
        ros::spinOnce();
        loop.sleep();

    }

    // Ensure 0 speed at the end
    twist.linear.x = 0;
    twist.angular.z = 0;
    speedPublisher.publish(twist);
    ros::spinOnce();

}

void mobile_base::Base::setConfig(const aidu_mobile_base::Config::ConstPtr& msg) {
    this->maximumLinearVelocity = msg->maximumLinearSpeed;
    this->maximumAngularVelocity = msg->maximumAngularSpeed;
    if (this->getLinearVelocity() > maximumLinearVelocity || this->getAngularVelocity() > maximumAngularVelocity) {
        geometry_msgs::Twist twist;
        twist.linear.x = BOUND(getLinearVelocity(), maximumLinearVelocity);
        twist.angular.z = BOUND(getAngularVelocity(), maximumAngularVelocity);
        speedPublisher.publish(twist);
    }
}

double mobile_base::Base::getAngle() {
    return ((getRightPos() / radiusBase) - (getLeftPos() / radiusBase)) / 2.0;
}

double mobile_base::Base::getLeftPos() {
    leftWheelMotor->motor->getLinearPos();
    return leftWheelMotor->motor->presentLinearPos() - initialLeftPos;
}

double mobile_base::Base::getRightPos() {
    rightWheelMotor->motor->getLinearPos();
    return rightWheelMotor->motor->presentLinearPos() - initialRightPos;
}

double mobile_base::Base::getLinearVelocity() {
    return ((leftWheelMotor->currentVelocity + rightWheelMotor->currentVelocity) / 2) * radiusWheel;
}

double mobile_base::Base::getAngularVelocity() {
    return ((leftWheelMotor->currentVelocity - rightWheelMotor->currentVelocity) / 2) * (radiusWheel / radiusBase);
}

void mobile_base::Base::resetPos() {
    leftWheelMotor->motor->getLinearPos();
    rightWheelMotor->motor->getLinearPos();
    initialLeftPos = leftWheelMotor->motor->presentLinearPos();
    initialRightPos = rightWheelMotor->motor->presentLinearPos();
}

void mobile_base::Base::publishState() {

    // Initialize
    aidu_mobile_base::State state;
    int countdown = 15;

    // Getting position, speed and angle
    leftWheelMotor->motor->getPosAndSpeed();
    state.leftpos = getLeftPos();
    state.leftspeed = leftWheelMotor->motor->presentSpeed();
    rightWheelMotor->motor->getPosAndSpeed();
    state.rightpos = getRightPos();
    state.rightspeed = rightWheelMotor->motor->presentSpeed();
    state.angle = getAngle();

    // Only publish state when the motors have been recently active
    if(state.rightspeed == 0 && state.leftspeed == 0) {
        if(countdown > 0) {
        countdown--;
        }
    } else {
        countdown = 15;
    }
    if(countdown > 0) {
        statePublisher.publish(state);
    }

}

void mobile_base::Base::spin(){

    ros::Rate rate(30); // rate at which position published (hertz)
    resetPos(); // Reset the position initially

    while(ros::ok()) {

        // Publish information
        publishState();

        // Spin ros
        ros::spinOnce();
        rate.sleep();

    }
}

mobile_base::Base::~Base() {
    delete leftWheelMotor;
    delete rightWheelMotor;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "base");
    mobile_base::Base base;
    base.spin();
    return 0;
}
<|endoftext|>


<commit_before>/*
    Author : David Barnett
    Date   : 03-10-2013
    License: MIT License
*/
#include "Settings.h"

#include <iosfwd>
#include <iostream>

using namespace IO;

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sstream>
#include <algorithm>
#include <iterator>
#include "../etc/string.h"

//#define SETTINGS_VERBOSE

Settings::Settings() {
    this->loading_flag = SETTING_LOAD_NOTHING;
}

Settings::Settings(SettingsLoadFlags flag)
{
    this->loading_flag = flag;
}

Settings::~Settings()
{
    this->stored_settings.clear();
}

/**
 * @brief removes all currently loaded settings
 */
void Settings::clear()
{
    this->stored_settings.clear();
}

/**
 * @brief Checks if a given setting exists, and loads it if flags permit
 * @param header section header
 * @param key name of the setting to be accessed
 * @return true on exists
 */
bool Settings::exists (std::string header ,std::string  key)
{
    if (this->exists(header))
    {
        INISection* section = &(this->stored_settings[header]);
        if (section->loaded) {
            return section->properties.find(key) != section->properties.end();
        } else {
            //Load Header's properties?
            if ( this->loading_flag == SETTING_LOAD_ON_REQUEST ) {
                this->load_section ( header , SETTINGS_DUPLICATES_INGORED );
                return this->exists ( header , key);
            } else {
                return false;
            }
        }
    } else {
        return false;
    }
}

/**
 * @brief Checks if a header exits
 * @param header Name of section
 * @return true on success
 */
bool Settings::exists (std::string header)
{
    return this->stored_settings.find(header) != this->stored_settings.end();
}

/**
 * @brief Loads settings from a file
 * @param file_name
 * @param flag
 */
void Settings::load(std::string file_name , SettingsDuplicateFlags flag)
{
#ifdef SETTINGS_VERBOSE
    std::cout << "Loading " << file_name << std::endl;
#endif
    std::fstream file;
    file.open(file_name.c_str());
    this->file_name = file_name;
    //Check if the file is open
    if (!(file.is_open()))
    {
        return;
    }

    //Seek to the beginning of the file
    file.seekg( 0 , file.beg );
    file.peek();

    //Calculate the offset from the 1st byte to the 0th element
    int sys_error = file.tellg();

    std::string line;
    INISection section;
    section.header_name = "";
    section.loaded = false;
    section.start_index = 0;
#ifdef SETTINGS_VERBOSE
    std::cout << "sys error is " << sys_error << " Pos " << file.tellg() << std::endl;
    std::cout << "root  S:" << section.start_index;
#endif
    while ( ! file.eof() )
    {
        //Get current position with the sys error offset
        int start_pos = (int)(file.tellg()) - sys_error;
        std::getline( file , line );

        if (line.length() == 0)
            continue;

        line = etc::trim(line);

        //Treat lines starting with ; as comments and do not process
        if ( etc::startswith( line , ";" ) )
        {
            continue;
        }
        //Check if the line has the beginning of a section
        if (etc::startswith( line , "[" ) && etc::endswith( line , "]" ))
        {
            //Close last section
            section.end_index = (int)file.tellg();
            this->stored_settings[section.header_name] = section;

#ifdef SETTINGS_VERBOSE
            std::cout << " E:" << section.end_index << std::endl;
#endif

            //Refresh section
            section = INISection();
            //Remove the brackets
            section.header_name = line.substr ( 1 , line.size() - 2 );

            if ( this->exists(section.header_name)) {
                //Warning text
                std::cout << "ERROR : Another " << this->file_name << "::" << section.header_name << " has been defined!\nSections will be damaged." << std::endl;
            } else {
                //-2 constant is to correct the position of the start index
                section.start_index = start_pos ;
                section.loaded = false;
                section.properties = SettingsMap();

#ifdef SETTINGS_VERBOSE
            std::cout << section.header_name  << " S:" << section.start_index;
#endif

            }
            continue;
        }
    }

    file.seekg (0 , file.end);
    section.end_index = (int)file.tellg() - sys_error;
    this->stored_settings[section.header_name] = section;

#ifdef SETTINGS_VERBOSE
            std::cout << " E:" << section.end_index << std::endl;
#endif

    file.close();
}

/**
 * @brief Loads a section's data into memory
 * @param header Section name
 * @param flag Settings Duplicate Flags
 */
void Settings::load_section ( std::string header , SettingsDuplicateFlags flag) {
    std::fstream file;

    if (this->exists(header) == false)
        return;

    INISection* section = &(this->stored_settings[header]);

    file.open( this->file_name.c_str() );
    int file_pos_max = section->end_index;
    //Check if the file is open
    if (!(file.is_open()))
    {
        return;
    }

    //Seek to the beginning of the file
    file.seekg( 0 , file.beg );
    file.peek();

    file.seekg( section->start_index , file.beg );

    bool start = false;
    while ( file.tellg() < file_pos_max && !file.eof() )
    {
        std::string line;
        std::getline ( file, line );
        line = etc::trim ( line );

        if (line == "")
        {
            continue;
        }
        if ( etc::startswith( line , ";" ) )
        {
            continue;
        }
        //Check if the line has the beginning of a section
        if ( etc::startswith( line , "[" ) || etc::endswith( line , "]") )
        {
            std::string header_name = line.substr ( 1 , line.size() - 2 );
            //If another section is in contact, LEAVE NOW
            if (header_name != section->header_name) {
                break;
            } else {
                //Found the start of the section we are looking for
                start = true;
            }
            continue;
        }

        if (start == false)
        {
            continue;
        }

        //Find the equal part of the line
        std::size_t equal_pos = line.find_first_of('=', 0);

        if (equal_pos == line.npos) {
            continue;
        }

        std::string key = etc::trim ( line.substr( 0 , equal_pos ) );
        std::string value = etc::trim ( line.substr( equal_pos + 1  , line.length() - 1 ) );

        if (key == "") {
            continue;
        }

        if ( section->properties.find( key ) !=  section->properties.end() )
        {
            //There is another copy of the key, check if it is OK to override
            if (flag == SETTINGS_DUPLICATES_OVERRIDE)
            {
                section->properties[key] = value;
            }
        }
        else
        {
            section->properties[key] = value;
#ifdef SETTINGS_VERBOSE
            std::cout << section->header_name << "::" << key << "=" << value << std::endl;
#endif
        }
    }
    //Update loaded status
    section->loaded = true;
    file.close();

}

/**
 * @brief Unloads a specified section
 * @param header a section name
 */
void Settings::unload_section (std::string header)
{
    if (this->exists(header)) {
        this->stored_settings[header].properties.clear();
        this->stored_settings[header].loaded = false;
    }
}

/**
 * @brief Gets the string of the given setting and copies it into the out string
 * @param header the name of the section
 * @param key the name of setting
 * @param out A string to be written to
 * @return true on successful copy
 */
bool Settings::get (std::string header , std::string  key, std::string* out)
{
    if (this->exists(header ,key))
    {
        *out = this->stored_settings[header].properties[key];
        return true;
    }
    else
    {
        return false;
    }
}

/**
 * @brief Gets the value and converts it to a bool
 * @see Settings::get
 * @param bol Returns the value
 * @return
 */
bool Settings::getBool (std::string header , std::string  key , bool* bol)
{
    std::string b;
    if (this->get(header, key , &b) == false)
        return false;
    b = etc::toLower(b);
    if (b != "")
    {
        *bol = ( b == "true" ? true : false );
        return true;
    }
    else
    {
        return false;
    }
}

bool Settings::getInt (std::string header , std::string  key , int* num)
{
    std::string b;
    if (this->get(header, key , &b) == false)
        return false;

    if ( etc::is_number(b) )
    {
        *num = atoi( b.c_str() );
        return true;
    }
    else
    {
        return false;
    }
}

bool Settings::add (std::string header , std::string key , std::string value)
{
    if (!this->exists(header , key))
    {
        this->stored_settings[header].properties[key] = value;
        return true;
    }
    else
    {
        return false;
    }
}

bool Settings::set (std::string header , std::string key , std::string value)
{
     if (this->exists( header , key ) ) {
        this->stored_settings[header].properties[key] = value;
        return true;
     }
     return false;
}

/**
 * @brief Saves all the sections of a given file
 * @warning Deletes original file and then renames a temp file to replace it
 * @param filename Saves all headers that belong to a file
 * @return bool true on success
 */
bool Settings::save ()
{
    //The File to be read from
    std::ifstream file_in;
    //The File to be written to
    std::fstream file_out;
    //Stream are separate to prevent cross-contamination

    //Open the input file
    file_in.open( this->file_name.c_str() );

    //Check for errors
    if (file_in.is_open() == false) {
        std::cout << "Error input file : " << this->file_name.c_str() << " could not be opened." << std::endl;
        return false;
    }

    //Open the output file
    file_out.open( ("temp~" + this->file_name).c_str() , std::ofstream::out );

    //Check for errors
    if (file_out.is_open() == false) {
        std::cout << "Error output file : " << ("temp~" + this->file_name).c_str() << " could not be opened." << std::endl;
        return false;
    }

    //Go through whole file
    INISection* current_section = &(this->stored_settings[""]);
    while ( file_in.eof() == false )
    {
        std::string line;
        std::getline ( file_in , line );
        line = etc::trim ( line );

        std::size_t equal_pos = line.find_first_of('=', 0);

        if (line == "")
        {

        }
        else if ( etc::startswith( line , ";" ) )
        {

        }
        else if ( etc::startswith( line , "[" ) || etc::endswith( line , "]") )
        {
            //Check if the line has the beginning of a section
            //Update current_section
            if (current_section != nullptr)
            {
                current_section->end_index = file_out.tellg();
            }

            current_section = &( this->stored_settings[ line.substr ( 1 , line.size() - 2 )] );
            current_section->start_index = (int)file_out.tellg() - 1;
        }
        else if (equal_pos != line.npos) {
            std::string key = etc::trim ( line.substr( 0 , equal_pos ) );
            std::string value = current_section->properties[key];
            if (value == "") {
                value = etc::trim ( line.substr( equal_pos + 1  , line.length() - 1 ) );
            }

            file_out << key << '=' << value << std::endl;
            continue;
        }

        //Standard copy line out
        file_out << line << std::endl;
    }

    file_out.close();
    file_in.close();

    //File operations
    //File operation return value
    int file_op = 0;

    //Remove old version
    file_op = std::remove( this->file_name.c_str() );

    //Check if there was an error
    if (file_op != 0) {
        std::cout << "Error while removing the file " << this->file_name.c_str() << std::endl;
        return false;
    }

    //Rename the temp version to replace the deleted version
    file_op = std::rename( ("temp~" + this->file_name).c_str()  , this->file_name.c_str() );

    //Check if there was an error
    if (file_op != 0) {
        std::cout << "Error while renaming the file from " << ("temp~" + this->file_name).c_str() << " to " << this->file_name.c_str() << std::endl;
        return false;
    }

    return true;
}
<commit_msg>Disabled Verbose * and fixed spelling Conflicts: 	io/Settings.cpp<commit_after>/*
    Author : David Barnett
    Date   : 03-10-2013
    License: MIT License
*/
#include "Settings.h"

#include <iosfwd>
#include <iostream>

using namespace IO;

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sstream>
#include <algorithm>
#include <iterator>
#include "../etc/string.h"

//#define SETTINGS_VERBOSE

Settings::Settings() {
    this->loading_flag = SETTING_LOAD_NOTHING;
}

Settings::Settings(SettingsLoadFlags flag)
{
    this->loading_flag = flag;
}

Settings::~Settings()
{
    this->stored_settings.clear();
}

/**
 * @brief removes all currently loaded settings
 */
void Settings::clear()
{
    this->stored_settings.clear();
}

/**
 * @brief Checks if a given setting exists, and loads it if flags permit
 * @param header section header
 * @param key name of the setting to be accessed
 * @return true on exists
 */
bool Settings::exists (std::string header ,std::string  key)
{
    if (this->exists(header))
    {
        INISection* section = &(this->stored_settings[header]);
        if (section->loaded) {
            return section->properties.find(key) != section->properties.end();
        } else {
            //Load Header's properties?
            if ( this->loading_flag == SETTING_LOAD_ON_REQUEST ) {
                this->load_section ( header , SETTINGS_DUPLICATES_INGORED );
                return this->exists ( header , key);
            } else {
                return false;
            }
        }
    } else {
        return false;
    }
}

/**
 * @brief Checks if a header exits
 * @param header Name of section
 * @return true on success
 */
bool Settings::exists (std::string header)
{
    return this->stored_settings.find(header) != this->stored_settings.end();
}

/**
 * @brief Loads settings from a file
 * @param file_name
 * @param flag
 */
void Settings::load(std::string file_name , SettingsDuplicateFlags flag)
{
#ifdef SETTINGS_VERBOSE
    std::cout << "Loading " << file_name << std::endl;
#endif
    std::fstream file;
    file.open(file_name.c_str());
    this->file_name = file_name;
    //Check if the file is open
    if (!(file.is_open()))
    {
        return;
    }

    //Seek to the beginning of the file
    file.seekg( 0 , file.beg );
    file.peek();

    //Calculate the offset from the 1st byte to the 0th element
    int sys_error = file.tellg();

    std::string line;
    INISection section;
    section.header_name = "";
    section.loaded = false;
    section.start_index = 0;
#ifdef SETTINGS_VERBOSE
    std::cout << "sys error is " << sys_error << " Pos " << file.tellg() << std::endl;
    std::cout << "root  S:" << section.start_index;
#endif
    while ( ! file.eof() )
    {
        //Get current position with the sys error offset
        int start_pos = (int)(file.tellg()) - sys_error;
        std::getline( file , line );

        if (line.length() == 0)
            continue;

        line = etc::trim(line);

        //Treat lines starting with ; as comments and do not process
        if ( etc::startswith( line , ";" ) )
        {
            continue;
        }
        //Check if the line has the beginning of a section
        if (etc::startswith( line , "[" ) && etc::endswith( line , "]" ))
        {
            //Close last section
            section.end_index = (int)file.tellg();
            this->stored_settings[section.header_name] = section;
#ifdef SETTINGS_VERBOSE
            std::cout << " E:" << section.end_index << std::endl;
#endif

            //Refresh section
            section = INISection();
            //Remove the brackets
            section.header_name = line.substr ( 1 , line.size() - 2 );

            if ( this->exists(section.header_name)) {
                //Warning text
                std::cout << "ERROR : Another " << this->file_name << "::" << section.header_name << " has been defined!\nSections will be damaged." << std::endl;
            } else {
                //-2 constant is to correct the position of the start index
                section.start_index = start_pos ;
                section.loaded = false;
                section.properties = SettingsMap();

#ifdef SETTINGS_VERBOSE
            std::cout << section.header_name  << " S:" << section.start_index;
#endif

            }
            continue;
        }
    }

    file.seekg (0 , file.end);
    section.end_index = (int)file.tellg() - sys_error;
    this->stored_settings[section.header_name] = section;

#ifdef SETTINGS_VERBOSE
            std::cout << " E:" << section.end_index << std::endl;
#endif

    file.close();
}

/**
 * @brief Loads a section's data into memory
 * @param header Section name
 * @param flag Settings Duplicate Flags
 */
void Settings::load_section ( std::string header , SettingsDuplicateFlags flag) {
    std::fstream file;

    if (this->exists(header) == false)
        return;

    INISection* section = &(this->stored_settings[header]);

    file.open( this->file_name.c_str() );
    int file_pos_max = section->end_index;
    //Check if the file is open
    if (!(file.is_open()))
    {
        return;
    }

    //Seek to the beginning of the file
    file.seekg( 0 , file.beg );
    file.peek();

    file.seekg( section->start_index , file.beg );

    bool start = false;
    while ( file.tellg() < file_pos_max && !file.eof() )
    {
        std::string line;
        std::getline ( file, line );
        line = etc::trim ( line );

        if (line == "")
        {
            continue;
        }
        if ( etc::startswith( line , ";" ) )
        {
            continue;
        }
        //Check if the line has the beginning of a section
        if ( etc::startswith( line , "[" ) || etc::endswith( line , "]") )
        {
            std::string header_name = line.substr ( 1 , line.size() - 2 );
            //If another section is in contact, LEAVE NOW
            if (header_name != section->header_name) {
                break;
            } else {
                //Found the start of the section we are looking for
                start = true;
            }
            continue;
        }

        if (start == false)
        {
            continue;
        }

        //Find the equal part of the line
        std::size_t equal_pos = line.find_first_of('=', 0);

        if (equal_pos == line.npos) {
            continue;
        }

        std::string key = etc::trim ( line.substr( 0 , equal_pos ) );
        std::string value = etc::trim ( line.substr( equal_pos + 1  , line.length() - 1 ) );

        if (key == "") {
            continue;
        }

        if ( section->properties.find( key ) !=  section->properties.end() )
        {
            //There is another copy of the key, check if it is OK to override
            if (flag == SETTINGS_DUPLICATES_OVERRIDE)
            {
                section->properties[key] = value;
            }
        }
        else
        {
            section->properties[key] = value;
#ifdef SETTINGS_VERBOSE
            std::cout << section->header_name << "::" << key << "=" << value << std::endl;
#endif
        }
    }
    //Update loaded status
    section->loaded = true;
    file.close();

}

/**
 * @brief Unloads a specified section
 * @param header a section name
 */
void Settings::unload_section (std::string header)
{
    if (this->exists(header)) {
        this->stored_settings[header].properties.clear();
        this->stored_settings[header].loaded = false;
    }
}

/**
 * @brief Gets the string of the given setting and copies it into the out string
 * @param header the name of the section
 * @param key the name of setting
 * @param out A string to be written to
 * @return true on successful copy
 */
bool Settings::get (std::string header , std::string  key, std::string* out)
{
    if (this->exists(header ,key))
    {
        *out = this->stored_settings[header].properties[key];
        return true;
    }
    else
    {
        return false;
    }
}

/**
 * @brief Gets the value and converts it to a bool
 * @see Settings::get
 * @param bol Returns the value
 * @return
 */
bool Settings::getBool (std::string header , std::string  key , bool* bol)
{
    std::string b;
    if (this->get(header, key , &b) == false)
        return false;
    b = etc::toLower(b);
    if (b != "")
    {
        *bol = ( b == "true" ? true : false );
        return true;
    }
    else
    {
        return false;
    }
}

bool Settings::getInt (std::string header , std::string  key , int* num)
{
    std::string b;
    if (this->get(header, key , &b) == false)
        return false;

    if ( etc::is_number(b) )
    {
        *num = atoi( b.c_str() );
        return true;
    }
    else
    {
        return false;
    }
}

bool Settings::add (std::string header , std::string key , std::string value)
{
    if (!this->exists(header , key))
    {
        this->stored_settings[header].properties[key] = value;
        return true;
    }
    else
    {
        return false;
    }
}

bool Settings::set (std::string header , std::string key , std::string value)
{
     if (this->exists( header , key ) ) {
        this->stored_settings[header].properties[key] = value;
        return true;
     }
     return false;
}

/**
 * @brief Saves all the sections of a given file
 * @warning Deletes original file and then renames a temp file to replace it
 * @param filename Saves all headers that belong to a file
 * @return bool true on success
 */
bool Settings::save ()
{
    //The File to be read from
    std::ifstream file_in;
    //The File to be written to
    std::fstream file_out;
    //Stream are separate to prevent cross-contamination

    //Open the input file
    file_in.open( this->file_name.c_str() );

    //Check for errors
    if (file_in.is_open() == false) {
        std::cout << "Error input file : " << this->file_name.c_str() << " could not be opened." << std::endl;
        return false;
    }

    //Open the output file
    file_out.open( ("temp~" + this->file_name).c_str() , std::ofstream::out );

    //Check for errors
    if (file_out.is_open() == false) {
        std::cout << "Error output file : " << ("temp~" + this->file_name).c_str() << " could not be opened." << std::endl;
        return false;
    }

    //Go through whole file
    INISection* current_section = &(this->stored_settings[""]);
    while ( file_in.eof() == false )
    {
        std::string line;
        std::getline ( file_in , line );
        line = etc::trim ( line );

        std::size_t equal_pos = line.find_first_of('=', 0);

        if (line == "")
        {

        }
        else if ( etc::startswith( line , ";" ) )
        {

        }
        else if ( etc::startswith( line , "[" ) || etc::endswith( line , "]") )
        {
            //Check if the line has the beginning of a section
            //Update current_section
            if (current_section != nullptr)
            {
                current_section->end_index = file_out.tellg();
            }

            current_section = &( this->stored_settings[ line.substr ( 1 , line.size() - 2 )] );
            current_section->start_index = (int)file_out.tellg() - 1;
        }
        else if (equal_pos != line.npos) {
            std::string key = etc::trim ( line.substr( 0 , equal_pos ) );
            std::string value = current_section->properties[key];
            if (value == "") {
                value = etc::trim ( line.substr( equal_pos + 1  , line.length() - 1 ) );
            }

            file_out << key << '=' << value << std::endl;
            continue;
        }

        //Standard copy line out
        file_out << line << std::endl;
    }

    file_out.close();
    file_in.close();

    //File operations
    //File operation return value
    int file_op = 0;

    //Remove old version
    file_op = std::remove( this->file_name.c_str() );

    //Check if there was an error
    if (file_op != 0) {
        std::cout << "Error while removing the file " << this->file_name.c_str() << std::endl;
        return false;
    }

    //Rename the temp version to replace the deleted version
    file_op = std::rename( ("temp~" + this->file_name).c_str()  , this->file_name.c_str() );

    //Check if there was an error
    if (file_op != 0) {
        std::cout << "Error while renaming the file from " << ("temp~" + this->file_name).c_str() << " to " << this->file_name.c_str() << std::endl;
        return false;
    }

    return true;
}
<|endoftext|>


<commit_before>/*
** Copyright 2009-2014 Merethis
**
** This file is part of Centreon Broker.
**
** Centreon Broker is free software: you can redistribute it and/or
** modify it under the terms of the GNU General Public License version 2
** as published by the Free Software Foundation.
**
** Centreon Broker 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
** General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with Centreon Broker. If not, see
** <http://www.gnu.org/licenses/>.
*/

#include "com/centreon/broker/notification/utilities/qhash_func.hh"
#include "com/centreon/broker/notification/state.hh"
#include "com/centreon/broker/notification/utilities/data_loggers.hh"

#include "com/centreon/broker/notification/builders/composed_acknowledgement_builder.hh"
#include "com/centreon/broker/notification/builders/composed_command_builder.hh"
#include "com/centreon/broker/notification/builders/composed_contact_builder.hh"
#include "com/centreon/broker/notification/builders/composed_dependency_builder.hh"
#include "com/centreon/broker/notification/builders/composed_downtime_builder.hh"
#include "com/centreon/broker/notification/builders/composed_escalation_builder.hh"
#include "com/centreon/broker/notification/builders/composed_node_builder.hh"
#include "com/centreon/broker/notification/builders/composed_timeperiod_builder.hh"

#include "com/centreon/broker/notification/builders/acknowledgement_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/command_by_name_builder.hh"
#include "com/centreon/broker/notification/builders/contact_by_command_builder.hh"
#include "com/centreon/broker/notification/builders/contact_by_name_builder.hh"
#include "com/centreon/broker/notification/builders/contact_by_node_builder.hh"
#include "com/centreon/broker/notification/builders/dependency_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/downtime_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/escalation_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/node_set_builder.hh"
#include "com/centreon/broker/notification/builders/node_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/timeperiod_by_name_builder.hh"
#include "com/centreon/broker/notification/builders/command_by_contact_builder.hh"

using namespace com::centreon::broker::notification;
using namespace com::centreon::broker::notification::objects;

/**
 *  Default constructor.
 */
state::state() :
  _state_mutex(QMutex::Recursive) {}

/**
 *  Copy constructor.
 *
 *  @param obj  The obj to copy.
 */
state::state(state const& obj) {
  state::operator=(obj);
}

/**
 *  Assignment operator.
 *
 *  @param obj  The obj to copy.
 *
 *  @return     A reference to this object.
 */
state& state::operator=(state const& obj) {
  if (this != &obj) {
    _nodes = obj._nodes;
    _node_by_id = obj._node_by_id;
    _acks = obj._acks;
    _commands = obj._commands;
    _contact_by_name = obj._contact_by_name;
    _contacts = obj._contacts;
    _contact_by_command = obj._contact_by_command;
    _host_command_by_contact = obj._host_command_by_contact;
    _service_command_by_contact = obj._service_command_by_contact;
    _dependency_by_child_id = obj._dependency_by_child_id;
    _dependency_by_parent_id = obj._dependency_by_parent_id;
    _downtimes = obj._downtimes;
    _escalations = obj._escalations;
    _timeperiod_by_name = obj._timeperiod_by_name;
  }
  return (*this);
}

/**
 *  Get the objects from the db.
 */
void state::update_objects_from_db(QSqlDatabase& centreon_db,
                                   QSqlDatabase& centreon_storage_db) {
  // Acquire mutex.
  QMutexLocker lock(&_state_mutex);

  // Remove old objects.
  _nodes.clear();
  _node_by_id.clear();
  _acks.clear();
  _commands.clear();
  _contact_by_name.clear();
  _contacts.clear();
  _contact_by_command.clear();
  _host_command_by_contact.clear();
  _service_command_by_contact.clear();
  _dependency_by_child_id.clear();
  _dependency_by_parent_id.clear();
  _downtimes.clear();
  _escalations.clear();
  _timeperiod_by_name.clear();

  // Get new objects
  {
    // Get nodes.
    node_loader node;
    composed_node_builder composed;
    node_set_builder set_builder(_nodes);
    node_by_node_id_builder by_node_id_builder(_node_by_id);
    composed.push_back(set_builder);
    composed.push_back(by_node_id_builder);
    node.load(&centreon_db, &composed);
  }
  {
    // Get commands.
    command_loader command;
    composed_command_builder composed;
    command_by_name_builder by_name_builder(_commands);
    composed.push_back(by_name_builder);
    command.load(&centreon_db, &composed);
  }
  {
    // Get contacts.
    contact_loader contact;
    composed_contact_builder composed;
    contact_by_command_builder by_command_builder(_contact_by_command);
    contact_by_name_builder by_name_builder(_contact_by_name);
    contact_by_node_builder by_node_builder(_contacts);
    command_by_contact_builder by_contact_builder(_host_command_by_contact,
                                                  _service_command_by_contact);
    composed.push_back(by_command_builder);
    composed.push_back(by_name_builder);
    composed.push_back(by_node_builder);
    composed.push_back(by_contact_builder);
    contact.load(&centreon_db, &composed);
  }
  {
    // Get dependencies.
    dependency_loader dependency;
    composed_dependency_builder composed;
    dependency_by_node_id_builder by_node_builder(_dependency_by_child_id,
                                                  _dependency_by_parent_id);
    composed.push_back(by_node_builder);
    dependency.load(&centreon_db, &composed);
  }
  {
    // Get escalations.
    escalation_loader escalation;
    composed_escalation_builder composed;
    escalation_by_node_id_builder by_node_builder(_escalations);
    composed.push_back(by_node_builder);
    escalation.load(&centreon_db, &composed);
  }
  {
    // Get timeperiods.
    timeperiod_loader timeperiod;
    composed_timeperiod_builder composed;
    timeperiod_by_name_builder by_name_builder(_timeperiod_by_name);
    composed.push_back(by_name_builder);
    timeperiod.load(&centreon_db, &composed);
  }
  {
    // Get downtimes.
    downtime_loader downtime;
    composed_downtime_builder composed;
    downtime_by_node_id_builder by_node_builder(_downtimes);
    composed.push_back(by_node_builder);
    downtime.load(&centreon_storage_db, &composed);
  }
  {
    // Get acknowledgements.
    acknowledgement_loader ack;
    composed_acknowledgement_builder composed;
    acknowledgement_by_node_id_builder by_node_builder(_acks);
    composed.push_back(by_node_builder);
    ack.load(&centreon_storage_db, &composed);
  }

  // Debug logging for all the data loaded.
#ifndef NDEBUG
    data_logger::log_container("_nodes", _nodes);
    data_logger::log_container("_node_by_id", _node_by_id);
    data_logger::log_container("_acks", _acks);
    data_logger::log_container("_commands", _commands);
    data_logger::log_container("_contact_by_command", _contact_by_name);
    data_logger::log_container("_host_command_by_contact", _host_command_by_contact);
    data_logger::log_container("_service_command_by_contact", _service_command_by_contact);
    data_logger::log_container("_contact_by_name", _contact_by_name);
    data_logger::log_container("_contacts", _contacts);
    data_logger::log_container("_dependency_by_child_id",
                               _dependency_by_child_id);
    data_logger::log_container("_dependency_by_parent_id",
                               _dependency_by_parent_id);
    data_logger::log_container("_downtimes", _downtimes);
    data_logger::log_container("_escalations", _escalations);
    data_logger::log_container("_timeperiod_by_name", _timeperiod_by_name);
#endif //!NDEBUG
}

node::ptr state::get_node_by_id(node_id id) {
  return (_node_by_id[id]);
}

objects::node::ptr state::get_host_from_service(objects::node_id service_id) {
  return (_node_by_id[node_id(service_id.get_host_id())]);
}

objects::timeperiod::ptr state::get_timeperiod_by_name(std::string const& name) {
  return (_timeperiod_by_name[name]);
}

QList<objects::contact::ptr> state::get_contacts_by_node(objects::node_id id) {
  return (_contacts.values(id));
}

QList<command::ptr> state::get_host_commands_by_contact(contact::ptr cnt) {
  QList<command::ptr> commands;
  QList<std::string> command_names = _host_command_by_contact.values(cnt);

  for (QList<std::string>::iterator it(command_names.begin()),
        end(command_names.end()); it != end; ++it)
    commands.push_back(_commands[*it]);
  return (commands);
}

QList<command::ptr> state::get_service_commands_by_contact(contact::ptr cnt) {
  QList<command::ptr> commands;
  QList<std::string> command_names = _service_command_by_contact.values(cnt);

  for (QList<std::string>::iterator it(command_names.begin()),
        end(command_names.end()); it != end; ++it)
    commands.push_back(_commands[*it]);
  return (commands);
}

/**
 *  Lock the internal mutex of this state.
 *
 *  @return  A QMutexLocker locking the internal mutex of this state.
 */
std::auto_ptr<QMutexLocker> state::lock() {
  std::auto_ptr<QMutexLocker> ret(new QMutexLocker(&_state_mutex));
  return (ret);
}

bool state::is_node_in_downtime(objects::node_id id) {
  time_t current_time = time(NULL);
  QList<downtime::ptr> downtimes = _downtimes.values(id);

  for (QList<downtime::ptr>::iterator it(downtimes.begin()), end(downtimes.end());
       it != end;
       ++it) {
    if ((*it)->get_actual_end_time() > current_time &&
        (*it)->get_cancelled() == false &&
        (*it)->get_started() == true &&
        (*it)->get_actual_start_time() + (*it)->get_duration() > current_time)
      return (true);
  }

  return (false);
}

bool state::has_node_been_acknowledged(objects::node_id id) {
  time_t current_time = time(NULL);
  QList<acknowledgement::ptr> acknowledgements = _acks.values(id);

  for (QList<acknowledgement::ptr>::iterator it (acknowledgements.begin()), end(acknowledgements.end());
       it != end;
       ++it) {
    return (true);
  }
  return (false);
}
<commit_msg>Notification: Use value() on QHash instead of the operator [].<commit_after>/*
** Copyright 2009-2014 Merethis
**
** This file is part of Centreon Broker.
**
** Centreon Broker is free software: you can redistribute it and/or
** modify it under the terms of the GNU General Public License version 2
** as published by the Free Software Foundation.
**
** Centreon Broker 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
** General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with Centreon Broker. If not, see
** <http://www.gnu.org/licenses/>.
*/

#include "com/centreon/broker/notification/utilities/qhash_func.hh"
#include "com/centreon/broker/notification/state.hh"
#include "com/centreon/broker/notification/utilities/data_loggers.hh"

#include "com/centreon/broker/notification/builders/composed_acknowledgement_builder.hh"
#include "com/centreon/broker/notification/builders/composed_command_builder.hh"
#include "com/centreon/broker/notification/builders/composed_contact_builder.hh"
#include "com/centreon/broker/notification/builders/composed_dependency_builder.hh"
#include "com/centreon/broker/notification/builders/composed_downtime_builder.hh"
#include "com/centreon/broker/notification/builders/composed_escalation_builder.hh"
#include "com/centreon/broker/notification/builders/composed_node_builder.hh"
#include "com/centreon/broker/notification/builders/composed_timeperiod_builder.hh"

#include "com/centreon/broker/notification/builders/acknowledgement_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/command_by_name_builder.hh"
#include "com/centreon/broker/notification/builders/contact_by_command_builder.hh"
#include "com/centreon/broker/notification/builders/contact_by_name_builder.hh"
#include "com/centreon/broker/notification/builders/contact_by_node_builder.hh"
#include "com/centreon/broker/notification/builders/dependency_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/downtime_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/escalation_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/node_set_builder.hh"
#include "com/centreon/broker/notification/builders/node_by_node_id_builder.hh"
#include "com/centreon/broker/notification/builders/timeperiod_by_name_builder.hh"
#include "com/centreon/broker/notification/builders/command_by_contact_builder.hh"

using namespace com::centreon::broker::notification;
using namespace com::centreon::broker::notification::objects;

/**
 *  Default constructor.
 */
state::state() :
  _state_mutex(QMutex::Recursive) {}

/**
 *  Copy constructor.
 *
 *  @param obj  The obj to copy.
 */
state::state(state const& obj) {
  state::operator=(obj);
}

/**
 *  Assignment operator.
 *
 *  @param obj  The obj to copy.
 *
 *  @return     A reference to this object.
 */
state& state::operator=(state const& obj) {
  if (this != &obj) {
    _nodes = obj._nodes;
    _node_by_id = obj._node_by_id;
    _acks = obj._acks;
    _commands = obj._commands;
    _contact_by_name = obj._contact_by_name;
    _contacts = obj._contacts;
    _contact_by_command = obj._contact_by_command;
    _host_command_by_contact = obj._host_command_by_contact;
    _service_command_by_contact = obj._service_command_by_contact;
    _dependency_by_child_id = obj._dependency_by_child_id;
    _dependency_by_parent_id = obj._dependency_by_parent_id;
    _downtimes = obj._downtimes;
    _escalations = obj._escalations;
    _timeperiod_by_name = obj._timeperiod_by_name;
  }
  return (*this);
}

/**
 *  Get the objects from the db.
 */
void state::update_objects_from_db(QSqlDatabase& centreon_db,
                                   QSqlDatabase& centreon_storage_db) {
  // Acquire mutex.
  QMutexLocker lock(&_state_mutex);

  // Remove old objects.
  _nodes.clear();
  _node_by_id.clear();
  _acks.clear();
  _commands.clear();
  _contact_by_name.clear();
  _contacts.clear();
  _contact_by_command.clear();
  _host_command_by_contact.clear();
  _service_command_by_contact.clear();
  _dependency_by_child_id.clear();
  _dependency_by_parent_id.clear();
  _downtimes.clear();
  _escalations.clear();
  _timeperiod_by_name.clear();

  // Get new objects
  {
    // Get nodes.
    node_loader node;
    composed_node_builder composed;
    node_set_builder set_builder(_nodes);
    node_by_node_id_builder by_node_id_builder(_node_by_id);
    composed.push_back(set_builder);
    composed.push_back(by_node_id_builder);
    node.load(&centreon_db, &composed);
  }
  {
    // Get commands.
    command_loader command;
    composed_command_builder composed;
    command_by_name_builder by_name_builder(_commands);
    composed.push_back(by_name_builder);
    command.load(&centreon_db, &composed);
  }
  {
    // Get contacts.
    contact_loader contact;
    composed_contact_builder composed;
    contact_by_command_builder by_command_builder(_contact_by_command);
    contact_by_name_builder by_name_builder(_contact_by_name);
    contact_by_node_builder by_node_builder(_contacts);
    command_by_contact_builder by_contact_builder(_host_command_by_contact,
                                                  _service_command_by_contact);
    composed.push_back(by_command_builder);
    composed.push_back(by_name_builder);
    composed.push_back(by_node_builder);
    composed.push_back(by_contact_builder);
    contact.load(&centreon_db, &composed);
  }
  {
    // Get dependencies.
    dependency_loader dependency;
    composed_dependency_builder composed;
    dependency_by_node_id_builder by_node_builder(_dependency_by_child_id,
                                                  _dependency_by_parent_id);
    composed.push_back(by_node_builder);
    dependency.load(&centreon_db, &composed);
  }
  {
    // Get escalations.
    escalation_loader escalation;
    composed_escalation_builder composed;
    escalation_by_node_id_builder by_node_builder(_escalations);
    composed.push_back(by_node_builder);
    escalation.load(&centreon_db, &composed);
  }
  {
    // Get timeperiods.
    timeperiod_loader timeperiod;
    composed_timeperiod_builder composed;
    timeperiod_by_name_builder by_name_builder(_timeperiod_by_name);
    composed.push_back(by_name_builder);
    timeperiod.load(&centreon_db, &composed);
  }
  {
    // Get downtimes.
    downtime_loader downtime;
    composed_downtime_builder composed;
    downtime_by_node_id_builder by_node_builder(_downtimes);
    composed.push_back(by_node_builder);
    downtime.load(&centreon_storage_db, &composed);
  }
  {
    // Get acknowledgements.
    acknowledgement_loader ack;
    composed_acknowledgement_builder composed;
    acknowledgement_by_node_id_builder by_node_builder(_acks);
    composed.push_back(by_node_builder);
    ack.load(&centreon_storage_db, &composed);
  }

  // Debug logging for all the data loaded.
#ifndef NDEBUG
    data_logger::log_container("_nodes", _nodes);
    data_logger::log_container("_node_by_id", _node_by_id);
    data_logger::log_container("_acks", _acks);
    data_logger::log_container("_commands", _commands);
    data_logger::log_container("_contact_by_command", _contact_by_name);
    data_logger::log_container("_host_command_by_contact", _host_command_by_contact);
    data_logger::log_container("_service_command_by_contact", _service_command_by_contact);
    data_logger::log_container("_contact_by_name", _contact_by_name);
    data_logger::log_container("_contacts", _contacts);
    data_logger::log_container("_dependency_by_child_id",
                               _dependency_by_child_id);
    data_logger::log_container("_dependency_by_parent_id",
                               _dependency_by_parent_id);
    data_logger::log_container("_downtimes", _downtimes);
    data_logger::log_container("_escalations", _escalations);
    data_logger::log_container("_timeperiod_by_name", _timeperiod_by_name);
#endif //!NDEBUG
}

node::ptr state::get_node_by_id(node_id id) {
  return (_node_by_id.value(id));
}

node::ptr state::get_host_from_service(objects::node_id service_id) {
  return (_node_by_id.value(node_id(service_id.get_host_id())));
}

timeperiod::ptr state::get_timeperiod_by_name(std::string const& name) {
  return (_timeperiod_by_name.value(name));
}

QList<objects::contact::ptr> state::get_contacts_by_node(objects::node_id id) {
  return (_contacts.values(id));
}

QList<command::ptr> state::get_host_commands_by_contact(contact::ptr cnt) {
  QList<command::ptr> commands;
  QList<std::string> command_names = _host_command_by_contact.values(cnt);

  for (QList<std::string>::iterator it(command_names.begin()),
        end(command_names.end()); it != end; ++it)
    commands.push_back(_commands[*it]);
  return (commands);
}

QList<command::ptr> state::get_service_commands_by_contact(contact::ptr cnt) {
  QList<command::ptr> commands;
  QList<std::string> command_names = _service_command_by_contact.values(cnt);

  for (QList<std::string>::iterator it(command_names.begin()),
        end(command_names.end()); it != end; ++it)
    commands.push_back(_commands[*it]);
  return (commands);
}

/**
 *  Lock the internal mutex of this state.
 *
 *  @return  A QMutexLocker locking the internal mutex of this state.
 */
std::auto_ptr<QMutexLocker> state::lock() {
  std::auto_ptr<QMutexLocker> ret(new QMutexLocker(&_state_mutex));
  return (ret);
}

bool state::is_node_in_downtime(objects::node_id id) {
  time_t current_time = time(NULL);
  QList<downtime::ptr> downtimes = _downtimes.values(id);

  for (QList<downtime::ptr>::iterator it(downtimes.begin()),
                                      end(downtimes.end());
       it != end;
       ++it) {
    if ((*it)->get_actual_end_time() > current_time &&
        (*it)->get_cancelled() == false &&
        (*it)->get_started() == true &&
        (*it)->get_actual_start_time() + (*it)->get_duration() > current_time)
      return (true);
  }

  return (false);
}

bool state::has_node_been_acknowledged(objects::node_id id) {
  time_t current_time = time(NULL);
  QList<acknowledgement::ptr> acknowledgements = _acks.values(id);

  for (QList<acknowledgement::ptr>::iterator it(acknowledgements.begin()),
                                             end(acknowledgements.end());
       it != end;
       ++it) {
    return (true);
  }
  return (false);
}
<|endoftext|>


<commit_before>// Time:  O(log(min(m, n)))
// Space: O(1)

class Solution {
public:
    double findMedianSortedArrays(vector<int>& nums1, vector<int>& nums2) {
        if ((nums1.size() + nums2.size()) % 2 == 1) {
            return findKthInTwoSortedArrays(nums1, nums2, (nums1.size() + nums2.size()) / 2 + 1);
        } else {
            return (findKthInTwoSortedArrays(nums1, nums2, (nums1.size() + nums2.size()) / 2) +
                    findKthInTwoSortedArrays(nums1, nums2, (nums1.size() + nums2.size()) / 2 + 1)) / 2.0;
        }
    }

    int findKthInTwoSortedArrays(const vector<int>& A, const vector<int>& B,
                                 int k) {
        const int m = A.size();
        const int n = B.size();

        // Make sure m is the smaller one.
        if (m > n) {
            return findKthInTwoSortedArrays(B, A, k);
        }

        int left = 0;
        int right = m;
        // Find a partition of A and B
        // where min left s.t. A[left - 1] >= B[k - 1 - left]. Thus A[left - 1] is the kth element.
        while (left < right) {
            int mid = left + (right - left) / 2;
            if (0 <= k - 1 - mid && k - 1 - mid < n && A[mid] >= B[k - 1 - mid]) {
                right = mid;
            } else {
                left = mid + 1;
            }
        }

        int Ai_minus_1 = left - 1 >= 0 ? A[left - 1] : numeric_limits<int>::min();
        int Bj = k - 1 - left >= 0 ? B[k - 1 - left] : numeric_limits<int>::min();

        // kth element would be A[left - 1] or B[k - 1 - left].
        return max(Ai_minus_1, Bj);
    }
};

// Time:  O(log(max(m, n)) * log(max_val - min_val))
// Space: O(1)
// Generic solution.
class Solution_Generic {
public:
    double findMedianSortedArrays(vector<int>& nums1, vector<int>& nums2) {
        vector<vector<int> *> arrays{&nums1, &nums2};
        if ((nums1.size() + nums2.size()) % 2 == 1) {
            return findKthInSortedArrays(arrays, (nums1.size() + nums2.size()) / 2 + 1);
        } else {
            return (findKthInSortedArrays(arrays, (nums1.size() + nums2.size()) / 2) +
                    findKthInSortedArrays(arrays, (nums1.size() + nums2.size()) / 2 + 1)) / 2.0;
        }
    }

private:
    int findKthInSortedArrays(const vector<vector<int> *>& arrays, int k) {
        int left = numeric_limits<int>::max();
        int right = numeric_limits<int>::min();
        for (const auto array : arrays) {
            if (!array->empty()) {
                left = min(left, array->front());
                right = max(right, array->back());
            }
        }
        // left xxxxxxxooooooo right, find first xo or oo
        while (left <= right) {
            const auto mid = left + (right - left) / 2;
            if (match(arrays, mid, k)) {
                right = mid - 1;
            } else {
                left = mid + 1;
            }
        }
        return left;
    }

    bool match(const vector<vector<int> *>& arrays, int num, int target) {
        int res = 0;
        for (const auto array : arrays) {
            if (!array->empty()) {
                res += distance(upper_bound(array->cbegin(), array->cend(), num),
                                array->cend());
            }
        }
        return res < target;
    }
};
<commit_msg>Update median-of-two-sorted-arrays.cpp<commit_after>// Time:  O(log(min(m, n)))
// Space: O(1)

class Solution {
public:
    double findMedianSortedArrays(vector<int>& nums1, vector<int>& nums2) {
        if ((nums1.size() + nums2.size()) % 2 == 1) {
            return findKthInTwoSortedArrays(nums1, nums2, (nums1.size() + nums2.size()) / 2 + 1);
        } else {
            return (findKthInTwoSortedArrays(nums1, nums2, (nums1.size() + nums2.size()) / 2) +
                    findKthInTwoSortedArrays(nums1, nums2, (nums1.size() + nums2.size()) / 2 + 1)) / 2.0;
        }
    }

    int findKthInTwoSortedArrays(const vector<int>& A, const vector<int>& B,
                                 int k) {
        const int m = A.size();
        const int n = B.size();

        // Make sure m is the smaller one.
        if (m > n) {
            return findKthInTwoSortedArrays(B, A, k);
        }

        int left = 0;
        int right = m;
        // Find a partition of A and B
        // where min left s.t. A[left] >= B[k - 1 - left]. Thus A[left] is the (k+1)-th or above element.
        while (left < right) {
            int mid = left + (right - left) / 2;
            if (0 <= k - 1 - mid && k - 1 - mid < n && A[mid] >= B[k - 1 - mid]) {
                right = mid;
            } else {
                left = mid + 1;
            }
        }

        int Ai_minus_1 = left - 1 >= 0 ? A[left - 1] : numeric_limits<int>::min();
        int Bj = k - 1 - left >= 0 ? B[k - 1 - left] : numeric_limits<int>::min();

        // kth element would be A[left - 1] or B[k - 1 - left].
        return max(Ai_minus_1, Bj);
    }
};

// Time:  O(log(max(m, n)) * log(max_val - min_val))
// Space: O(1)
// Generic solution.
class Solution_Generic {
public:
    double findMedianSortedArrays(vector<int>& nums1, vector<int>& nums2) {
        vector<vector<int> *> arrays{&nums1, &nums2};
        if ((nums1.size() + nums2.size()) % 2 == 1) {
            return findKthInSortedArrays(arrays, (nums1.size() + nums2.size()) / 2 + 1);
        } else {
            return (findKthInSortedArrays(arrays, (nums1.size() + nums2.size()) / 2) +
                    findKthInSortedArrays(arrays, (nums1.size() + nums2.size()) / 2 + 1)) / 2.0;
        }
    }

private:
    int findKthInSortedArrays(const vector<vector<int> *>& arrays, int k) {
        int left = numeric_limits<int>::max();
        int right = numeric_limits<int>::min();
        for (const auto array : arrays) {
            if (!array->empty()) {
                left = min(left, array->front());
                right = max(right, array->back());
            }
        }
        // left xxxxxxxooooooo right, find first xo or oo
        while (left <= right) {
            const auto mid = left + (right - left) / 2;
            if (match(arrays, mid, k)) {
                right = mid - 1;
            } else {
                left = mid + 1;
            }
        }
        return left;
    }

    bool match(const vector<vector<int> *>& arrays, int num, int target) {
        int res = 0;
        for (const auto array : arrays) {
            if (!array->empty()) {
                res += distance(upper_bound(array->cbegin(), array->cend(), num),
                                array->cend());
            }
        }
        return res < target;
    }
};
<|endoftext|>


<commit_before>/**
 * \brief
 *
 **/
/**
 \file LaunchOptions.cpp
 \author steiner
 \date Apr 14, 2016
 **/

#include "LaunchOptions.h"

namespace TwitterTetris
{
LaunchOptions::LaunchOptions(int argc, char* argv[])
{
   // Copy command into strings for safer use
   std::vector<std::string> command_args;
   for(int i = 0; i < argc; ++i)
     {
        std::ostringstream text;
        text << argv[i];

        if(text.str().size() > 0)
           command_args.push_back(text.str());
     }

   // Loop on command line arguments
   for(uint i = 0; i < command_args.size(); ++i)
   {
      // Command options begins with -
      if(command_args[i] == std::string("--action") || command_args[i] == std::string("-a"))
      {
         // Next argument, if it exists is the action
         if(command_args.size() > i+1) this->maction = command_args[i+1][0];
      }

      if(command_args[i] == std::string("--last") || command_args[i] == std::string("-l"))
      {
         // Next argument, if it exists is the filename
         if(command_args.size() > i+1) this->mlast_state = command_args[i+1];
      }

      if(command_args[i] == std::string("--img"))
      {
         // Next argument, if it exists is the image filename
         if(command_args.size() > i+1) this->mimg_file = command_args[i+1];
      }

      if(command_args[i] == std::string("--ascii"))
      {
         this->mascii_export = true;
         // Next argument, if it exists is the ascii filename
         if(command_args.size() > i+1)
            if(command_args[i+1][0] != '-') this->mascii_file = command_args[i+1];
      }

      if(command_args[i] == std::string("--help"))
      {
         mdirectExit = true;
         std::cout << "Usage: TwitterTetris [options]" << std::endl
                   << "Options:" << std::endl << std::left

                   << std::setw(25) << " --help" << "Display this information and exit" << std::endl

                   << std::setw(25) << " --action, -a <char>" << "Define next action to be applied. Available actions are : " << std::endl
                   << std::setw(25) << " " << "'l' (move left), 'r' (move right), 's' (spin the piece), 'a' (anti-spin the piece)," << std::endl
                   << std::setw(25) << " " << "'d' (fall all the way down), 'f' (fall 1 unit down), 'n' (no action)" << std::endl

                   << std::setw(25) << " --last, -l <file>" << "XML file with last available state" << std::endl

                   << std::setw(25) << " --img <file>" << "File prefix for the GIF image to generate" << std::endl

                   << std::setw(25) << " --ascii <optional file> " << "Display state with ascii characters in file or in console if no file supplied" << std::endl

                   << std::setw(25) << " --properties, -p <file> " << "If file exists, read properties. If not, write default properties to file." << std::endl;

      }
   }
}
} /* namespace TwitterTetris */
<commit_msg>Change help text<commit_after>/**
 * \brief
 *
 **/
/**
 \file LaunchOptions.cpp
 \author steiner
 \date Apr 14, 2016
 **/

#include "LaunchOptions.h"

namespace TwitterTetris
{
LaunchOptions::LaunchOptions(int argc, char* argv[])
{
   // Copy command into strings for safer use
   std::vector<std::string> command_args;
   for(int i = 0; i < argc; ++i)
     {
        std::ostringstream text;
        text << argv[i];

        if(text.str().size() > 0)
           command_args.push_back(text.str());
     }

   // Loop on command line arguments
   for(uint i = 0; i < command_args.size(); ++i)
   {
      // Command options begins with -
      if(command_args[i] == std::string("--action") || command_args[i] == std::string("-a"))
      {
         // Next argument, if it exists is the action
         if(command_args.size() > i+1) this->maction = command_args[i+1][0];
      }

      if(command_args[i] == std::string("--last") || command_args[i] == std::string("-l"))
      {
         // Next argument, if it exists is the filename
         if(command_args.size() > i+1) this->mlast_state = command_args[i+1];
      }

      if(command_args[i] == std::string("--img"))
      {
         // Next argument, if it exists is the image filename
         if(command_args.size() > i+1) this->mimg_file = command_args[i+1];
      }

      if(command_args[i] == std::string("--ascii"))
      {
         this->mascii_export = true;
         // Next argument, if it exists is the ascii filename
         if(command_args.size() > i+1)
            if(command_args[i+1][0] != '-') this->mascii_file = command_args[i+1];
      }

      if(command_args[i] == std::string("--help"))
      {
         mdirectExit = true;
         std::cout << "Usage: TwitterTetris [options]" << std::endl
                   << "Options:" << std::endl << std::left

                   << std::setw(25) << " --help" << "Display this information and exit" << std::endl

                   << std::setw(25) << " --action, -a <char>" << "Define next action to be applied. Available actions are : " << std::endl
                   << std::setw(25) << " " << "'l' (move left), 'r' (move right), 's' (spin the piece), 'a' (anti-spin the piece)," << std::endl
                   << std::setw(25) << " " << "'f' (fall all the way down), 'd' (fall 1 unit down), 'n' (no action)" << std::endl

                   << std::setw(25) << " --last, -l <file>" << "XML file with last available state" << std::endl

                   << std::setw(25) << " --img <file>" << "File prefix for the GIF image to generate" << std::endl

                   << std::setw(25) << " --ascii <optional file> " << "Display state with ascii characters in file or in console if no file supplied" << std::endl

                   << std::setw(25) << " --properties, -p <file> " << "If file exists, read properties. If not, write default properties to file." << std::endl;

      }
   }
}
} /* namespace TwitterTetris */
<|endoftext|>