Revision: 8677
http://playerstage.svn.sourceforge.net/playerstage/?rev=8677&view=rev
Author: natepak
Date: 2010-05-13 20:29:21 +0000 (Thu, 13 May 2010)
Log Message:
-----------
Update the gui. Improved mouse selection, allow for creation of simple shapes in the GUI
Modified Paths:
--------------
code/gazebo/trunk/Media/materials/scripts/Gazebo.material
code/gazebo/trunk/Media/materials/textures/CMakeLists.txt
code/gazebo/trunk/Media/materials/textures/cursor.png
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Entity.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/Simulator.hh
code/gazebo/trunk/server/Vector3.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/World.hh
code/gazebo/trunk/server/gui/CMakeLists.txt
code/gazebo/trunk/server/gui/GLFrame.cc
code/gazebo/trunk/server/gui/GLWindow.cc
code/gazebo/trunk/server/gui/GLWindow.hh
code/gazebo/trunk/server/gui/Gui.cc
code/gazebo/trunk/server/gui/Gui.hh
code/gazebo/trunk/server/gui/Sidebar.cc
code/gazebo/trunk/server/gui/Sidebar.hh
code/gazebo/trunk/server/gui/Toolbar.cc
code/gazebo/trunk/server/gui/Toolbar.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Joint.cc
code/gazebo/trunk/server/physics/ode/ODEBody.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/rendering/CameraManager.cc
code/gazebo/trunk/server/rendering/CameraManager.hh
code/gazebo/trunk/server/rendering/OgreAdaptor.cc
code/gazebo/trunk/server/rendering/OgreAdaptor.hh
code/gazebo/trunk/server/rendering/OgreCamera.cc
code/gazebo/trunk/server/rendering/OgreCamera.hh
code/gazebo/trunk/server/rendering/OgreCreator.cc
code/gazebo/trunk/server/rendering/OgreCreator.hh
code/gazebo/trunk/server/rendering/OgreDynamicLines.cc
code/gazebo/trunk/server/rendering/OgreDynamicLines.hh
code/gazebo/trunk/server/rendering/OgreFrameListener.cc
code/gazebo/trunk/server/rendering/OgreFrameListener.hh
code/gazebo/trunk/server/rendering/SelectionObj.cc
code/gazebo/trunk/server/rendering/UserCamera.cc
code/gazebo/trunk/worlds/pioneer2dx.world
code/gazebo/trunk/worlds/simpleshapes.world
Added Paths:
-----------
code/gazebo/trunk/Media/materials/textures/box_create_blue.png
code/gazebo/trunk/Media/materials/textures/box_create_grey.png
code/gazebo/trunk/Media/materials/textures/control_end.png
code/gazebo/trunk/Media/materials/textures/control_end_blue.png
code/gazebo/trunk/Media/materials/textures/control_pause.png
code/gazebo/trunk/Media/materials/textures/control_pause_blue.png
code/gazebo/trunk/Media/materials/textures/control_play.png
code/gazebo/trunk/Media/materials/textures/control_play_blue.png
code/gazebo/trunk/Media/materials/textures/cylinder_create_blue.png
code/gazebo/trunk/Media/materials/textures/sphere_create_blue.png
code/gazebo/trunk/Media/materials/textures/sphere_create_grey.png
code/gazebo/trunk/server/gui/BoxMaker.cc
code/gazebo/trunk/server/gui/BoxMaker.hh
code/gazebo/trunk/server/gui/CylinderMaker.cc
code/gazebo/trunk/server/gui/CylinderMaker.hh
code/gazebo/trunk/server/gui/Events.cc
code/gazebo/trunk/server/gui/Events.hh
code/gazebo/trunk/server/gui/HingeJointMaker.cc
code/gazebo/trunk/server/gui/HingeJointMaker.hh
code/gazebo/trunk/server/gui/SphereMaker.cc
code/gazebo/trunk/server/gui/SphereMaker.hh
Modified: code/gazebo/trunk/Media/materials/scripts/Gazebo.material
===================================================================
--- code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2010-05-13 20:29:21 UTC (rev 8677)
@@ -159,6 +159,7 @@
}
}
+
material Gazebo/Red
{
receive_shadows on
@@ -167,7 +168,6 @@
{
pass
{
-
ambient 1.000000 0.000000 0.000000 1.000000
diffuse 1.000000 0.000000 0.000000 1.000000
specular 0.200000 0.000000 0.000000 1.000000
@@ -278,11 +278,11 @@
{
pass
{
- ambient 1.000000 0.000000 0.000000 1.000000
+ ambient 1.000000 0.000000 0.000000 1.000000
diffuse 1.000000 0.000000 0.000000 1.000000
specular 0.200000 0.000000 0.000000 1.000000
emissive 1.000000 0.000000 0.000000 1.000000
- lighting on
+ lighting on
}
}
}
@@ -959,7 +959,7 @@
texture_unit
{
colour_op_ex source1 src_current src_current 1 0 0
- alpha_op_ex source1 src_manual src_current 0.1
+ alpha_op_ex source1 src_manual src_current 0.4
}
}
}
@@ -980,12 +980,33 @@
texture_unit
{
colour_op_ex source1 src_current src_current 0 1 0
- alpha_op_ex source1 src_manual src_current 0.1
+ alpha_op_ex source1 src_manual src_current 0.4
}
}
}
}
+material Gazebo/BlueTransparent
+{
+ technique
+ {
+ pass
+ {
+ scene_blend alpha_blend
+ depth_write off
+
+ ambient 0.0 0.0 1.0 1
+ diffuse 0.0 0.0 1.0 1
+
+ texture_unit
+ {
+ colour_op_ex source1 src_current src_current 0 1 0
+ alpha_op_ex source1 src_manual src_current 0.4
+ }
+ }
+ }
+}
+
material Gazebo/DepthMap
{
technique
Modified: code/gazebo/trunk/Media/materials/textures/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/Media/materials/textures/CMakeLists.txt 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/Media/materials/textures/CMakeLists.txt 2010-05-13 20:29:21 UTC (rev 8677)
@@ -107,6 +107,18 @@
hanoi_red_disk.tga
hanoi_green_disk.tga
hanoi_blue_disk.tga
+ control_pause.png
+ control_pause_blue.png
+ control_play.png
+ control_play_blue.png
+ control_end.png
+ control_end_blue.png
+ box_create_blue.png
+ box_create_grey.png
+ sphere_create_blue.png
+ sphere_create_grey.png
+ cylinder_create_blue.png
+ cursor.png
)
INSTALL(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo/Media/materials/textures/)
Added: code/gazebo/trunk/Media/materials/textures/box_create_blue.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/box_create_blue.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/box_create_grey.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/box_create_grey.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/control_end.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/control_end.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/control_end_blue.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/control_end_blue.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/control_pause.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/control_pause.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/control_pause_blue.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/control_pause_blue.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/control_play.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/control_play.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/control_play_blue.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/control_play_blue.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Modified: code/gazebo/trunk/Media/materials/textures/cursor.png
===================================================================
(Binary files differ)
Added: code/gazebo/trunk/Media/materials/textures/cylinder_create_blue.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/cylinder_create_blue.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/sphere_create_blue.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/sphere_create_blue.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: code/gazebo/trunk/Media/materials/textures/sphere_create_grey.png
===================================================================
(Binary files differ)
Property changes on: code/gazebo/trunk/Media/materials/textures/sphere_create_grey.png
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/Entity.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -258,7 +258,7 @@
////////////////////////////////////////////////////////////////////////////////
/// Return the name of this entity with the model scope
/// model1::...::modelN::entityName
-std::string Entity::GetScopedName()
+std::string Entity::GetScopedName() const
{
Entity *p = this->parent;
std::string scopedName = this->GetName();
@@ -279,7 +279,7 @@
////////////////////////////////////////////////////////////////////////////////
/// Return the name of this entity with the model scope
/// model1::...::modelN::entityName
-std::string Entity::GetCompleteScopedName()
+std::string Entity::GetCompleteScopedName() const
{
Entity *p = this->parent;
std::string scopedName = this->GetName();
Modified: code/gazebo/trunk/server/Entity.hh
===================================================================
--- code/gazebo/trunk/server/Entity.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/Entity.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -72,7 +72,7 @@
/// \brief Get the parent
/// \return Pointer to the parent entity
public: Entity *GetParent() const;
-
+
/// \brief Add a child to this entity
/// \param child Child entity
public: void AddChild(Entity *child);
@@ -142,11 +142,11 @@
/// \brief Return the name of this entity with the model scope
/// model1::...::modelN::entityName
- public: std::string GetScopedName();
+ public: std::string GetScopedName() const;
/// \brief Return the name of this entity with the model+body+geom scope
/// model1::...::modelN::bodyN::entityName
- public: std::string GetCompleteScopedName();
+ public: std::string GetCompleteScopedName() const;
/// \brief Set the type of this entity
public: void SetType(Type type);
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/Model.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -880,7 +880,6 @@
gzthrow( "can't have two joint with the same name");
this->joints.push_back( joint );
-
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/Simulator.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -70,15 +70,13 @@
guiEnabled(true),
renderEngineEnabled(true),
physicsEnabled(true),
- timeout(-1),
- selectedEntity(NULL),
- selectedBody(NULL)
+ timeout(-1)
{
- this->selectedEntity = NULL;
this->render_mutex = new boost::recursive_mutex();
this->model_delete_mutex = new boost::recursive_mutex();
this->startTime = this->GetWallTime();
this->gazeboConfig=new gazebo::GazeboConfig();
+ this->pause = false;
}
////////////////////////////////////////////////////////////////////////////////
@@ -495,6 +493,9 @@
{
boost::recursive_mutex::scoped_lock lock(*this->GetMRMutex());
this->stepInc = step;
+ this->stepSignal(step);
+
+ this->SetPaused(!step);
}
////////////////////////////////////////////////////////////////////////////////
@@ -547,37 +548,6 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Set the selected entity
-void Simulator::SetSelectedEntity( Entity *ent )
-{
- // unselect selectedEntity
- if (this->selectedEntity)
- {
- this->selectedEntity->GetVisualNode()->ShowSelectionBox(false);
- this->selectedEntity->SetSelected(false);
- }
-
- // if a different entity is selected, show bounding box and SetSelected(true)
- if (this->selectedEntity != ent)
- {
- // set selected entity to ent
- this->selectedEntity = ent;
- this->selectedEntity->GetVisualNode()->ShowSelectionBox(true);
- this->selectedEntity->SetSelected(true);
- }
- else
- this->selectedEntity = NULL;
-
-}
-
-////////////////////////////////////////////////////////////////////////////////
-/// Get the selected entity
-Entity *Simulator::GetSelectedEntity() const
-{
- return this->selectedEntity;
-}
-
-////////////////////////////////////////////////////////////////////////////////
/// Get the model that contains the entity
Model *Simulator::GetParentModel( Entity *entity ) const
{
@@ -643,20 +613,13 @@
currTime = this->GetRealTime();
userStepped = false;
-
- // Update the physics engine
- //if (!this->GetUserPause() && !this->IsPaused() ||
- // (this->GetUserPause() && this->GetUserStepInc()))
- if (!this->IsPaused() || this->GetStepInc())
- {
+ if (this->IsPaused())
+ this->pauseTime += step;
+ else
this->simTime += step;
- if (this->GetStepInc())
- userStepped = true;
- this->SetPaused(false);
- }
- else
- this->pauseTime += step;
+ if (this->GetStepInc())
+ userStepped = true;
lastTime = this->GetRealTime();
Modified: code/gazebo/trunk/server/Simulator.hh
===================================================================
--- code/gazebo/trunk/server/Simulator.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/Simulator.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -156,15 +156,6 @@
/// \brief Get the physics enabled/disabled
public: bool GetPhysicsEnabled() const;
- /// \brief Set the selected entity
- public: void SetSelectedEntity( Entity *ent );
-
- /// \brief Set the selected body
- public: void SetSelectedBody( Body *bod );
-
- /// \brief Get the selected entity
- public: Entity *GetSelectedEntity() const;
-
/// \brief Get the model that contains the entity
public: Model *GetParentModel( Entity *entity ) const;
@@ -186,6 +177,13 @@
{
pauseSignal.connect(subscriber);
}
+
+ /// \brief Connect a boost::slot the the step signal
+ public: template<typename T>
+ void ConnectStepSignal( T subscriber )
+ {
+ stepSignal.connect(subscriber);
+ }
/// \brief Function to run gui. Used by guiThread
private: void PhysicsLoop();
@@ -241,10 +239,6 @@
/// Length of time the simulator is allowed to run
private: double updateTime;
- /// The entity currently selected by the user
- private: Entity *selectedEntity;
- private: Body *selectedBody;
-
/// Thread in which to run the gui
private: boost::thread *physicsThread;
@@ -254,6 +248,7 @@
private: State state;
private: boost::signal<void (bool)> pauseSignal;
+ private: boost::signal<void (bool)> stepSignal;
//Singleton implementation
private: friend class DestroyerT<Simulator>;
Modified: code/gazebo/trunk/server/Vector3.cc
===================================================================
--- code/gazebo/trunk/server/Vector3.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/Vector3.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -104,11 +104,11 @@
////////////////////////////////////////////////////////////////////////////////
Vector3 Vector3::GetCrossProd(const Vector3 &pt) const
{
- Vector3 c;
+ Vector3 c(0,0,0);
- c.x = this->y * pt.z - this->z * pt.y;
- c.y = -this->x * pt.z + this->z * pt.x;
- c.z = this->x * pt.y - this->y * pt.x;
+ c.x = this->y * pt.z - this->z * pt.y;
+ c.y = this->z * pt.x - this->x * pt.z;
+ c.z = this->x * pt.y - this->y * pt.x;
return c;
}
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/World.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -29,6 +29,7 @@
#include <fstream>
#include <sys/time.h> //gettimeofday
+#include "OgreVisual.hh"
#include "Body.hh"
#include "Factory.hh"
#include "GraphicsIfaceHandler.hh"
@@ -50,7 +51,6 @@
using namespace gazebo;
-
////////////////////////////////////////////////////////////////////////////////
// Private constructor
World::World()
@@ -68,6 +68,7 @@
this->server = NULL;
this->graphics = NULL;
this->openAL = NULL;
+ this->selectedEntity = NULL;
PhysicsFactory::RegisterAll();
this->factory = NULL;
@@ -1355,9 +1356,6 @@
}
}
- /*for(unsigned int ii=0;ii<candids.size();ii++)
- printf("candidatetypes: %s\n",candids[ii].c_str());*/
-
for(i=0; i<candids.size(); i++)
{
if(candids[i][0]=='>')
@@ -1381,8 +1379,6 @@
response->strValue[511]='\0';
}
- //printf("-> modeltype: %s \n", response->modelType);
-
response++;
this->simIface->data->responseCount += 1;
@@ -1408,7 +1404,6 @@
unsigned int index = list[jj].find(">>");
if(index !=std::string::npos)
list[jj].replace(index,list[jj].size(),"");
- //printf("-->> %s \n",list[jj].c_str());
}
@@ -1520,7 +1515,7 @@
case SimulationRequestData::GO:
{
int sec = req->runTime/1000;
- int nsec = (req->runTime - sec) * 1e9;
+ int nsec = (req->runTime - sec*1000) * 1e9;
this->simPauseTime = Simulator::Instance()->GetSimTime()
+ Time(sec, nsec);
@@ -1667,3 +1662,35 @@
if (!p)
this->worldStatesInsertIter = this->worldStatesCurrentIter;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the selected entity
+void World::SetSelectedEntity( Entity *ent )
+{
+ // unselect selectedEntity
+ if (this->selectedEntity)
+ {
+ this->selectedEntity->GetVisualNode()->ShowSelectionBox(false);
+ this->selectedEntity->SetSelected(false);
+ }
+
+ // if a different entity is selected, show bounding box and SetSelected(true)
+ if (ent && this->selectedEntity != ent)
+ {
+ // set selected entity to ent
+ this->selectedEntity = ent;
+ this->selectedEntity->GetVisualNode()->ShowSelectionBox(true);
+ this->selectedEntity->SetSelected(true);
+ }
+ else
+ this->selectedEntity = NULL;
+
+ this->entitySelectedSignal(this->selectedEntity);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the selected entity
+Entity *World::GetSelectedEntity() const
+{
+ return this->selectedEntity;
+}
Modified: code/gazebo/trunk/server/World.hh
===================================================================
--- code/gazebo/trunk/server/World.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/World.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -223,6 +223,12 @@
/// \brief Goto a position in time
public: void GotoTime(double pos);
+ /// \brief Set the selected entity
+ public: void SetSelectedEntity( Entity *ent );
+
+ /// \brief Get the selected entity
+ public: Entity *GetSelectedEntity() const;
+
/// \brief Save the state of the world
private: void SaveState();
@@ -346,8 +352,15 @@
void DisconnectWorldUpdateEndSignal( T subscriber )
{ worldUpdateEndSignal.disconnect(subscriber); }
+ /// \brief Connect a boost::slot the the entity selected signal
+ public: template<typename T>
+ boost::signals::connection ConnectEntitySelectedSignal(T subscriber)
+ { return entitySelectedSignal.connect(subscriber); }
+ /// \brief Disconnect a boost::slot the the entity selected signal
+ public: template<typename T>
+ void DisconnectEntitySelectedSignal( T subscriber )
+ { entitySelectedSignal.disconnect(subscriber); }
-
/// \brief Get the names of interfaces defined in the tree of a model
private: void GetInterfaceNames(Entity* m, std::vector<std::string>& list);
@@ -389,6 +402,10 @@
private: friend class DestroyerT<World>;
private: friend class SingletonT<World>;
+ /// The entity currently selected by the user
+ private: Entity *selectedEntity;
+
+
private: boost::signal<void (Entity*)> addEntitySignal;
private: boost::signal<void (bool)> showLightsSignal;
private: boost::signal<void (bool)> showCamerasSignal;
@@ -397,6 +414,7 @@
private: boost::signal<void (bool)> showPhysicsSignal;
private: boost::signal<void (bool)> showJointsSignal;
private: boost::signal<void (bool)> showBoundingBoxesSignal;
+ private: boost::signal<void (Entity*)> entitySelectedSignal;
private: boost::signal<void ()> worldUpdateStartSignal;
private: boost::signal<void ()> worldUpdateEndSignal;
Added: code/gazebo/trunk/server/gui/BoxMaker.cc
===================================================================
--- code/gazebo/trunk/server/gui/BoxMaker.cc (rev 0)
+++ code/gazebo/trunk/server/gui/BoxMaker.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,171 @@
+#include <iostream>
+#include <FL/Fl.H>
+
+#include "Simulator.hh"
+#include "GLWindow.hh"
+#include "OgreVisual.hh"
+#include "OgreCreator.hh"
+#include "CameraManager.hh"
+#include "OgreCamera.hh"
+#include "World.hh"
+#include "BoxMaker.hh"
+
+using namespace gazebo;
+
+BoxMaker::BoxMaker()
+{
+ this->state = 0;
+ this->visualName = "";
+ this->leftMousePressed = false;
+ this->index = 0;
+}
+
+BoxMaker::~BoxMaker()
+{
+}
+
+void BoxMaker::Start()
+{
+ std::ostringstream stream;
+ std::string boxName = "user_box";
+
+ do
+ {
+ stream.str("");
+ stream << boxName << index;
+ this->index++;
+ } while (OgreCreator::Instance()->GetVisual(stream.str()));
+
+ this->visualName = stream.str();
+ this->state = 1;
+}
+
+void BoxMaker::Stop()
+{
+ this->state = 0;
+}
+
+bool BoxMaker::IsActive() const
+{
+ return this->state > 0;
+}
+
+void BoxMaker::MousePushCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->leftMousePressed = false;
+
+ this->mousePushPos = mousePos;
+
+ switch (Fl::event_button())
+ {
+ case FL_LEFT_MOUSE:
+ this->leftMousePressed = true;
+ break;
+ }
+}
+
+void BoxMaker::MouseReleaseCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->state++;
+
+ if (this->state == 3)
+ {
+ this->CreateTheBox();
+ this->Start();
+ }
+}
+
+void BoxMaker::MouseDragCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ Vector3 norm;
+ Vector3 p1, p2;
+
+ //if (this->state == 1)
+ norm.Set(0,0,1);
+ /*else if (this->state == 2)
+ {
+ norm = CameraManager::Instance()->GetActiveCamera()->GetDirection();
+ }*/
+
+ p1 = GLWindow::GetWorldPointOnPlane(this->mousePushPos.x, this->mousePushPos.y, norm, 0);
+ p2 = GLWindow::GetWorldPointOnPlane(mousePos.x, mousePos.y ,norm, 0);
+
+ OgreVisual *vis = NULL;
+ if (OgreCreator::Instance()->GetVisual(this->visualName))
+ vis = OgreCreator::Instance()->GetVisual(this->visualName);
+ else
+ {
+ vis = OgreCreator::Instance()->CreateVisual(this->visualName);
+ vis->AttachMesh("unit_box_U1V1");
+ vis->SetPosition(p1);
+ }
+
+ Vector3 scale = p1-p2;
+ Vector3 p = vis->GetPosition();
+
+ if (this->state == 1)
+ {
+ scale.z = 0.01;
+ p.x = p1.x - scale.x/2.0;
+ p.y = p1.y - scale.y/2.0;
+ }
+ else
+ {
+ scale = vis->GetScale();
+ scale.z = (this->mousePushPos.y - mousePos.y)*0.01;
+ p.z = scale.z/2.0;
+ }
+
+ vis->SetPosition(p);
+
+ vis->SetScale(scale);
+
+}
+
+void BoxMaker::CreateTheBox()
+{
+ boost::recursive_mutex::scoped_lock lock( *Simulator::Instance()->GetMRMutex());
+
+ std::ostringstream newModelStr;
+
+
+ OgreVisual *vis = OgreCreator::Instance()->GetVisual(this->visualName);
+ if (vis == NULL)
+ {
+ return;
+ }
+
+ newModelStr << "<?xml version='1.0'?> <gazebo:world xmlns:xi='http://www.w3.org/2001/XInclude' xmlns:gazebo='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz' xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model' xmlns:sensor='http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor' xmlns:body='http://playerstage.sourceforge.net/gazebo/xmlschema/#body' xmlns:geom='http://playerstage.sourceforge.net/gazebo/xmlschema/#geom' xmlns:joint='http://playerstage.sourceforge.net/gazebo/xmlschema/#joint' xmlns:interface='http://playerstage.sourceforge.net/gazebo/xmlschema/#interface' xmlns:rendering='http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering' xmlns:renderable='http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable' xmlns:controller='http://playerstage.sourceforge.net/gazebo/xmlschema/#controller' xmlns:physics='http://playerstage.sourceforge.net/gazebo/xmlschema/#physics' >";
+
+
+ newModelStr << "<model:physical name=\"" << this->visualName << "\">\
+ <xyz>" << vis->GetPosition() << "</xyz>\
+ <body:box name=\"body\">\
+ <geom:box name=\"geom\">\
+ <size>" << vis->GetScale() << "</size>\
+ <mass>0.5</mass>\
+ <visual>\
+ <mesh>unit_box_U1V1</mesh>\
+ <size>" << vis->GetScale() << "</size>\
+ <material>Gazebo/Grey</material>\
+ </visual>\
+ </geom:box>\
+ </body:box>\
+ </model:physical>";
+
+ newModelStr << "</gazebo:world>";
+
+ OgreCreator::Instance()->DeleteVisual(this->visualName);
+
+ World::Instance()->InsertEntity(newModelStr.str());
+}
+
Added: code/gazebo/trunk/server/gui/BoxMaker.hh
===================================================================
--- code/gazebo/trunk/server/gui/BoxMaker.hh (rev 0)
+++ code/gazebo/trunk/server/gui/BoxMaker.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,30 @@
+#ifndef BOXMAKER_HH
+#define BOXMAKER_HH
+
+#include "Vector2.hh"
+
+namespace gazebo
+{
+ class BoxMaker
+ {
+ public: BoxMaker();
+ public: virtual ~BoxMaker();
+
+ public: void Start();
+ public: void Stop();
+ public: bool IsActive() const;
+
+ public: void MousePushCB(Vector2<int> mousePos);
+ public: void MouseReleaseCB(Vector2<int> mousePos);
+ public: void MouseDragCB(Vector2<int> mousePos);
+
+ private: void CreateTheBox();
+ private: int state;
+ private: bool leftMousePressed;
+ private: Vector2<int> mousePushPos;
+ private: std::string visualName;
+ private: int index;
+ };
+}
+
+#endif
Modified: code/gazebo/trunk/server/gui/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/gui/CMakeLists.txt 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/CMakeLists.txt 2010-05-13 20:29:21 UTC (rev 8677)
@@ -18,6 +18,11 @@
StatusBar.cc
GLFrameManager.cc
GLFrame.cc
+ BoxMaker.cc
+ SphereMaker.cc
+ CylinderMaker.cc
+ HingeJointMaker.cc
+ Events.cc
)
set (headers Gui.hh
@@ -28,6 +33,11 @@
StatusBar.hh
GLFrameManager.hh
GLFrame.hh
+ BoxMaker.hh
+ SphereMaker.hh
+ CylinderMaker.hh
+ HingeJointMaker.hh
+ Events.hh
)
LIST_TO_STRING(GAZEBO_CFLAGS "${gazeboserver_cflags}")
Added: code/gazebo/trunk/server/gui/CylinderMaker.cc
===================================================================
--- code/gazebo/trunk/server/gui/CylinderMaker.cc (rev 0)
+++ code/gazebo/trunk/server/gui/CylinderMaker.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,161 @@
+#include <iostream>
+#include <FL/Fl.H>
+
+#include "Simulator.hh"
+#include "GLWindow.hh"
+#include "OgreVisual.hh"
+#include "OgreCreator.hh"
+#include "World.hh"
+#include "CylinderMaker.hh"
+
+using namespace gazebo;
+
+CylinderMaker::CylinderMaker()
+{
+ this->state = 0;
+ this->visualName = "";
+ this->leftMousePressed = false;
+ this->index = 0;
+}
+
+CylinderMaker::~CylinderMaker()
+{
+}
+
+void CylinderMaker::Start()
+{
+ std::ostringstream stream;
+ std::string name = "user_cylinder";
+
+ do
+ {
+ stream.str("");
+ stream << name << index;
+ this->index++;
+ } while (OgreCreator::Instance()->GetVisual(stream.str()));
+
+ this->visualName = stream.str();
+ this->state = 1;
+}
+
+void CylinderMaker::Stop()
+{
+ this->state = 0;
+}
+
+bool CylinderMaker::IsActive() const
+{
+ return this->state > 0;
+}
+
+void CylinderMaker::MousePushCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->leftMousePressed = false;
+
+ this->mousePushPos = mousePos;
+
+ switch (Fl::event_button())
+ {
+ case FL_LEFT_MOUSE:
+ this->leftMousePressed = true;
+ break;
+ }
+}
+
+void CylinderMaker::MouseReleaseCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->state++;
+
+ if (this->state == 3)
+ {
+ this->CreateTheCylinder();
+ this->Start();
+ }
+}
+
+void CylinderMaker::MouseDragCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ Vector3 norm;
+ Vector3 p1, p2;
+
+ if (this->state == 1)
+ norm.Set(0,0,1);
+ else if (this->state == 2)
+ norm.Set(1,0,0);
+
+ p1 = GLWindow::GetWorldPointOnPlane(this->mousePushPos.x, this->mousePushPos.y, norm, 0);
+ p2 = GLWindow::GetWorldPointOnPlane(mousePos.x, mousePos.y ,norm, 0);
+
+ OgreVisual *vis = NULL;
+ if (OgreCreator::Instance()->GetVisual(this->visualName))
+ vis = OgreCreator::Instance()->GetVisual(this->visualName);
+ else
+ {
+ vis = OgreCreator::Instance()->CreateVisual(this->visualName);
+ vis->AttachMesh("unit_cylinder");
+ vis->SetPosition(p1);
+ }
+
+ Vector3 p = vis->GetPosition();
+ Vector3 scale;
+
+ if (this->state == 1)
+ {
+ double dist = p1.Distance(p2);
+ scale.x = dist*2;
+ scale.y = dist*2;
+ scale.z = 0.01;
+ }
+ else
+ {
+ scale = vis->GetScale();
+ // scale.z = p2.z - p1.z;
+ scale.z = (this->mousePushPos.y - mousePos.y)*0.01;
+ p.z = scale.z/2.0;
+ }
+
+ vis->SetPosition(p);
+ vis->SetScale(scale);
+}
+
+void CylinderMaker::CreateTheCylinder()
+{
+ boost::recursive_mutex::scoped_lock lock( *Simulator::Instance()->GetMRMutex());
+
+ std::ostringstream newModelStr;
+
+ OgreVisual *vis = OgreCreator::Instance()->GetVisual(this->visualName);
+
+ newModelStr << "<?xml version='1.0'?> <gazebo:world xmlns:xi='http://www.w3.org/2001/XInclude' xmlns:gazebo='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz' xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model' xmlns:sensor='http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor' xmlns:body='http://playerstage.sourceforge.net/gazebo/xmlschema/#body' xmlns:geom='http://playerstage.sourceforge.net/gazebo/xmlschema/#geom' xmlns:joint='http://playerstage.sourceforge.net/gazebo/xmlschema/#joint' xmlns:interface='http://playerstage.sourceforge.net/gazebo/xmlschema/#interface' xmlns:rendering='http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering' xmlns:renderable='http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable' xmlns:controller='http://playerstage.sourceforge.net/gazebo/xmlschema/#controller' xmlns:physics='http://playerstage.sourceforge.net/gazebo/xmlschema/#physics' >";
+
+
+ newModelStr << "<model:physical name=\"" << this->visualName << "\">\
+ <xyz>" << vis->GetPosition() << "</xyz>\
+ <body:cylinder name=\"body\">\
+ <geom:cylinder name=\"geom\">\
+ <size>" << vis->GetScale().x*.5 << " " << vis->GetScale().z << "</size>\
+ <mass>0.5</mass>\
+ <visual>\
+ <mesh>unit_cylinder</mesh>\
+ <size>" << vis->GetScale() << "</size>\
+ <material>Gazebo/Grey</material>\
+ </visual>\
+ </geom:cylinder>\
+ </body:cylinder>\
+ </model:physical>";
+
+ newModelStr << "</gazebo:world>";
+
+ OgreCreator::Instance()->DeleteVisual(this->visualName);
+ World::Instance()->InsertEntity(newModelStr.str());
+}
+
Added: code/gazebo/trunk/server/gui/CylinderMaker.hh
===================================================================
--- code/gazebo/trunk/server/gui/CylinderMaker.hh (rev 0)
+++ code/gazebo/trunk/server/gui/CylinderMaker.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,29 @@
+#ifndef CYLINDERMAKER_HH
+#define CYLINDERMAKER_HH
+
+#include "Vector2.hh"
+
+namespace gazebo
+{
+ class CylinderMaker
+ {
+ public: CylinderMaker();
+ public: virtual ~CylinderMaker();
+
+ public: void Start();
+ public: void Stop();
+ public: bool IsActive() const;
+
+ public: void MousePushCB(Vector2<int> mousePos);
+ public: void MouseReleaseCB(Vector2<int> mousePos);
+ public: void MouseDragCB(Vector2<int> mousePos);
+
+ private: void CreateTheCylinder();
+ private: int state;
+ private: bool leftMousePressed;
+ private: Vector2<int> mousePushPos;
+ private: std::string visualName;
+ private: int index;
+ };
+}
+#endif
Added: code/gazebo/trunk/server/gui/Events.cc
===================================================================
--- code/gazebo/trunk/server/gui/Events.cc (rev 0)
+++ code/gazebo/trunk/server/gui/Events.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,5 @@
+#include "Events.hh"
+
+using namespace gazebo;
+
+boost::signal<void (std::string)> Events::createEntitySignal;
Added: code/gazebo/trunk/server/gui/Events.hh
===================================================================
--- code/gazebo/trunk/server/gui/Events.hh (rev 0)
+++ code/gazebo/trunk/server/gui/Events.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,25 @@
+#ifndef EVENTS_HH
+#define EVENTS_HH
+
+#include <boost/signal.hpp>
+
+namespace gazebo
+{
+ class Events
+ {
+ /// \brief Connect a boost::slot the the add entity signal
+ public: template<typename T>
+ static boost::signals::connection ConnectCreateEntitySignal( T subscriber )
+ { return createEntitySignal.connect(subscriber); }
+
+ public: template<typename T>
+ static void DisconnectCreateEntitySignal( T subscriber)
+ { createEntitySignal.disconnect(subscriber); }
+
+ public: static boost::signal<void (std::string)> createEntitySignal;
+ };
+
+}
+
+
+#endif
Modified: code/gazebo/trunk/server/gui/GLFrame.cc
===================================================================
--- code/gazebo/trunk/server/gui/GLFrame.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/GLFrame.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -316,4 +316,6 @@
Model *model = dynamic_cast<Model*>(World::Instance()->GetEntityByName(choice->text() ));
frame->glWindow->GetCamera()->TrackModel( model );
}
+
+ frame->glWindow->take_focus();
}
Modified: code/gazebo/trunk/server/gui/GLWindow.cc
===================================================================
--- code/gazebo/trunk/server/gui/GLWindow.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/GLWindow.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -48,6 +48,7 @@
#include "CameraManager.hh"
#include "UserCamera.hh"
#include "World.hh"
+#include "Gui.hh"
#include "OgreHUD.hh"
#include "OgreAdaptor.hh"
@@ -56,6 +57,7 @@
#include "GLWindow.hh"
#include <boost/thread.hpp>
+#include "Events.hh"
using namespace gazebo;
@@ -84,12 +86,16 @@
if (activeWin == NULL)
activeWin = this;
+
+ Events::ConnectCreateEntitySignal( boost::bind(&GLWindow::CreateEntity, this, _1) );
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
GLWindow::~GLWindow()
{
+ Events::DisconnectCreateEntitySignal( boost::bind(&GLWindow::CreateEntity, this, _1) );
+
if (this->userCamera)
delete this->userCamera;
}
@@ -166,7 +172,7 @@
this->lastUpdateTime = Simulator::Instance()->GetRealTime();
// continuously apply force to selected body
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
+ Entity *entity = World::Instance()->GetSelectedEntity();
if (entity && entity->GetType() == Entity::BODY &&
(this->keys[FL_Control_L] || this->keys[FL_Control_R]) )
@@ -176,10 +182,6 @@
{
body->SetForce(this->forceVec);
}
- if (this->leftMousePressed && body)
- {
- body->SetTorque(this->torqueVec);
- }
}
}
@@ -215,7 +217,16 @@
this->mousePushPos = this->mousePos;
- if (Simulator::Instance()->GetSelectedEntity())
+ this->boxMaker.MousePushCB(this->mousePos);
+ this->sphereMaker.MousePushCB(this->mousePos);
+ this->cylinderMaker.MousePushCB(this->mousePos);
+ this->hingeJointMaker.MousePushCB(this->mousePos);
+
+ if (this->boxMaker.IsActive() || this->sphereMaker.IsActive() ||
+ this->cylinderMaker.IsActive() || this->hingeJointMaker.IsActive())
+ return;
+
+ if (World::Instance()->GetSelectedEntity())
{
OgreAdaptor::Instance()->GetEntityAt(this->activeCamera,
this->mousePos, this->mouseModifier);
@@ -244,6 +255,17 @@
/// Handle a mouse button release
void GLWindow::HandleMouseRelease()
{
+ OgreCreator::SetVisible("guiline", false);
+
+ this->boxMaker.MouseReleaseCB(this->mousePos);
+ this->sphereMaker.MouseReleaseCB(this->mousePos);
+ this->cylinderMaker.MouseReleaseCB(this->mousePos);
+ this->hingeJointMaker.MouseReleaseCB(this->mousePos);
+
+ if (this->boxMaker.IsActive() || this->sphereMaker.IsActive() ||
+ this->cylinderMaker.IsActive() || this->hingeJointMaker.IsActive())
+ return;
+
// Get the mouse button that was pressed (if one was pressed)
switch (Fl::event_button())
{
@@ -277,13 +299,13 @@
{
case FL_LEFT_MOUSE:
if (model)
- Simulator::Instance()->SetSelectedEntity( model );
+ World::Instance()->SetSelectedEntity( model );
this->mouseOriginPos = Vector2<int>( Fl::event_x(), Fl::event_y() );
break;
case FL_RIGHT_MOUSE:
if (body)
- Simulator::Instance()->SetSelectedEntity( body );
+ World::Instance()->SetSelectedEntity( body );
this->mouseOriginPos = Vector2<int>( Fl::event_x(), Fl::event_y() );
break;
@@ -301,7 +323,16 @@
{
// stop simulation when this is happening
boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMRMutex());
+ this->mouseDrag = true;
+ this->boxMaker.MouseDragCB(this->mousePos);
+ this->sphereMaker.MouseDragCB(this->mousePos);
+ this->cylinderMaker.MouseDragCB(this->mousePos);
+ this->hingeJointMaker.MouseDragCB(this->mousePos);
+ if (this->boxMaker.IsActive() || this->sphereMaker.IsActive() ||
+ this->cylinderMaker.IsActive() || this->hingeJointMaker.IsActive())
+ return;
+
if (this->activeCamera && this->activeCamera->GetUserMovable())
{
Vector2<int> drag = this->mousePos - this->prevMousePos;
@@ -311,171 +342,29 @@
Vector3 camUp = this->activeCamera->GetUp();
Vector3 camRight = this->activeCamera->GetRight();
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
+ Entity *entity = World::Instance()->GetSelectedEntity();
if (this->leftMousePressed)
{
+
if ( entity && (entity->GetType() == Entity::MODEL ||
entity->GetType() == Entity::BODY) &&
(this->keys[FL_Control_L] || this->keys[FL_Control_R]) )
{
- if (entity->GetType() == Entity::MODEL && this->mouseModifier.substr(0,3) == "rot")
- {
- Model *model = (Model*)(entity);
-
- //
- // interactively change rotation pose to selected item,
- // rotate about axis perpendicular to view port plane
- //
- Pose3d modelPose = model->GetRelativePose();
-
- /*double distance = (modelPose.pos - this->activeCamera->GetCameraPosition()).GetLength();
- double scaleX = distance * tan (this->activeCamera->GetHFOV().GetAsRadian() / 2.0f ) * 2.0f;
- double scaleY = distance * tan (this->activeCamera->GetVFOV().GetAsRadian() / 2.0f ) * 2.0f;
- */
-
- Vector3 planeNorm, planeNorm2;
- Vector3 origin1, dir1, p1;
- Vector3 origin2, dir2, p2;
- Vector3 a,b;
- Vector3 ray(0,0,0);
-
- // Cast two rays from the camera into the world
- this->activeCamera->GetCameraToViewportRay(this->mousePos.x,
- this->mousePos.y, origin1, dir1);
- this->activeCamera->GetCameraToViewportRay(this->prevMousePos.x,
- this->prevMousePos.y, origin2, dir2);
-
- // Figure out which axis to rotate around
- if (this->mouseModifier == "rotx")
- ray.x = 1.0;
- else if (this->mouseModifier == "roty")
- ray.y = 1.0;
- else if (this->mouseModifier == "rotz")
- ray.z = 1.0;
-
- // Compute the normal to the plane on which to rotate
- planeNorm = modelPose.rot * ray;
-
- // Compute the distance from the camera to plane of rotation
- double d = modelPose.pos.GetDotProd(ray);
- double dist1 = origin1.GetDistToPlane(dir1, planeNorm, -d);
- double dist2 = origin2.GetDistToPlane(dir2, planeNorm, -d);
-
- // Compute two points on the plane. The first point is the current
- // mouse position, the second is the previous mouse position
- p1 = origin1 + dir1 * dist1;
- p2 = origin2 + dir2 * dist2;
-
- // Get point vectors relative to the entitie's pose
- a = p1 - entity->GetAbsPose().pos;
- b = p2 - entity->GetAbsPose().pos;
-
- a.Normalize();
- b.Normalize();
-
- // Get the angle between the two vectors. This is the amount to
- // rotate the entity
- float angle = acos(a.GetDotProd(b));
- if (isnan(angle))
- angle = 0;
-
- // Compute the normal to the plane which is defined by the
- // direction of rotation
- planeNorm2 = a.GetCrossProd(b);
- planeNorm2.Normalize();
-
- // Switch rotation direction if the two normals don't line up
- if ( planeNorm.GetDotProd(planeNorm2) > 0)
- angle *= -1;
-
- Quatern delta;
- delta.SetFromAxis( ray.x, ray.y, ray.z,angle);
- modelPose.rot = modelPose.rot * delta;
-
- model->SetAbsPose(modelPose);
- }
+ if ((entity->GetType() == Entity::MODEL ||
+ entity->GetType() == Entity::BODY) &&
+ this->mouseModifier.substr(0,3) == "rot")
+ this->EntityRotate(entity);
else if (this->mouseModifier.substr(0,5) == "trans")
- {
- Model *model = (Model*)(entity);
- Pose3d modelPose = model->GetRelativePose();
-
- Vector3 origin1, dir1, p1;
- Vector3 origin2, dir2, p2;
-
- // Cast two rays from the camera into the world
- this->activeCamera->GetCameraToViewportRay(this->mousePos.x,
- this->mousePos.y, origin1, dir1);
- this->activeCamera->GetCameraToViewportRay(this->prevMousePos.x,
- this->prevMousePos.y, origin2, dir2);
-
- Vector3 moveVector(0,0,0);
- Vector3 planeNorm(0,0,1);
- if (this->mouseModifier == "transx")
- moveVector.x = 1;
- else if (this->mouseModifier == "transy")
- moveVector.y = 1;
- else if (this->mouseModifier == "transz")
- {
- moveVector.z = 1;
- planeNorm.Set(1,0,0);
- }
-
- // Compute the distance from the camera to plane of translation
- double d = modelPose.pos.GetDotProd(planeNorm);
- double dist1 = origin1.GetDistToPlane(dir1, planeNorm, -d);
- double dist2 = origin2.GetDistToPlane(dir2, planeNorm, -d);
-
- // Compute two points on the plane. The first point is the current
- // mouse position, the second is the previous mouse position
- p1 = origin1 + dir1 * dist1;
- p2 = origin2 + dir2 * dist2;
-
- moveVector *= p1 - p2;
-
- modelPose.pos += moveVector;
- model->SetRelativePose(modelPose);
- }
-
- if (entity->GetType() == Entity::BODY)
- {
- Body *body = (Body*)(entity);
- double distance, scaleX, scaleY, torqueScale;
- Quatern qUp, qRight, upRightDragQ;
-
- //
- // interactively set torque selected item
- //
- Pose3d bodyPose = body->GetAbsPose();
-
- distance = (bodyPose.pos -
- this->activeCamera->GetCameraPosition()).GetLength();
- scaleX = distance * tan
- (this->activeCamera->GetHFOV().GetAsRadian() / 2.0f ) * 2.0f;
- scaleY = distance * tan
- (this->activeCamera->GetVFOV().GetAsRadian() / 2.0f ) * 2.0f;
-
- qUp.SetFromAxis(camUp.x,camUp.y,camUp.z,(double)drag.x/vpw*scaleX);
-
- qRight.SetFromAxis(camRight.x,camRight.y,camRight.z,
- (double)drag.y/vpw*scaleY);
- upRightDragQ = qUp * qRight;
-
- torqueScale = 10000.0;
- this->torqueVec = upRightDragQ.GetAsEuler()*torqueScale;
- /*std::cout << "set euler torques (" << this->torqueVec
- << ") to body (" << body->GetName() << ")" << std::endl;
- */
- body->SetTorque(this->torqueVec);
- }
+ this->EntityTranslate(entity);
}
else
{
//
// interactively rotate view
//
- this->activeCamera->RotateYaw(DTOR(-drag.x * this->rotateAmount));
- this->activeCamera->RotatePitch(DTOR(drag.y * this->rotateAmount));
+ this->activeCamera->RotateYaw(DTOR(drag.x * this->rotateAmount));
+ this->activeCamera->RotatePitch(DTOR(-drag.y * this->rotateAmount));
}
}
else if (this->rightMousePressed)
@@ -599,7 +488,6 @@
}
}
- this->mouseDrag = true;
}
////////////////////////////////////////////////////////////////////////////////
@@ -609,7 +497,7 @@
// stop simulation when this is happening
boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMRMutex());
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
+ Entity *entity = World::Instance()->GetSelectedEntity();
if ( entity && (entity->GetType() == Entity::MODEL || entity->GetType() == Entity::BODY) &&
(this->keys[FL_Control_L] || this->keys[FL_Control_R]) )
@@ -649,6 +537,10 @@
/// Handle a key press
void GLWindow::HandleKeyPress(int keyNum)
{
+ if (this->boxMaker.IsActive() || this->sphereMaker.IsActive() ||
+ this->cylinderMaker.IsActive() || this->hingeJointMaker.IsActive())
+ return;
+
std::map<int,int>::iterator iter;
this->keys[keyNum] = 1;
@@ -664,6 +556,9 @@
case FL_Control_R:
moveAmount = this->moveAmount * 10;
break;
+ case FL_CTRL+'q':
+ Simulator::Instance()->SetUserQuit();
+ break;
}
}
}
@@ -674,6 +569,10 @@
{
switch (iter->first)
{
+ case ' ':
+ Simulator::Instance()->SetPaused(!Simulator::Instance()->IsPaused() );
+ break;
+
case '=':
case '+':
this->moveAmount *= 2;
@@ -760,7 +659,6 @@
CameraManager::Instance()->DecActiveCamera();
break;
}
-
}
////////////////////////////////////////////////////////////////////////////////
@@ -808,7 +706,12 @@
case FL_SHORTCUT:
case FL_KEYDOWN:
- if (activeWin != this)
+ if (Fl::event_key() == FL_Escape)
+ World::Instance()->SetSelectedEntity(NULL);
+ else if ((Fl::event_state() | FL_CTRL) && Fl::event_key() == 113)
+ // Capture CTRL-q
+ Simulator::Instance()->SetUserQuit();
+ else if (activeWin != this)
activeWin->HandleKeyPress(Fl::event_key());
else
this->HandleKeyPress(Fl::event_key());
@@ -816,7 +719,9 @@
break;
case FL_KEYUP:
- if (activeWin != this)
+ if (Fl::event_key() == FL_Escape)
+ World::Instance()->SetSelectedEntity(NULL);
+ else if (activeWin != this)
activeWin->HandleKeyRelease(Fl::event_key());
else
this->HandleKeyRelease(Fl::event_key());
@@ -827,13 +732,16 @@
this->HandleMouseWheel(Fl::event_dx(), Fl::event_dy());
handled = true;
break;
-
- default:
- break;
}
this->prevMousePos = this->mousePos;
+ if (Fl::event_key() == FL_Escape)
+ {
+ return 1;
+ }
+
+
if (!handled)
return Fl_Gl_Window::handle(event);
else
@@ -899,3 +807,164 @@
cam->SetWorldPose( pose );
}
+
+////////////////////////////////////////////////////////////////////////////////
+// Get point on a plane
+Vector3 GLWindow::GetWorldPointOnPlane(int x, int y, Vector3 planeNorm, double d)
+{
+ Vector3 origin, dir;
+ double dist;
+
+ // Cast two rays from the camera into the world
+ CameraManager::Instance()->GetActiveCamera()->GetCameraToViewportRay(x, y, origin, dir);
+
+ dist = origin.GetDistToPlane(dir, planeNorm, d);
+
+ // Compute two points on the plane. The first point is the current
+ // mouse position, the second is the previous mouse position
+ return origin + dir * dist;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Rotate and entity, or apply torque
+void GLWindow::EntityRotate(Entity *entity)
+{
+ Vector3 planeNorm, planeNorm2;
+ Vector3 p1, p2;
+ Vector3 a,b;
+ Vector3 ray(0,0,0);
+
+ Pose3d modelPose = entity->GetAbsPose();
+
+ // Figure out which axis to rotate around
+ if (this->mouseModifier == "rotx")
+ ray.x = 1.0;
+ else if (this->mouseModifier == "roty")
+ ray.y = 1.0;
+ else if (this->mouseModifier == "rotz")
+ ray.z = 1.0;
+
+ // Compute the normal to the plane on which to rotate
+ planeNorm = modelPose.rot.RotateVector(ray);
+ double d = -modelPose.pos.GetDotProd(planeNorm);
+
+ p1 = this->GetWorldPointOnPlane( this->mousePos.x, this->mousePos.y,
+ planeNorm, d);
+
+ p2 = this->GetWorldPointOnPlane( this->prevMousePos.x,
+ this->prevMousePos.y, planeNorm, d);
+
+ OgreCreator::DrawLine(entity->GetAbsPose().pos,p1, "guiline");
+
+ // Get point vectors relative to the entity's pose
+ a = p1 - entity->GetAbsPose().pos;
+ b = p2 - entity->GetAbsPose().pos;
+
+ a.Normalize();
+ b.Normalize();
+
+ // Get the angle between the two vectors. This is the amount to
+ // rotate the entity
+ float angle = acos(a.GetDotProd(b));
+ if (isnan(angle))
+ angle = 0;
+
+ // Compute the normal to the plane which is defined by the
+ // direction of rotation
+ planeNorm2 = a.GetCrossProd(b);
+ planeNorm2.Normalize();
+
+ // Switch rotation direction if the two normals don't line up
+ if ( planeNorm.GetDotProd(planeNorm2) > 0)
+ angle *= -1;
+
+ if (entity->GetType() == Entity::MODEL)
+ {
+ Quatern delta;
+ delta.SetFromAxis( ray.x, ray.y, ray.z,angle);
+
+ modelPose.rot = modelPose.rot * delta;
+ entity->SetAbsPose(modelPose);
+ }
+ else
+ {
+ ((Body*)entity)->SetTorque(planeNorm * angle * Gui::forceMultiplier);
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Translate an entity, or apply force
+void GLWindow::EntityTranslate(Entity *entity)
+{
+ Pose3d pose = entity->GetAbsPose();
+
+ Vector3 origin1, dir1, p1;
+ Vector3 origin2, dir2, p2;
+
+ // Cast two rays from the camera into the world
+ this->activeCamera->GetCameraToViewportRay(this->mousePos.x,
+ this->mousePos.y, origin1, dir1);
+ this->activeCamera->GetCameraToViewportRay(this->prevMousePos.x,
+ this->prevMousePos.y, origin2, dir2);
+
+ Vector3 moveVector(0,0,0);
+ Vector3 planeNorm(0,0,1);
+ if (this->mouseModifier == "transx")
+ moveVector.x = 1;
+ else if (this->mouseModifier == "transy")
+ moveVector.y = 1;
+ else if (this->mouseModifier == "transz")
+ {
+ moveVector.z = 1;
+ planeNorm.Set(1,0,0);
+ }
+
+ // Compute the distance from the camera to plane of translation
+ double d = -pose.pos.GetDotProd(planeNorm);
+ double dist1 = origin1.GetDistToPlane(dir1, planeNorm, d);
+ double dist2 = origin2.GetDistToPlane(dir2, planeNorm, d);
+
+ // Compute two points on the plane. The first point is the current
+ // mouse position, the second is the previous mouse position
+ p1 = origin1 + dir1 * dist1;
+ p2 = origin2 + dir2 * dist2;
+
+ OgreCreator::DrawLine(entity->GetAbsPose().pos,p2, "guiline");
+
+ moveVector *= p1 - p2;
+
+ if (entity->GetType() == Entity::MODEL)
+ {
+ pose.pos += moveVector;
+ entity->SetRelativePose(pose);
+ }
+ else if (entity->GetType() == Entity::BODY)
+ {
+ Body *body = (Body*)(entity);
+ moveVector *= Gui::forceMultiplier;
+ body->SetForce(moveVector);
+ }
+}
+
+void GLWindow::CreateEntity(std::string name)
+{
+ World::Instance()->SetSelectedEntity(NULL);
+ this->boxMaker.Stop();
+ this->sphereMaker.Stop();
+ this->cylinderMaker.Stop();
+ this->hingeJointMaker.Stop();
+
+ this->cursor(FL_CURSOR_CROSS);
+
+ if (name == "box")
+ this->boxMaker.Start();
+ else if (name == "cylinder")
+ this->cylinderMaker.Start();
+ else if (name == "sphere")
+ this->sphereMaker.Start();
+ else if (name == "hingejoint")
+ this->hingeJointMaker.Start();
+ else
+ this->cursor(FL_CURSOR_DEFAULT);
+
+}
Modified: code/gazebo/trunk/server/gui/GLWindow.hh
===================================================================
--- code/gazebo/trunk/server/gui/GLWindow.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/GLWindow.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -43,14 +43,18 @@
#include "Pose3d.hh"
#include "Vector3.hh"
#include "Vector2.hh"
+#include "SphereMaker.hh"
+#include "BoxMaker.hh"
+#include "CylinderMaker.hh"
+#include "HingeJointMaker.hh"
-
namespace gazebo
{
class UserCamera;
class OgreCamera;
class GLFrame;
class WindowManager;
+ class Entity;
/// \brief OpenGL window to display camera data
class GLWindow : public Fl_Gl_Window
@@ -105,6 +109,9 @@
/// \brief Set the style of the view = "front, left, top, user"
public: void SetViewStyle(std::string view);
+ public: static Vector3 GetWorldPointOnPlane(int x, int y,
+ Vector3 planeNorm, double d);
+
/// \brief Handle a mouse button push
private: void HandleMousePush();
@@ -123,6 +130,18 @@
/// \brief Handle mouse wheel movement
private: void HandleMouseWheel(int dx, int dy);
+
+ /// \brief Clear selections
+ private: void ClearSelections();
+
+ /// \brief Rotate and entity, or apply torque
+ private: void EntityRotate(Entity *entity);
+
+ /// \brief Translate an entity, or apply force
+ private: void EntityTranslate(Entity *entity);
+
+ private: void CreateEntity(std::string name);
+
/// ID of the window
private: Window windowId;
@@ -165,12 +184,18 @@
private: std::string mouseModifier;
+ private: BoxMaker boxMaker;
+ private: SphereMaker sphereMaker;
+ private: CylinderMaker cylinderMaker;
+ private: HingeJointMaker hingeJointMaker;
+
/// gui interface, prerequisite to selecting Model / Body
/// press control+left click Model to toggle select. Left mouse button drag updates model rotation about camera view axis, right mouse button drag udpates model position in camera view plane.
/// press control+right click Body to toggle select body select. Left mouse button drag applies torque, right mouse button drag applies linear force.
/// press and hold shift to move the user camera around faster
};
+
}
#endif
Modified: code/gazebo/trunk/server/gui/Gui.cc
===================================================================
--- code/gazebo/trunk/server/gui/Gui.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/Gui.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -52,6 +52,7 @@
using namespace gazebo;
+double Gui::forceMultiplier = 1;
////////////////////////////////////////////////////////////////////////////////
/// Constructor
@@ -80,11 +81,11 @@
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() - 115);
+ this->toolbar->gui = this;
+ this->sidebar = new Sidebar(0, this->toolbar->y() + this->toolbar->h(), toolbarWidth, this->h() - 115);
// Create the frame mamanger
- this->frameMgr = new GLFrameManager(toolbarWidth, 60,
- this->w()-toolbarWidth, this->h()-115, "");
+ this->frameMgr = new GLFrameManager(toolbarWidth, this->toolbar->y() + this->toolbar->h(), 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);
@@ -113,6 +114,7 @@
Fl::check();
Fl::wait(0.3);
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -214,6 +216,10 @@
{
switch(event)
{
+ case FL_SHORTCUT:
+ if ( (Fl::event_state() | FL_CTRL) && Fl::event_key() == 113)
+ Simulator::Instance()->SetUserQuit();
+ break;
case FL_FOCUS:
this->hasFocus = true;
break;
@@ -226,6 +232,12 @@
break;
}
+ if (Fl::event_key() == FL_Escape)
+ {
+ World::Instance()->SetSelectedEntity(NULL);
+ return 1;
+ }
+
return Fl_Window::handle(event);
}
Modified: code/gazebo/trunk/server/gui/Gui.hh
===================================================================
--- code/gazebo/trunk/server/gui/Gui.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/Gui.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -101,6 +101,8 @@
private: std::vector<Param*> parameters;
private: Fl_Slider *timeSlider;
+
+ public: static double forceMultiplier;
};
}
Added: code/gazebo/trunk/server/gui/HingeJointMaker.cc
===================================================================
--- code/gazebo/trunk/server/gui/HingeJointMaker.cc (rev 0)
+++ code/gazebo/trunk/server/gui/HingeJointMaker.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,147 @@
+#include <iostream>
+#include <FL/Fl.H>
+
+#include "PhysicsEngine.hh"
+#include "Joint.hh"
+#include "XMLConfig.hh"
+#include "Body.hh"
+#include "Simulator.hh"
+#include "OgreAdaptor.hh"
+#include "Entity.hh"
+#include "Pose3d.hh"
+#include "GLWindow.hh"
+#include "OgreVisual.hh"
+#include "OgreCreator.hh"
+#include "CameraManager.hh"
+#include "OgreCamera.hh"
+#include "World.hh"
+#include "HingeJointMaker.hh"
+
+using namespace gazebo;
+
+HingeJointMaker::HingeJointMaker()
+{
+ this->state = 0;
+ this->jointName = "";
+ this->leftMousePressed = false;
+}
+
+HingeJointMaker::~HingeJointMaker()
+{
+}
+
+void HingeJointMaker::Start()
+{
+ this->jointName = "user_hingejoint";
+ this->first = NULL;
+ this->second = NULL;
+ this->state = 1;
+}
+
+void HingeJointMaker::Stop()
+{
+ this->state = 0;
+}
+
+bool HingeJointMaker::IsActive() const
+{
+ return this->state > 0;
+}
+
+void HingeJointMaker::MousePushCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->leftMousePressed = false;
+
+ this->mousePushPos = mousePos;
+
+ std::string mod;
+ OgreCamera *cam = CameraManager::Instance()->GetActiveCamera();
+ this->first = OgreAdaptor::Instance()->GetEntityAt(cam, mousePos, mod);
+
+ switch (Fl::event_button())
+ {
+ case FL_LEFT_MOUSE:
+ this->leftMousePressed = true;
+ break;
+ }
+}
+
+void HingeJointMaker::MouseReleaseCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->state++;
+
+ std::string mod;
+ OgreCamera *cam = CameraManager::Instance()->GetActiveCamera();
+ this->second = OgreAdaptor::Instance()->GetEntityAt(cam, mousePos, mod);
+ this->CreateTheHingeJoint();
+}
+
+void HingeJointMaker::MouseDragCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ Vector3 norm;
+ Vector3 p1, p2;
+
+ p1 = this->first->GetAbsPose().pos;
+ p2 = OgreAdaptor::Instance()->GetFirstContact(CameraManager::Instance()->GetActiveCamera(), mousePos);
+
+ OgreCreator::DrawLine(p1,p2,"HingeMaker");
+
+}
+
+void HingeJointMaker::CreateTheHingeJoint()
+{
+ boost::recursive_mutex::scoped_lock lock( *Simulator::Instance()->GetMRMutex());
+
+
+ Body *body1 = Simulator::Instance()->GetParentBody(this->first);
+ Body *body2 = Simulator::Instance()->GetParentBody(this->second);
+
+ Joint *joint;
+ Joint::Type jtype = Joint::HINGE;
+
+ joint = World::Instance()->GetPhysicsEngine()->CreateJoint(jtype);
+
+ std::ostringstream newJointStr;
+
+ newJointStr << "<?xml version='1.0'?> <gazebo:world xmlns:xi='http://www.w3.org/2001/XInclude' xmlns:gazebo='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz' xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model' xmlns:sensor='http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor' xmlns:body='http://playerstage.sourceforge.net/gazebo/xmlschema/#body' xmlns:geom='http://playerstage.sourceforge.net/gazebo/xmlschema/#geom' xmlns:joint='http://playerstage.sourceforge.net/gazebo/xmlschema/#joint' xmlns:interface='http://playerstage.sourceforge.net/gazebo/xmlschema/#interface' xmlns:rendering='http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering' xmlns:renderable='http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable' xmlns:controller='http://playerstage.sourceforge.net/gazebo/xmlschema/#controller' xmlns:physics='http://playerstage.sourceforge.net/gazebo/xmlschema/#physics' >";
+
+
+ newJointStr << "<joint:hinge name=\"" << this->jointName << "\">\
+ <body1>" << body1->GetCompleteScopedName() << "</body1>\
+ <body2>" << body2->GetCompleteScopedName() << "</body2>\
+ <anchor>" <<body1->GetCompleteScopedName() << "</anchor>\
+ <anchorOffset>0 0 0</anchorOffset>\
+ <axis>0 0 1</axis>\
+ <erp>0.8</erp>\
+ <cfm>10e-5</cfm>\
+ </joint:hinge>";
+
+ newJointStr << "</gazebo:world>";
+
+
+ XMLConfig *xmlConfig = new XMLConfig();
+
+ // Load the XML tree from the given string
+ try
+ {
+ xmlConfig->LoadString( newJointStr.str() );
+ }
+ catch (gazebo::GazeboError e)
+ {
+ gzerr(0) << "The world could not load the XML data [" << e << "]\n";
+ }
+
+ XMLConfigNode *root = xmlConfig->GetRootNode();
+ joint->Load( root->GetChild() );
+
+ OgreCreator::SetVisible("guiline", false);
+}
Added: code/gazebo/trunk/server/gui/HingeJointMaker.hh
===================================================================
--- code/gazebo/trunk/server/gui/HingeJointMaker.hh (rev 0)
+++ code/gazebo/trunk/server/gui/HingeJointMaker.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,33 @@
+#ifndef HINGEJOINTMAKER_HH
+#define HINGEJOINTMAKER_HH
+
+#include "Vector2.hh"
+
+namespace gazebo
+{
+ class Entity;
+
+ class HingeJointMaker
+ {
+ public: HingeJointMaker();
+ public: virtual ~HingeJointMaker();
+
+ public: void Start();
+ public: void Stop();
+ public: bool IsActive() const;
+
+ public: void MousePushCB(Vector2<int> mousePos);
+ public: void MouseReleaseCB(Vector2<int> mousePos);
+ public: void MouseDragCB(Vector2<int> mousePos);
+
+ private: void CreateTheHingeJoint();
+ private: int state;
+ private: bool leftMousePressed;
+ private: Vector2<int> mousePushPos;
+ private: std::string jointName;
+
+ private: Entity *first, *second;
+ };
+}
+
+#endif
Modified: code/gazebo/trunk/server/gui/Sidebar.cc
===================================================================
--- code/gazebo/trunk/server/gui/Sidebar.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/Sidebar.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -85,25 +85,9 @@
this->paramInput->when( FL_WHEN_ENTER_KEY | FL_WHEN_RELEASE );
this->paramInput->callback(&Sidebar::ParamInputCB, this);
this->paramInput->color(FL_WHITE);
+ this->paramInput->value("1.0");
+ this->paramInput->hide();
- /*
- y = this->paramInput->y() + this->paramInput->h() + 20;
- this->jointChoice = new Fl_Choice(x+50, y, w-70, 20, "Joint:");
- this->jointChoice->callback(&Sidebar::JointChoiceCB, this);
- this->jointChoice->labelsize(12);
-
- y = this->jointChoice->y() + this->jointChoice->h() + 20;
- this->jointForceSlider = new Fl_Value_Slider(x+50, y, w-70, 20, "Force:");
- this->jointForceSlider->labelsize(12);
- this->jointForceSlider->type(FL_HOR_NICE_SLIDER);
- this->jointForceSlider->callback(&Sidebar::JointForceSliderCB, this);
-
- y = this->jointForceSlider->y() + this->jointForceSlider->h() + 20;
- this->jointVelocitySlider = new Fl_Value_Slider(x+50, y, w-70, 20, "Velocity:");
- this->jointVelocitySlider->labelsize(12);
- this->jointVelocitySlider->type(FL_HOR_NICE_SLIDER);
- this->jointVelocitySlider->callback(&Sidebar::JointVelocitySliderCB, this);
- */
this->end();
this->resizable( this->entityBrowser );
@@ -112,6 +96,8 @@
World::Instance()->ConnectAddEntitySignal(
boost::bind(&Sidebar::AddEntityToBrowser, this, _1) );
+ World::Instance()->ConnectEntitySelectedSignal(
+ boost::bind(&Sidebar::SetSelectedEntity, this, _1) );
}
////////////////////////////////////////////////////////////////////////////////
@@ -122,23 +108,33 @@
delete this->paramInput;
}
-
-
////////////////////////////////////////////////////////////////////////////////
/// Update the toolbar data
void Sidebar::Update()
{
- if (this->entityBrowser->size() == 0)
- this->UpdateEntityBrowser();
+// if (this->entityBrowser->size() == 0)
+ //this->UpdateEntityBrowser();
+}
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
-
+////////////////////////////////////////////////////////////////////////////////
+// Set the selected entity
+void Sidebar::SetSelectedEntity(Entity *entity)
+{
this->paramCount = 0;
+
if (entity && entity->GetType() == Entity::MODEL)
{
+ this->paramBrowser->deselect();
Model *model = (Model*)(entity);
+ for (int i=1; i <= this->entityBrowser->size(); i++)
+ {
+ std::string lineText = this->entityBrowser->text(i);
+ if (lineText == entity->GetCompleteScopedName())
+ this->entityBrowser->value(i);
+ }
+
std::string value = "@b@... ";
this->AddToParamBrowser(value);
this->AddEntityToParamBrowser(model, "");
@@ -148,12 +144,6 @@
std::vector<Entity*>::const_iterator iter;
std::map<std::string, Geom*>::const_iterator giter;
- /*for (unsigned int i=0; i < model->GetJointCount(); i++)
- {
- Joint *joint = model->GetJoint(i);
- this->jointChoice->add(joint->GetName().c_str(),0,0);
- }*/
-
for (iter = bodies.begin(); iter != bodies.end(); iter++)
{
Body *body = dynamic_cast<Body*>(*iter);
@@ -190,10 +180,25 @@
this->AddToParamBrowser("");
}
+ this->paramInput->hide();
}
+ else if (entity && entity->GetType() == Entity::BODY)
+ {
+ this->paramInput->show();
+ this->paramInput->value(boost::lexical_cast<std::string>(Gui::forceMultiplier).c_str());
+ this->paramInput->label("Force Multiplier:");
+ }
+ else if (!entity)
+ {
+ this->entityBrowser->deselect();
+ this->paramBrowser->deselect();
+ this->paramBrowser->value(0);
+ this->paramBrowser->clear();
+ }
}
+
////////////////////////////////////////////////////////////////////////////////
// Attribute browser callback
void Sidebar::ParamBrowserCB( Fl_Widget * w, void *data)
@@ -213,34 +218,33 @@
if (lineText.find("-Body") != std::string::npos)
{
- /*beginLbl = lineText.rfind("@") + 2;
+ Entity *selected = World::Instance()->GetSelectedEntity();
+ beginLbl = lineText.rfind("@") + 2;
+
std::string bodyName = lineText.substr(beginLbl, lineText.size()-beginLbl);
- std::cout << "Body Name[" << bodyName << "]\n";
- Model *model = Simulator::Instance()->GetSelectedModel();
- Body *body = model->GetBody(bodyName);
- Simulator::Instance()->SetSelectedEntity(body);
- */
+ std::string modelName = toolbar->entityBrowser->text( toolbar->entityBrowser->value() );
+ std::string scopedName = modelName + "::" + bodyName;
+
+ Entity *ent = World::Instance()->GetEntityByName(scopedName);
+
+ if (selected != ent)
+ World::Instance()->SetSelectedEntity(ent);
}
else if (lineText.find("-Geom") != std::string::npos)
{
- /*beginLbl = lineText.rfind("@") + 2;
+ beginLbl = lineText.rfind("@") + 2;
std::string geomName = lineText.substr(beginLbl, lineText.size()-beginLbl);
- std::cout << "Geom Name[" << geomName << "]\n";
- Model *model = Simulator::Instance()->GetSelectedModel();
- Geom *geom = model->GetGeom(geomName);
- Simulator::Instance()->SetSelectedEntity(geom);
+ Entity *ent = World::Instance()->GetEntityByName(geomName);
- toolbar->paramInput->deactivate();
- Simulator::Instance()->SetSelectedEntity( );
- */
+ if (ent)
+ World::Instance()->SetSelectedEntity(ent);
+
return;
}
- else
- toolbar->paramInput->activate();
endLbl = lineText.find("~");
while (lineText[beginLbl] == '@') beginLbl+=2;
@@ -249,13 +253,14 @@
beginValue = endLbl+1;
while (lineText[beginValue] == '@') beginValue+=2;
- toolbar->paramInputLbl = lineText.substr(beginLbl, endLbl-beginLbl);
+ /*toolbar->paramInputLbl = lineText.substr(beginLbl, endLbl-beginLbl);
toolbar->paramInput->label(toolbar->paramInputLbl.c_str());
toolbar->paramInput->value( lineText.substr(beginValue, lineText.size() - beginValue).c_str() );
toolbar->paramInput->redraw();
+ */
}
////////////////////////////////////////////////////////////////////////////////
@@ -263,10 +268,16 @@
void Sidebar::ParamInputCB( Fl_Widget *w, void *data)
{
Fl_Input *input = (Fl_Input*)(w);
+
+ std::string value = input->value();
+ double dblValue = boost::lexical_cast<double>(value);
+ Gui::forceMultiplier = dblValue;
+
+ /*Fl_Input *input = (Fl_Input*)(w);
Sidebar *toolbar = (Sidebar*)(data);
Fl_Hold_Browser *browser = toolbar->paramBrowser;
int selected = browser->value();
- Model *model = dynamic_cast<Model*>(Simulator::Instance()->GetSelectedEntity());
+ Model *model = dynamic_cast<Model*>(World::Instance()->GetSelectedEntity());
Body *body = NULL;
@@ -330,6 +341,7 @@
{
param->SetFromString( value, true );
}
+ */
}
////////////////////////////////////////////////////////////////////////////////
@@ -348,7 +360,12 @@
Entity *ent = World::Instance()->GetEntityByName(lineText);
if (ent)
- Simulator::Instance()->SetSelectedEntity(ent);
+ {
+ World::Instance()->SetSelectedEntity(ent);
+ OgreCamera *cam = CameraManager::Instance()->GetActiveCamera();
+ if (cam)
+ cam->MoveToEntity(ent);
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -405,7 +422,7 @@
// Add an entity to the browser
void Sidebar::AddEntityToBrowser(const Entity *entity)
{
- this->entityBrowser->add( entity->GetName().c_str() );
+ this->entityBrowser->add( entity->GetCompleteScopedName().c_str() );
}
////////////////////////////////////////////////////////////////////////////////
@@ -415,7 +432,7 @@
Sidebar *self = (Sidebar*)(data);
Fl_Choice *choice = (Fl_Choice*)(w);
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
+ Entity *entity = World::Instance()->GetSelectedEntity();
if (entity->GetType() == Entity::MODEL)
{
Model *model = (Model*)(entity);
@@ -442,7 +459,7 @@
double value = slider->value();
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
+ Entity *entity = World::Instance()->GetSelectedEntity();
if (entity->GetType() == Entity::MODEL)
{
Model *model = (Model*)(entity);
@@ -450,7 +467,6 @@
Joint *joint = model->GetJoint( self->jointChoice->text() );
joint->SetMaxForce( 0, value );
joint->SetForce( 0, value );
- std::cout << "Set Force[" << value << "]\n";
}
}
@@ -467,7 +483,7 @@
double value = slider->value();
- Entity *entity = Simulator::Instance()->GetSelectedEntity();
+ Entity *entity = World::Instance()->GetSelectedEntity();
if (entity->GetType() == Entity::MODEL)
{
Model *model = (Model*)(entity);
Modified: code/gazebo/trunk/server/gui/Sidebar.hh
===================================================================
--- code/gazebo/trunk/server/gui/Sidebar.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/Sidebar.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -53,6 +53,9 @@
/// \brief Update the toolbar data
public: void Update();
+ /// \brief Set the selected entity
+ public: void SetSelectedEntity(Entity *entity);
+
/// \brief Add an entity to the browser
public: void AddEntityToBrowser(const Entity *model);
Added: code/gazebo/trunk/server/gui/SphereMaker.cc
===================================================================
--- code/gazebo/trunk/server/gui/SphereMaker.cc (rev 0)
+++ code/gazebo/trunk/server/gui/SphereMaker.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,144 @@
+#include <iostream>
+#include <FL/Fl.H>
+
+#include "Simulator.hh"
+#include "GLWindow.hh"
+#include "OgreVisual.hh"
+#include "OgreCreator.hh"
+#include "World.hh"
+#include "SphereMaker.hh"
+
+using namespace gazebo;
+
+SphereMaker::SphereMaker()
+{
+ this->state = 0;
+ this->visualName = "";
+ this->leftMousePressed = false;
+ this->index = 0;
+}
+
+SphereMaker::~SphereMaker()
+{
+}
+
+void SphereMaker::Start()
+{
+ std::ostringstream stream;
+ std::string name = "user_sphere";
+
+ do
+ {
+ stream.str("");
+ stream << name << index;
+ this->index++;
+ } while (OgreCreator::Instance()->GetVisual(stream.str()));
+
+ this->visualName = stream.str();
+ this->state = 1;
+}
+
+void SphereMaker::Stop()
+{
+ this->state = 0;
+}
+
+bool SphereMaker::IsActive() const
+{
+ return this->state > 0;
+}
+
+void SphereMaker::MousePushCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->leftMousePressed = false;
+
+ this->mousePushPos = mousePos;
+
+ switch (Fl::event_button())
+ {
+ case FL_LEFT_MOUSE:
+ this->leftMousePressed = true;
+ break;
+ }
+}
+
+void SphereMaker::MouseReleaseCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ this->state++;
+
+ if (this->state == 2)
+ {
+ this->CreateTheSphere();
+ this->Start();
+ }
+}
+
+void SphereMaker::MouseDragCB(Vector2<int> mousePos)
+{
+ if (this->state == 0)
+ return;
+
+ Vector3 norm;
+ Vector3 p1, p2;
+
+ norm.Set(0,0,1);
+
+ p1 = GLWindow::GetWorldPointOnPlane(this->mousePushPos.x, this->mousePushPos.y, norm, 0);
+ p2 = GLWindow::GetWorldPointOnPlane(mousePos.x, mousePos.y ,norm, 0);
+
+ OgreVisual *vis = NULL;
+ if (OgreCreator::Instance()->GetVisual(this->visualName))
+ vis = OgreCreator::Instance()->GetVisual(this->visualName);
+ else
+ {
+ vis = OgreCreator::Instance()->CreateVisual(this->visualName);
+ vis->AttachMesh("unit_sphere");
+ vis->SetPosition(p1);
+ }
+
+ double scale = p1.Distance(p2);
+ Vector3 p = vis->GetPosition();
+
+ p.z = scale;
+
+ vis->SetPosition(p);
+ vis->SetScale(Vector3(scale,scale,scale));
+}
+
+void SphereMaker::CreateTheSphere()
+{
+ boost::recursive_mutex::scoped_lock lock( *Simulator::Instance()->GetMRMutex());
+ std::ostringstream newModelStr;
+
+ OgreVisual *vis = OgreCreator::Instance()->GetVisual(this->visualName);
+
+ newModelStr << "<?xml version='1.0'?> <gazebo:world xmlns:xi='http://www.w3.org/2001/XInclude' xmlns:gazebo='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz' xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model' xmlns:sensor='http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor' xmlns:body='http://playerstage.sourceforge.net/gazebo/xmlschema/#body' xmlns:geom='http://playerstage.sourceforge.net/gazebo/xmlschema/#geom' xmlns:joint='http://playerstage.sourceforge.net/gazebo/xmlschema/#joint' xmlns:interface='http://playerstage.sourceforge.net/gazebo/xmlschema/#interface' xmlns:rendering='http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering' xmlns:renderable='http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable' xmlns:controller='http://playerstage.sourceforge.net/gazebo/xmlschema/#controller' xmlns:physics='http://playerstage.sourceforge.net/gazebo/xmlschema/#physics' >";
+
+
+ newModelStr << "<model:physical name=\"" << this->visualName << "\">\
+ <xyz>" << vis->GetPosition() << "</xyz>\
+ <body:sphere name=\"body\">\
+ <geom:sphere name=\"geom\">\
+ <size>" << vis->GetScale().x << "</size>\
+ <mass>0.5</mass>\
+ <visual>\
+ <mesh>unit_sphere</mesh>\
+ <size>" << vis->GetScale()*2 << "</size>\
+ <material>Gazebo/Grey</material>\
+ </visual>\
+ </geom:sphere>\
+ </body:sphere>\
+ </model:physical>";
+
+ newModelStr << "</gazebo:world>";
+
+ World::Instance()->InsertEntity(newModelStr.str());
+
+ OgreCreator::Instance()->DeleteVisual(this->visualName);
+}
Added: code/gazebo/trunk/server/gui/SphereMaker.hh
===================================================================
--- code/gazebo/trunk/server/gui/SphereMaker.hh (rev 0)
+++ code/gazebo/trunk/server/gui/SphereMaker.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -0,0 +1,30 @@
+#ifndef SPHEREMAKER_HH
+#define SPHEREMAKER_HH
+
+#include "Vector2.hh"
+
+namespace gazebo
+{
+ class SphereMaker
+ {
+ public: SphereMaker();
+ public: virtual ~SphereMaker();
+
+ public: void Start();
+ public: void Stop();
+ public: bool IsActive() const;
+
+ public: void MousePushCB(Vector2<int> mousePos);
+ public: void MouseReleaseCB(Vector2<int> mousePos);
+ public: void MouseDragCB(Vector2<int> mousePos);
+
+ private: void CreateTheSphere();
+ private: int state;
+ private: bool leftMousePressed;
+ private: Vector2<int> mousePushPos;
+ private: std::string visualName;
+ private: int index;
+
+ };
+}
+#endif
Modified: code/gazebo/trunk/server/gui/Toolbar.cc
===================================================================
--- code/gazebo/trunk/server/gui/Toolbar.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/Toolbar.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -1,6 +1,6 @@
/*
* Gazebo - Outdoor Multi-Robot Simulator
- * Copyright (C) 2003
+ * Copyright (C) 2203
* Nate Koenig & Andrew Howard
*
* This program is free software; you can redistribute it and/or modify
@@ -15,22 +15,22 @@
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * Foundation, Inc., 59 Temple Place, Suite 322, Boston, MA 02111-1227 USA
*
*/
/* Desc: Toolbar
* Author: Nate Koenig
- * Date: 13 Feb 2006
+ * Date: 13 Feb 2206
* SVN: $Id$
*/
#include <stdio.h>
-#include <FL/Fl_Button.H>
#include <FL/Fl_Box.H>
#include <FL/Fl_Bitmap.H>
#include <FL/Fl_Image.H>
+#include "Events.hh"
#include "Image.hh"
#include "Gui.hh"
#include "Global.hh"
@@ -44,7 +44,7 @@
Toolbar::Toolbar(int x, int y, int w, int h, const char *l)
: Fl_Group(x,y,w,h,l)
{
- this->box(FL_THIN_DOWN_BOX);
+ this->box(FL_NO_BOX);
this->color(BG_COLOR);
@@ -52,76 +52,133 @@
unsigned int dataCount;
Image image;
- image.Load("blue_play_button.png");
- image.Rescale(20,20);
+ image.Load("control_play_blue.png");
+ image.Rescale(22,22);
image.GetData(&data, dataCount);
this->playImage[0] = new Fl_RGB_Image(data, image.GetWidth(),
image.GetHeight(), 4);
data = NULL;
- image.Load("grey_play_button.png");
- image.Rescale(20,20);
+ image.Load("control_play.png");
+ image.Rescale(22,22);
image.GetData(&data, dataCount);
this->playImage[1] = new Fl_RGB_Image(data, image.GetWidth(),
image.GetHeight(), 4);
data = NULL;
- image.Load("blue_pause_button.png");
- image.Rescale(20,20);
+ image.Load("control_pause_blue.png");
+ image.Rescale(22,22);
image.GetData(&data, dataCount);
this->pauseImage[0] = new Fl_RGB_Image(data, image.GetWidth(),
image.GetHeight(), 4);
data = NULL;
- image.Load("grey_pause_button.png");
- image.Rescale(20,20);
+ image.Load("control_pause.png");
+ image.Rescale(22,22);
image.GetData(&data, dataCount);
this->pauseImage[1] = new Fl_RGB_Image(data, image.GetWidth(),
image.GetHeight(), 4);
data = NULL;
- image.Load("blue_step_button.png");
- image.Rescale(20,20);
+ image.Load("control_end_blue.png");
+ image.Rescale(22,22);
image.GetData(&data, dataCount);
this->stepImage[0] = new Fl_RGB_Image(data, image.GetWidth(),
image.GetHeight(), 4);
data = NULL;
- image.Load("grey_step_button.png");
- image.Rescale(20,20);
+ image.Load("control_end.png");
+ image.Rescale(22,22);
image.GetData(&data, dataCount);
this->stepImage[1] = new Fl_RGB_Image(data, image.GetWidth(),
image.GetHeight(), 4);
+ data = NULL;
+ image.Load("box_create_blue.png");
+ image.Rescale(22,22);
+ image.GetData(&data, dataCount);
+ this->boxImage[0] = new Fl_RGB_Image(data, image.GetWidth(),
+ image.GetHeight(), 4);
+ data = NULL;
+ image.Load("sphere_create_blue.png");
+ image.Rescale(22,22);
+ image.GetData(&data, dataCount);
+ this->sphereImage[0] = new Fl_RGB_Image(data, image.GetWidth(),
+ image.GetHeight(), 4);
+
+ data = NULL;
+ image.Load("cylinder_create_blue.png");
+ image.Rescale(22,22);
+ image.GetData(&data, dataCount);
+ this->cylinderImage[0] = new Fl_RGB_Image(data, image.GetWidth(),
+ image.GetHeight(), 4);
+
+
+ data = NULL;
+ image.Load("cursor.png");
+ image.Rescale(22,22);
+ image.GetData(&data, dataCount);
+ this->cursorImage = new Fl_RGB_Image(data, image.GetWidth(),
+ image.GetHeight(), 4);
+
+
+ y += 4;
x += 5;
- y += 5;
- this->playButton = new Fl_Button(x, y, 20, 20);
+ y += 0;
+ this->playButton = new Fl_Button(x, y, 22, 22);
this->playButton->callback( &gazebo::Toolbar::PlayButtonCB, this );
this->playButton->color(BG_COLOR, BG_COLOR);
this->playButton->image(this->playImage[1]);
this->playButton->box(FL_NO_BOX);
this->playButton->deactivate();
+ this->playButton->clear_visible_focus();
x = this->playButton->x() + this->playButton->w() + 10;
- this->pauseButton = new Fl_Button(x, y, 20, 20);
+ this->pauseButton = new Fl_Button(x, y, 22, 22);
this->pauseButton->callback( &gazebo::Toolbar::PauseButtonCB, this );
this->pauseButton->color(BG_COLOR, BG_COLOR);
this->pauseButton->image(this->pauseImage[0]);
this->pauseButton->box(FL_NO_BOX);
+ this->pauseButton->clear_visible_focus();
x = this->pauseButton->x() + this->pauseButton->w() + 10;
- this->stepButton = new Fl_Button(x, y, 20, 20);
+ this->stepButton = new Fl_Button(x, y, 22, 22);
this->stepButton->callback( &gazebo::Toolbar::StepButtonCB, this );
this->stepButton->color(BG_COLOR, BG_COLOR);
this->stepButton->image(this->stepImage[1]);
this->stepButton->box(FL_NO_BOX);
this->stepButton->deactivate();
+ this->stepButton->clear_visible_focus();
- //x = this->stepButton->x() + this->stepButton->w() + 10;
- //this->moveButton = new Fl_Button(x,y,20,20);
+ y = this->y();
+ x = this->stepButton->x() + this->stepButton->w() + 10;
+ this->cursorButton = new ToolbarButton(x, y, 30, 30);
+ this->cursorButton->callback( &gazebo::Toolbar::CursorButtonCB, this );
+ this->cursorButton->image(this->cursorImage);
+ this->cursorButton->set();
+ this->cursorButton->clear_visible_focus();
+
+ x = this->cursorButton->x() + this->cursorButton->w() + 10;
+ this->boxButton = new ToolbarButton(x, y, 30, 30);
+ this->boxButton->callback( &gazebo::Toolbar::BoxButtonCB, this );
+ this->boxButton->image(this->boxImage[0]);
+ this->boxButton->clear_visible_focus();
+
+ x = this->boxButton->x() + this->boxButton->w() + 10;
+ this->sphereButton = new ToolbarButton(x, y, 30, 30);
+ this->sphereButton->callback( &gazebo::Toolbar::SphereButtonCB, this );
+ this->sphereButton->image(this->sphereImage[0]);
+ this->sphereButton->clear_visible_focus();
+
+ x = this->sphereButton->x() + this->sphereButton->w() + 10;
+ this->cylinderButton = new ToolbarButton(x, y, 30, 30);
+ this->cylinderButton->callback( &gazebo::Toolbar::CylinderButtonCB, this );
+ this->cylinderButton->image(this->cylinderImage[0]);
+ this->cylinderButton->clear_visible_focus();
+
this->end();
this->resizable(NULL);
}
@@ -213,3 +270,31 @@
Simulator::Instance()->SetStepInc( true );
w->clear_visible_focus();
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Box button callback
+void Toolbar::BoxButtonCB( Fl_Widget *w, void * /*data*/ )
+{
+ Events::createEntitySignal("box");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Sphere button callback
+void Toolbar::SphereButtonCB( Fl_Widget *w, void * /*data*/ )
+{
+ Events::createEntitySignal("sphere");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Box button callback
+void Toolbar::CylinderButtonCB( Fl_Widget *w, void * /*data*/ )
+{
+ Events::createEntitySignal("cylinder");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Cursor button callback
+void Toolbar::CursorButtonCB( Fl_Widget *w, void * /*data*/ )
+{
+ Events::createEntitySignal("");
+}
Modified: code/gazebo/trunk/server/gui/Toolbar.hh
===================================================================
--- code/gazebo/trunk/server/gui/Toolbar.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/gui/Toolbar.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -28,13 +28,14 @@
#define TOOLBAR_HH
#include <FL/Fl_Group.H>
+#include <FL/Fl_Button.H>
-class Fl_Button;
class Fl_Box;
namespace gazebo
{
class Common;
+ class Gui;
/// \brief Toolbar
class Toolbar : public Fl_Group
@@ -57,15 +58,67 @@
/// \brief Set button callback
public: static void StepButtonCB( Fl_Widget * /*w*/, void * /*data*/ );
+ /// \brief Box button callback
+ public: static void BoxButtonCB( Fl_Widget *w, void * /*data*/ );
+
+ /// \brief Sphere button callback
+ public: static void SphereButtonCB( Fl_Widget *w, void * /*data*/ );
+
+ /// \brief Cylinder button callback
+ public: static void CylinderButtonCB( Fl_Widget *w, void * /*data*/ );
+
+ /// \brief Cursor button callback
+ public: static void CursorButtonCB( Fl_Widget *w, void * /*data*/ );
+
+
+ private: class ToolbarButton : public Fl_Button
+ {
+ public: ToolbarButton(int x, int y, int w, int h, const char *l=0)
+ : Fl_Button(x,y,w,h,l)
+ {
+ this->box(FL_FLAT_BOX);
+ this->down_box(FL_DOWN_BOX);
+ this->type(FL_RADIO_BUTTON);
+ }
+ public: virtual ~ToolbarButton() {}
+ public: int handle(int event)
+ {
+ int ret = Fl_Button::handle(event);
+ switch(event)
+ {
+ case FL_ENTER:
+ this->box(FL_UP_BOX);
+ this->redraw();
+ break;
+ case FL_LEAVE:
+ this->box(FL_FLAT_BOX);
+ this->redraw();
+ break;
+ }
+
+ return ret;
+ }
+ };
+
+
private: Fl_Button *playButton;
private: Fl_Button *pauseButton;
private: Fl_Button *stepButton;
private: Fl_Button *moveButton;
+ private: ToolbarButton *boxButton;
+ private: ToolbarButton *sphereButton;
+ private: ToolbarButton *cylinderButton;
+ private: ToolbarButton *cursorButton;
private: Fl_RGB_Image *playImage[2];
private: Fl_RGB_Image *pauseImage[2];
private: Fl_RGB_Image *stepImage[2];
+ private: Fl_RGB_Image *boxImage[2];
+ private: Fl_RGB_Image *sphereImage[2];
+ private: Fl_RGB_Image *cylinderImage[2];
+ private: Fl_RGB_Image *cursorImage;
+ public: Gui *gui;
};
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/physics/Body.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -368,8 +368,6 @@
this->linearAccel.Set(0,0,0);
this->angularAccel.Set(0,0,0);
- std::cout << this->GetName() << "Mass[" << this->mass.GetAsDouble() << "]\n";
-
/// Attach mesh for CG visualization
/// Add a renderable visual for CG, make visible in Update()
if (this->mass.GetAsDouble() > 0.0)
@@ -377,7 +375,6 @@
std::ostringstream visname;
visname << this->GetCompleteScopedName() + ":" + this->GetName() << "_CGVISUAL" ;
- std::cout << "CG Visual Name[" << visname.str() << "]\n";
if (this->cgVisual == NULL)
{
this->cgVisual = OgreCreator::Instance()->CreateVisual(visname.str(),
@@ -391,7 +388,7 @@
this->cgVisual->AttachMesh("body_cg");
this->cgVisual->SetMaterial("Gazebo/Red");
this->cgVisual->SetCastShadows(false);
- this->cgVisual->AttachAxes();
+ //this->cgVisual->AttachAxes();
std::map< std::string, Geom* >::iterator giter;
Modified: code/gazebo/trunk/server/physics/Joint.cc
===================================================================
--- code/gazebo/trunk/server/physics/Joint.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/physics/Joint.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -121,12 +121,22 @@
this->fudgeFactorP->Load(node);
std::ostringstream visname;
- visname << this->model->GetScopedName() << "::" << this->GetName() << "_VISUAL";
- this->body1 = this->model->GetBody( **(this->body1NameP));
- this->body2 = this->model->GetBody(**(this->body2NameP));
+ if (this->model)
+ {
+ visname << this->model->GetScopedName() << "::" << this->GetName() << "_VISUAL";
- this->anchorBody = this->model->GetBody(**(this->anchorBodyNameP));
+ this->body1 = this->model->GetBody( **(this->body1NameP));
+ this->body2 = this->model->GetBody(**(this->body2NameP));
+ this->anchorBody = this->model->GetBody(**(this->anchorBodyNameP));
+ }
+ else
+ {
+ visname << this->GetName() << "_VISUAL";
+ this->body1 = dynamic_cast<Body*>(World::Instance()->GetEntityByName( **(this->body1NameP) ));
+ this->body2 = dynamic_cast<Body*>(World::Instance()->GetEntityByName( **(this->body2NameP) ));
+ this->anchorBody = dynamic_cast<Body*>(World::Instance()->GetEntityByName( **(this->anchorBodyNameP) ));
+ }
if (!this->body1 && this->body1NameP->GetValue() != std::string("world"))
gzthrow("Couldn't Find Body[" + node->GetString("body1","",1));
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -499,6 +499,7 @@
{
if (this->bodyId)
{
+ this->SetEnabled(true);
this->physicsEngine->LockMutex();
dBodyAddTorque(this->bodyId, torque.x, torque.y, torque.z);
this->physicsEngine->UnlockMutex();
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -79,6 +79,14 @@
this->contactGroup = dJointGroupCreate(0);
+ // If auto-disable is active, then user interaction with the joints
+ // doesn't behave properly
+ dWorldSetAutoDisableFlag(this->worldId, 0);
+ dWorldSetAutoDisableTime(this->worldId, 2.0);
+ dWorldSetAutoDisableLinearThreshold(this->worldId, 0.001);
+ dWorldSetAutoDisableAngularThreshold(this->worldId, 0.001);
+ dWorldSetAutoDisableSteps(this->worldId, 50);
+
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
this->globalERPP = new ParamT<double>("erp", 0.2, 0);
Modified: code/gazebo/trunk/server/rendering/CameraManager.cc
===================================================================
--- code/gazebo/trunk/server/rendering/CameraManager.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/CameraManager.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -131,3 +131,13 @@
{
this->activeCamera = (this->activeCamera-1) % this->cameras.size();
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// A frame has started event
+void CameraManager::FrameStarted(double timeSinceLastFrame)
+{
+ if (this->activeCamera < this->cameras.size())
+ this->cameras[this->activeCamera]->FrameStarted(timeSinceLastFrame);
+}
+
+
Modified: code/gazebo/trunk/server/rendering/CameraManager.hh
===================================================================
--- code/gazebo/trunk/server/rendering/CameraManager.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/CameraManager.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -77,6 +77,9 @@
/// \brief Set the prev camera in the queue to be active
public: void DecActiveCamera();
+ /// \brief A frame has started event
+ public: void FrameStarted(double timeSinceLastFrame);
+
/// \brief Connect a boost::slot the the AddCamera signal
public: template<typename T>
void ConnectAddCameraSignal( T subscriber )
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -618,7 +618,7 @@
OgreCreator::Instance()->Update();
- this->root->_fireFrameStarted();
+ /*this->root->_fireFrameStarted();
// Draw all the non-user cameras
for (iter = this->cameras.begin(); iter != this->cameras.end(); iter++)
@@ -627,6 +627,9 @@
(*iter)->Render();
}
+ this->root->_fireFrameEnded();
+*/
+ this->root->renderOneFrame();
// Must update the user camera's last.
for (iter = this->cameras.begin(); iter != this->cameras.end(); iter++)
{
@@ -635,7 +638,7 @@
userCam->Update();
}
- this->root->_fireFrameEnded();
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -724,7 +727,27 @@
return NULL;
}
+////////////////////////////////////////////////////////////////////////////////
+/// Get the world pos of a the first contact at a pixel location
+Vector3 OgreAdaptor::GetFirstContact(OgreCamera *camera, Vector2<int> mousePos)
+{
+ Ogre::Camera *ogreCam = camera->GetOgreCamera();
+ Ogre::Real closest_distance = -1.0f;
+ Ogre::Ray mouseRay = ogreCam->getCameraToViewportRay(
+ (float)mousePos.x / ogreCam->getViewport()->getActualWidth(),
+ (float)mousePos.y / ogreCam->getViewport()->getActualHeight() );
+ this->raySceneQuery->setRay( mouseRay );
+
+ // Perform the scene query
+ Ogre::RaySceneQueryResult &result = this->raySceneQuery->execute();
+ Ogre::RaySceneQueryResult::iterator iter = result.begin();
+
+ Ogre::Vector3 pt = mouseRay.getPoint(iter->distance);
+
+ return Vector3(pt.x, pt.y, pt.z);
+}
+
////////////////////////////////////////////////////////////////////////////////
/// Register a user camera
void OgreAdaptor::RegisterCamera( OgreCamera *cam )
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.hh
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -110,6 +110,9 @@
/// \return The selected entity, or NULL
public: Entity *GetEntityAt(OgreCamera *camera, Vector2<int> mousePos, std::string &mod);
+ /// \brief Get the world pos of a the first contact at a pixel location
+ public: Vector3 GetFirstContact(OgreCamera *camera, Vector2<int> mousePos);
+
/// \brief Register a user camera
public: void RegisterCamera( OgreCamera *cam );
public: void UnregisterCamera( OgreCamera *cam );
Modified: code/gazebo/trunk/server/rendering/OgreCamera.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCamera.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/OgreCamera.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -54,6 +54,7 @@
{
this->name = "DefaultCameraName";
+ this->animState = NULL;
this->textureWidth = this->textureHeight = 0;
this->saveFrameBuffer = NULL;
@@ -249,6 +250,19 @@
// Update the drawing
void OgreCamera::UpdateCam()
{
+
+ if (this->animState)
+ {
+ this->animState->addTime(0.01);
+ if (this->animState->hasEnded())
+ {
+ this->animState = NULL;
+
+ OgreAdaptor::Instance()->sceneMgr->destroyAnimation("cameratrack");
+ OgreAdaptor::Instance()->sceneMgr->destroyAnimationState("cameratrack");
+ }
+ }
+
if (!Simulator::Instance()->GetRenderEngineEnabled())
return;
@@ -738,6 +752,75 @@
}
//////////////////////////////////////////////////////////////////////////////
+// Move the camera to focus on an entity
+void OgreCamera::MoveToEntity(Entity *entity)
+{
+ if (!entity)
+ return;
+
+ if (OgreAdaptor::Instance()->sceneMgr->hasAnimation("cameratrack"))
+ {
+ OgreAdaptor::Instance()->sceneMgr->destroyAnimation("cameratrack");
+ OgreAdaptor::Instance()->sceneMgr->destroyAnimationState("cameratrack");
+ }
+
+ Ogre::Animation *anim = OgreAdaptor::Instance()->sceneMgr->createAnimation("cameratrack",.5);
+ anim->setInterpolationMode(Ogre::Animation::IM_SPLINE);
+
+ Ogre::NodeAnimationTrack *strack = anim->createNodeTrack(0,this->sceneNode);
+ Ogre::NodeAnimationTrack *ptrack = anim->createNodeTrack(1,this->pitchNode);
+
+ Vector3 start = this->GetWorldPose().pos;
+ Vector3 end = entity->GetAbsPose().pos;
+ Vector3 dir = end - start;
+
+ double yawAngle = atan2(dir.y,dir.x);
+ double pitchAngle = atan2(-dir.z, sqrt(dir.x*dir.x + dir.y*dir.y));
+ Ogre::Quaternion yawFinal(Ogre::Radian(yawAngle), Ogre::Vector3(0,0,1));
+ Ogre::Quaternion pitchFinal(Ogre::Radian(pitchAngle), Ogre::Vector3(0,1,0));
+
+ Ogre::TransformKeyFrame *key;
+
+ key = strack->createNodeKeyFrame(0);
+ key->setTranslate(Ogre::Vector3(start.x, start.y, start.z));
+ key->setRotation(this->sceneNode->getOrientation());
+
+ key = ptrack->createNodeKeyFrame(0);
+ key->setRotation(this->pitchNode->getOrientation());
+
+ Vector3 min, max, size;
+ OgreVisual *vis = entity->GetVisualNode();
+ OgreCreator::GetVisualBounds(vis, min,max);
+ size = max-min;
+
+ double scale = std::max(std::max(size.x, size.y), size.z);
+ scale += 0.5;
+
+ dir.Normalize();
+ double dist = start.Distance(end);
+
+ Vector3 mid = start + dir*(dist*.5 - scale);
+ key = strack->createNodeKeyFrame(.2);
+ key->setTranslate( Ogre::Vector3(mid.x, mid.y, mid.z));
+ key->setRotation(yawFinal);
+
+ key = ptrack->createNodeKeyFrame(.2);
+ key->setRotation(pitchFinal);
+
+ end = start + dir*(dist - scale);
+ key = strack->createNodeKeyFrame(.5);
+ key->setTranslate( Ogre::Vector3(end.x, end.y, end.z));
+ key->setRotation(yawFinal);
+
+ key = ptrack->createNodeKeyFrame(.5);
+ key->setRotation(pitchFinal);
+
+ this->animState = OgreAdaptor::Instance()->sceneMgr->createAnimationState("cameratrack");
+ this->animState->setEnabled(true);
+ this->animState->setLoop(false);
+}
+
+//////////////////////////////////////////////////////////////////////////////
/// Set the camera to track an entity
void OgreCamera::TrackModel( Model *model )
{
@@ -747,10 +830,14 @@
{
Body *b = model->GetCanonicalBody();
b->GetVisualNode()->GetSceneNode()->addChild(this->sceneNode);
+ this->camera->setAutoTracking(true, b->GetVisualNode()->GetSceneNode() );
}
else
{
this->origParentNode->addChild(this->sceneNode);
+ this->camera->setAutoTracking(false, NULL);
+ this->camera->setPosition(Ogre::Vector3(0,0,0));
+ this->camera->setOrientation(Ogre::Quaternion(-.5,-.5,.5,.5));
}
}
@@ -890,3 +977,27 @@
}
}
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Frame started
+void OgreCamera::FrameStarted(double timeSinceLastFrame)
+{
+ if (this->animState)
+ {
+ //std::cout << "Add Time[" << timeSinceLastFrame << "]\n";
+ //this->animState->addTime(timeSinceLastFrame);
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the direction the camera is facing
+Vector3 OgreCamera::GetDirection() const
+{
+ Vector3 result;
+ result.x = this->camera->getDerivedDirection().x;
+ result.y = this->camera->getDerivedDirection().y;
+ result.z = this->camera->getDerivedDirection().z;
+ return result;
+}
+
+
Modified: code/gazebo/trunk/server/rendering/OgreCamera.hh
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCamera.hh 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/OgreCamera.hh 2010-05-13 20:29:21 UTC (rev 8677)
@@ -42,12 +42,14 @@
class Camera;
class Viewport;
class SceneNode;
+ class AnimationState;
}
namespace gazebo
{
class XMLConfigNode;
class Model;
+ class Entity;
/// \addtogroup gazebo_rendering
/// \brief Basic camera
@@ -212,6 +214,9 @@
/// \brief Set the camera's name
public: void SetCamName( const std::string &name );
+ /// \brief Move the camera to focus on an entity
+ public: void MoveToEntity(Entity *entity);
+
/// \brief Set the camera to track an entity
public: void TrackModel( Model *model );
@@ -222,6 +227,12 @@
public: void GetCameraToViewportRay(int screenx, int screeny,
Vector3 &origin, Vector3 &dir);
+ /// \brief Frame started
+ public: void FrameStarted(double timeSinceLastFrame);
+
+ /// \brief Get the direction the camera is facing
+ public: Vector3 GetDirection() const;
+
/// \brief if user requests bayer image, post process rgb from ogre to generate bayer formats
private: void ConvertRGBToBAYER(unsigned char* dst, unsigned char* src, std::string format,int width, int height);
@@ -275,6 +286,7 @@
protected: Time renderPeriod;
protected: Time lastUpdate;
+ private: Ogre::AnimationState *animState;
};
/// \}
Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCreator.cc 2010-05-13 06:38:16 UTC (rev 8676)
+++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2010-05-13 20:29:21 UTC (rev 8677)
@@ -474,6 +474,51 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Draw a named line
+void OgreCreator::DrawLine(const Vector3 &start, const Vector3 &end,
+ const std::string &name)
+{
+ Ogre::SceneNode *node = NULL;
+ Ogre::ManualObject *obj = NULL;
+ bool attached = false;
+
@@ Diff output truncated at 100000 characters. @@
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|