Revision: 8606
          http://playerstage.svn.sourceforge.net/playerstage/?rev=8606&view=rev
Author:   natepak
Date:     2010-04-02 15:07:45 +0000 (Fri, 02 Apr 2010)

Log Message:
-----------
Added benchmarks

Added Paths:
-----------
    code/gazebo/trunk/benchmarks/
    code/gazebo/trunk/benchmarks/CMakeLists.txt
    code/gazebo/trunk/benchmarks/box_grid_benchmark.cc
    code/gazebo/trunk/benchmarks/box_pile_benchmark.cc
    code/gazebo/trunk/benchmarks/camera_benchmark.cc
    code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc
    code/gazebo/trunk/benchmarks/laser_benchmark.cc

Added: code/gazebo/trunk/benchmarks/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/benchmarks/CMakeLists.txt                         (rev 0)
+++ code/gazebo/trunk/benchmarks/CMakeLists.txt 2010-04-02 15:07:45 UTC (rev 
8606)
@@ -0,0 +1,36 @@
+project(factory)
+
+cmake_minimum_required( VERSION 2.4 FATAL_ERROR )
+
+if(COMMAND cmake_policy)
+  cmake_policy(SET CMP0003 NEW)
+  cmake_policy(SET CMP0004 NEW)
+endif(COMMAND cmake_policy)
+
+include (FindPkgConfig)
+
+if (PKG_CONFIG_FOUND)
+  pkg_check_modules(PKG_CFGS REQUIRED libgazebo)
+  set (INCLUDE_DIRS ${PKG_CFGS_INCLUDE_DIRS})
+  set (LINK_DIRS  ${PKG_CFGS_LIBRARY_DIRS})
+  set (LINK_LIBS ${PKG_CFGS_LINK_LIBS} ${PKG_CFGS_LIBRARIES})
+endif (PKG_CONFIG_FOUND)
+
+include_directories( ${INCLUDE_DIRS} )
+link_directories( ${LINK_DIRS} )
+
+include_directories(.)
+
+set (sources laser_benchmark.cc box_grid_benchmark.cc box_pile_benchmark.cc 
joint_chain_benchmark.cc)
+
+set_source_files_properties(${sources} PROPERTIES COMPILE_FLAGS "-ggdb -g2 
-Wall")
+
+add_executable(laser_benchmark laser_benchmark.cc)
+add_executable(box_pile_benchmark box_pile_benchmark.cc)
+add_executable(box_grid_benchmark box_grid_benchmark.cc)
+add_executable(joint_chain_benchmark joint_chain_benchmark.cc)
+
+target_link_libraries(laser_benchmark ${LINK_LIBS} )
+target_link_libraries(box_grid_benchmark ${LINK_LIBS} )
+target_link_libraries(box_pile_benchmark ${LINK_LIBS} )
+target_link_libraries(joint_chain_benchmark ${LINK_LIBS} )

Added: code/gazebo/trunk/benchmarks/box_grid_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/box_grid_benchmark.cc                          
(rev 0)
+++ code/gazebo/trunk/benchmarks/box_grid_benchmark.cc  2010-04-02 15:07:45 UTC 
(rev 8606)
@@ -0,0 +1,133 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <gazebo/gazebo.h>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+int laser_count = 0;
+
+std::string test_name="Box Grid Benchmark";
+std::string xlabel = "Box Count";
+std::string ylabel = "Simtime / Realtime";
+std::string data_filename = "/tmp/" + test_name + ".data";
+
+void make_plot()
+{
+  std::ostringstream cmd;
+
+  cmd << "echo \"";
+  cmd << "set xlabel '" << xlabel << "'\n";
+  cmd << "set ylabel '" << ylabel << "'\n";
+  cmd << "set title '" << test_name << "'\n";
+  cmd << "set terminal png\n";
+  cmd << "set output '" << test_name << ".png'\n";
+  cmd << "plot '" << data_filename << "' with lines\n";
+  cmd << "\" | gnuplot";
+
+  if (system(cmd.str().c_str() ) < 0)
+    std::cerr << "Error\n";
+}
+
+void spawn_box(double x, double y, double z=.25)
+{
+  std::ostringstream model;
+
+  model << "<model:physical name='box" << laser_count++ << "'>";
+  model << "  <xyz>" << x << " " << y << " " << z << "</xyz>";
+  model << "  <rpy>0 0 0</rpy>";
+  model << "  <body:box name='body'>";
+
+  model << "    <geom:box name='geom'>";
+  model << "      <size>0.5 0.5 0.5</size>";
+  model << "      <mass>1</mass>";
+  model << "      <visual>";
+  model << "        <size>0.5 0.5 0.5</size>";
+  model << "        <mesh>unit_box</mesh>";
+  model << "        <material>Gazebo/Rocky</material>";
+  model << "      </visual>";
+  model << "    </geom:box>";
+
+  model << "  </body:box>";
+  model << "</model:physical>";
+
+  factoryIface->Lock(1);
+  strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+  factoryIface->Unlock();
+}
+
+int main()
+{
+  client = new gazebo::Client();
+  simIface = new gazebo::SimulationIface();
+  factoryIface = new gazebo::FactoryIface();
+
+  try
+  {
+    client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+  }
+  catch(std::string e)
+  {
+    std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+    return -1;
+  }
+
+  /// Open the sim iface
+  try
+  {
+    simIface->Open(client, "default");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+    return -1;
+  }
+
+  // Open the factory iface
+  try
+  {
+    factoryIface->Open(client, "default");
+  }
+  catch( std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to the factory Iface:" 
+              << e << "\n";
+    return -1;
+  }
+
+  FILE *out = fopen(data_filename.c_str(), "w");
+
+  // 400 blocks in a grid patter
+  for (int y=-10; y < 10; y++)
+  {
+    for ( int x=-10; x < 10; x++)
+    {
+      spawn_box(x, y);
+      double simTime = 0;
+      double realTime = 0;
+
+      for (unsigned int i=0; i < 30; i++)
+      {
+        simTime += simIface->data->simTime;
+        realTime += simIface->data->realTime;
+
+        /// Wait .1 seconds 
+        usleep(100000);
+      }
+
+      double percent = simTime / realTime;
+      fprintf(out,"%f\n",percent);
+      std::cout << "Index[" << y <<  " " << x <<  "] " << percent << "\n";
+    }
+  }
+
+  fclose(out);
+
+  make_plot();
+
+  factoryIface->Close();
+  simIface->Close();
+  client->Disconnect();
+}

Added: code/gazebo/trunk/benchmarks/box_pile_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/box_pile_benchmark.cc                          
(rev 0)
+++ code/gazebo/trunk/benchmarks/box_pile_benchmark.cc  2010-04-02 15:07:45 UTC 
(rev 8606)
@@ -0,0 +1,130 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <gazebo/gazebo.h>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+int laser_count = 0;
+
+std::string test_name="Boxes Benchmark";
+std::string xlabel = "Box Count";
+std::string ylabel = "Simtime / Realtime";
+std::string data_filename = "/tmp/" + test_name + ".data";
+
+void make_plot()
+{
+  std::ostringstream cmd;
+
+  cmd << "echo \"";
+  cmd << "set xlabel '" << xlabel << "'\n";
+  cmd << "set ylabel '" << ylabel << "'\n";
+  cmd << "set title '" << test_name << "'\n";
+  cmd << "set terminal png\n";
+  cmd << "set output '" << test_name << ".png'\n";
+  cmd << "plot '" << data_filename << "' with lines\n";
+  cmd << "\" | gnuplot";
+
+  if (system(cmd.str().c_str() ) < 0)
+    std::cerr << "Error\n";
+}
+
+void spawn_box(double x, double y, double z=10)
+{
+  std::ostringstream model;
+
+  model << "<model:physical name='box" << laser_count++ << "'>";
+  model << "  <xyz>" << x << " " << y << " " << z << "</xyz>";
+  model << "  <rpy>30 40 0</rpy>";
+  model << "  <body:box name='body'>";
+
+  model << "    <geom:box name='geom'>";
+  model << "      <size>0.5 0.5 0.5</size>";
+  model << "      <mass>1</mass>";
+  model << "      <visual>";
+  model << "        <size>0.5 0.5 0.5</size>";
+  model << "        <mesh>unit_box</mesh>";
+  model << "        <material>Gazebo/Rocky</material>";
+  model << "      </visual>";
+  model << "    </geom:box>";
+
+  model << "  </body:box>";
+  model << "</model:physical>";
+
+  factoryIface->Lock(1);
+  strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+  factoryIface->Unlock();
+}
+
+int main()
+{
+  client = new gazebo::Client();
+  simIface = new gazebo::SimulationIface();
+  factoryIface = new gazebo::FactoryIface();
+
+  try
+  {
+    client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+  }
+  catch(std::string e)
+  {
+    std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+    return -1;
+  }
+
+  /// Open the sim iface
+  try
+  {
+    simIface->Open(client, "default");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+    return -1;
+  }
+
+  // Open the factory iface
+  try
+  {
+    factoryIface->Open(client, "default");
+  }
+  catch( std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to the factory Iface:" 
+              << e << "\n";
+    return -1;
+  }
+
+  FILE *out = fopen(data_filename.c_str(), "w");
+
+  for (unsigned int j=0; j < 400; j++)
+  {
+    spawn_box(0, 0);
+
+    double simTime = 0;
+    double realTime = 0;
+
+    for (unsigned int i=0; i < 50; i++)
+    {
+      simTime += simIface->data->simTime;
+      realTime += simIface->data->realTime;
+
+      /// Wait .1 seconds 
+      usleep(100000);
+    }
+
+    double percent = simTime / realTime;
+    fprintf(out,"%f\n",percent);
+
+    std::cout << "Index[" << j << "] = " << percent << "\n";
+  }
+  fclose(out);
+
+  make_plot();
+
+  factoryIface->Close();
+  simIface->Close();
+  client->Disconnect();
+}

Added: code/gazebo/trunk/benchmarks/camera_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/camera_benchmark.cc                            
(rev 0)
+++ code/gazebo/trunk/benchmarks/camera_benchmark.cc    2010-04-02 15:07:45 UTC 
(rev 8606)
@@ -0,0 +1,132 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <gazebo/gazebo.h>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+int camera_count = 0;
+
+std::string test_name="Camera Benchmark";
+std::string xlabel = "Camera Count";
+std::string ylabel = "Simtime / Realtime";
+std::string data_filename = "/tmp/" + test_name + ".data";
+
+void make_plot()
+{
+  std::ostringstream cmd;
+
+  cmd << "echo \"";
+  cmd << "set xlabel '" << xlabel << "'\n";
+  cmd << "set ylabel '" << ylabel << "'\n";
+  cmd << "set title '" << test_name << "'\n";
+  cmd << "set terminal png\n";
+  cmd << "set output '" << test_name << ".png'\n";
+  cmd << "plot '" << data_filename << "' with lines\n";
+  cmd << "\" | gnuplot";
+
+  if (system(cmd.str().c_str() ) < 0)
+    std::cerr << "Error\n";
+}
+
+void spawn_box(double x, double y, double z=10)
+{
+  std::ostringstream model;
+
+  model << "<model:physical name='camera" << camera_count++ << "'>";
+  model << "  <xyz>" << x << " " << y << " " << z << "</xyz>";
+  model << "  <rpy>30 40 0</rpy>";
+  model << "  <body:box name='body'>";
+
+  model << "    <geom:box name='geom'>";
+  model << "      <size>0.5 0.5 0.5</size>";
+  model << "      <mass>1</mass>";
+  model << "      <visual>";
+  model << "        <size>0.5 0.5 0.5</size>";
+  model << "        <mesh>unit_box</mesh>";
+  model << "        <material>Gazebo/Rocky</material>";
+  model << "      </visual>";
+  model << "    </geom:box>";
+
+  model << "   <sensor:camera name='camera'>";
+  model << "     <imageSize>640 480</imageSize>";
+  model << "     
+
+  model << "  </body:box>";
+  model << "</model:physical>";
+
+  factoryIface->Lock(1);
+  strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+  factoryIface->Unlock();
+}
+
+int main()
+{
+  client = new gazebo::Client();
+  simIface = new gazebo::SimulationIface();
+  factoryIface = new gazebo::FactoryIface();
+
+  try
+  {
+    client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+  }
+  catch(std::string e)
+  {
+    std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+    return -1;
+  }
+
+  /// Open the sim iface
+  try
+  {
+    simIface->Open(client, "default");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+    return -1;
+  }
+
+  // Open the factory iface
+  try
+  {
+    factoryIface->Open(client, "default");
+  }
+  catch( std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to the factory Iface:" 
+              << e << "\n";
+    return -1;
+  }
+
+  FILE *out = fopen(data_filename.c_str(), "w");
+
+  for (unsigned int j=0; j < 100; j++)
+  {
+    spawn_box(j+1, 0);
+
+    double simTime = 0;
+    double realTime = 0;
+
+    for (unsigned int i=0; i < 50; i++)
+    {
+      simTime += simIface->data->simTime;
+      realTime += simIface->data->realTime;
+
+      /// Wait .1 seconds 
+      usleep(100000);
+    }
+
+    double percent = simTime / realTime;
+    fprintf(out,"%f\n",percent);
+  }
+  fclose(out);
+
+  make_plot();
+
+  factoryIface->Close();
+  simIface->Close();
+  client->Disconnect();
+}

Added: code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc                       
        (rev 0)
+++ code/gazebo/trunk/benchmarks/joint_chain_benchmark.cc       2010-04-02 
15:07:45 UTC (rev 8606)
@@ -0,0 +1,163 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <gazebo/gazebo.h>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+int joint_count = 0;
+
+std::string test_name="Joint Chain Benchmark";
+std::string xlabel = "Joint Count";
+std::string ylabel = "Simtime / Realtime";
+std::string data_filename = "/tmp/" + test_name + ".data";
+
+void make_plot()
+{
+  std::ostringstream cmd;
+
+  cmd << "echo \"";
+  cmd << "set xlabel '" << xlabel << "'\n";
+  cmd << "set ylabel '" << ylabel << "'\n";
+  cmd << "set title '" << test_name << "'\n";
+  cmd << "set terminal png\n";
+  cmd << "set output '" << test_name << ".png'\n";
+  cmd << "plot '" << data_filename << "' with lines\n";
+  cmd << "\" | gnuplot";
+
+  if (system(cmd.str().c_str() ) < 0)
+    std::cerr << "Error\n";
+}
+
+void spawn_chain(int joint_count)
+{
+  std::ostringstream model;
+
+  model << "<model:physical name='joint_model'>";
+  model << "  <xyz>0 0 0</xyz>";
+  model << "  <rpy>0 0 0</rpy>";
+
+  model << "  <body:box name='body0'>";
+  model << "    <xyz>0 0 0.25</xyz>";
+  model << "    <geom:box name='geom'>";
+  model << "      <size>0.5 0.5 0.5</size>";
+  model << "      <mass>1</mass>";
+  model << "      <visual>";
+  model << "        <size>0.5 0.5 0.5</size>";
+  model << "        <mesh>unit_box</mesh>";
+  model << "        <material>Gazebo/Rocky</material>";
+  model << "      </visual>";
+  model << "    </geom:box>";
+  model << "  </body:box>";
+
+  for (int i=1; i < joint_count+1; i++)
+  {
+    model << "  <body:box name='body" << i << "'>";
+    model << "    <xyz>" << i << " 0 0.25</xyz>";
+    model << "    <geom:box name='geom'>";
+    model << "      <size>0.5 0.5 0.5</size>";
+    model << "      <mass>1</mass>";
+    model << "      <visual>";
+    model << "        <size>0.5 0.5 0.5</size>";
+    model << "        <mesh>unit_box</mesh>";
+    model << "        <material>Gazebo/Rocky</material>";
+    model << "      </visual>";
+    model << "    </geom:box>";
+    model << "  </body:box>";
+  }
+
+  for (int i=1; i < joint_count+1; i++)
+  {
+    model << "  <joint:hinge name='joint" << i << "'>";
+    model << "    <body1>body" << i-1 << "</body1>";
+    model << "    <body2>body" << i << "</body2>";
+    model << "    <anchor>body" << i << "</anchor>";
+    model << "    <axis>0 0 1</axis>";
+    model << "    <erp>0.8</erp>";
+    model << "    <cfm>10e-5</cfm>";
+    model << "  </joint:hinge>";
+  }
+
+  model << "</model:physical>";
+
+  factoryIface->Lock(1);
+  strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+  factoryIface->Unlock();
+}
+
+int main()
+{
+  client = new gazebo::Client();
+  simIface = new gazebo::SimulationIface();
+  factoryIface = new gazebo::FactoryIface();
+
+  try
+  {
+    client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+  }
+  catch(std::string e)
+  {
+    std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+    return -1;
+  }
+
+  /// Open the sim iface
+  try
+  {
+    simIface->Open(client, "default");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+    return -1;
+  }
+
+  // Open the factory iface
+  try
+  {
+    factoryIface->Open(client, "default");
+  }
+  catch( std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to the factory Iface:" 
+              << e << "\n";
+    return -1;
+  }
+
+  FILE *out = fopen(data_filename.c_str(), "w");
+
+  for (unsigned int j=1; j < 400; j++)
+  {
+
+    spawn_chain(j);
+
+    double simTime = 0;
+    double realTime = 0;
+
+    for (unsigned int i=0; i < 50; i++)
+    {
+      simTime += simIface->data->simTime;
+      realTime += simIface->data->realTime;
+
+      /// Wait .1 seconds 
+      usleep(100000);
+    }
+    strcpy( (char*)factoryIface->data->deleteModel, "joint_model");
+    while ( strcmp((char*)factoryIface->data->deleteModel, "") != 0)
+      usleep(10000);
+
+    double percent = simTime / realTime;
+    fprintf(out,"%f\n",percent);
+
+    std::cout << "Index[" << j << "] = " << percent << "\n";
+  }
+  fclose(out);
+
+  make_plot();
+
+  factoryIface->Close();
+  simIface->Close();
+  client->Disconnect();
+}

Added: code/gazebo/trunk/benchmarks/laser_benchmark.cc
===================================================================
--- code/gazebo/trunk/benchmarks/laser_benchmark.cc                             
(rev 0)
+++ code/gazebo/trunk/benchmarks/laser_benchmark.cc     2010-04-02 15:07:45 UTC 
(rev 8606)
@@ -0,0 +1,147 @@
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include <gazebo/gazebo.h>
+
+gazebo::Client *client = NULL;
+gazebo::SimulationIface *simIface = NULL;
+gazebo::FactoryIface *factoryIface = NULL;
+int laser_count = 0;
+
+std::string test_name="Laser Benchmark";
+std::string xlabel = "Laser Count";
+std::string ylabel = "Simtime / Realtime";
+std::string data_filename = "/tmp/" + test_name + ".data";
+
+void make_plot()
+{
+  std::ostringstream cmd;
+
+  cmd << "echo \"";
+  cmd << "set xlabel '" << xlabel << "'\n";
+  cmd << "set ylabel '" << ylabel << "'\n";
+  cmd << "set title '" << test_name << "'\n";
+  cmd << "set terminal png\n";
+  cmd << "set output '" << test_name << ".png'\n";
+  cmd << "plot '" << data_filename << "' with lines\n";
+  cmd << "\" | gnuplot";
+
+  if (system(cmd.str().c_str() ) < 0)
+    std::cerr << "Error\n";
+}
+
+void spawn_laser(double x, double y, double z=0.05)
+{
+  std::ostringstream model;
+
+  model << "<model:physical name='laser" << laser_count++ << "'>";
+  model << "  <xyz>" << x << " " << y << " " << z << "</xyz>";
+  model << "  <static>true</static>";
+  model << "  <body:box name='body'>";
+
+  model << "    <geom:box name='geom'>";
+  model << "      <size>0.1 0.1 0.1</size>";
+  model << "      <mass>0.1</mass>";
+  model << "      <visual>";
+  model << "        <size>0.1 0.1 0.1</size>";
+  model << "        <mesh>unit_box</mesh>";
+  model << "      </visual>";
+  model << "    </geom:box>";
+
+  model << "    <sensor:ray name='laser'>";
+  model << "      <alwaysActive>true</alwaysActive>";
+  model << "      <rayCount>260</rayCount>";
+  model << "      <rangeCount>260</rangeCount>";
+  model << "      <origin>0.02 0 0</origin>";
+  model << "      <displayRays>fan</displayRays>";
+  model << "      <minAngle>-90</minAngle>";
+  model << "      <maxAngle>90</maxAngle>";
+  model << "      <minRange>0.1</minRange>";
+  model << "      <maxRange>8.0</maxRange>";
+  model << "      <resRange>0.1</resRange>";
+  model << "    </sensor:ray>";
+
+  model << "  </body:box>";
+  model << "</model:physical>";
+
+  factoryIface->Lock(1);
+  strcpy( (char*)factoryIface->data->newModel, model.str().c_str() );
+  factoryIface->Unlock();
+}
+
+int main()
+{
+  client = new gazebo::Client();
+  simIface = new gazebo::SimulationIface();
+  factoryIface = new gazebo::FactoryIface();
+
+  try
+  {
+    client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
+  }
+  catch(std::string e)
+  {
+    std::cerr << "Gazebo Error: Unable to connect: " << e << "\n";
+    return -1;
+  }
+
+  /// Open the sim iface
+  try
+  {
+    simIface->Open(client, "default");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to sim iface:" << e << "\n";
+    return -1;
+  }
+
+  // Open the factory iface
+  try
+  {
+    factoryIface->Open(client, "default");
+  }
+  catch( std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to the factory Iface:" 
+              << e << "\n";
+    return -1;
+  }
+
+  FILE *out = fopen(data_filename.c_str(), "w");
+
+  if (!out)
+    std::cerr << "Unable to open log file";
+
+  for (int y = -5; y < 5; y++)
+  {
+    for (int x = -5; x < 5; x++)
+    {
+      spawn_laser(x,y);
+
+      double simTime = 0;
+      double realTime = 0;
+
+      for (unsigned int i=0; i < 30; i++)
+      {
+        simTime += simIface->data->simTime;
+        realTime += simIface->data->realTime;
+
+        /// Wait .1 seconds 
+        usleep(100000);
+      }
+
+      double percent = simTime / realTime;
+      fprintf(out,"%f\n",percent);
+      std::cout << "Index[" << y << " " << x << "] = " << percent << "\n";
+    }
+  }
+  fclose(out);
+
+  make_plot();
+
+  factoryIface->Close();
+  simIface->Close();
+  client->Disconnect();
+}


This was sent by the SourceForge.net collaborative development platform, the 
world's largest Open Source development site.

------------------------------------------------------------------------------
Download Intel&#174; Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit

Reply via email to