Revision: 8424
http://playerstage.svn.sourceforge.net/playerstage/?rev=8424&view=rev
Author: natepak
Date: 2009-11-20 02:52:19 +0000 (Fri, 20 Nov 2009)
Log Message:
-----------
Added beginning of replay
Modified Paths:
--------------
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/World.hh
code/gazebo/trunk/server/gui/Gui.cc
code/gazebo/trunk/server/gui/Gui.hh
code/gazebo/trunk/server/physics/Body.cc
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-11-19 19:18:09 UTC (rev 8423)
+++ code/gazebo/trunk/server/World.cc 2009-11-20 02:52:19 UTC (rev 8424)
@@ -29,6 +29,7 @@
#include <fstream>
#include <sys/time.h> //gettimeofday
+#include "Body.hh"
#include "Factory.hh"
#include "GraphicsIfaceHandler.hh"
#include "Global.hh"
@@ -72,6 +73,11 @@
this->threadsP = new ParamT<int>("threads",2,0);
Param::End();
#endif
+
+ this->worldStates.resize(10000);
+ this->worldStatesInsertIter = this->worldStates.begin();
+ this->worldStatesEndIter = this->worldStates.end()-1;
+ this->worldStatesCurrentIter = this->worldStatesInsertIter;
}
////////////////////////////////////////////////////////////////////////////////
@@ -326,6 +332,8 @@
Simulator::Instance()->GetPhysicsEnabled())
{
this->physicsEngine->UpdatePhysics();
+
+ this->SaveState();
}
this->factory->Update();
@@ -602,13 +610,20 @@
}
////////////////////////////////////////////////////////////////////////////////
-// The plane and heighweighmap will not be registered
+// Register a geom
void World::RegisterGeom(Geom *geom)
{
this->geometries.push_back(geom);
}
////////////////////////////////////////////////////////////////////////////////
+// Register a body
+void World::RegisterBody( Body *body )
+{
+ this->bodies.push_back(body);
+}
+
+////////////////////////////////////////////////////////////////////////////////
// True if the bounding boxes of the models are being shown
bool World::GetShowBoundingBoxes()
{
@@ -1302,25 +1317,93 @@
this->toDeleteModels.clear();
}
-void World::GetInterfaceNames(Entity* en, std::vector<std::string>& list){
-
-
+void World::GetInterfaceNames(Entity* en, std::vector<std::string>& list)
+{
Model* m = dynamic_cast<Model*>(en);
- if(m){
+
+ if(m)
m->GetModelInterfaceNames(list);
- }
std::vector<Entity*>::iterator citer;
- for (citer=en->GetChildren().begin(); citer!=en->GetChildren().end();
citer++)
- {
-
-
+ for (citer=en->GetChildren().begin(); citer!=en->GetChildren().end();
citer++)
this->GetInterfaceNames((*citer),list);
-
+}
- }
+////////////////////////////////////////////////////////////////////////////////
+// Save the state of the world
+void World::SaveState()
+{
+ std::vector<Model*>::iterator mIter;
+ std::vector<Body*>::iterator bIter;
+ std::vector<Geom*>::iterator gIter;
+ WorldState *ws = &(*this->worldStatesInsertIter);
+ for (mIter = this->models.begin(); mIter != this->models.end(); mIter++)
+ ws->modelPoses[(*mIter)->GetName()] = (*mIter)->GetRelativePose();
+
+ for (bIter = this->bodies.begin(); bIter !=this->bodies.end(); bIter++)
+ ws->bodyPoses[(*bIter)->GetName()] = (*bIter)->GetRelativePose();
+
+ for (gIter = this->geometries.begin(); gIter !=this->geometries.end();
+ gIter++)
+ ws->geomPoses[(*gIter)->GetName()] = (*gIter)->GetRelativePose();
+
+ this->worldStatesInsertIter++;
+ if (this->worldStatesInsertIter == this->worldStates.end())
+ this->worldStatesInsertIter = this->worldStates.begin();
+
+ if (this->worldStatesInsertIter == this->worldStatesEndIter)
+ {
+ this->worldStatesEndIter++;
+ if (this->worldStatesEndIter == this->worldStates.end())
+ this->worldStatesEndIter = this->worldStates.begin();
+ }
}
+////////////////////////////////////////////////////////////////////////////////
+// Set the state of the world to the pos pointed to by the iterator
+void World::SetState(std::deque<WorldState>::iterator iter)
+{
+ std::vector<Model*>::iterator mIter;
+ std::vector<Body*>::iterator bIter;
+ std::vector<Geom*>::iterator gIter;
+ WorldState *ws = &(*iter);
+ for (mIter = this->models.begin(); mIter != this->models.end(); mIter++)
+ (*mIter)->SetRelativePose( ws->modelPoses[(*mIter)->GetName()] );
+ for (bIter = this->bodies.begin(); bIter !=this->bodies.end(); bIter++)
+ (*bIter)->SetRelativePose( ws->bodyPoses[(*bIter)->GetName()] );
+
+ for (gIter = this->geometries.begin(); gIter !=this->geometries.end();
+ gIter++)
+ (*gIter)->SetRelativePose( ws->geomPoses[(*gIter)->GetName()] );
+
+ this->worldStatesCurrentIter = iter;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Goto a position in time
+void World::GotoTime(double pos)
+{
+ Simulator::Instance()->SetPaused(true);
+
+ this->worldStatesCurrentIter = this->worldStatesInsertIter;
+
+ int i = (int)(this->worldStates.size() * (1.0-pos));
+
+ if (this->worldStatesCurrentIter == this->worldStates.begin())
+ this->worldStatesCurrentIter = this->worldStates.end()--;
+
+ for (;i>=0;i--, this->worldStatesCurrentIter--)
+ {
+ if (this->worldStatesCurrentIter == this->worldStates.begin())
+ this->worldStatesCurrentIter = this->worldStates.end()-1;
+
+ if (this->worldStatesCurrentIter == this->worldStatesEndIter)
+ break;
+ }
+
+ this->SetState(this->worldStatesCurrentIter);
+}
Modified: code/gazebo/trunk/server/World.hh
===================================================================
--- code/gazebo/trunk/server/World.hh 2009-11-19 19:18:09 UTC (rev 8423)
+++ code/gazebo/trunk/server/World.hh 2009-11-20 02:52:19 UTC (rev 8424)
@@ -29,6 +29,10 @@
#include <iostream>
#include <vector>
+#include <deque>
+#include <map>
+#include <string>
+
#include <boost/tuple/tuple.hpp>
#include <boost/signal.hpp>
@@ -59,6 +63,7 @@
class GraphicsIfaceHandler;
class OpenAL;
class Factory;
+ class WorldState;
/// \brief The World
/*
@@ -154,9 +159,12 @@
/// \brief Reset the simulation to the initial settings
public: void Reset();
- /// \brief register a geom This method is no more than a manually-done
signal system
+ /// \brief register a geom
public: void RegisterGeom(Geom *geom);
+ /// \brief Register a body
+ public: void RegisterBody(Body *body);
+
// User control of how the world is viewed
// If this section grows it may become a model-view structure ...
/// \brief Return true if the bounding boxes should be shown
@@ -183,6 +191,15 @@
/// \brief Get whether to view as wireframe
public: bool GetShowPhysics();
+ /// \brief Goto a position in time
+ public: void GotoTime(double pos);
+
+ /// \brief Save the state of the world
+ private: void SaveState();
+
+ /// \breif Set the state of the world to the pos pointed to by the iterator
+ private: void SetState(std::deque<WorldState>::iterator iter);
+
/// Set to true to show bounding boxes
private: bool showBoundingBoxes;
@@ -228,6 +245,9 @@
/// List of all the registered geometries
private: std::vector< Geom* > geometries;
+ /// List of all the registered bodies
+ private: std::vector< Body* > bodies;
+
/// List of models to delete from the world
private: std::vector< Model* > toDeleteModels;
@@ -259,8 +279,19 @@
private: friend class SingletonT<World>;
private: boost::signal<void (Entity*)> addEntitySignal;
+
+ private: std::deque<WorldState> worldStates;
+ private: std::deque<WorldState>::iterator worldStatesInsertIter;
+ private: std::deque<WorldState>::iterator worldStatesEndIter;
+ private: std::deque<WorldState>::iterator worldStatesCurrentIter;
};
+class WorldState
+{
+ public: std::map<std::string, Pose3d> modelPoses;
+ public: std::map<std::string, Pose3d> bodyPoses;
+ public: std::map<std::string, Pose3d> geomPoses;
+};
/// \}
}
Modified: code/gazebo/trunk/server/gui/Gui.cc
===================================================================
--- code/gazebo/trunk/server/gui/Gui.cc 2009-11-19 19:18:09 UTC (rev 8423)
+++ code/gazebo/trunk/server/gui/Gui.cc 2009-11-20 02:52:19 UTC (rev 8424)
@@ -32,6 +32,7 @@
#include <FL/Fl_Gl_Window.H>
#include <FL/fl_draw.H>
+#include "World.hh"
#include "Global.hh"
#include "XMLConfig.hh"
#include "GLFrameManager.hh"
@@ -69,12 +70,19 @@
new MainMenu(0, 0, w(), 20, (char *)"MainMenu");
this->toolbar = new Toolbar(0, 20, this->w(), 30);
- this->sidebar = new Sidebar(0, 60, toolbarWidth, this->h() - 90);
+ this->sidebar = new Sidebar(0, 60, toolbarWidth, this->h() - 115);
// Create the frame mamanger
this->frameMgr = new GLFrameManager(toolbarWidth, 60,
- this->w()-toolbarWidth, this->h()-90, "");
+ this->w()-toolbarWidth, this->h()-115, "");
+ this->timeSlider = new Fl_Slider(35,this->h()-50,this->w()-35,20,"Time:" );
+ this->timeSlider->type(FL_HOR_NICE_SLIDER);
+ this->timeSlider->align(FL_ALIGN_LEFT);
+ this->timeSlider->labelsize(10);
+ this->timeSlider->callback(&Gui::TimeSliderCB, this);
+ this->timeSlider->value(1.0);
+
// Create the status bar
this->statusbar = new StatusBar(0, this->h()-30,
width, 30);
@@ -156,6 +164,9 @@
this->toolbar->Update();
this->statusbar->Update();
this->frameMgr->Update();
+
+ if (!Simulator::Instance()->IsPaused())
+ this->timeSlider->value(1.0);
Fl::check();
@@ -204,4 +215,11 @@
return this->frameMgr->GetFPS();
}
-
+////////////////////////////////////////////////////////////////////////////////
+// Time slider CB
+void Gui::TimeSliderCB( Fl_Widget * w, void *data)
+{
+ //Gui *self = (Gui*)(data);
+ Fl_Slider *slider = (Fl_Slider*)(w);
+ World::Instance()->GotoTime( slider->value() );
+}
Modified: code/gazebo/trunk/server/gui/Gui.hh
===================================================================
--- code/gazebo/trunk/server/gui/Gui.hh 2009-11-19 19:18:09 UTC (rev 8423)
+++ code/gazebo/trunk/server/gui/Gui.hh 2009-11-20 02:52:19 UTC (rev 8424)
@@ -32,17 +32,18 @@
#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
+#include <FL/Fl_Slider.H>
#include <string>
#include <iostream>
#include "Param.hh"
#include "Vector2.hh"
-#define BG_COLOR fl_rgb_color(235,225,208 )
+//#define BG_COLOR fl_rgb_color(235,225,208 )
+#define BG_COLOR FL_BACKGROUND_COLOR
namespace gazebo
{
-
class GLWindow;
class Sidebar;
class Toolbar;
@@ -85,6 +86,9 @@
/// \brief Get the average FPS
public: float GetAvgFPS() const;
+ /// \brief Time slider cb
+ private: static void TimeSliderCB( Fl_Widget * w, void *data);
+
private: Toolbar *toolbar;
private: Sidebar *sidebar;
private: StatusBar *statusbar;
@@ -95,8 +99,9 @@
private: ParamT<Vector2<int> > *sizeP;
private: ParamT<Vector2<int> > *posP;
private: std::vector<Param*> parameters;
+
+ private: Fl_Slider *timeSlider;
};
-
}
#endif
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-11-19 19:18:09 UTC (rev
8423)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-11-20 02:52:19 UTC (rev
8424)
@@ -206,6 +206,8 @@
this->LoadSensor(childNode);
childNode = childNode->GetNextByNSPrefix("sensor");
}
+
+ World::Instance()->RegisterBody(this);
}
////////////////////////////////////////////////////////////////////////////////
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Let Crystal Reports handle the reporting - Free Crystal Reports 2008 30-Day
trial. Simplify your report design, integration and deployment - and focus on
what you do best, core application coding. Discover what's new with
Crystal Reports now. http://p.sf.net/sfu/bobj-july
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
