Revision: 8576
http://playerstage.svn.sourceforge.net/playerstage/?rev=8576&view=rev
Author: hsujohnhsu
Date: 2010-03-09 23:32:32 +0000 (Tue, 09 Mar 2010)
Log Message:
-----------
cleanup GetCanonicalBody for Model
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2010-03-09 21:48:34 UTC (rev 8575)
+++ code/gazebo/trunk/server/Model.cc 2010-03-09 23:32:32 UTC (rev 8576)
@@ -393,25 +393,9 @@
// this updates the relativePose of the Model Entity
// set model pointer of the canonical body so when body updates,
// one can callback to the model
- if (!this->bodies.empty())
- {
- Body* canonicalBody;
- if (this->bodies.find(this->canonicalBodyNameP->GetValue()) !=
this->bodies.end())
- {
- canonicalBody = this->bodies[this->canonicalBodyNameP->GetValue()];
- canonicalBody->SetCanonicalModel(this);
- // std::cout << "model " << this->GetName()
- // << " canoncal body: " << canonicalBody->GetName() <<
std::endl;
- }
- else
- {
- //std::cout << "bad canonical body?" << std::endl;
- }
- }
- else
- {
- //std::cout << "model " << this->GetName() << " has no bodies" <<
std::endl;
- }
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ cb->SetCanonicalModel(this);
}
@@ -524,28 +508,10 @@
/// \brief set the pose of the model, which is the pose of the canonical body
// this updates the relativePose of the Model Entity
- if (!this->bodies.empty())
- {
- Body* canonicalBody;
- if (this->bodies.find(this->canonicalBodyNameP->GetValue()) !=
this->bodies.end())
- {
- canonicalBody = this->bodies[this->canonicalBodyNameP->GetValue()];
- this->SetAbsPose(canonicalBody->GetAbsPose(),false); // do not recurse
- // std::cout << " Model OnPoseChange " << this->GetName()
- // << " canoncal body: " << canonicalBody->GetName()
- // << " pose " << this->GetRelativePose() << std::endl;
- }
- else
- {
- //std::cout << " has no canonical body" << std::endl;
- }
- }
- else
- {
- //std::cout << " Model " << this->GetName() << " OnPoseChange has no
bodies" << std::endl;
- }
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ this->SetAbsPose(cb->GetAbsPose(),false);
-
}
@@ -703,36 +669,44 @@
/// Get the linear velocity of the model
Vector3 Model::GetLinearVel() const
{
- std::map<std::string, Body* >::const_iterator iter;
- iter = this->bodies.begin();
- return iter->second->GetLinearVel();
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ return cb->GetLinearVel();
+ else // return 0 vector if model has no body
+ return Vector3(0,0,0);
}
////////////////////////////////////////////////////////////////////////////////
/// Get the angular velocity of the model
Vector3 Model::GetAngularVel() const
{
- std::map<std::string, Body* >::const_iterator iter;
- iter = this->bodies.begin();
- return iter->second->GetAngularVel();
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ return cb->GetAngularVel();
+ else // return 0 vector if model has no body
+ return Vector3(0,0,0);
}
////////////////////////////////////////////////////////////////////////////////
/// Get the linear acceleration of the model
Vector3 Model::GetLinearAccel() const
{
- std::map<std::string, Body* >::const_iterator iter;
- iter = this->bodies.begin();
- return iter->second->GetLinearAccel();
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ return cb->GetLinearAccel();
+ else // return 0 vector if model has no body
+ return Vector3(0,0,0);
}
////////////////////////////////////////////////////////////////////////////////
/// Get the angular acceleration of the model
Vector3 Model::GetAngularAccel() const
{
- std::map<std::string, Body* >::const_iterator iter;
- iter = this->bodies.begin();
- return iter->second->GetAngularAccel();
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ return cb->GetAngularAccel();
+ else // return 0 vector if model has no body
+ return Vector3(0,0,0);
}
////////////////////////////////////////////////////////////////////////////////
@@ -1010,9 +984,17 @@
////////////////////////////////////////////////////////////////////////////////
/// Get the canonical body. Used for connected Model heirarchies
-Body *Model::GetCanonicalBody()
+Body * Model::GetCanonicalBody() const
{
- return this->bodies[this->canonicalBodyNameP->GetValue()];
+ if (!this->bodies.empty())
+ {
+ if (this->bodies.find(this->canonicalBodyNameP->GetValue()) !=
this->bodies.end())
+ {
+ return this->bodies.find(this->canonicalBodyNameP->GetValue())->second;
+ }
+ }
+
+ return NULL;
}
////////////////////////////////////////////////////////////////////////////////
@@ -1199,5 +1181,9 @@
/// which is done in Model::OnPoseChange and during Model initialization
Pose3d Model::GetAbsPose()
{
- return this->bodies[**this->canonicalBodyNameP]->GetAbsPose();
+ Body* cb = this->GetCanonicalBody();
+ if (cb != NULL)
+ return cb->GetAbsPose();
+ else // GetCanonicalBody() returns NULL only if this->bodies is empty
+ return Pose3d();
}
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2010-03-09 21:48:34 UTC (rev 8575)
+++ code/gazebo/trunk/server/Model.hh 2010-03-09 23:32:32 UTC (rev 8576)
@@ -180,7 +180,7 @@
/// \brief Get the canonical body. Used for connected Model heirarchies
/// \return Pointer to the body
- public: Body *GetCanonicalBody();
+ public: Body *GetCanonicalBody() const;
/// \brief Called when the pose of the entity (or one of its parents) has
/// changed
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
