Revision: 7542
http://playerstage.svn.sourceforge.net/playerstage/?rev=7542&view=rev
Author: natepak
Date: 2009-03-24 19:40:33 +0000 (Tue, 24 Mar 2009)
Log Message:
-----------
Updated the gripper
Modified Paths:
--------------
code/branches/federation/gazebo/server/Angle.hh
code/branches/federation/gazebo/server/Model.cc
code/branches/federation/gazebo/server/Model.hh
code/branches/federation/gazebo/server/Simulator.cc
code/branches/federation/gazebo/server/World.cc
code/branches/federation/gazebo/server/controllers/Controller.cc
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
code/branches/federation/gazebo/server/main.cc
code/branches/federation/gazebo/server/physics/Body.cc
code/branches/federation/gazebo/server/physics/Body.hh
code/branches/federation/gazebo/server/physics/Joint.cc
code/branches/federation/gazebo/server/physics/RayGeom.cc
code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc
code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
code/branches/federation/gazebo/worlds/models/pioneer2gripper.model
code/branches/federation/gazebo/worlds/models/sicklms200.model
Modified: code/branches/federation/gazebo/server/Angle.hh
===================================================================
--- code/branches/federation/gazebo/server/Angle.hh 2009-03-24 18:50:06 UTC
(rev 7541)
+++ code/branches/federation/gazebo/server/Angle.hh 2009-03-24 19:40:33 UTC
(rev 7542)
@@ -28,6 +28,7 @@
#define ANGLE_HH
#include <iostream>
+#include <math.h>
namespace gazebo
{
Modified: code/branches/federation/gazebo/server/Model.cc
===================================================================
--- code/branches/federation/gazebo/server/Model.cc 2009-03-24 18:50:06 UTC
(rev 7541)
+++ code/branches/federation/gazebo/server/Model.cc 2009-03-24 19:40:33 UTC
(rev 7542)
@@ -96,29 +96,53 @@
std::map< std::string, Controller* >::iterator citer;
if (this->graphicsHandler)
+ {
delete this->graphicsHandler;
+ this->graphicsHandler = NULL;
+ }
for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
{
- GZ_DELETE(biter->second);
+ if (biter->second)
+ {
+ delete biter->second;
+ biter->second = NULL;
+ }
}
this->bodies.clear();
for (jiter = this->joints.begin(); jiter != this->joints.end(); jiter++)
{
- GZ_DELETE( jiter->second );
+ if (jiter->second)
+ {
+ delete jiter->second;
+ jiter->second=NULL;
+ }
}
this->joints.clear();
for (citer = this->controllers.begin();
citer != this->controllers.end(); citer++)
{
- GZ_DELETE( citer->second );
+ if (citer->second)
+ {
+ delete citer->second;
+ citer->second = NULL;
+ }
}
this->controllers.clear();
- GZ_DELETE(this->parentBodyNameP);
- GZ_DELETE(this->myBodyNameP);
+ if (this->parentBodyNameP)
+ {
+ delete this->parentBodyNameP;
+ this->parentBodyNameP = NULL;
+ }
+
+ if (this->myBodyNameP)
+ {
+ delete this->myBodyNameP;
+ this->myBodyNameP = NULL;
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -741,6 +765,7 @@
std::cerr << "Error Loading Controller[" << controllerName
<< "]\n" << e << std::endl;
delete controller;
+ controller = NULL;
return;
}
@@ -766,8 +791,40 @@
{
return &(this->bodies);
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get a sensor by name
+Sensor *Model::GetSensor(const std::string &name) const
+{
+ Sensor *sensor = NULL;
+ std::map< std::string, Body* >::const_iterator biter;
+
+ for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
+ {
+ if ( (sensor = biter->second->GetSensor(name)) != NULL)
+ break;
+ }
+
+ return sensor;
+}
////////////////////////////////////////////////////////////////////////////////
+/// Get a geom by name
+Geom *Model::GetGeom(const std::string &name) const
+{
+ Geom *geom = NULL;
+ std::map< std::string, Body* >::const_iterator biter;
+
+ for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
+ {
+ if ( (geom = biter->second->GetGeom(name)) != NULL)
+ break;
+ }
+
+ return geom;
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get a body by name
Body *Model::GetBody(const std::string &name)
{
Modified: code/branches/federation/gazebo/server/Model.hh
===================================================================
--- code/branches/federation/gazebo/server/Model.hh 2009-03-24 18:50:06 UTC
(rev 7541)
+++ code/branches/federation/gazebo/server/Model.hh 2009-03-24 19:40:33 UTC
(rev 7542)
@@ -49,6 +49,8 @@
class Controller;
class HingeJoint;
class GraphicsIfaceHandler;
+ class Sensor;
+ class Geom;
/// \addtogroup gazebo_server
/// \brief A model
@@ -149,6 +151,12 @@
/// \brief Get a map of all the bodies
public: const std::map<std::string, Body*> *GetBodies() const;
+
+ /// \brief Get a sensor by name
+ public: Sensor *GetSensor(const std::string &name) const;
+
+ /// \brief Get a geom by name
+ public: Geom *GetGeom(const std::string &name) const;
/// \brief Attach this model to its parent
public: void Attach(XMLConfigNode *node);
Modified: code/branches/federation/gazebo/server/Simulator.cc
===================================================================
--- code/branches/federation/gazebo/server/Simulator.cc 2009-03-24 18:50:06 UTC
(rev 7541)
+++ code/branches/federation/gazebo/server/Simulator.cc 2009-03-24 19:40:33 UTC
(rev 7542)
@@ -84,10 +84,42 @@
// Destructor
Simulator::~Simulator()
{
- this->Close();
- delete this->mutex;
- this->mutex = NULL;
+ if (this->gazeboConfig)
+ {
+ delete this->gazeboConfig;
+ this->gazeboConfig = NULL;
+ }
+
+ if (this->xmlFile)
+ {
+ delete this->xmlFile;
+ this->xmlFile = NULL;
+ }
+
+ if (this->mutex)
+ {
+ delete this->mutex;
+ this->mutex = NULL;
+ }
+
+ if (this->gui)
+ {
+ delete this->gui;
+ this->gui = NULL;
+ }
+
+ if (this->physicsThread)
+ {
+ delete this->physicsThread;
+ this->physicsThread = NULL;
+ }
+
+ if (this->mutex)
+ {
+ delete this->mutex;
+ this->mutex = NULL;
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -97,13 +129,8 @@
if (!this->loaded)
return;
- GZ_DELETE (this->gui)
- GZ_DELETE (this->xmlFile)
- GZ_DELETE (this->gazeboConfig)
gazebo::World::Instance()->Close();
gazebo::OgreAdaptor::Instance()->Close();
-
- //GZ_DELETE(this->renderEngine);
}
////////////////////////////////////////////////////////////////////////////////
@@ -280,6 +307,8 @@
void Simulator::Fini( )
{
gazebo::World::Instance()->Fini();
+
+ this->Close();
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/branches/federation/gazebo/server/World.cc
===================================================================
--- code/branches/federation/gazebo/server/World.cc 2009-03-24 18:50:06 UTC
(rev 7541)
+++ code/branches/federation/gazebo/server/World.cc 2009-03-24 19:40:33 UTC
(rev 7542)
@@ -87,12 +87,25 @@
this->models.clear();
this->geometries.clear();
- GZ_DELETE (this->physicsEngine)
- GZ_DELETE (this->server)
+ if (this->physicsEngine)
+ {
+ delete this->physicsEngine;
+ this->physicsEngine = NULL;
+ }
+ if (this->server)
+ {
+ delete this->server;
+ this->server =NULL;
+ }
+
try
{
- GZ_DELETE (this->simIface)
+ if (this->simIface)
+ {
+ delete this->simIface;
+ this->simIface = NULL;
+ }
}
catch (std::string e)
{
Modified: code/branches/federation/gazebo/server/controllers/Controller.cc
===================================================================
--- code/branches/federation/gazebo/server/controllers/Controller.cc
2009-03-24 18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/controllers/Controller.cc
2009-03-24 19:40:33 UTC (rev 7542)
@@ -58,6 +58,7 @@
/// Destructor
Controller::~Controller()
{
+ std::cout << "Deleting Controller[" << **this->nameP << "]\n";
delete this->nameP;
delete this->updatePeriodP;
}
Modified:
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
===================================================================
---
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
2009-03-24 18:50:06 UTC (rev 7541)
+++
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
2009-03-24 19:40:33 UTC (rev 7542)
@@ -25,6 +25,13 @@
* SVN info: $Id$
*/
+#include <boost/bind.hpp>
+
+#include "PhysicsEngine.hh"
+#include "Geom.hh"
+#include "ContactParams.hh"
+#include "Simulator.hh"
+#include "RaySensor.hh"
#include "Global.hh"
#include "XMLConfig.hh"
#include "Model.hh"
@@ -57,12 +64,35 @@
// Destructor
Pioneer2_Gripper::~Pioneer2_Gripper()
{
+ if (this->holdJoint)
+ delete this->holdJoint;
+ this->holdJoint = NULL;
+
for (int i=0; i<3; i++)
{
- GZ_DELETE(this->jointNamesP[i]);
- GZ_DELETE(this->gainsP[i]);
- GZ_DELETE(this->forcesP[i]);
+ if (this->jointNamesP[i])
+ delete this->jointNamesP[i];
+ this->jointNamesP[i] = NULL;
+
+ if (this->gainsP[i])
+ delete this->gainsP[i];
+ this->gainsP[i] = NULL;
+
+ if (this->forcesP[i])
+ delete this->forcesP[i];
+ this->forcesP[i] = NULL;
}
+
+ for (int i =0; i<2; i++)
+ {
+ if (this->breakBeamNamesP[i])
+ delete this->breakBeamNamesP[i];
+ this->breakBeamNamesP[i] = NULL;
+
+ if (this->paddleNamesP[i])
+ delete this->paddleNamesP[i];
+ this->paddleNamesP[i] = NULL;
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -133,8 +163,26 @@
this->joints[LIFT] =
dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->jointNamesP[LIFT]->GetValue()));
}
+ this->breakBeamNamesP[0] = new ParamT<std::string>("name","",1);
+ this->breakBeamNamesP[1] = new ParamT<std::string>("name","",1);
+
+ this->breakBeamNamesP[0]->Load(node->GetChild("outerBreakBeam"));
+ this->breakBeamNamesP[1]->Load(node->GetChild("innerBreakBeam"));
+
+ this->paddleNamesP[LEFT] = new ParamT<std::string>("name","",1);
+ this->paddleNamesP[RIGHT] = new ParamT<std::string>("name","",1);
+
+ this->paddleNamesP[LEFT]->Load(node->GetChild("leftPaddle"));
+ this->paddleNamesP[RIGHT]->Load(node->GetChild("rightPaddle"));
+
Param::End();
+ this->breakBeams[0] =
dynamic_cast<RaySensor*>(this->myParent->GetSensor(**this->breakBeamNamesP[0]));
+ this->breakBeams[1] =
dynamic_cast<RaySensor*>(this->myParent->GetSensor(**this->breakBeamNamesP[1]));
+
+ this->paddles[LEFT] =
dynamic_cast<Geom*>(this->myParent->GetGeom(**this->paddleNamesP[LEFT]));
+ this->paddles[RIGHT] =
dynamic_cast<Geom*>(this->myParent->GetGeom(**this->paddleNamesP[RIGHT]));
+
if (!this->joints[LEFT])
gzthrow("couldn't get left slider joint");
@@ -143,6 +191,24 @@
if (!this->joints[LIFT])
gzthrow("couldn't get lift slider joint");
+
+ if (!this->breakBeams[0])
+ gzthrow("Couldn't get outer breakbeam sensor");
+
+ if (!this->breakBeams[1])
+ gzthrow("Couldn't get inner breakbeam sensor");
+
+ if (!this->paddles[LEFT])
+ gzthrow("Couldn't get the left paddle geom");
+
+ if (!this->paddles[RIGHT])
+ gzthrow("Couldn't get the right paddle geom");
+
+ this->holdJoint =
(SliderJoint*)World::Instance()->GetPhysicsEngine()->CreateJoint(Joint::SLIDER);
+ this->holdJoint->SetName(this->GetName() + "_Hold_Joint");
+
+ this->paddles[LEFT]->contact->Callback(&Pioneer2_Gripper::LeftPaddleCB,
this);
+ this->paddles[RIGHT]->contact->Callback(&Pioneer2_Gripper::RightPaddleCB,
this);
}
////////////////////////////////////////////////////////////////////////////////
@@ -163,23 +229,48 @@
// Initialize the controller
void Pioneer2_Gripper::InitChild()
{
+ // Initially keep the gripper closed
+ this->joints[RIGHT]->SetParam(dParamVel,-0.1);
+ this->joints[LEFT]->SetParam(dParamVel,0.1);
+ this->joints[LEFT]->SetParam(dParamFMax, **(this->forcesP[LEFT]));
+ this->joints[RIGHT]->SetParam(dParamFMax, **(this->forcesP[RIGHT]));
+
+
+ // Initially lower the lift
+ this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT]));
+ this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT]));
+
+ this->contactGeoms[LEFT] = this->contactGeoms[RIGHT] = NULL;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void Pioneer2_Gripper::UpdateChild()
{
- /*double leftPaddleHiStop = this->joints[LEFT]->GetParam(dParamHiStop);
- double leftPaddleLoStop = this->joints[LEFT]->GetParam(dParamLoStop);
- double rightPaddleHiStop = this->joints[RIGHT]->GetParam(dParamHiStop);
- double rightPaddleLoStop = this->joints[RIGHT]->GetParam(dParamLoStop);
+ // Create a hold joint, if both paddles are touching the same geom
+ if (this->contactGeoms[LEFT] && this->contactGeoms[RIGHT] &&
+ this->contactGeoms[LEFT]->GetName() ==
+ this->contactGeoms[RIGHT]->GetName())
+ {
+ if (!this->holdJoint->AreConnected(this->myParent->GetBody(),
+ this->contactGeoms[LEFT]->GetBody()))
+ {
+ this->holdJoint->Attach(this->myParent->GetBody(),
+ this->contactGeoms[LEFT]->GetBody());
+ this->holdJoint->SetAxis(Vector3(0,0,1));
- double leftPaddlePos = this->joints[LEFT]->GetPosition();
- double rightPaddlePos = this->joints[RIGHT]->GetPosition();
- */
+ }
+ }
+ // Otherwise disconnect the joint if it has been created
+ else if (this->holdJoint->GetJointBody(0))
+ {
+ this->holdJoint->Detach();
+ this->contactGeoms[LEFT] = this->contactGeoms[RIGHT] = NULL;
+ }
this->gripIface->Lock(1);
+ // Move the paddles
switch( this->gripIface->data->cmd)
{
case GAZEBO_GRIPPER_CMD_OPEN:
@@ -192,58 +283,54 @@
this->joints[LEFT]->SetParam(dParamVel,0.1);
break;
-/* case GAZEBO_GRIPPER_CMD_STORE:
- this->joints[LIFT]->SetParam(dParamVel, 0.2);
- break;
-
- case GAZEBO_GRIPPER_CMD_RETRIEVE:
- this->joints[LIFT]->SetParam(dParamVel, -0.2);
- break;
- */
-
case GAZEBO_GRIPPER_CMD_STOP:
this->joints[RIGHT]->SetParam(dParamVel,0);
this->joints[LEFT]->SetParam(dParamVel,0);
- // this->joints[LIFT]->SetParam(dParamVel,0);
break;
-
-
- /*default:
- this->joints[RIGHT]->SetParam(dParamVel,0.0);
- this->joints[LEFT]->SetParam(dParamVel,0.0);
- this->joints[LIFT]->SetParam(dParamVel,0.0);
- break;
- */
}
// Move the lift
if (this->actIface->data->cmd_pos[0] > 0.5)
- this->joints[LIFT]->SetParam(dParamVel, 0.2);
+ {
+ this->joints[LIFT]->SetParam(dParamVel, 0.2);
+ }
else if (this->actIface->data->cmd_pos[0] < 0.5)
- this->joints[LIFT]->SetParam(dParamVel, -0.2);
+ {
+ this->joints[LIFT]->SetParam(dParamVel, -0.2);
+ }
-
this->joints[LEFT]->SetParam(dParamFMax, **(this->forcesP[LEFT]));
this->joints[RIGHT]->SetParam(dParamFMax, **(this->forcesP[RIGHT]));
this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT]));
+ // DEBUG Statements
/*printf("Left Pos[%f] High[%f]
Low[%f]\n",this->joints[LEFT]->GetPosition(),
this->joints[LEFT]->GetHighStop(),this->joints[LEFT]->GetLowStop() );
printf("Right Pos[%f] High[%f]
Low[%f]\n",this->joints[RIGHT]->GetPosition(),
this->joints[RIGHT]->GetHighStop(),this->joints[RIGHT]->GetLowStop() );
- */
printf("Lift Pos[%f] High[%f] Low[%f]\n", this->joints[LIFT]->GetPosition(),
this->joints[LIFT]->GetHighStop(), this->joints[LIFT]->GetLowStop());
-
+ */
- // Set the state of the paddles
+ // Set the state of the left paddle pressure sensor
+ if (this->contactGeoms[LEFT])
+ this->gripIface->data->left_paddle_open = 0;
+ else
+ this->gripIface->data->left_paddle_open = 1;
+
+ // Set the state of the right paddle pressure sensor
+ if (this->contactGeoms[RIGHT])
+ this->gripIface->data->right_paddle_open = 0;
+ else
+ this->gripIface->data->right_paddle_open = 1;
+
+
+ // Set the OPEN/CLOSED/MOVING state of the gripper
if (fabs(this->joints[LEFT]->GetPosition() -
this->joints[LEFT]->GetHighStop()) < 0.01 &&
fabs(this->joints[RIGHT]->GetPosition() -
this->joints[RIGHT]->GetLowStop()) < 0.01)
{
this->gripIface->data->state = GAZEBO_GRIPPER_STATE_CLOSED;
- this->gripIface->data->left_paddle_open = 0;
- this->gripIface->data->right_paddle_open = 0;
}
else if (fabs(this->joints[LEFT]->GetPosition() -
this->joints[LEFT]->GetLowStop()) < 0.01 &&
@@ -251,26 +338,47 @@
this->joints[RIGHT]->GetHighStop()) < 0.01)
{
this->gripIface->data->state = GAZEBO_GRIPPER_STATE_OPEN;
- this->gripIface->data->left_paddle_open = 1;
- this->gripIface->data->right_paddle_open = 1;
}
else
{
this->gripIface->data->state = GAZEBO_GRIPPER_STATE_MOVING;
- this->gripIface->data->left_paddle_open = 0;
- this->gripIface->data->right_paddle_open = 0;
}
- // Set the state of the lift
+ // Set the UP/DOWN state of the lift
if (fabs(this->joints[LIFT]->GetPosition() -
this->joints[LIFT]->GetHighStop()) < 0.01)
+ {
this->actIface->data->actuators[0].position = 1;
+ }
else if (fabs(this->joints[LIFT]->GetPosition() -
this->joints[LIFT]->GetLowStop()) < 0.01)
+ {
this->actIface->data->actuators[0].position = 0;
+ }
+
+ // Check the break beams
+ if (this->gripIface->data->state == GAZEBO_GRIPPER_STATE_OPEN)
+ {
+ if (this->breakBeams[0]->GetRange(0) < 0.22)
+ this->gripIface->data->outer_beam_obstruct = 1;
+ else
+ this->gripIface->data->outer_beam_obstruct = 0;
+
+ if (this->breakBeams[1]->GetRange(0) < 0.22)
+ this->gripIface->data->inner_beam_obstruct = 1;
+ else
+ this->gripIface->data->inner_beam_obstruct = 0;
+ }
+
this->actIface->data->actuators_count = 1;
+
+ this->gripIface->data->head.time = Simulator::Instance()->GetSimTime();
+ this->gripIface->Post();
this->gripIface->Unlock();
+
+ // Reset these flags.
+ this->contactGeoms[LEFT] = this->contactGeoms[RIGHT] = NULL;
}
////////////////////////////////////////////////////////////////////////////////
@@ -278,3 +386,23 @@
void Pioneer2_Gripper::FiniChild()
{
}
+
+////////////////////////////////////////////////////////////////////////////////
+// Left paddle contact callback
+void Pioneer2_Gripper::LeftPaddleCB(Geom *g1, Geom *g2)
+{
+ if (g1->GetName() != this->paddles[LEFT]->GetName())
+ this->contactGeoms[LEFT] = g1;
+ else
+ this->contactGeoms[LEFT] = g2;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Right paddle contact callback
+void Pioneer2_Gripper::RightPaddleCB(Geom *g1, Geom *g2)
+{
+ if (g1->GetName() != this->paddles[RIGHT]->GetName())
+ this->contactGeoms[RIGHT] = g1;
+ else
+ this->contactGeoms[RIGHT] = g2;
+}
Modified:
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
===================================================================
---
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
2009-03-24 18:50:06 UTC (rev 7541)
+++
code/branches/federation/gazebo/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
2009-03-24 19:40:33 UTC (rev 7542)
@@ -37,6 +37,7 @@
class SliderJoint;
class GripperIface;
class ActarrayIface;
+ class RaySensor;
/// \addtogroup gazebo_controller
/// \{
@@ -61,13 +62,19 @@
/// This is a controller that simulates a Pioneer 2DX motion
class Pioneer2_Gripper : public Controller
{
- /// Constructor
+ /// \brief Constructor
public: Pioneer2_Gripper(Entity *parent);
- /// Destructor
+ /// \brief Destructor
public: virtual ~Pioneer2_Gripper();
- /// Load the controller
+ /// \brief Left paddle contact callback
+ public: void LeftPaddleCB(Geom *g1, Geom *g2);
+
+ /// \brief Right paddle contact callback
+ public: void RightPaddleCB(Geom *g1, Geom *g2);
+
+ /// \brief Load the controller
/// \param node XML config node
/// \return 0 on success
protected: virtual void LoadChild(XMLConfigNode *node);
@@ -94,15 +101,22 @@
// The interface for the lift
private: ActarrayIface *actIface;
+ private: RaySensor *breakBeams[2];
+
/// The parent Model
private: Model *myParent;
private: SliderJoint *joints[3];
+ private: SliderJoint *holdJoint;
+ private: Geom *paddles[2];
+ private: Geom *contactGeoms[2];
+
private: ParamT<std::string> *jointNamesP[3];
private: ParamT<float> *gainsP[3];
private: ParamT<float> *forcesP[3];
-
+ private: ParamT<std::string> *breakBeamNamesP[2];
+ private: ParamT<std::string> *paddleNamesP[2];
};
/** \} */
Modified: code/branches/federation/gazebo/server/main.cc
===================================================================
--- code/branches/federation/gazebo/server/main.cc 2009-03-24 18:50:06 UTC
(rev 7541)
+++ code/branches/federation/gazebo/server/main.cc 2009-03-24 19:40:33 UTC
(rev 7542)
@@ -277,8 +277,9 @@
}
catch (gazebo::GazeboError e)
{
- std::cerr << "Erro Loading Gazebo" << std::endl;
+ std::cerr << "Error Loading Gazebo" << std::endl;
std::cerr << e << std::endl;
+ gazebo::Simulator::Instance()->Fini();
return -1;
}
@@ -291,6 +292,7 @@
{
std::cerr << "Initialization failed" << std::endl;
std::cerr << e << std::endl;
+ gazebo::Simulator::Instance()->Fini();
return -1;
}
@@ -303,6 +305,7 @@
{
std::cerr << "Main simulation loop failed" << std::endl;
std::cerr << e << std::endl;
+ gazebo::Simulator::Instance()->Fini();
return -1;
}
Modified: code/branches/federation/gazebo/server/physics/Body.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.cc 2009-03-24
18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/physics/Body.cc 2009-03-24
19:40:33 UTC (rev 7542)
@@ -793,19 +793,33 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Get a sensor by name
+Sensor *Body::GetSensor( const std::string &name ) const
+{
+ Sensor *sensor = NULL;
+ std::vector< Sensor* >::const_iterator iter;
+
+ for (iter = this->sensors.begin(); iter != this->sensors.end(); iter++)
+ {
+ if ((*iter)->GetName() == name)
+ {
+ sensor = (*iter);
+ break;
+ }
+ }
+
+ return sensor;
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get a geom by name
Geom *Body::GetGeom(const std::string &name) const
{
std::map<std::string, Geom*>::const_iterator iter = this->geoms.find(name);
if (iter != this->geoms.end())
- {
return iter->second;
- }
else
- {
- gzerr(0) << "Unknown geom[" << name << "]\n";
return NULL;
- }
}
Modified: code/branches/federation/gazebo/server/physics/Body.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.hh 2009-03-24
18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/physics/Body.hh 2009-03-24
19:40:33 UTC (rev 7542)
@@ -175,6 +175,9 @@
/// \brief Get the mass of the body
public: float GetMass() const { return mass.mass; }
+ /// \brief Get a sensor by name
+ public: Sensor *GetSensor( const std::string &name ) const;
+
/// Load a new geom helper function
/// \param node XMLConfigNode used to load the geom
private: void LoadGeom(XMLConfigNode *node);
Modified: code/branches/federation/gazebo/server/physics/Joint.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-24
18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/physics/Joint.cc 2009-03-24
19:40:33 UTC (rev 7542)
@@ -52,6 +52,9 @@
this->provideFeedbackP = new ParamT<bool>("provideFeedback", false, 0);
this->fudgeFactorP = new ParamT<double>( "fudgeFactor", 1.0, 0 );
Param::End();
+
+ this->body1 = NULL;
+ this->body2 = NULL;
}
@@ -236,9 +239,10 @@
if ( index==0 || index==1 )
{
- if (dJointGetBody( this->jointId, index ) == this->body1->GetId())
+ if (this->body1 &&
+ dJointGetBody( this->jointId, index ) == this->body1->GetId())
result = this->body1;
- else
+ else if (this->body2)
result = this->body2;
}
@@ -298,6 +302,8 @@
// Detach this joint from all bodies
void Joint::Detach()
{
+ this->body1 = NULL;
+ this->body2 = NULL;
dJointAttach( this->jointId, 0, 0 );
return;
}
Modified: code/branches/federation/gazebo/server/physics/RayGeom.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/RayGeom.cc 2009-03-24
18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/physics/RayGeom.cc 2009-03-24
19:40:33 UTC (rev 7542)
@@ -50,7 +50,6 @@
if (displayRays)
{
- //this->line = new OgreDynamicLines(OgreDynamicRenderable::OT_LINE_LIST);
this->line =
OgreCreator::Instance()->CreateDynamicLine(OgreDynamicRenderable::OT_LINE_LIST);
// Add two points
Modified: code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc
2009-03-24 18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/rendering/OgreDynamicLines.cc
2009-03-24 19:40:33 UTC (rev 7542)
@@ -27,6 +27,7 @@
#include <Ogre.h>
#include <cassert>
#include <cmath>
+#include <math.h>
#include <sstream>
#include "GazeboError.hh"
@@ -90,7 +91,7 @@
void OgreDynamicLines::Update()
{
- if (this->dirty)
+ if (this->dirty && this->points.size() > 1)
this->FillHardwareBuffers();
}
@@ -156,8 +157,26 @@
if ((float)vaabMin.z >= (float)vaabMax.z)
vaabMin.z = vaabMax.z - 10;
+ if (!finite(vaabMin.x))
+ vaabMin.x = 0;
+ if (!finite(vaabMin.y))
+ vaabMin.y = 0;
+ if (!finite(vaabMin.z))
+ vaabMin.z = 0;
+
+ if (!finite(vaabMax.x))
+ vaabMax.x = 0;
+ if (!finite(vaabMax.y))
+ vaabMax.y = 0;
+ if (!finite(vaabMax.z))
+ vaabMax.z = 0;
+
+ /*printf("Min[%f %f %f] Max[%f %f %f]\n",vaabMin.x, vaabMin.y, vaabMin.z,
+ vaabMax.x, vaabMax.y, vaabMax.z);
+ */
+
this->mBox.setExtents(Ogre::Vector3(vaabMin.x, vaabMin.y, vaabMin.z),
- Ogre::Vector3(vaabMax.x, vaabMax.y, vaabMax.z) );
+ Ogre::Vector3(vaabMax.x, vaabMax.y, vaabMax.z) );
this->dirty = false;
}
Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
===================================================================
--- code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
2009-03-24 18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.cc
2009-03-24 19:40:33 UTC (rev 7542)
@@ -73,7 +73,7 @@
this->minRangeP = new ParamT<double>("minRange",0,1);
this->maxRangeP = new ParamT<double>("maxRange",0,1);
this->originP = new ParamT<Vector3>("origin", Vector3(0,0,0), 0);
- this->displayRaysP = new ParamT<bool>("displayRays", true, 0);
+ this->displayRaysP = new ParamT<std::string>("displayRays", "off", 0);
Param::End();
}
@@ -82,8 +82,12 @@
// Destructor
RaySensor::~RaySensor()
{
- delete this->rayFan;
+ if (this->rayFan)
+ delete this->rayFan;
+ if (this->rayFanOutline)
+ delete this->rayFanOutline;
+
delete this->rayCountP;
delete this->rangeCountP;
delete this->minAngleP;
@@ -151,43 +155,50 @@
bodyPose = this->body->GetPose();
this->prevPose = bodyPose;
-
- this->rayFan->AddPoint(Vector3(0,0,0));
- this->rayFanOutline->AddPoint(Vector3(0,0,0));
-
// Create and array of ray geoms
for (int i = 0; i < this->rayCountP->GetValue(); i++)
{
double diff = (**(this->maxAngleP) - **(this->minAngleP)).GetAsRadian();
- angle = i * diff / (rayCountP->GetValue() - 1) +
(**(this->minAngleP)).GetAsRadian();
+ angle = i * diff / (rayCountP->GetValue()) +
(**(this->minAngleP)).GetAsRadian();
axis.Set(cos(angle), sin(angle),0);
start = (axis * this->minRangeP->GetValue()) + this->originP->GetValue();
end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue();
- ray = new RayGeom(this->body, false);
+ ray = new RayGeom(this->body, (**this->displayRaysP) == "lines");
- this->rayFan->AddPoint(end);
- this->rayFanOutline->AddPoint(end);
+ if ((**this->displayRaysP) == "fan")
+ {
+ if (i == 0)
+ {
+ this->rayFan->AddPoint(start);
+ this->rayFanOutline->AddPoint(start);
+ }
+ this->rayFan->AddPoint(end);
+ this->rayFanOutline->AddPoint(end);
+ }
+
+
ray->SetPoints(start, end);
this->rays.push_back(ray);
}
- this->rayFan->AddPoint(Vector3(0,0,0));
- this->rayFan->setMaterial("Gazebo/BlueLaser");
+ if ((**this->displayRaysP) == "fan")
+ {
+ this->rayFan->AddPoint(this->rayFan->GetPoint(0));
+ this->rayFan->setMaterial("Gazebo/BlueLaser");
- this->rayFanOutline->AddPoint(Vector3(0,0,0));
- this->rayFanOutline->setMaterial("Gazebo/BlueEmissive");
+ this->rayFanOutline->AddPoint(this->rayFanOutline->GetPoint(0));
+ this->rayFanOutline->setMaterial("Gazebo/BlueEmissive");
- if (**displayRaysP)
- {
this->visualNode->AttachObject(this->rayFan);
this->visualNode->AttachObject(this->rayFanOutline);
}
+
}
//////////////////////////////////////////////////////////////////////////////
@@ -295,7 +306,7 @@
// Update the sensor information
void RaySensor::UpdateChild()
{
-// if (this->active)
+ // if (this->active)
{
std::vector<RayGeom*>::iterator iter;
Pose3d poseDelta;
@@ -329,18 +340,21 @@
// Do collision detection
dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ),
- ( dGeomID ) ( ode->GetSpaceId() ),
- this, &UpdateCallback );
-
- i = 1;
- for (iter = this->rays.begin(); iter != this->rays.end(); iter++, i++)
- {
- (*iter)->Update();
+ ( dGeomID ) ( ode->GetSpaceId() ),
+ this, &UpdateCallback );
- (*iter)->GetRelativePoints(a,b);
+ if ((**this->displayRaysP) == "fan")
+ {
+ i = 1;
+ for (iter = this->rays.begin(); iter != this->rays.end(); iter++, i++)
+ {
+ (*iter)->Update();
- this->rayFan->SetPoint(i,b);
- this->rayFanOutline->SetPoint(i,b);
+ (*iter)->GetRelativePoints(a,b);
+
+ this->rayFan->SetPoint(i,b);
+ this->rayFanOutline->SetPoint(i,b);
+ }
}
}
}
Modified: code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
===================================================================
--- code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
2009-03-24 18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/server/sensors/ray/RaySensor.hh
2009-03-24 19:40:33 UTC (rev 7542)
@@ -142,7 +142,7 @@
private: ParamT<int> *rangeCountP;
/// Display rays when rendering images
- private: ParamT<bool> *displayRaysP;
+ private: ParamT<std::string> *displayRaysP;
private: OgreDynamicLines *rayFan;
private: OgreDynamicLines *rayFanOutline;
Modified: code/branches/federation/gazebo/worlds/models/pioneer2gripper.model
===================================================================
--- code/branches/federation/gazebo/worlds/models/pioneer2gripper.model
2009-03-24 18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/worlds/models/pioneer2gripper.model
2009-03-24 19:40:33 UTC (rev 7542)
@@ -77,6 +77,14 @@
<material>Gazebo/Black</material>
</visual>
</geom:box>
+
+ <sensor:ray name="breakbeam1">
+ <rayCount>1</rayCount>
+ <origin>0 0 0</origin>
+ <displayRays>true</displayRays>
+ <minRange>0.01</minRange>
+ <maxRange>0.5</maxRange>
+ </sensor:ray>
</body:box>
Modified: code/branches/federation/gazebo/worlds/models/sicklms200.model
===================================================================
--- code/branches/federation/gazebo/worlds/models/sicklms200.model
2009-03-24 18:50:06 UTC (rev 7541)
+++ code/branches/federation/gazebo/worlds/models/sicklms200.model
2009-03-24 19:40:33 UTC (rev 7542)
@@ -46,7 +46,6 @@
<displayRays>false</displayRays>
-
<minAngle>-90</minAngle>
<maxAngle>90</maxAngle>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
