<commit_before>/*!
* \file youbot_joy_teleop.cpp
* \brief Allows for control of the KUKA youBot with a joystick.
*
* youbot_joy_teleop creates a ROS node that allows the control of a KUKA youBot with a joystick.
* This node listens to a /joy topic and sends messages to the /cmd_vel topic. Arm control is currently unimplemented.
*
* \author Russell Toris, WPI - rctoris@wpi.edu
* \date May 21, 2013
*/

#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <youbot_joy_arm/youbot_joy_teleop.h>

#include <iostream>
#include <assert.h>
#include "trajectory_msgs/JointTrajectory.h"
#include "brics_actuator/CartesianWrench.h"
#include <boost/units/io.hpp>

#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <math.h>
#include "brics_actuator/JointPositions.h"
#include "sensor_msgs/JointState.h"

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/io.hpp>

#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include "std_msgs/String.h"

ros::Time T;
bool receivedmsg = false;

double joint[5];
double lastJoint[5];
double gripperr = 0;
double gripperl = 0;

double jointMax[] = {5.840139, 2.617989, -0.0157081, 3.42919, 5.641589};
double jointMin[] = {0.01006921, 0.01006921, -5.0264, 0.0221391, 0.11062};
double gripperMax = 0.0115;
double gripperMin = 0;

double jointDelta[5];
double gripperDelta = (gripperMax - gripperMin) * 0.02;

double jointHome[] = {0.01007,0.01007,-0.15709,0.02214,0.1107};
double jointCamera[] = {3.0,0.5,-0.9,0.1,3.0};
double jointObject[] = {3.04171,0.63597,-1.017845,0.36284,2.876194};
double jointGrasp[] = {3.04171,2.04427,-1.5189129,2.5434289757,2.8761944};
double jointInitialize[] = {0.01007,.635971,-1.91989,1.04424,2.87619};

using namespace std;

void position_listener(const sensor_msgs::JointState::ConstPtr& msg)
{
    if(msg->name[0] == "arm_joint_1")
    {
        joint[0] = msg->position[0];
        joint[1] = msg->position[1];
        joint[2] = msg->position[2];
        joint[3] = msg->position[3];
        joint[4] = msg->position[4];

        gripperl = msg->position[5];
        gripperr = msg->position[6];
    }
}

youbot_joy_teleop::youbot_joy_teleop()
{
  // create the ROS topics
  cmd_vel = node.advertise < geometry_msgs::Twist > ("cmd_vel", 10);
  joy_sub = node.subscribe < sensor_msgs::Joy > ("joy", 10, &youbot_joy_teleop::joy_cback, this);
  armPositionsPublisher = node.advertise<brics_actuator::JointPositions > ("arm_1/arm_controller/position_command", 1);
  gripperPositionPublisher = node.advertise<brics_actuator::JointPositions > ("arm_1/gripper_controller/position_command", 1);
  armPositionSubscriber = node.subscribe("joint_states", 1000, position_listener);

  const int numberOfArmJoints = 5;
  const int numberOfGripperJoints = 2;

  armJointPositions.resize(numberOfArmJoints);
  gripperJointPositions.resize(numberOfGripperJoints);

  ROS_INFO("youBot Joystick Teleop Started");
}

void youbot_joy_teleop::joy_cback(const sensor_msgs::Joy::ConstPtr& joy)
{
  if(!receivedmsg)
  {
    receivedmsg = true;
  }
  T = ros::Time::now();

  // create the twist message
  geometry_msgs::Twist twist;
  // left joystick controls the linear movement
  twist.linear.x = joy->axes.at(1);
  twist.linear.y = joy->axes.at(0);
  twist.linear.z = 0;
  // right joystick controls the angular movement
  twist.angular.x = 0;
  twist.angular.y = 0;
  twist.angular.z = joy->axes.at(2);
  // send the twist command
  cmd_vel.publish(twist);

  //Joint 0 Cross up/down
  if(joy->axes.at(7) > 0) {
      joint[0] += jointDelta[0];
      ROS_INFO("Joint0: %f", joint[0]);
  } else if (joy->axes.at(7) < 0){
      joint[0] -= jointDelta[0];
      ROS_INFO("Joint0: %f", joint[0]);
  }

  //Joint 1 Cross left/right
  if(joy->axes.at(6) > 0) {
      joint[1] += jointDelta[1];
      ROS_INFO("Joint1: %f", joint[1]);
  } else if (joy->axes.at(6) < 0){
      joint[1] -= jointDelta[1];
      ROS_INFO("Joint1: %f", joint[1]);
  }

  //Joint 2 Button Y/A
  if(joy->buttons.at(3)) {
      joint[2] += jointDelta[2];
      ROS_INFO("Joint2: %f", joint[2]);
  }
  if(joy->buttons.at(0)) {
      joint[2] -= jointDelta[2];
      ROS_INFO("Joint2: %f", joint[2]);
  }

  //Joint 3 Button Y/A
  if(joy->buttons.at(2)) {
      joint[3] += jointDelta[3];
      ROS_INFO("Joint3: %f", joint[3]);
  }
  if(joy->buttons.at(1)) {
      joint[3] -= jointDelta[3];
      ROS_INFO("Joint3: %f", joint[3]);
  }

  //Joint 4 LT/RT
  if(joy->axes.at(2) < 0) {
      joint[4] += jointDelta[4];
      ROS_INFO("Joint4: %f", joint[4]);
  }
  if(joy->axes.at(5) < 0) {
      joint[4] -= jointDelta[4];
      ROS_INFO("Joint4: %f", joint[4]);
  }

  //Gripper LB(open)/RB(close)
  if(joy->buttons.at(4)) {
      gripperl = gripperMax;
      gripperr = gripperMax;
      ROS_INFO("Gripper Open");
  }
  if(joy->buttons.at(5)) {
      gripperl = gripperMin;
      gripperr = gripperMin;
      ROS_INFO("Gripper Close");
  }

  //for(int i = 0; i < numberOfArmJoints; i++)
  for(int i = 0; i < 5; i++)
  {
      jointName.str("");
      jointName << "arm_joint_" << (i + 1);

      if(joint[i] < jointMin[i])
          joint[i] = jointMin[i];
      if(joint[i] > jointMax[i])
          joint[i] = jointMax[i];

      armJointPositions[i].joint_uri = jointName.str();
      armJointPositions[i].value = joint[i];

      armJointPositions[i].unit = boost::units::to_string(boost::units::si::radians);
  }

  if(gripperl < gripperMin)
      gripperl = gripperMin;
  if(gripperr < gripperMin)
      gripperr = gripperMin;
  if(gripperl > gripperMax)
      gripperl = gripperMax;
  if(gripperr > gripperMax)
      gripperr = gripperMax;

  gripperJointPositions[0].joint_uri = "gripper_finger_joint_l";
  gripperJointPositions[0].value = gripperl;
  gripperJointPositions[0].unit = boost::units::to_string(boost::units::si::meter);

  gripperJointPositions[1].joint_uri = "gripper_finger_joint_r";
  gripperJointPositions[1].value = gripperr;
  gripperJointPositions[1].unit = boost::units::to_string(boost::units::si::meter);

  command.positions = armJointPositions;
  armPositionsPublisher.publish(command);

  command.positions = gripperJointPositions;
  gripperPositionPublisher.publish(command);

  //for(int i = 0; i <numberOfArmJoints; i++)
  for(int i = 0; i <5; i++)
  {
      lastJoint[i] = joint[i];
  }
}

void youbot_joy_teleop::joy_check()
{
    if( (receivedmsg) && ( (ros::Time::now().toSec() - T.toSec() ) > .15) )
    {
        geometry_msgs::Twist zero;
        cmd_vel.publish(zero);
    }
}

int main(int argc, char **argv)
{
  // initialize ROS and the node
  ros::init(argc, argv, "youbot_joy_arm");

  // initialize the joystick controller
  youbot_joy_teleop controller;

  std::fill_n(joint, 5, 0);
  gripperl = 0;
  gripperr = 0;

  ros::Rate rate(10); //Hz
  static const int numberOfArmJoints = 5;
  static const int numberOfGripperJoints = 2;

  for(int i = 0; i < numberOfArmJoints; i++)
  {
      jointDelta[i] = (jointMax[i] - jointMin[i]) * 0.02;
  }

  ros::spinOnce();

  for(int i = 0; i < numberOfArmJoints; i++)
  {
      lastJoint[i] = joint[i];
  }

  // continue until a ctrl-c has occurred
  while(ros::ok())
  {
    ros::spinOnce();

    controller.joy_check();

    rate.sleep();
  }
  //ros::spin();
  return 0;
}
<commit_msg>Disable cmd<commit_after>/*!
* \file youbot_joy_teleop.cpp
* \brief Allows for control of the KUKA youBot with a joystick.
*
* youbot_joy_teleop creates a ROS node that allows the control of a KUKA youBot with a joystick.
* This node listens to a /joy topic and sends messages to the /cmd_vel topic. Arm control is currently unimplemented.
*
* \author Russell Toris, WPI - rctoris@wpi.edu
* \date May 21, 2013
*/

#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <youbot_joy_arm/youbot_joy_teleop.h>

#include <iostream>
#include <assert.h>
#include "trajectory_msgs/JointTrajectory.h"
#include "brics_actuator/CartesianWrench.h"
#include <boost/units/io.hpp>

#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <math.h>
#include "brics_actuator/JointPositions.h"
#include "sensor_msgs/JointState.h"

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/io.hpp>

#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include "std_msgs/String.h"

ros::Time T;
bool receivedmsg = false;

double joint[5];
double lastJoint[5];
double gripperr = 0;
double gripperl = 0;

double jointMax[] = {5.840139, 2.617989, -0.0157081, 3.42919, 5.641589};
double jointMin[] = {0.01006921, 0.01006921, -5.0264, 0.0221391, 0.11062};
double gripperMax = 0.0115;
double gripperMin = 0;

double jointDelta[5];
double gripperDelta = (gripperMax - gripperMin) * 0.02;

double jointHome[] = {0.01007,0.01007,-0.15709,0.02214,0.1107};
double jointCamera[] = {3.0,0.5,-0.9,0.1,3.0};
double jointObject[] = {3.04171,0.63597,-1.017845,0.36284,2.876194};
double jointGrasp[] = {3.04171,2.04427,-1.5189129,2.5434289757,2.8761944};
double jointInitialize[] = {0.01007,.635971,-1.91989,1.04424,2.87619};

using namespace std;

void position_listener(const sensor_msgs::JointState::ConstPtr& msg)
{
    if(msg->name[0] == "arm_joint_1")
    {
        joint[0] = msg->position[0];
        joint[1] = msg->position[1];
        joint[2] = msg->position[2];
        joint[3] = msg->position[3];
        joint[4] = msg->position[4];

        gripperl = msg->position[5];
        gripperr = msg->position[6];
    }
}

youbot_joy_teleop::youbot_joy_teleop()
{
  // create the ROS topics
  //cmd_vel = node.advertise < geometry_msgs::Twist > ("cmd_vel", 10);
  joy_sub = node.subscribe < sensor_msgs::Joy > ("joy", 10, &youbot_joy_teleop::joy_cback, this);
  armPositionsPublisher = node.advertise<brics_actuator::JointPositions > ("arm_1/arm_controller/position_command", 1);
  gripperPositionPublisher = node.advertise<brics_actuator::JointPositions > ("arm_1/gripper_controller/position_command", 1);
  armPositionSubscriber = node.subscribe("joint_states", 1000, position_listener);

  const int numberOfArmJoints = 5;
  const int numberOfGripperJoints = 2;

  armJointPositions.resize(numberOfArmJoints);
  gripperJointPositions.resize(numberOfGripperJoints);

  ROS_INFO("youBot Joystick Teleop Started");
}

void youbot_joy_teleop::joy_cback(const sensor_msgs::Joy::ConstPtr& joy)
{
  if(!receivedmsg)
  {
    receivedmsg = true;
  }
  T = ros::Time::now();

  // create the twist message
  geometry_msgs::Twist twist;
  // left joystick controls the linear movement
  twist.linear.x = joy->axes.at(1);
  twist.linear.y = joy->axes.at(0);
  twist.linear.z = 0;
  // right joystick controls the angular movement
  twist.angular.x = 0;
  twist.angular.y = 0;
  twist.angular.z = joy->axes.at(2);
  // send the twist command
  //cmd_vel.publish(twist);

  //Joint 0 Cross up/down
  if(joy->axes.at(7) > 0) {
      joint[0] += jointDelta[0];
      ROS_INFO("Joint0: %f", joint[0]);
  } else if (joy->axes.at(7) < 0){
      joint[0] -= jointDelta[0];
      ROS_INFO("Joint0: %f", joint[0]);
  }

  //Joint 1 Cross left/right
  if(joy->axes.at(6) > 0) {
      joint[1] += jointDelta[1];
      ROS_INFO("Joint1: %f", joint[1]);
  } else if (joy->axes.at(6) < 0){
      joint[1] -= jointDelta[1];
      ROS_INFO("Joint1: %f", joint[1]);
  }

  //Joint 2 Button Y/A
  if(joy->buttons.at(3)) {
      joint[2] += jointDelta[2];
      ROS_INFO("Joint2: %f", joint[2]);
  }
  if(joy->buttons.at(0)) {
      joint[2] -= jointDelta[2];
      ROS_INFO("Joint2: %f", joint[2]);
  }

  //Joint 3 Button Y/A
  if(joy->buttons.at(2)) {
      joint[3] += jointDelta[3];
      ROS_INFO("Joint3: %f", joint[3]);
  }
  if(joy->buttons.at(1)) {
      joint[3] -= jointDelta[3];
      ROS_INFO("Joint3: %f", joint[3]);
  }

  //Joint 4 LT/RT
  if(joy->axes.at(2) < 0) {
      joint[4] += jointDelta[4];
      ROS_INFO("Joint4: %f", joint[4]);
  }
  if(joy->axes.at(5) < 0) {
      joint[4] -= jointDelta[4];
      ROS_INFO("Joint4: %f", joint[4]);
  }

  //Gripper LB(open)/RB(close)
  if(joy->buttons.at(4)) {
      gripperl = gripperMax;
      gripperr = gripperMax;
      ROS_INFO("Gripper Open");
  }
  if(joy->buttons.at(5)) {
      gripperl = gripperMin;
      gripperr = gripperMin;
      ROS_INFO("Gripper Close");
  }

  //for(int i = 0; i < numberOfArmJoints; i++)
  for(int i = 0; i < 5; i++)
  {
      jointName.str("");
      jointName << "arm_joint_" << (i + 1);

      if(joint[i] < jointMin[i])
          joint[i] = jointMin[i];
      if(joint[i] > jointMax[i])
          joint[i] = jointMax[i];

      armJointPositions[i].joint_uri = jointName.str();
      armJointPositions[i].value = joint[i];

      armJointPositions[i].unit = boost::units::to_string(boost::units::si::radians);
  }

  if(gripperl < gripperMin)
      gripperl = gripperMin;
  if(gripperr < gripperMin)
      gripperr = gripperMin;
  if(gripperl > gripperMax)
      gripperl = gripperMax;
  if(gripperr > gripperMax)
      gripperr = gripperMax;

  gripperJointPositions[0].joint_uri = "gripper_finger_joint_l";
  gripperJointPositions[0].value = gripperl;
  gripperJointPositions[0].unit = boost::units::to_string(boost::units::si::meter);

  gripperJointPositions[1].joint_uri = "gripper_finger_joint_r";
  gripperJointPositions[1].value = gripperr;
  gripperJointPositions[1].unit = boost::units::to_string(boost::units::si::meter);

  command.positions = armJointPositions;
  armPositionsPublisher.publish(command);

  command.positions = gripperJointPositions;
  gripperPositionPublisher.publish(command);

  //for(int i = 0; i <numberOfArmJoints; i++)
  for(int i = 0; i <5; i++)
  {
      lastJoint[i] = joint[i];
  }
}

void youbot_joy_teleop::joy_check()
{
    if( (receivedmsg) && ( (ros::Time::now().toSec() - T.toSec() ) > .15) )
    {
        geometry_msgs::Twist zero;
        //cmd_vel.publish(zero);
    }
}

int main(int argc, char **argv)
{
  // initialize ROS and the node
  ros::init(argc, argv, "youbot_joy_arm");

  // initialize the joystick controller
  youbot_joy_teleop controller;

  std::fill_n(joint, 5, 0);
  gripperl = 0;
  gripperr = 0;

  ros::Rate rate(10); //Hz
  static const int numberOfArmJoints = 5;
  static const int numberOfGripperJoints = 2;

  for(int i = 0; i < numberOfArmJoints; i++)
  {
      jointDelta[i] = (jointMax[i] - jointMin[i]) * 0.02;
  }

  ros::spinOnce();

  for(int i = 0; i < numberOfArmJoints; i++)
  {
      lastJoint[i] = joint[i];
  }

  // continue until a ctrl-c has occurred
  while(ros::ok())
  {
    ros::spinOnce();

    controller.joy_check();

    rate.sleep();
  }
  //ros::spin();
  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2012 by Glenn Hickey (hickey@soe.ucsc.edu)
 *
 * Released under the MIT license, see LICENSE.txt
 */

#include <cstdlib>
#include <iostream>
#include "halStats.h"

using namespace std;
using namespace hal;

static void printGenomes(ostream& os, AlignmentConstPtr alignment);
static void printSequences(ostream& os, AlignmentConstPtr alignment,
                          const string& genomeName);
static void printSequenceStats(ostream& os, AlignmentConstPtr alignment,
                               const string& genomeName);
static void printBranchPath(ostream& os, AlignmentConstPtr alignment,
                            const vector<string>& genomeNames);
static void printBranches(ostream& os, AlignmentConstPtr alignment);
static void printChildren(ostream& os, AlignmentConstPtr alignment,
                          const string& genomeName);
static void printParent(ostream& os, AlignmentConstPtr alignment,
                        const string& genomeName);
static void printRootName(ostream& os, AlignmentConstPtr alignment);

int main(int argc, char** argv)
{
  CLParserPtr optionsParser = hdf5CLParserInstance();
  optionsParser->setDescription("Rertrieve basic statics from a hal database");
  optionsParser->addArgument("halFile", "path to hal file to analyze");
  optionsParser->addOptionFlag("genomes", "print only a list of genomes "
                               "in alignment", false);
  optionsParser->addOption("sequences", "print list of sequences in given "
                           "genome", "\"\"");
  optionsParser->addOption("sequenceStats", "print stats for each sequence in "
                           "given genome", "\"\"");
  optionsParser->addOptionFlag("tree", "print only the NEWICK tree", false);
  optionsParser->addOptionFlag("branches", "print list of branches. "
                               "Each branch is specified by the child genome",
                               false);
  optionsParser->addOption("path", "print branches on path between comma "
                           "separated list of genomes", "\"\"");
  optionsParser->addOption("children", "print names of children of given "
                           "genome", "\"\"");
  optionsParser->addOptionFlag("root", "print root genome name", false);
  optionsParser->addOption("parent", "print name of parent of given genome",
                           "\"\"");

  string path;
  bool listGenomes;
  string sequencesFromGenome;
  string sequenceStatsFromGenome;
  string pathGenomes;
  bool tree;
  bool branches;
  string childrenFromGenome;
  string parentFromGenome;
  bool printRoot;
  try
  {
    optionsParser->parseOptions(argc, argv);
    path = optionsParser->getArgument<string>("halFile");
    listGenomes = optionsParser->getFlag("genomes");
    sequencesFromGenome = optionsParser->getOption<string>("sequences");
    sequenceStatsFromGenome = optionsParser->getOption<string>("sequenceStats");
    tree = optionsParser->getFlag("tree");
    pathGenomes = optionsParser->getOption<string>("path");
    branches = optionsParser->getFlag("branches");
    childrenFromGenome = optionsParser->getOption<string>("children");
    parentFromGenome = optionsParser->getOption<string>("parent");
    printRoot = optionsParser->getFlag("root");

    size_t optCount = listGenomes == true ? 1 : 0;
    if (sequencesFromGenome != "\"\"") ++optCount;
    if (tree == true) ++optCount;
    if (sequenceStatsFromGenome != "\"\"") ++optCount;
    if (pathGenomes != "\"\"") ++optCount;
    if (branches) ++ optCount;
    if (childrenFromGenome != "\"\"") ++optCount;
    if (parentFromGenome != "\"\"") ++optCount;
    if (printRoot) ++optCount;
    if (optCount > 1)
    {
      throw hal_exception("--genomes, --sequences, --tree, --path, --branches, "
                          "--sequenceStats, --children, --parent and --root "
                          "options are mutually exclusive");
    }
  }
  catch(exception& e)
  {
    cerr << e.what() << endl;
    optionsParser->printUsage(cerr);
    exit(1);
  }
  try
  {
    AlignmentConstPtr alignment = openHalAlignmentReadOnly(path, optionsParser);

    if (listGenomes == true && alignment->getNumGenomes() > 0)
    {
      printGenomes(cout, alignment);
    }
    else if (sequencesFromGenome != "\"\"")
    {
      printSequences(cout, alignment, sequencesFromGenome);
    }
    else if (tree == true)
    {
      cout << alignment->getNewickTree() << endl;
    }
    else if (sequenceStatsFromGenome != "\"\"")
    {
      printSequenceStats(cout, alignment, sequenceStatsFromGenome);
    }
    else if (pathGenomes !=  "\"\"")
    {
      printBranchPath(cout, alignment, chopString(pathGenomes, ","));
    }
    else if (branches == true)
    {
      printBranches(cout, alignment);
    }
    else if (childrenFromGenome != "\"\"")
    {
      printChildren(cout, alignment, childrenFromGenome);
    }
    else if (parentFromGenome != "\"\"")
    {
      printParent(cout, alignment, parentFromGenome);
    }
    else if (printRoot == true)
    {
      printRootName(cout, alignment);
    }
    else
    {
      HalStats halStats(alignment);
      cout << endl << "hal v" << alignment->getVersion() << "\n" << halStats;
    }
  }
  catch(hal_exception& e)
  {
    cerr << "hal exception caught: " << e.what() << endl;
    return 1;
  }
  catch(exception& e)
  {
    cerr << "Exception caught: " << e.what() << endl;
    return 1;
  }

  return 0;
}

void printGenomes(ostream& os, AlignmentConstPtr alignment)
{
  const Genome* root = alignment->openGenome(alignment->getRootName());
  set<const Genome*> genomes;
  getGenomesInSubTree(root, genomes);
  genomes.insert(root);
  for (set<const Genome*>::iterator i = genomes.begin(); i != genomes.end();
       ++i)
  {
    if (i != genomes.begin())
    {
      os << ",";
    }
    os << (*i)->getName();
  }
  os << endl;
}

void printSequences(ostream& os, AlignmentConstPtr alignment,
                   const string& genomeName)
{
  const Genome* genome = alignment->openGenome(genomeName);
  if (genome == NULL)
  {
    throw hal_exception(string("Genome ") + genomeName + " not found.");
  }
  if (genome->getNumSequences() > 0)
  {
    SequenceIteratorConstPtr seqIt = genome->getSequenceIterator();
    SequenceIteratorConstPtr seqEnd = genome->getSequenceEndIterator();
    for (; !seqIt->equals(seqEnd); seqIt->toNext())
    {
      if (!seqIt->equals(genome->getSequenceIterator()))
      {
        os << ",";
      }
      os << seqIt->getSequence()->getName();
    }
  }
  os << endl;
}

void printSequenceStats(ostream& os, AlignmentConstPtr alignment,
                        const string& genomeName)
{
  const Genome* genome = alignment->openGenome(genomeName);
  if (genome == NULL)
  {
    throw hal_exception(string("Genome ") + genomeName + " not found.");
  }
  if (genome->getNumSequences() > 0)
  {
    SequenceIteratorConstPtr seqIt = genome->getSequenceIterator();
    SequenceIteratorConstPtr seqEnd = genome->getSequenceEndIterator();
    os << "SequenceName, Length, NumTopSegments, NumBottomSegments" << endl;

    for (; !seqIt->equals(seqEnd); seqIt->toNext())
    {
      os << seqIt->getSequence()->getName() << ", "
         << seqIt->getSequence()->getSequenceLength() << ", "
         << seqIt->getSequence()->getNumTopSegments() << ", "
         << seqIt->getSequence()->getNumBottomSegments() << "\n";
    }
  }
  os << endl;
}

static void printBranchPath(ostream& os, AlignmentConstPtr alignment,
                            const vector<string>& genomeNames)
{
  set<const Genome*> inputSet;
  for (size_t i = 0; i < genomeNames.size(); ++i)
  {
    const Genome* genome = alignment->openGenome(genomeNames[i]);
    if (genome == NULL)
    {
      throw hal_exception(string("Genome ") + genomeNames[i] + " not found");
    }
    inputSet.insert(genome);
  }
  set<const Genome*> outputSet;
  getGenomesInSpanningTree(inputSet, outputSet);

  for (set<const Genome*>::const_iterator j = outputSet.begin();
       j != outputSet.end(); ++j)
  {
    const Genome* genome = *j;
    if (genome->getParent() != NULL &&
        outputSet.find(genome->getParent()) != outputSet.end())
    {
      os << genome->getName() << " ";
    }
  }
  os << endl;
}

static void printBranches(ostream& os, AlignmentConstPtr alignment)
{
  const Genome* root = alignment->openGenome(alignment->getRootName());
  set<const Genome*> genomes;
  getGenomesInSubTree(root, genomes);
  genomes.insert(root);
  bool first = true;
  for (set<const Genome*>::iterator i = genomes.begin(); i != genomes.end();
       ++i)
  {
    if ((*i)->getParent() != NULL)
    {
      if (!first)
      {
        os << " ";
      }
      else
      {
        first = false;
      }
      os << (*i)->getName();
    }
  }
  os << endl;
}

void printChildren(ostream& os, AlignmentConstPtr alignment,
                   const string& genomeName)
{
  vector<string> children = alignment->getChildNames(genomeName);
  for (size_t i = 0; i < children.size(); ++i)
  {
    os << children[i];
    if (i != children.size() - 1)
    {
      os << " ";
    }
  }
  os << endl;
}

void printParent(ostream& os, AlignmentConstPtr alignment,
                        const string& genomeName)
{
  if (genomeName != alignment->getRootName())
  {
    os << alignment->getParentName(genomeName) << endl;
  }
}

void printRootName(ostream& os, AlignmentConstPtr alignment)
{
  os << alignment->getRootName() << endl;
}
<commit_msg>option for halstats to make a bed file<commit_after>/*
 * Copyright (C) 2012 by Glenn Hickey (hickey@soe.ucsc.edu)
 *
 * Released under the MIT license, see LICENSE.txt
 */

#include <cstdlib>
#include <iostream>
#include "halStats.h"

using namespace std;
using namespace hal;

static void printGenomes(ostream& os, AlignmentConstPtr alignment);
static void printSequences(ostream& os, AlignmentConstPtr alignment,
                          const string& genomeName);
static void printSequenceStats(ostream& os, AlignmentConstPtr alignment,
                               const string& genomeName);
static void printBedSequenceStats(ostream& os, AlignmentConstPtr alignment,
                                  const string& genomeName);
static void printBranchPath(ostream& os, AlignmentConstPtr alignment,
                            const vector<string>& genomeNames);
static void printBranches(ostream& os, AlignmentConstPtr alignment);
static void printChildren(ostream& os, AlignmentConstPtr alignment,
                          const string& genomeName);
static void printParent(ostream& os, AlignmentConstPtr alignment,
                        const string& genomeName);
static void printRootName(ostream& os, AlignmentConstPtr alignment);

int main(int argc, char** argv)
{
  CLParserPtr optionsParser = hdf5CLParserInstance();
  optionsParser->setDescription("Rertrieve basic statics from a hal database");
  optionsParser->addArgument("halFile", "path to hal file to analyze");
  optionsParser->addOptionFlag("genomes", "print only a list of genomes "
                               "in alignment", false);
  optionsParser->addOption("sequences", "print list of sequences in given "
                           "genome", "\"\"");
  optionsParser->addOption("sequenceStats", "print stats for each sequence in "
                           "given genome", "\"\"");
  optionsParser->addOption("bedSequences", "print sequences of given genome "
                           "in bed format",
                           "\"\"");
  optionsParser->addOptionFlag("tree", "print only the NEWICK tree", false);
  optionsParser->addOptionFlag("branches", "print list of branches. "
                               "Each branch is specified by the child genome",
                               false);
  optionsParser->addOption("path", "print branches on path between comma "
                           "separated list of genomes", "\"\"");
  optionsParser->addOption("children", "print names of children of given "
                           "genome", "\"\"");
  optionsParser->addOptionFlag("root", "print root genome name", false);
  optionsParser->addOption("parent", "print name of parent of given genome",
                           "\"\"");

  string path;
  bool listGenomes;
  string sequencesFromGenome;
  string sequenceStatsFromGenome;
  string bedSequencesFromGenome;
  string pathGenomes;
  bool tree;
  bool branches;
  string childrenFromGenome;
  string parentFromGenome;
  bool printRoot;
  try
  {
    optionsParser->parseOptions(argc, argv);
    path = optionsParser->getArgument<string>("halFile");
    listGenomes = optionsParser->getFlag("genomes");
    sequencesFromGenome = optionsParser->getOption<string>("sequences");
    sequenceStatsFromGenome = optionsParser->getOption<string>("sequenceStats");
    bedSequencesFromGenome = optionsParser->getOption<string>("bedSequences");
    tree = optionsParser->getFlag("tree");
    pathGenomes = optionsParser->getOption<string>("path");
    branches = optionsParser->getFlag("branches");
    childrenFromGenome = optionsParser->getOption<string>("children");
    parentFromGenome = optionsParser->getOption<string>("parent");
    printRoot = optionsParser->getFlag("root");

    size_t optCount = listGenomes == true ? 1 : 0;
    if (sequencesFromGenome != "\"\"") ++optCount;
    if (tree == true) ++optCount;
    if (sequenceStatsFromGenome != "\"\"") ++optCount;
    if (bedSequencesFromGenome != "\"\"") ++optCount;
    if (pathGenomes != "\"\"") ++optCount;
    if (branches) ++ optCount;
    if (childrenFromGenome != "\"\"") ++optCount;
    if (parentFromGenome != "\"\"") ++optCount;
    if (printRoot) ++optCount;
    if (optCount > 1)
    {
      throw hal_exception("--genomes, --sequences, --tree, --path, --branches, "
                          "--sequenceStats, --children, --parent, "
                          "--bedSequences abd --root "
                          "options are mutually exclusive");
    }
  }
  catch(exception& e)
  {
    cerr << e.what() << endl;
    optionsParser->printUsage(cerr);
    exit(1);
  }
  try
  {
    AlignmentConstPtr alignment = openHalAlignmentReadOnly(path, optionsParser);

    if (listGenomes == true && alignment->getNumGenomes() > 0)
    {
      printGenomes(cout, alignment);
    }
    else if (sequencesFromGenome != "\"\"")
    {
      printSequences(cout, alignment, sequencesFromGenome);
    }
    else if (tree == true)
    {
      cout << alignment->getNewickTree() << endl;
    }
    else if (sequenceStatsFromGenome != "\"\"")
    {
      printSequenceStats(cout, alignment, sequenceStatsFromGenome);
    }
    else if (bedSequencesFromGenome != "\"\"")
    {
      printBedSequenceStats(cout, alignment, bedSequencesFromGenome);
    }
    else if (pathGenomes !=  "\"\"")
    {
      printBranchPath(cout, alignment, chopString(pathGenomes, ","));
    }
    else if (branches == true)
    {
      printBranches(cout, alignment);
    }
    else if (childrenFromGenome != "\"\"")
    {
      printChildren(cout, alignment, childrenFromGenome);
    }
    else if (parentFromGenome != "\"\"")
    {
      printParent(cout, alignment, parentFromGenome);
    }
    else if (printRoot == true)
    {
      printRootName(cout, alignment);
    }
    else
    {
      HalStats halStats(alignment);
      cout << endl << "hal v" << alignment->getVersion() << "\n" << halStats;
    }
  }
  catch(hal_exception& e)
  {
    cerr << "hal exception caught: " << e.what() << endl;
    return 1;
  }
  catch(exception& e)
  {
    cerr << "Exception caught: " << e.what() << endl;
    return 1;
  }

  return 0;
}

void printGenomes(ostream& os, AlignmentConstPtr alignment)
{
  const Genome* root = alignment->openGenome(alignment->getRootName());
  set<const Genome*> genomes;
  getGenomesInSubTree(root, genomes);
  genomes.insert(root);
  for (set<const Genome*>::iterator i = genomes.begin(); i != genomes.end();
       ++i)
  {
    if (i != genomes.begin())
    {
      os << ",";
    }
    os << (*i)->getName();
  }
  os << endl;
}

void printSequences(ostream& os, AlignmentConstPtr alignment,
                   const string& genomeName)
{
  const Genome* genome = alignment->openGenome(genomeName);
  if (genome == NULL)
  {
    throw hal_exception(string("Genome ") + genomeName + " not found.");
  }
  if (genome->getNumSequences() > 0)
  {
    SequenceIteratorConstPtr seqIt = genome->getSequenceIterator();
    SequenceIteratorConstPtr seqEnd = genome->getSequenceEndIterator();
    for (; !seqIt->equals(seqEnd); seqIt->toNext())
    {
      if (!seqIt->equals(genome->getSequenceIterator()))
      {
        os << ",";
      }
      os << seqIt->getSequence()->getName();
    }
  }
  os << endl;
}

void printSequenceStats(ostream& os, AlignmentConstPtr alignment,
                        const string& genomeName)
{
  const Genome* genome = alignment->openGenome(genomeName);
  if (genome == NULL)
  {
    throw hal_exception(string("Genome ") + genomeName + " not found.");
  }
  if (genome->getNumSequences() > 0)
  {
    SequenceIteratorConstPtr seqIt = genome->getSequenceIterator();
    SequenceIteratorConstPtr seqEnd = genome->getSequenceEndIterator();
    os << "SequenceName, Length, NumTopSegments, NumBottomSegments" << endl;

    for (; !seqIt->equals(seqEnd); seqIt->toNext())
    {
      os << seqIt->getSequence()->getName() << ", "
         << seqIt->getSequence()->getSequenceLength() << ", "
         << seqIt->getSequence()->getNumTopSegments() << ", "
         << seqIt->getSequence()->getNumBottomSegments() << "\n";
    }
  }
  os << endl;
}

void printBedSequenceStats(ostream& os, AlignmentConstPtr alignment,
                           const string& genomeName)
{
  const Genome* genome = alignment->openGenome(genomeName);
  if (genome == NULL)
  {
    throw hal_exception(string("Genome ") + genomeName + " not found.");
  }
  if (genome->getNumSequences() > 0)
  {
    SequenceIteratorConstPtr seqIt = genome->getSequenceIterator();
    SequenceIteratorConstPtr seqEnd = genome->getSequenceEndIterator();

    for (; !seqIt->equals(seqEnd); seqIt->toNext())
    {
      os << seqIt->getSequence()->getName() << "\t"
         << 0 << "\t"
         << seqIt->getSequence()->getSequenceLength() << "\n";
    }
  }
  os << endl;
}

static void printBranchPath(ostream& os, AlignmentConstPtr alignment,
                            const vector<string>& genomeNames)
{
  set<const Genome*> inputSet;
  for (size_t i = 0; i < genomeNames.size(); ++i)
  {
    const Genome* genome = alignment->openGenome(genomeNames[i]);
    if (genome == NULL)
    {
      throw hal_exception(string("Genome ") + genomeNames[i] + " not found");
    }
    inputSet.insert(genome);
  }
  set<const Genome*> outputSet;
  getGenomesInSpanningTree(inputSet, outputSet);

  for (set<const Genome*>::const_iterator j = outputSet.begin();
       j != outputSet.end(); ++j)
  {
    const Genome* genome = *j;
    if (genome->getParent() != NULL &&
        outputSet.find(genome->getParent()) != outputSet.end())
    {
      os << genome->getName() << " ";
    }
  }
  os << endl;
}

static void printBranches(ostream& os, AlignmentConstPtr alignment)
{
  const Genome* root = alignment->openGenome(alignment->getRootName());
  set<const Genome*> genomes;
  getGenomesInSubTree(root, genomes);
  genomes.insert(root);
  bool first = true;
  for (set<const Genome*>::iterator i = genomes.begin(); i != genomes.end();
       ++i)
  {
    if ((*i)->getParent() != NULL)
    {
      if (!first)
      {
        os << " ";
      }
      else
      {
        first = false;
      }
      os << (*i)->getName();
    }
  }
  os << endl;
}

void printChildren(ostream& os, AlignmentConstPtr alignment,
                   const string& genomeName)
{
  vector<string> children = alignment->getChildNames(genomeName);
  for (size_t i = 0; i < children.size(); ++i)
  {
    os << children[i];
    if (i != children.size() - 1)
    {
      os << " ";
    }
  }
  os << endl;
}

void printParent(ostream& os, AlignmentConstPtr alignment,
                        const string& genomeName)
{
  if (genomeName != alignment->getRootName())
  {
    os << alignment->getParentName(genomeName) << endl;
  }
}

void printRootName(ostream& os, AlignmentConstPtr alignment)
{
  os << alignment->getRootName() << endl;
}
<|endoftext|>


<commit_before>// //////////////////////////////////////////////////////////////////////
// Import section
// //////////////////////////////////////////////////////////////////////
// STL
#include <cassert>
#include <sstream>
// StdAir
#include <stdair/basic/BasConst_General.hpp>
#include <stdair/basic/BasConst_Inventory.hpp>
#include <stdair/basic/RandomGeneration.hpp>
#include <stdair/bom/BookingClass.hpp>

namespace stdair {

  // ////////////////////////////////////////////////////////////////////
  BookingClass::BookingClass() : _key (DEFAULT_CLASS_CODE), _parent (NULL) {
    assert (false);
  }

  // ////////////////////////////////////////////////////////////////////
  BookingClass::BookingClass (const BookingClass& iBookingClass)
    : _key (iBookingClass._key),
      _parent (NULL),
      _subclassCode (iBookingClass._subclassCode),
      _cumulatedProtection (iBookingClass._cumulatedProtection),
      _protection (iBookingClass._protection),
      _cumulatedBookingLimit (iBookingClass._cumulatedBookingLimit),
      _au (iBookingClass._au),
      _nego (iBookingClass._nego),
      _noShowPercentage (iBookingClass._noShowPercentage),
      _cancellationPercentage (iBookingClass._cancellationPercentage),
      _nbOfBookings (iBookingClass._nbOfBookings),
      _groupNbOfBookings (iBookingClass._groupNbOfBookings),
      _groupPendingNbOfBookings (iBookingClass._groupPendingNbOfBookings),
      _staffNbOfBookings (iBookingClass._staffNbOfBookings),
      _wlNbOfBookings (iBookingClass._wlNbOfBookings),
      _nbOfCancellations (iBookingClass._nbOfCancellations),
      _etb (iBookingClass._etb),
      _netClassAvailability (iBookingClass._netClassAvailability),
      _segmentAvailability (iBookingClass._segmentAvailability),
      _netRevenueAvailability (iBookingClass._netRevenueAvailability),
      _yield (iBookingClass._yield),
      _adjustedYield (iBookingClass._adjustedYield),
      _mean (iBookingClass._mean),
      _stdDev (iBookingClass._stdDev) {
  }

  // ////////////////////////////////////////////////////////////////////
  BookingClass::BookingClass (const Key_T& iKey)
    : _key (iKey), _parent (NULL), _subclassCode(0), _cumulatedProtection (0.0),
      _protection (0.0), _cumulatedBookingLimit (0.0), _au (0.0), _nego (0.0),
      _noShowPercentage (0.0), _cancellationPercentage (0.0),
      _nbOfBookings (0.0), _groupNbOfBookings (0.0),
      _groupPendingNbOfBookings (0.0), _staffNbOfBookings (0.0),
      _wlNbOfBookings (0.0), _nbOfCancellations (0.), _etb (0.0),
      _netClassAvailability (0.0), _segmentAvailability (0.0),
      _netRevenueAvailability (0.0), _yield (0.0), _mean (0.0), _stdDev (0.0) {
  }

  // ////////////////////////////////////////////////////////////////////
  BookingClass::~BookingClass() {
  }

  // ////////////////////////////////////////////////////////////////////
  std::string BookingClass::toString() const {
    std::ostringstream oStr;
    oStr << describeKey();
    return oStr.str();
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::sell (const NbOfBookings_T& iNbOfBookings) {
    _nbOfBookings += iNbOfBookings;
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::cancel (const NbOfBookings_T& iNbOfCancellations) {
    _nbOfBookings -= iNbOfCancellations;
    _nbOfCancellations += iNbOfCancellations;
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::generateDemandSamples (const NbOfSamples_T& K) {
    _generatedDemandVector.clear();
    if (_stdDev > 0) {
      RandomGeneration lGenerator (DEFAULT_RANDOM_SEED);
      for (unsigned int i = 0; i < K; ++i) {
        RealNumber_T lDemandSample = lGenerator.generateNormal (_mean, _stdDev);
        _generatedDemandVector.push_back (lDemandSample);
      }
    }
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::generateDemandSamples (const NbOfSamples_T& K,
                                            const RandomSeed_T& iSeed) {
    _generatedDemandVector.clear();
    if (_stdDev > 0) {
      RandomGeneration lGenerator (iSeed);
      for (unsigned int i = 0; i < K; ++i) {
        RealNumber_T lDemandSample = lGenerator.generateNormal (_mean, _stdDev);
        _generatedDemandVector.push_back (lDemandSample);
      }
    }
  }

}

<commit_msg>Factorized the generateDemandSamples() source code.<commit_after>// //////////////////////////////////////////////////////////////////////
// Import section
// //////////////////////////////////////////////////////////////////////
// STL
#include <cassert>
#include <sstream>
// StdAir
#include <stdair/basic/BasConst_General.hpp>
#include <stdair/basic/BasConst_Inventory.hpp>
#include <stdair/basic/RandomGeneration.hpp>
#include <stdair/bom/BookingClass.hpp>

namespace stdair {

  // ////////////////////////////////////////////////////////////////////
  BookingClass::BookingClass() : _key (DEFAULT_CLASS_CODE), _parent (NULL) {
    assert (false);
  }

  // ////////////////////////////////////////////////////////////////////
  BookingClass::BookingClass (const BookingClass& iBookingClass)
    : _key (iBookingClass._key),
      _parent (NULL),
      _subclassCode (iBookingClass._subclassCode),
      _cumulatedProtection (iBookingClass._cumulatedProtection),
      _protection (iBookingClass._protection),
      _cumulatedBookingLimit (iBookingClass._cumulatedBookingLimit),
      _au (iBookingClass._au),
      _nego (iBookingClass._nego),
      _noShowPercentage (iBookingClass._noShowPercentage),
      _cancellationPercentage (iBookingClass._cancellationPercentage),
      _nbOfBookings (iBookingClass._nbOfBookings),
      _groupNbOfBookings (iBookingClass._groupNbOfBookings),
      _groupPendingNbOfBookings (iBookingClass._groupPendingNbOfBookings),
      _staffNbOfBookings (iBookingClass._staffNbOfBookings),
      _wlNbOfBookings (iBookingClass._wlNbOfBookings),
      _nbOfCancellations (iBookingClass._nbOfCancellations),
      _etb (iBookingClass._etb),
      _netClassAvailability (iBookingClass._netClassAvailability),
      _segmentAvailability (iBookingClass._segmentAvailability),
      _netRevenueAvailability (iBookingClass._netRevenueAvailability),
      _yield (iBookingClass._yield),
      _adjustedYield (iBookingClass._adjustedYield),
      _mean (iBookingClass._mean),
      _stdDev (iBookingClass._stdDev) {
  }

  // ////////////////////////////////////////////////////////////////////
  BookingClass::BookingClass (const Key_T& iKey)
    : _key (iKey), _parent (NULL), _subclassCode(0), _cumulatedProtection (0.0),
      _protection (0.0), _cumulatedBookingLimit (0.0), _au (0.0), _nego (0.0),
      _noShowPercentage (0.0), _cancellationPercentage (0.0),
      _nbOfBookings (0.0), _groupNbOfBookings (0.0),
      _groupPendingNbOfBookings (0.0), _staffNbOfBookings (0.0),
      _wlNbOfBookings (0.0), _nbOfCancellations (0.), _etb (0.0),
      _netClassAvailability (0.0), _segmentAvailability (0.0),
      _netRevenueAvailability (0.0), _yield (0.0), _mean (0.0), _stdDev (0.0) {
  }

  // ////////////////////////////////////////////////////////////////////
  BookingClass::~BookingClass() {
  }

  // ////////////////////////////////////////////////////////////////////
  std::string BookingClass::toString() const {
    std::ostringstream oStr;
    oStr << describeKey();
    return oStr.str();
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::sell (const NbOfBookings_T& iNbOfBookings) {
    _nbOfBookings += iNbOfBookings;
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::cancel (const NbOfBookings_T& iNbOfCancellations) {
    _nbOfBookings -= iNbOfCancellations;
    _nbOfCancellations += iNbOfCancellations;
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::generateDemandSamples (const NbOfSamples_T& K) {
    generateDemandSamples (K, DEFAULT_RANDOM_SEED);
  }

  // ////////////////////////////////////////////////////////////////////
  void BookingClass::generateDemandSamples (const NbOfSamples_T& K,
                                            const RandomSeed_T& iSeed) {
    _generatedDemandVector.clear();
    if (_stdDev > 0) {
      RandomGeneration lGenerator (iSeed);
      for (unsigned int i = 0; i < K; ++i) {
        const RealNumber_T& lDemandSample = lGenerator.generateNormal (_mean,
                                                                       _stdDev);
        _generatedDemandVector.push_back (lDemandSample);
      }
    }
  }

}

<|endoftext|>


<commit_before>#include "tangram.h"

const GLfloat vertices[] = {
	0.0f, 1.0f,
	-1.0f, 0.0f,
	1.0f, 0.0f
};

ShaderProgram simpleShader;
GLuint vbo;
ViewModule *view = new ViewModule();
float t;

const std::string vertShaderSrc =
    "#ifdef GL_ES\n"
    "precision mediump float;\n"
    "#endif\n"
    "attribute vec4 a_position;\n"
	"void main() {\n"
	"  gl_Position = a_position;\n"
	"}\n";

const std::string fragShaderSrc =
	"#ifdef GL_ES\n"
	"precision mediump float;\n"
	"#endif\n"
	"void main(void) {\n"
	"  gl_FragColor = vec4(93.0/255.0, 141.0/255.0, 148.0/255.0, 1.0);\n"
	"}\n";

void initializeOpenGL()
{

	// Make a VBO
	glGenBuffers(1, &vbo);
	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

	// Make a shader program
	simpleShader.buildFromSourceStrings(fragShaderSrc, vertShaderSrc);

	t = 0;

	logMsg("%s\n", "initialize");

}

void resizeViewport(int newWidth, int newHeight)
{
	glViewport(0, 0, newWidth, newHeight);

	view->setAspect(newWidth, newHeight);

	logMsg("%s\n", "resizeViewport");
}

void renderFrame()
{

	// Draw a triangle!
	t += 0.016;
	float sintsqr = pow(sin(t), 2);
	glClearColor(0.8f * sintsqr, 0.32f * sintsqr, 0.3f * sintsqr, 1.0f);
	glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
	simpleShader.use();
	GLint posAttrib = simpleShader.getAttribLocation("a_position");
	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	glVertexAttribPointer(posAttrib, 2, GL_FLOAT, GL_FALSE, 0, 0);
	glEnableVertexAttribArray(posAttrib);

	glDrawArrays(GL_TRIANGLES, 0, 3);

	GLenum glError = glGetError();
	if (glError) {
		logMsg("%s\n", "glError!!");
	}

}
<commit_msg>retabbing<commit_after>#include "tangram.h"

const GLfloat vertices[] = {
    0.0f, 1.0f,
    -1.0f, 0.0f,
    1.0f, 0.0f
};

ShaderProgram simpleShader;
GLuint vbo;
ViewModule *view = new ViewModule();
float t;

const std::string vertShaderSrc =
    "#ifdef GL_ES\n"
    "precision mediump float;\n"
    "#endif\n"
    "attribute vec4 a_position;\n"
    "void main() {\n"
    "  gl_Position = a_position;\n"
    "}\n";

const std::string fragShaderSrc =
    "#ifdef GL_ES\n"
    "precision mediump float;\n"
    "#endif\n"
    "void main(void) {\n"
    "  gl_FragColor = vec4(93.0/255.0, 141.0/255.0, 148.0/255.0, 1.0);\n"
    "}\n";

void initializeOpenGL()
{

    // Make a VBO
    glGenBuffers(1, &vbo);
    glBindBuffer(GL_ARRAY_BUFFER, vbo);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    // Make a shader program
    simpleShader.buildFromSourceStrings(fragShaderSrc, vertShaderSrc);

    t = 0;

    logMsg("%s\n", "initialize");

}

void resizeViewport(int newWidth, int newHeight)
{
    glViewport(0, 0, newWidth, newHeight);

    view->setAspect(newWidth, newHeight);

    logMsg("%s\n", "resizeViewport");
}

void renderFrame()
{

    // Draw a triangle!
    t += 0.016;
    float sintsqr = pow(sin(t), 2);
    glClearColor(0.8f * sintsqr, 0.32f * sintsqr, 0.3f * sintsqr, 1.0f);
    glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    simpleShader.use();
    GLint posAttrib = simpleShader.getAttribLocation("a_position");
    glBindBuffer(GL_ARRAY_BUFFER, vbo);
    glVertexAttribPointer(posAttrib, 2, GL_FLOAT, GL_FALSE, 0, 0);
    glEnableVertexAttribArray(posAttrib);

    glDrawArrays(GL_TRIANGLES, 0, 3);

    GLenum glError = glGetError();
    if (glError) {
        logMsg("%s\n", "glError!!");
    }

}
<|endoftext|>


<commit_before>// Partial trace stress test on a density matrix of n qubits
// Reduce the number of NQ_END in run.sh to accomodate for density matrices
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include <omp.h>

#include "qpp.h"

int main(int argc, char **argv) {
    using namespace qpp;
    if (argc != 3) {
        std::cerr << "Please specify the number of cores and qubits!\n";
        exit(EXIT_FAILURE);
    }

    idx num_cores = std::stoi(argv[1]);   // number of cores
    idx n = std::stoi(argv[2]);           // number of qubits
    idx D = std::round(std::pow(2, n));   // dimension
    omp_set_num_threads(num_cores);       // number of cores

    cmat randcmat = cmat::Random(D, D);   // random matrix
    std::vector<idx> subsys_ptrace = {0}; // partial trace over first qubit

    Timer<> t;
    ptrace(randcmat, subsys_ptrace);
    std::cout << num_cores << ", " << n << ", " << t.toc() << '\n';
}
<commit_msg>commit<commit_after>// Partial trace stress test on a density matrix of n qubits
// Reduce the number of NQ_END in run.sh to accommodate for density matrices
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include <omp.h>

#include "qpp.h"

int main(int argc, char **argv) {
    using namespace qpp;
    if (argc != 3) {
        std::cerr << "Please specify the number of cores and qubits!\n";
        exit(EXIT_FAILURE);
    }

    idx num_cores = std::stoi(argv[1]);   // number of cores
    idx n = std::stoi(argv[2]);           // number of qubits
    idx D = std::round(std::pow(2, n));   // dimension
    omp_set_num_threads(num_cores);       // number of cores

    cmat randcmat = cmat::Random(D, D);   // random matrix
    std::vector<idx> subsys_ptrace = {0}; // partial trace over first qubit

    Timer<> t;
    ptrace(randcmat, subsys_ptrace);
    std::cout << num_cores << ", " << n << ", " << t.toc() << '\n';
}
<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// STL A* Search implementation
// (C)2001 Justin Heyes-Jones
//
// Finding a path on a simple grid maze
// This shows how to do shortest path finding using A*

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

#include "stlastar.h" // See header for copyright and usage information

#include <iostream>
#include <stdio.h>

#define DEBUG_LISTS 0
#define DEBUG_LIST_LENGTHS_ONLY 0

using namespace std;

// Global data

// The world map

const int MAP_WIDTH = 20;
const int MAP_HEIGHT = 20;

int world_map[ MAP_WIDTH * MAP_HEIGHT ] =
{

// 0001020304050607080910111213141516171819
	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 00
	1,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,1,   // 01
	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 02
	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 03
	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 04
	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 05
	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 06
	1,9,9,9,9,9,9,9,9,1,1,1,9,9,9,9,9,9,9,1,   // 07
	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 08
	1,9,1,9,9,9,9,9,9,9,1,1,9,9,9,9,9,9,9,1,   // 09
	1,9,1,1,1,1,9,1,1,9,1,1,1,1,1,1,1,1,1,1,   // 10
	1,9,9,9,9,9,1,9,1,9,1,9,9,9,9,9,1,1,1,1,   // 11
	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 12
	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 13
	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 14
	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 15
	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 16
	1,1,9,9,9,9,9,9,9,1,1,1,9,9,9,1,9,9,9,9,   // 17
	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 18
	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 19

};

// map helper functions

int GetMap( int x, int y )
{
	if( x < 0 ||
	    x >= MAP_WIDTH ||
		 y < 0 ||
		 y >= MAP_HEIGHT
	  )
	{
		return 9;
	}

	return world_map[(y*MAP_WIDTH)+x];
}

// Definitions

class MapSearchNode
{
public:
	int x;	 // the (x,y) positions of the node
	int y;

	MapSearchNode() { x = y = 0; }
	MapSearchNode( int px, int py ) { x=px; y=py; }

	float GoalDistanceEstimate( MapSearchNode &nodeGoal );
	bool IsGoal( MapSearchNode &nodeGoal );
	bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
	float GetCost( MapSearchNode &successor );
	bool IsSameState( MapSearchNode &rhs );

	void PrintNodeInfo();

};

bool MapSearchNode::IsSameState( MapSearchNode &rhs )
{

	// same state in a maze search is simply when (x,y) are the same
	if( (x == rhs.x) &&
		(y == rhs.y) )
	{
		return true;
	}
	else
	{
		return false;
	}

}

void MapSearchNode::PrintNodeInfo()
{
	char str[100];
	sprintf( str, "Node position : (%d,%d)\n", x,y );

	cout << str;
}

// Here's the heuristic function that estimates the distance from a Node
// to the Goal.

float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
{
	float xd = float( ( (float)x - (float)nodeGoal.x ) );
	float yd = float( ( (float)y - (float)nodeGoal.y) );

	return xd + yd;

}

bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
{

	if( (x == nodeGoal.x) &&
		(y == nodeGoal.y) )
	{
		return true;
	}

	return false;
}

// This generates the successors to the given Node. It uses a helper function called
// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
// is done for each node internally, so here you just set the state information that
// is specific to the application
bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node )
{

	int parent_x = -1;
	int parent_y = -1;

	if( parent_node )
	{
		parent_x = parent_node->x;
		parent_y = parent_node->y;
	}

	MapSearchNode NewNode;

	// push each possible move except allowing the search to go backwards

	if( (GetMap( x-1, y ) < 9)
		&& !((parent_x == x-1) && (parent_y == y))
	  )
	{
		NewNode = MapSearchNode( x-1, y );
		astarsearch->AddSuccessor( NewNode );
	}

	if( (GetMap( x, y-1 ) < 9)
		&& !((parent_x == x) && (parent_y == y-1))
	  )
	{
		NewNode = MapSearchNode( x, y-1 );
		astarsearch->AddSuccessor( NewNode );
	}

	if( (GetMap( x+1, y ) < 9)
		&& !((parent_x == x+1) && (parent_y == y))
	  )
	{
		NewNode = MapSearchNode( x+1, y );
		astarsearch->AddSuccessor( NewNode );
	}

	if( (GetMap( x, y+1 ) < 9)
		&& !((parent_x == x) && (parent_y == y+1))
		)
	{
		NewNode = MapSearchNode( x, y+1 );
		astarsearch->AddSuccessor( NewNode );
	}

	return true;
}

// given this node, what does it cost to move to successor. In the case
// of our map the answer is the map terrain value at this node since that is
// conceptually where we're moving

float MapSearchNode::GetCost( MapSearchNode &successor )
{
	return (float) GetMap( x, y );

}

// Main

int main( int argc, char *argv[] )
{

	cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";

	// Our sample problem defines the world as a 2d array representing a terrain
	// Each element contains an integer from 0 to 5 which indicates the cost
	// of travel across the terrain. Zero means the least possible difficulty
	// in travelling (think ice rink if you can skate) whilst 5 represents the
	// most difficult. 9 indicates that we cannot pass.

	// Create an instance of the search class...

	AStarSearch<MapSearchNode> astarsearch;

	unsigned int SearchCount = 0;

	const unsigned int NumSearches = 1;

	while(SearchCount < NumSearches)
	{

		// Create a start state
		MapSearchNode nodeStart;
		nodeStart.x = rand()%MAP_WIDTH;
		nodeStart.y = rand()%MAP_HEIGHT;

		// Define the goal state
		MapSearchNode nodeEnd;
		nodeEnd.x = rand()%MAP_WIDTH;
		nodeEnd.y = rand()%MAP_HEIGHT;

		// Set Start and goal states

		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

		unsigned int SearchState;
		unsigned int SearchSteps = 0;

		do
		{
			SearchState = astarsearch.SearchStep();

			SearchSteps++;

	#if DEBUG_LISTS

			cout << "Steps:" << SearchSteps << "\n";

			int len = 0;

			cout << "Open:\n";
			MapSearchNode *p = astarsearch.GetOpenListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY
				((MapSearchNode *)p)->PrintNodeInfo();
	#endif
				p = astarsearch.GetOpenListNext();

			}

			cout << "Open list has " << len << " nodes\n";

			len = 0;

			cout << "Closed:\n";
			p = astarsearch.GetClosedListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY
				p->PrintNodeInfo();
	#endif
				p = astarsearch.GetClosedListNext();
			}

			cout << "Closed list has " << len << " nodes\n";
	#endif

		}
		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );

		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
		{
			cout << "Search found goal state\n";

				MapSearchNode *node = astarsearch.GetSolutionStart();

	#if DISPLAY_SOLUTION
				cout << "Displaying solution\n";
	#endif
				int steps = 0;

				node->PrintNodeInfo();
				for( ;; )
				{
					node = astarsearch.GetSolutionNext();

					if( !node )
					{
						break;
					}

					node->PrintNodeInfo();
					steps ++;

				};

				cout << "Solution steps " << steps << endl;

				// Once you're done with the solution you can free the nodes up
				astarsearch.FreeSolutionNodes();

		}
		else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED )
		{
			cout << "Search terminated. Did not find goal state\n";

		}

		// Display the number of loops the search went through
		cout << "SearchSteps : " << SearchSteps << "\n";

		SearchCount ++;

		astarsearch.EnsureMemoryFreed();
	}

	return 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
<commit_msg>fix goal distance estimate in findpath<commit_after>////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// STL A* Search implementation
// (C)2001 Justin Heyes-Jones
//
// Finding a path on a simple grid maze
// This shows how to do shortest path finding using A*

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

#include "stlastar.h" // See header for copyright and usage information

#include <iostream>
#include <stdio.h>
#include <math.h>

#define DEBUG_LISTS 0
#define DEBUG_LIST_LENGTHS_ONLY 0

using namespace std;

// Global data

// The world map

const int MAP_WIDTH = 20;
const int MAP_HEIGHT = 20;

int world_map[ MAP_WIDTH * MAP_HEIGHT ] =
{

// 0001020304050607080910111213141516171819
	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 00
	1,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,1,   // 01
	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 02
	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 03
	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 04
	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 05
	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 06
	1,9,9,9,9,9,9,9,9,1,1,1,9,9,9,9,9,9,9,1,   // 07
	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 08
	1,9,1,9,9,9,9,9,9,9,1,1,9,9,9,9,9,9,9,1,   // 09
	1,9,1,1,1,1,9,1,1,9,1,1,1,1,1,1,1,1,1,1,   // 10
	1,9,9,9,9,9,1,9,1,9,1,9,9,9,9,9,1,1,1,1,   // 11
	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 12
	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 13
	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 14
	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 15
	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 16
	1,1,9,9,9,9,9,9,9,1,1,1,9,9,9,1,9,9,9,9,   // 17
	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 18
	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 19

};

// map helper functions

int GetMap( int x, int y )
{
	if( x < 0 ||
	    x >= MAP_WIDTH ||
		 y < 0 ||
		 y >= MAP_HEIGHT
	  )
	{
		return 9;
	}

	return world_map[(y*MAP_WIDTH)+x];
}

// Definitions

class MapSearchNode
{
public:
	int x;	 // the (x,y) positions of the node
	int y;

	MapSearchNode() { x = y = 0; }
	MapSearchNode( int px, int py ) { x=px; y=py; }

	float GoalDistanceEstimate( MapSearchNode &nodeGoal );
	bool IsGoal( MapSearchNode &nodeGoal );
	bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
	float GetCost( MapSearchNode &successor );
	bool IsSameState( MapSearchNode &rhs );

	void PrintNodeInfo();

};

bool MapSearchNode::IsSameState( MapSearchNode &rhs )
{

	// same state in a maze search is simply when (x,y) are the same
	if( (x == rhs.x) &&
		(y == rhs.y) )
	{
		return true;
	}
	else
	{
		return false;
	}

}

void MapSearchNode::PrintNodeInfo()
{
	char str[100];
	sprintf( str, "Node position : (%d,%d)\n", x,y );

	cout << str;
}

// Here's the heuristic function that estimates the distance from a Node
// to the Goal.

float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
{
	return fabsf(x - nodeGoal.x) + fabsf(y - nodeGoal.y);
}

bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
{

	if( (x == nodeGoal.x) &&
		(y == nodeGoal.y) )
	{
		return true;
	}

	return false;
}

// This generates the successors to the given Node. It uses a helper function called
// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
// is done for each node internally, so here you just set the state information that
// is specific to the application
bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node )
{

	int parent_x = -1;
	int parent_y = -1;

	if( parent_node )
	{
		parent_x = parent_node->x;
		parent_y = parent_node->y;
	}

	MapSearchNode NewNode;

	// push each possible move except allowing the search to go backwards

	if( (GetMap( x-1, y ) < 9)
		&& !((parent_x == x-1) && (parent_y == y))
	  )
	{
		NewNode = MapSearchNode( x-1, y );
		astarsearch->AddSuccessor( NewNode );
	}

	if( (GetMap( x, y-1 ) < 9)
		&& !((parent_x == x) && (parent_y == y-1))
	  )
	{
		NewNode = MapSearchNode( x, y-1 );
		astarsearch->AddSuccessor( NewNode );
	}

	if( (GetMap( x+1, y ) < 9)
		&& !((parent_x == x+1) && (parent_y == y))
	  )
	{
		NewNode = MapSearchNode( x+1, y );
		astarsearch->AddSuccessor( NewNode );
	}

	if( (GetMap( x, y+1 ) < 9)
		&& !((parent_x == x) && (parent_y == y+1))
		)
	{
		NewNode = MapSearchNode( x, y+1 );
		astarsearch->AddSuccessor( NewNode );
	}

	return true;
}

// given this node, what does it cost to move to successor. In the case
// of our map the answer is the map terrain value at this node since that is
// conceptually where we're moving

float MapSearchNode::GetCost( MapSearchNode &successor )
{
	return (float) GetMap( x, y );

}

// Main

int main( int argc, char *argv[] )
{

	cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";

	// Our sample problem defines the world as a 2d array representing a terrain
	// Each element contains an integer from 0 to 5 which indicates the cost
	// of travel across the terrain. Zero means the least possible difficulty
	// in travelling (think ice rink if you can skate) whilst 5 represents the
	// most difficult. 9 indicates that we cannot pass.

	// Create an instance of the search class...

	AStarSearch<MapSearchNode> astarsearch;

	unsigned int SearchCount = 0;

	const unsigned int NumSearches = 1;

	while(SearchCount < NumSearches)
	{

		// Create a start state
		MapSearchNode nodeStart;
		nodeStart.x = rand()%MAP_WIDTH;
		nodeStart.y = rand()%MAP_HEIGHT;

		// Define the goal state
		MapSearchNode nodeEnd;
		nodeEnd.x = rand()%MAP_WIDTH;
		nodeEnd.y = rand()%MAP_HEIGHT;

		// Set Start and goal states

		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );

		unsigned int SearchState;
		unsigned int SearchSteps = 0;

		do
		{
			SearchState = astarsearch.SearchStep();

			SearchSteps++;

	#if DEBUG_LISTS

			cout << "Steps:" << SearchSteps << "\n";

			int len = 0;

			cout << "Open:\n";
			MapSearchNode *p = astarsearch.GetOpenListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY
				((MapSearchNode *)p)->PrintNodeInfo();
	#endif
				p = astarsearch.GetOpenListNext();

			}

			cout << "Open list has " << len << " nodes\n";

			len = 0;

			cout << "Closed:\n";
			p = astarsearch.GetClosedListStart();
			while( p )
			{
				len++;
	#if !DEBUG_LIST_LENGTHS_ONLY
				p->PrintNodeInfo();
	#endif
				p = astarsearch.GetClosedListNext();
			}

			cout << "Closed list has " << len << " nodes\n";
	#endif

		}
		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );

		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
		{
			cout << "Search found goal state\n";

				MapSearchNode *node = astarsearch.GetSolutionStart();

	#if DISPLAY_SOLUTION
				cout << "Displaying solution\n";
	#endif
				int steps = 0;

				node->PrintNodeInfo();
				for( ;; )
				{
					node = astarsearch.GetSolutionNext();

					if( !node )
					{
						break;
					}

					node->PrintNodeInfo();
					steps ++;

				};

				cout << "Solution steps " << steps << endl;

				// Once you're done with the solution you can free the nodes up
				astarsearch.FreeSolutionNodes();

		}
		else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED )
		{
			cout << "Search terminated. Did not find goal state\n";

		}

		// Display the number of loops the search went through
		cout << "SearchSteps : " << SearchSteps << "\n";

		SearchCount ++;

		astarsearch.EnsureMemoryFreed();
	}

	return 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
<|endoftext|>


<commit_before>/*
 * Funambol is a mobile platform developed by Funambol, Inc.
 * Copyright (C) 2003 - 2007 Funambol, Inc.
 *
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Affero General Public License version 3 as published by
 * the Free Software Foundation with the addition of the following permission
 * added to Section 15 as permitted in Section 7(a): FOR ANY PART OF THE COVERED
 * WORK IN WHICH THE COPYRIGHT IS OWNED BY FUNAMBOL, FUNAMBOL DISCLAIMS THE
 * WARRANTY OF NON INFRINGEMENT  OF THIRD PARTY RIGHTS.
 *
 * This program 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 Affero General Public License
 * along with this program; if not, see http://www.gnu.org/licenses or write to
 * the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 * MA 02110-1301 USA.
 *
 * You can contact Funambol, Inc. headquarters at 643 Bair Island Road, Suite
 * 305, Redwood City, CA 94063, USA, or at email address info@funambol.com.
 *
 * The interactive user interfaces in modified source and object code versions
 * of this program must display Appropriate Legal Notices, as required under
 * Section 5 of the GNU Affero General Public License version 3.
 *
 * In accordance with Section 7(b) of the GNU Affero General Public License
 * version 3, these Appropriate Legal Notices must retain the display of the
 * "Powered by Funambol" logo. If the display of the logo is not reasonably
 * feasible for technical reasons, the Appropriate Legal Notices must display
 * the words "Powered by Funambol".
 */

#include "base/globalsdef.h"
#include "base/posixlog.h"
#include "base/fscapi.h"
#include "base/util/utils.h"

#include <unistd.h>

USE_NAMESPACE

POSIXLog::POSIXLog() :
    logFile(NULL),
    logFileStdout(false),
    logName(LOG_NAME),
    logPath(NULL),
    logRedirectStderr(false),
    fderr(-1),
    prefix("")
{
}

void POSIXLog::setLogPath(const char*  configLogPath) {
    // If empty or null, set current dir.
    if(configLogPath == 0|| *configLogPath == '\0') {
        logPath = "./";
    }
    else {
        logPath.sprintf("%s/", configLogPath);
    }
}

void POSIXLog::setLogName(const char*  configLogName) {
    logName.sprintf("%s", configLogName ? configLogName : LOG_NAME);
}

void POSIXLog::setLogFile(const char *path, const char* name, bool redirectStderr) {
    if (logPath != path) {
        setLogPath(path);
    }
    if (logName != name) {
        setLogName(name);
    }
    logRedirectStderr = redirectStderr;

    if (logFile != NULL) {
        fclose(logFile);
        logFile = NULL;
    }
    logFileStdout = false;

    if (!strcmp(name, "-")) {
        // write to stdout
        logFileStdout = true;
    } else if (path) {
        char *filename = new char[strlen(path) + strlen(name) + 3];

        sprintf(filename, "%s/%s", path, name);
        logFile = fopen(filename, "a+" );
        delete [] filename;
    } else {
        logFile = fopen(name, "a+" );
    }

    if (logFile) {
        char buffer[256];
        struct tm tm;
        time_t t = time(NULL);

        // We log UTC at the start of each line.
        // Log the current user's time offset.
        localtime_r(&t, &tm);
        strftime(buffer, sizeof(buffer),
                 "local timezone: %Z = GMT %z",
                 &tm);
        developer("%s", buffer);
        asctime_r(&tm, buffer);
        developer("local time: %s", buffer);
        gmtime_r(&t, &tm);
        asctime_r(&tm, buffer);
        developer("world time: %s", buffer);
    }

    if (redirectStderr && logFile) {
        if (fderr == -1) {
            // remember original stderr
            fderr = dup(2);
        }
        // overwrite stderr with log file fd,
        // closing the current stderr if necessary
        dup2(fileno(logFile), 2);
    } else {
        if (fderr != -1) {
            // restore original stderr
            dup2(fderr, 2);
        }
    }
}

POSIXLog::~POSIXLog() {
    if (logFile != NULL) {
        fclose(logFile);
    }
}

void POSIXLog::error(const char*  msg, ...) {
    va_list argList;
    va_start (argList, msg);
    printMessage(LOG_LEVEL_NONE, LOG_ERROR, msg, argList);
    va_end(argList);
}

void POSIXLog::info(const char*  msg, ...) {
    if (isLoggable(LOG_LEVEL_INFO)) {
        va_list argList;
        va_start (argList, msg);
        printMessage(LOG_LEVEL_INFO, LOG_INFO, msg, argList);
        va_end(argList);
    }
}

void POSIXLog::developer(const char*  msg, ...) {
    if (isLoggable(LOG_LEVEL_INFO)) {
        va_list argList;
        va_start (argList, msg);
        printMessage(LOG_LEVEL_DEBUG, LOG_DEBUG, msg, argList);
        va_end(argList);
    }
}

void POSIXLog::debug(const char*  msg, ...) {
    if (isLoggable(LOG_LEVEL_DEBUG)) {
        va_list argList;
        va_start (argList, msg);
        printMessage(LOG_LEVEL_DEBUG, LOG_DEBUG, msg, argList);
        va_end(argList);
    }
}

void POSIXLog::printLine(bool firstLine,
                         time_t time,
                         const char *fullTime,
                         const char *shortTime,
                         const char *utcTime,
                         LogLevel level,
                         const char *levelPrefix,
                         const char *line)
{
    FILE *out = getLogFile();
    if (!out) {
        out = stdout;
    }
    if (firstLine) {
        fprintf(out, "%s [%s] %s%s\n",
                logFile ? utcTime : shortTime,
                levelPrefix,
                getPrefix().c_str(),
                line);
    } else {
        fprintf(out, "[%s] %s%s\n",
                levelPrefix,
                getPrefix().c_str(),
                line);
    }
    fflush(out);
}

void POSIXLog::printMessage(LogLevel level, const char* levelPrefix, const char* msg, va_list argList) {
    time_t t = time(NULL);
    struct tm sys_time;
    struct tm utc_time;
    char fullTime[64], shortTime[32];
    char utcTime[32];

    localtime_r(&t, &sys_time);
    gmtime_r(&t, &utc_time);

    strftime(fullTime, sizeof(fullTime), "%F %T GMT %z", &sys_time);
    strftime(shortTime, sizeof(shortTime), "%T", &sys_time);
    sprintf(utcTime, "%02d:%02d:%02d GMT",
            utc_time.tm_hour,
            utc_time.tm_min,
            utc_time.tm_sec);

    if (!logFileStdout && !logFile) {
         setLogFile(logPath, logName, logRedirectStderr);
    }

    StringBuffer buffer;
    buffer.vsprintf(msg, argList);
    const char *start = buffer.c_str();
    const char *eol = strchr(start, '\n');
    bool firstLine = true;
    while (eol) {
        /* hack: StringBuffer does not really allow write access, but do it anyway */
        *(char *)eol = 0;
        printLine(firstLine,
                  t,
                  fullTime,
                  shortTime,
                  utcTime,
                  level,
                  levelPrefix,
                  start);
        firstLine = false;
        *(char *)eol = '\n';
        start = eol + 1;
        eol = strchr(start, '\n');
    }
    printLine(firstLine,
              t,
              fullTime,
              shortTime,
              utcTime,
              level,
              levelPrefix,
              start);
}

void POSIXLog::reset(const char*  title) {
    setLogFile(logPath, logName, logRedirectStderr);

    if (logFile) {
        ftruncate(fileno(logFile), 0);
    }
}

size_t POSIXLog::getLogSize() {
    size_t ret = 0;

    if (logFile) {
        ret = fgetsize(logFile);
        fclose(logFile);
    }
    return ret;
}

Log *Log::logger;

Log &Log::instance() {
    if (!logger) {
        logger = new POSIXLog();
    }
    return *logger;
}

<commit_msg>fix posixlog bug: crash when reset log after getting the file size<commit_after>/*
 * Funambol is a mobile platform developed by Funambol, Inc.
 * Copyright (C) 2003 - 2007 Funambol, Inc.
 *
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Affero General Public License version 3 as published by
 * the Free Software Foundation with the addition of the following permission
 * added to Section 15 as permitted in Section 7(a): FOR ANY PART OF THE COVERED
 * WORK IN WHICH THE COPYRIGHT IS OWNED BY FUNAMBOL, FUNAMBOL DISCLAIMS THE
 * WARRANTY OF NON INFRINGEMENT  OF THIRD PARTY RIGHTS.
 *
 * This program 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 Affero General Public License
 * along with this program; if not, see http://www.gnu.org/licenses or write to
 * the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 * MA 02110-1301 USA.
 *
 * You can contact Funambol, Inc. headquarters at 643 Bair Island Road, Suite
 * 305, Redwood City, CA 94063, USA, or at email address info@funambol.com.
 *
 * The interactive user interfaces in modified source and object code versions
 * of this program must display Appropriate Legal Notices, as required under
 * Section 5 of the GNU Affero General Public License version 3.
 *
 * In accordance with Section 7(b) of the GNU Affero General Public License
 * version 3, these Appropriate Legal Notices must retain the display of the
 * "Powered by Funambol" logo. If the display of the logo is not reasonably
 * feasible for technical reasons, the Appropriate Legal Notices must display
 * the words "Powered by Funambol".
 */

#include "base/globalsdef.h"
#include "base/posixlog.h"
#include "base/fscapi.h"
#include "base/util/utils.h"

#include <unistd.h>

USE_NAMESPACE

POSIXLog::POSIXLog() :
    logFile(NULL),
    logFileStdout(false),
    logName(LOG_NAME),
    logPath(NULL),
    logRedirectStderr(false),
    fderr(-1),
    prefix("")
{
}

void POSIXLog::setLogPath(const char*  configLogPath) {
    // If empty or null, set current dir.
    if(configLogPath == 0|| *configLogPath == '\0') {
        logPath = "./";
    }
    else {
        logPath.sprintf("%s/", configLogPath);
    }
}

void POSIXLog::setLogName(const char*  configLogName) {
    logName.sprintf("%s", configLogName ? configLogName : LOG_NAME);
}

void POSIXLog::setLogFile(const char *path, const char* name, bool redirectStderr) {
    if (logPath != path) {
        setLogPath(path);
    }
    if (logName != name) {
        setLogName(name);
    }
    logRedirectStderr = redirectStderr;

    if (logFile != NULL) {
        fclose(logFile);
        logFile = NULL;
    }
    logFileStdout = false;

    if (!strcmp(name, "-")) {
        // write to stdout
        logFileStdout = true;
    } else if (path) {
        char *filename = new char[strlen(path) + strlen(name) + 3];

        sprintf(filename, "%s/%s", path, name);
        logFile = fopen(filename, "a+" );
        delete [] filename;
    } else {
        logFile = fopen(name, "a+" );
    }

    if (logFile) {
        char buffer[256];
        struct tm tm;
        time_t t = time(NULL);

        // We log UTC at the start of each line.
        // Log the current user's time offset.
        localtime_r(&t, &tm);
        strftime(buffer, sizeof(buffer),
                 "local timezone: %Z = GMT %z",
                 &tm);
        developer("%s", buffer);
        asctime_r(&tm, buffer);
        developer("local time: %s", buffer);
        gmtime_r(&t, &tm);
        asctime_r(&tm, buffer);
        developer("world time: %s", buffer);
    }

    if (redirectStderr && logFile) {
        if (fderr == -1) {
            // remember original stderr
            fderr = dup(2);
        }
        // overwrite stderr with log file fd,
        // closing the current stderr if necessary
        dup2(fileno(logFile), 2);
    } else {
        if (fderr != -1) {
            // restore original stderr
            dup2(fderr, 2);
        }
    }
}

POSIXLog::~POSIXLog() {
    if (logFile != NULL) {
        fclose(logFile);
    }
}

void POSIXLog::error(const char*  msg, ...) {
    va_list argList;
    va_start (argList, msg);
    printMessage(LOG_LEVEL_NONE, LOG_ERROR, msg, argList);
    va_end(argList);
}

void POSIXLog::info(const char*  msg, ...) {
    if (isLoggable(LOG_LEVEL_INFO)) {
        va_list argList;
        va_start (argList, msg);
        printMessage(LOG_LEVEL_INFO, LOG_INFO, msg, argList);
        va_end(argList);
    }
}

void POSIXLog::developer(const char*  msg, ...) {
    if (isLoggable(LOG_LEVEL_INFO)) {
        va_list argList;
        va_start (argList, msg);
        printMessage(LOG_LEVEL_DEBUG, LOG_DEBUG, msg, argList);
        va_end(argList);
    }
}

void POSIXLog::debug(const char*  msg, ...) {
    if (isLoggable(LOG_LEVEL_DEBUG)) {
        va_list argList;
        va_start (argList, msg);
        printMessage(LOG_LEVEL_DEBUG, LOG_DEBUG, msg, argList);
        va_end(argList);
    }
}

void POSIXLog::printLine(bool firstLine,
                         time_t time,
                         const char *fullTime,
                         const char *shortTime,
                         const char *utcTime,
                         LogLevel level,
                         const char *levelPrefix,
                         const char *line)
{
    FILE *out = getLogFile();
    if (!out) {
        out = stdout;
    }
    if (firstLine) {
        fprintf(out, "%s [%s] %s%s\n",
                logFile ? utcTime : shortTime,
                levelPrefix,
                getPrefix().c_str(),
                line);
    } else {
        fprintf(out, "[%s] %s%s\n",
                levelPrefix,
                getPrefix().c_str(),
                line);
    }
    fflush(out);
}

void POSIXLog::printMessage(LogLevel level, const char* levelPrefix, const char* msg, va_list argList) {
    time_t t = time(NULL);
    struct tm sys_time;
    struct tm utc_time;
    char fullTime[64], shortTime[32];
    char utcTime[32];

    localtime_r(&t, &sys_time);
    gmtime_r(&t, &utc_time);

    strftime(fullTime, sizeof(fullTime), "%F %T GMT %z", &sys_time);
    strftime(shortTime, sizeof(shortTime), "%T", &sys_time);
    sprintf(utcTime, "%02d:%02d:%02d GMT",
            utc_time.tm_hour,
            utc_time.tm_min,
            utc_time.tm_sec);

    if (!logFileStdout && !logFile) {
         setLogFile(logPath, logName, logRedirectStderr);
    }

    StringBuffer buffer;
    buffer.vsprintf(msg, argList);
    const char *start = buffer.c_str();
    const char *eol = strchr(start, '\n');
    bool firstLine = true;
    while (eol) {
        /* hack: StringBuffer does not really allow write access, but do it anyway */
        *(char *)eol = 0;
        printLine(firstLine,
                  t,
                  fullTime,
                  shortTime,
                  utcTime,
                  level,
                  levelPrefix,
                  start);
        firstLine = false;
        *(char *)eol = '\n';
        start = eol + 1;
        eol = strchr(start, '\n');
    }
    printLine(firstLine,
              t,
              fullTime,
              shortTime,
              utcTime,
              level,
              levelPrefix,
              start);
}

void POSIXLog::reset(const char*  title) {
    setLogFile(logPath, logName, logRedirectStderr);

    if (logFile) {
        ftruncate(fileno(logFile), 0);
    }
}

size_t POSIXLog::getLogSize() {
    size_t ret = 0;

    if (logFile) {
        ret = fgetsize(logFile);
        fclose(logFile);
        logFile = NULL;
    }
    return ret;
}

Log *Log::logger;

Log &Log::instance() {
    if (!logger) {
        logger = new POSIXLog();
    }
    return *logger;
}

<|endoftext|>


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

#include "video_recorder.h"

#include<stdio.h>
#include<iostream>
#include<chrono>
#include<errno.h>

#include<ros/ros.h>
#include<sensor_msgs/CompressedImage.h>

extern "C" {
#include <libavcodec/avcodec.h>
#include <libavutil/opt.h>
#include <libavutil/mathematics.h>
#include <libavformat/avformat.h>
}

namespace rospilot {

using namespace std::chrono;

SoftwareVideoRecorder::SoftwareVideoRecorder(AVPixelFormat pixelFormat, H264Settings settings, std::string mediaPath)
{
    av_register_all();
    this->width = settings.width;
    this->height = settings.height;
    this->pixelFormat = pixelFormat;
    this->settings = settings;
    this->tempFilename = mediaPath + "/.tmp.mp4";
}

void SoftwareVideoRecorder::addFrame(sensor_msgs::CompressedImage *image, bool keyFrame)
{
    // acquire lock so that we can read this->recording
    std::lock_guard<std::mutex> guard(lock);
    if (!recording) {
        return;
    }

    time_point<high_resolution_clock> currentTime = high_resolution_clock::now();
    if (!foundKeyframe && keyFrame) {
        foundKeyframe = true;
        firstFrameTime = currentTime;
        lastPTS = -1;
    }

    if (!foundKeyframe) {
        return;
    }

    duration<double> duration = (currentTime - firstFrameTime);
    int pts = (int) (duration.count() * FPS);
    if (pts == lastPTS) {
        // If we're receiving frames faster than 60 fps, just drop them.
        return;
    }
    lastPTS = pts;

    // Skip empty frames
    if (image->data.size() > 0) {
        AVPacket pkt;
        av_init_packet(&pkt);

        pkt.pts = av_rescale_q(pts, (AVRational){1, FPS},
                formatContext->streams[0]->time_base);
        if (keyFrame) {
            pkt.flags |= AV_PKT_FLAG_KEY;
        }

        pkt.stream_index = 0;
        pkt.data = image->data.data();
        pkt.size = image->data.size();

        int ret = av_interleaved_write_frame(formatContext, &pkt);
        if (ret != 0) {
            ROS_ERROR("Error while writing video frame: %d", ret);
        }
    }
}

AVStream *SoftwareVideoRecorder::createVideoStream(AVFormatContext *oc)
{
    AVCodecContext *c;
    AVStream *stream;
    AVCodec *codec;

    codec = avcodec_find_encoder(AV_CODEC_ID_H264);
    if (!codec) {
        ROS_ERROR("codec not found");
    }

    stream = avformat_new_stream(oc, codec);
    if (!stream) {
        ROS_ERROR("Could not alloc stream");
    }

    stream->time_base.den = FPS;
    stream->time_base.num = 1;
    c = stream->codec;

    c->codec_id = AV_CODEC_ID_H264;
    c->bit_rate = this->settings.bit_rate;
    c->width = this->width;
    c->height = this->height;
    c->gop_size = this->settings.gop_size;
    // Not sure this does anything, so set the "profile" on priv_data also
    if (settings.profile == CONSTRAINED_BASELINE) {
        c->profile = FF_PROFILE_H264_CONSTRAINED_BASELINE;
        av_opt_set(c->priv_data, "profile", "baseline", AV_OPT_SEARCH_CHILDREN);
    }
    else if (settings.profile == HIGH) {
        c->profile = FF_PROFILE_H264_HIGH;
        av_opt_set(c->priv_data, "profile", "high", AV_OPT_SEARCH_CHILDREN);
    }
    else {
        ROS_ERROR("Unknown H264 profile");
    }
    if (settings.zero_latency) {
        av_opt_set(c->priv_data, "tune", "zerolatency", AV_OPT_SEARCH_CHILDREN);
        av_opt_set(c->priv_data, "preset", "ultrafast", AV_OPT_SEARCH_CHILDREN);
    }
    c->level = this->settings.level;
    c->pix_fmt = this->pixelFormat;
    c->flags |= CODEC_FLAG_GLOBAL_HEADER;

    if (avcodec_open2(c, codec, nullptr) < 0) {
        ROS_ERROR("could not open codec");
    }

    return stream;
}

bool SoftwareVideoRecorder::start(const char *name)
{
    AVCodecID codecId = AV_CODEC_ID_H264;
    std::lock_guard<std::mutex> guard(lock);
    filename = std::string(name);
    formatContext = avformat_alloc_context();
    formatContext->oformat = av_guess_format(nullptr, name, nullptr);

    formatContext->priv_data =
        av_mallocz(formatContext->oformat->priv_data_size);
    if (formatContext->oformat->priv_class) {
        *(const AVClass**)formatContext->priv_data =
            formatContext->oformat->priv_class;
        av_opt_set_defaults(formatContext->priv_data);
    }

    // Clear the file before we start writing
    remove(tempFilename.c_str());

    strncpy(formatContext->filename,
            tempFilename.c_str(),
            sizeof(formatContext->filename));
    // Check that this is a valid combination
    if(avformat_query_codec(formatContext->oformat, codecId, FF_COMPLIANCE_NORMAL) != 1) {
        ROS_FATAL("Codec %d not compatible with output format %s", codecId,
                formatContext->oformat->long_name);
    }
    videoStream = createVideoStream(formatContext);
    av_dump_format(formatContext, 0, tempFilename.c_str(), 1);

    if (avio_open(&formatContext->pb,
                tempFilename.c_str(),
                AVIO_FLAG_WRITE) < 0) {
        ROS_ERROR("Could not open '%s'", tempFilename.c_str());
        return false;
    }

    avformat_write_header(formatContext, nullptr);
    foundKeyframe = false;
    recording = true;
    ROS_INFO("Start recording output to %s as %s with vcodec %d, short name = %s",
            tempFilename.c_str(),
            formatContext->oformat->long_name,
            codecId,
            formatContext->oformat->name);
    return true;
}

bool SoftwareVideoRecorder::stop()
{
    std::lock_guard<std::mutex> guard(lock);
    recording = false;
    av_write_trailer(formatContext);
    avcodec_close(videoStream->codec);
    for (int i = 0; i < formatContext->nb_streams; i++) {
        av_freep(&formatContext->streams[i]->codec);
        av_freep(&formatContext->streams[i]);
    }
    avio_close(formatContext->pb);
    av_free(formatContext);
    ROS_INFO("Finializing video file: %s", filename.c_str());
    if(rename(tempFilename.c_str(), filename.c_str()) != 0) {
        ROS_ERROR("Error moving temp file: %s", strerror(errno));
    }
    else {
        ROS_INFO("Finished recording video");
    }
    return true;
}

SoftwareVideoRecorder::~SoftwareVideoRecorder()
{
}

}
<commit_msg>Cleanup VideoRecorder::stop()<commit_after>/*********************************************************************
 *
 * Copyright 2012 the original author or authors.
 * See the NOTICE file distributed with this work for additional
 * information regarding copyright ownership.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *********************************************************************/

#include "video_recorder.h"

#include<stdio.h>
#include<iostream>
#include<chrono>
#include<errno.h>

#include<ros/ros.h>
#include<sensor_msgs/CompressedImage.h>

extern "C" {
#include <libavcodec/avcodec.h>
#include <libavutil/opt.h>
#include <libavutil/mathematics.h>
#include <libavformat/avformat.h>
}

namespace rospilot {

using namespace std::chrono;

SoftwareVideoRecorder::SoftwareVideoRecorder(AVPixelFormat pixelFormat, H264Settings settings, std::string mediaPath)
{
    av_register_all();
    this->width = settings.width;
    this->height = settings.height;
    this->pixelFormat = pixelFormat;
    this->settings = settings;
    this->tempFilename = mediaPath + "/.tmp.mp4";
}

void SoftwareVideoRecorder::addFrame(sensor_msgs::CompressedImage *image, bool keyFrame)
{
    // acquire lock so that we can read this->recording
    std::lock_guard<std::mutex> guard(lock);
    if (!recording) {
        return;
    }

    time_point<high_resolution_clock> currentTime = high_resolution_clock::now();
    if (!foundKeyframe && keyFrame) {
        foundKeyframe = true;
        firstFrameTime = currentTime;
        lastPTS = -1;
    }

    if (!foundKeyframe) {
        return;
    }

    duration<double> duration = (currentTime - firstFrameTime);
    int pts = (int) (duration.count() * FPS);
    if (pts == lastPTS) {
        // If we're receiving frames faster than 60 fps, just drop them.
        return;
    }
    lastPTS = pts;

    // Skip empty frames
    if (image->data.size() > 0) {
        AVPacket pkt;
        av_init_packet(&pkt);

        pkt.pts = av_rescale_q(pts, (AVRational){1, FPS},
                formatContext->streams[0]->time_base);
        if (keyFrame) {
            pkt.flags |= AV_PKT_FLAG_KEY;
        }

        pkt.stream_index = 0;
        pkt.data = image->data.data();
        pkt.size = image->data.size();

        int ret = av_interleaved_write_frame(formatContext, &pkt);
        if (ret != 0) {
            ROS_ERROR("Error while writing video frame: %d", ret);
        }
    }
}

AVStream *SoftwareVideoRecorder::createVideoStream(AVFormatContext *oc)
{
    AVCodecContext *c;
    AVStream *stream;
    AVCodec *codec;

    codec = avcodec_find_encoder(AV_CODEC_ID_H264);
    if (!codec) {
        ROS_ERROR("codec not found");
    }

    stream = avformat_new_stream(oc, codec);
    if (!stream) {
        ROS_ERROR("Could not alloc stream");
    }

    stream->time_base.den = FPS;
    stream->time_base.num = 1;
    c = stream->codec;

    c->codec_id = AV_CODEC_ID_H264;
    c->bit_rate = this->settings.bit_rate;
    c->width = this->width;
    c->height = this->height;
    c->gop_size = this->settings.gop_size;
    // Not sure this does anything, so set the "profile" on priv_data also
    if (settings.profile == CONSTRAINED_BASELINE) {
        c->profile = FF_PROFILE_H264_CONSTRAINED_BASELINE;
        av_opt_set(c->priv_data, "profile", "baseline", AV_OPT_SEARCH_CHILDREN);
    }
    else if (settings.profile == HIGH) {
        c->profile = FF_PROFILE_H264_HIGH;
        av_opt_set(c->priv_data, "profile", "high", AV_OPT_SEARCH_CHILDREN);
    }
    else {
        ROS_ERROR("Unknown H264 profile");
    }
    if (settings.zero_latency) {
        av_opt_set(c->priv_data, "tune", "zerolatency", AV_OPT_SEARCH_CHILDREN);
        av_opt_set(c->priv_data, "preset", "ultrafast", AV_OPT_SEARCH_CHILDREN);
    }
    c->level = this->settings.level;
    c->pix_fmt = this->pixelFormat;
    c->flags |= CODEC_FLAG_GLOBAL_HEADER;

    if (avcodec_open2(c, codec, nullptr) < 0) {
        ROS_ERROR("could not open codec");
    }

    return stream;
}

bool SoftwareVideoRecorder::start(const char *name)
{
    AVCodecID codecId = AV_CODEC_ID_H264;
    std::lock_guard<std::mutex> guard(lock);
    filename = std::string(name);
    formatContext = avformat_alloc_context();
    formatContext->oformat = av_guess_format(nullptr, name, nullptr);

    formatContext->priv_data =
        av_mallocz(formatContext->oformat->priv_data_size);
    if (formatContext->oformat->priv_class) {
        *(const AVClass**)formatContext->priv_data =
            formatContext->oformat->priv_class;
        av_opt_set_defaults(formatContext->priv_data);
    }

    // Clear the file before we start writing
    remove(tempFilename.c_str());

    strncpy(formatContext->filename,
            tempFilename.c_str(),
            sizeof(formatContext->filename));
    // Check that this is a valid combination
    if(avformat_query_codec(formatContext->oformat, codecId, FF_COMPLIANCE_NORMAL) != 1) {
        ROS_FATAL("Codec %d not compatible with output format %s", codecId,
                formatContext->oformat->long_name);
    }
    videoStream = createVideoStream(formatContext);
    av_dump_format(formatContext, 0, tempFilename.c_str(), 1);

    if (avio_open(&formatContext->pb,
                tempFilename.c_str(),
                AVIO_FLAG_WRITE) < 0) {
        ROS_ERROR("Could not open '%s'", tempFilename.c_str());
        return false;
    }

    avformat_write_header(formatContext, nullptr);
    foundKeyframe = false;
    recording = true;
    ROS_INFO("Start recording output to %s as %s with vcodec %d, short name = %s",
            tempFilename.c_str(),
            formatContext->oformat->long_name,
            codecId,
            formatContext->oformat->name);
    return true;
}

bool SoftwareVideoRecorder::stop()
{
    std::lock_guard<std::mutex> guard(lock);
    recording = false;
    av_write_trailer(formatContext);
    AVIOContext *pb = formatContext->pb;
    avcodec_close(videoStream->codec);
    avformat_free_context(formatContext);
    avio_close(pb);
    ROS_INFO("Finializing video file: %s", filename.c_str());
    if(rename(tempFilename.c_str(), filename.c_str()) != 0) {
        ROS_ERROR("Error moving temp file: %s", strerror(errno));
    }
    else {
        ROS_INFO("Finished recording video");
    }
    return true;
}

SoftwareVideoRecorder::~SoftwareVideoRecorder()
{
}

}
<|endoftext|>