Revision: 7365
http://playerstage.svn.sourceforge.net/playerstage/?rev=7365&view=rev
Author: natepak
Date: 2009-03-07 23:36:57 +0000 (Sat, 07 Mar 2009)
Log Message:
-----------
Set state of model through sim iface
Modified Paths:
--------------
code/gazebo/trunk/examples/libgazebo/simiface/SConstruct
code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/sensors/ray/RaySensor.cc
Modified: code/gazebo/trunk/examples/libgazebo/simiface/SConstruct
===================================================================
--- code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-07
08:19:41 UTC (rev 7364)
+++ code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-07
23:36:57 UTC (rev 7365)
@@ -1,7 +1,8 @@
# 3rd party packages
parseConfigs=['pkg-config --cflags --libs libgazebo',
- 'pkg-config --cflags --libs libgazeboServer']
+ 'pkg-config --cflags --libs libgazeboServer',
+ 'pkg-config --cflags --libs playerc++']
env = Environment (
Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-07
08:19:41 UTC (rev 7364)
+++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-07
23:36:57 UTC (rev 7365)
@@ -41,11 +41,30 @@
gazebo::SimulationRequestData *request =
&(simIface->data->requests[simIface->data->requestCount++]);
- request->type = gazebo::SimulationRequestData::SET_POSE2D;
+ request->type = gazebo::SimulationRequestData::SET_STATE;
memcpy(request->modelName, name, 512);
+ request->modelPose.pos.x = i+1;
+ request->modelPose.pos.y = 0;
+ request->modelPose.pos.z = 0;
+ request->modelPose.roll = 0;
+ request->modelPose.pitch = 0;
+ request->modelPose.yaw = 0;
+
+ request->modelLinearVel.x = 0.1;
+ request->modelLinearVel.y = 0;
+ request->modelLinearVel.z = 0;
+
+ request->modelAngularVel.x = 0.1;
+ request->modelAngularVel.y = 0;
+ request->modelAngularVel.z = 0;
+
+ /*request->type = gazebo::SimulationRequestData::SET_POSE2D;
+ memcpy(request->modelName, name, 512);
+
request->modelPose.pos.x = i+.1;
request->modelPose.pos.y = 0;
+ */
simIface->Unlock();
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2009-03-07 08:19:41 UTC (rev
7364)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2009-03-07 23:36:57 UTC (rev
7365)
@@ -375,12 +375,17 @@
GET_POSE2D,
SET_POSE3D,
SET_POSE2D,
+ SET_STATE
};
public: Type type;
public: char modelName[512];
public: Pose modelPose;
+ public: Vec3 modelLinearVel;
+ public: Vec3 modelAngularVel;
+ public: Vec3 modelLinearAccel;
+ public: Vec3 modelAngularAccel;
};
/// \brief Simulation interface data
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-03-07 08:19:41 UTC (rev 7364)
+++ code/gazebo/trunk/server/Model.cc 2009-03-07 23:36:57 UTC (rev 7365)
@@ -490,6 +490,36 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Set the linear velocity of the model
+void Model::SetLinearVel( const Vector3 &vel )
+{
+ Body *body;
+ std::map<std::string, Body* >::iterator iter;
+
+ for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
+ {
+ body = iter->second;
+
+ body->SetLinearVel( vel );
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the angular velocity of the model
+void Model::SetAngularVel( const Vector3 &vel )
+{
+ Body *body;
+ std::map<std::string, Body* >::iterator iter;
+
+ for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
+ {
+ body = iter->second;
+
+ body->SetAngularVel( vel );
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Get the current pose
const Pose3d &Model::GetPose() const
{
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2009-03-07 08:19:41 UTC (rev 7364)
+++ code/gazebo/trunk/server/Model.hh 2009-03-07 23:36:57 UTC (rev 7365)
@@ -107,6 +107,12 @@
/// \brief Set the rotation of the model
public: void SetRotation( const Quatern &rot );
+
+ /// \brief Set the linear velocity of the model
+ public: void SetLinearVel( const Vector3 &vel );
+
+ /// \brief Set the angular velocity of the model
+ public: void SetAngularVel( const Vector3 &vel );
/// \brief Get the current pose
public: const Pose3d &GetPose() const;
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-03-07 08:19:41 UTC (rev 7364)
+++ code/gazebo/trunk/server/World.cc 2009-03-07 23:36:57 UTC (rev 7365)
@@ -584,6 +584,40 @@
Simulator::Instance()->Save();
break;
+ case SimulationRequestData::SET_STATE:
+ {
+ Model *model = this->GetModelByName((char*)req->modelName);
+
+ if (model)
+ {
+ Pose3d pose;
+ Vector3 linearVel( req->modelLinearVel.x,
+ req->modelLinearVel.y,
+ req->modelLinearVel.z);
+ Vector3 angularVel( req->modelAngularVel.x,
+ req->modelAngularVel.y,
+ req->modelAngularVel.z);
+
+ pose.pos.x = req->modelPose.pos.x;
+ pose.pos.y = req->modelPose.pos.y;
+ pose.pos.z = req->modelPose.pos.z;
+
+ pose.rot.SetFromEuler(
+ Vector3(req->modelPose.roll,
+ req->modelPose.pitch,
+ req->modelPose.yaw));
+ model->SetPose(pose);
+
+ model->SetLinearVel(linearVel);
+ model->SetAngularVel(angularVel);
+ }
+ else
+ {
+ gzerr(0) << "Invalid model name[" << req->modelName
+ << "] in simulation interface Set State Request.\n";
+ }
+ break;
+ }
case SimulationRequestData::SET_POSE3D:
{
Pose3d pose;
Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-03-07 08:19:41 UTC
(rev 7364)
+++ code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2009-03-07 23:36:57 UTC
(rev 7365)
@@ -306,6 +306,7 @@
dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ),
( dGeomID ) ( ode->GetSpaceId() ),
this, &UpdateCallback );
+
}
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
