Revision: 8727
http://playerstage.svn.sourceforge.net/playerstage/?rev=8727&view=rev
Author: natepak
Date: 2010-05-28 21:22:39 +0000 (Fri, 28 May 2010)
Log Message:
-----------
Update the sprial and circle benchmarks
Modified Paths:
--------------
code/gazebo/branches/simpar/benchmarks/CMakeLists.txt
code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
Modified: code/gazebo/branches/simpar/benchmarks/CMakeLists.txt
===================================================================
--- code/gazebo/branches/simpar/benchmarks/CMakeLists.txt 2010-05-28
17:24:13 UTC (rev 8726)
+++ code/gazebo/branches/simpar/benchmarks/CMakeLists.txt 2010-05-28
21:22:39 UTC (rev 8727)
@@ -31,6 +31,7 @@
box_drop
table
pioneer_circle
+ pioneer_spiral
)
foreach (src ${benchmarks})
Modified: code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
2010-05-28 17:24:13 UTC (rev 8726)
+++ code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
2010-05-28 21:22:39 UTC (rev 8727)
@@ -4,7 +4,10 @@
#include <vector>
#include <libgazebo/gz.h>
#include <boost/lexical_cast.hpp>
+#include <math.h>
+#define DTOR(x) ( (x) * M_PI / 180.0)
+
gazebo::Client *client = NULL;
gazebo::SimulationIface *simIface = NULL;
gazebo::FactoryIface *factoryIface = NULL;
@@ -173,17 +176,36 @@
simIface->StartLogEntity("pioneer");
simIface->Unpause();
- double left = 1.0/stepTime;
- double right = left*1.8;
+ double angle = 10;
+ double speed = 0.2;
+ std::cout << "Radians[" << DTOR(angle) << "]\n";
+ double left = speed + DTOR(angle) * 0.39/2;
+ double right = speed - DTOR(angle) * 0.39/2;
- std::cout << "Left[" << left << "] Right[" << right << "]\n";
+ left = left / (.15 *0.5);
+ right = right / (.15 *0.5);
- double time = simIface->data->simTime;
- while(simIface->data->simTime - time < 60)
+ //std::cout << "Left[" << left << "] Right[" << right << "]\n";
+
+ gazebo::Pose pose;
+ double prevX = 0;
+ std::cout << "X[" << pose.pos.x << "] Y[" << pose.pos.y << "]\n";
+
+ double time = simIface->data->realTime;
+ int circles = 0;
+
+ while (circles < 4)
{
simIface->SetAngularVel("pioneer::left_wheel", gazebo::Vec3(0,0,left));
simIface->SetAngularVel("pioneer::right_wheel", gazebo::Vec3(0,0, right));
- usleep(10000);
+ simIface->GetPose2d("pioneer", pose);
+
+ if (simIface->data->realTime - time > 5.0 && fabs(pose.pos.x) < 0.1 )
+ {
+ time = simIface->data->realTime;
+ circles++;
+ std::cout << "Found a circle[" << circles << "]\n";
+ }
}
simIface->StopLogEntity("pioneer");
@@ -251,8 +273,8 @@
std::vector<std::string > step_types;
std::vector<std::string >::iterator iter;
//step_types.push_back("robust");
- //step_types.push_back("world");
- step_types.push_back("quick");
+ step_types.push_back("world");
+ //step_types.push_back("quick");
for (iter = step_types.begin(); iter != step_types.end(); iter++)
{
@@ -266,9 +288,9 @@
int i = 0;
simIface->SetStepType(*iter);
- for (double step=0.0001; step > 1e-5; step *= 0.5)
+ for (double step=0.001; step > 1e-5; step *= 0.5)
{
- unsigned int iterations = 10;
+ unsigned int iterations = 100;
if (*iter == "world")
iterations = 199;
for (; iterations < 200; iterations +=20, i++)
Modified: code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
2010-05-28 17:24:13 UTC (rev 8726)
+++ code/gazebo/branches/simpar/benchmarks/pioneer_spiral_benchmark.cc
2010-05-28 21:22:39 UTC (rev 8727)
@@ -168,23 +168,16 @@
factoryIface->Unlock();
}
-void RunSim(double stepTime)
+void RunSim()
{
simIface->StartLogEntity("pioneer");
simIface->Unpause();
- double left = 0.1/stepTime;
- if (left > 5)
- left = 5.0;
- double right = left * 0.5;
-
- std::cout << "Left[" << left << "] Right[" << right << "]\n";
-
double time = simIface->data->realTime;
while(simIface->data->realTime - time < 60)
{
- simIface->ApplyTorque("pioneer::left_wheel",gazebo::Vec3(0,0,-50));
- usleep(100000);
+ simIface->ApplyTorque("pioneer::left_wheel",gazebo::Vec3(0,0,-30));
+ usleep(20000);
}
simIface->StopLogEntity("pioneer");
@@ -255,7 +248,7 @@
//step_types.push_back("world");
//step_types.push_back("quick");
- for (iter = step_types.begin(); iter != step_types.end(); iter++)
+ /*for (iter = step_types.begin(); iter != step_types.end(); iter++)
{
std::string path =
std::string("/home/nate/work/simpar/data/pioneer_spiral/") + *iter + "/";
system((std::string("mkdir -p ")+path).c_str());
@@ -278,8 +271,9 @@
fprintf(out,"%d %f %d\n",i,step, iterations);
std::cout << "Type[" << *iter << "] Step[" << step << "] Iterations["
<< iterations << "]\n";
- RunSim(step);
-
+ */
+ RunSim();
+/*
std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + path +
"pioneer_spiral_benchmark_" + *iter + "_" + boost::lexical_cast<std::string>(i)
+ ".data";
system(mv_cmd.c_str());
}
@@ -287,6 +281,7 @@
fclose(out);
}
+ */
factoryIface->Close();
simIface->Close();
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
