Skip to content
Browse files

Merge pull request #23 from gerkey/master

Implement Get/Set GlobalVelocity, needed by stageros.
  • Loading branch information...
2 parents f0e1c49 + a879cca commit cffb7bf9819469e38396c7711cfde3f4c9573a0e @rtv committed
Showing with 37 additions and 7 deletions.
  1. +1 −1 libstage/blockgroup.cc
  2. +32 −0 libstage/model_position.cc
  3. +4 −6 libstage/stage.hh
View
2 libstage/blockgroup.cc
@@ -169,7 +169,7 @@ static void combineCallback(GLdouble coords[3],
// render each block as a polygon extruded into Z
void BlockGroup::BuildDisplayList()
{
- static bool init = false;
+ //static bool init = false;
static GLUtesselator *tobj = NULL;
if( ! mod.world->IsGUI() )
View
32 libstage/model_position.cc
@@ -158,6 +158,38 @@ void ModelPosition::SetVelocity( const Velocity& val )
CallCallbacks( CB_VELOCITY );
}
+// get the model's velocity in the global frame
+Velocity ModelPosition::GetGlobalVelocity() const
+{
+ Pose gpose = GetGlobalPose();
+
+ double cosa = cos( gpose.a );
+ double sina = sin( gpose.a );
+
+ Velocity gv;
+ gv.x = velocity.x * cosa - velocity.y * sina;
+ gv.y = velocity.x * sina + velocity.y * cosa;
+ gv.a = velocity.a;
+
+ return gv;
+}
+
+// set the model's velocity in the global frame
+void ModelPosition::SetGlobalVelocity( const Velocity& gv
+ )
+{
+ Pose gpose = GetGlobalPose();
+
+ double cosa = cos( gpose.a );
+ double sina = sin( gpose.a );
+
+ Velocity lv;
+ lv.x = gv.x * cosa + gv.y * sina;
+ lv.y = -gv.x * sina + gv.y * cosa;
+ lv.a = gv.a;
+
+ this->SetVelocity( lv );
+}
void ModelPosition::Load( void )
{
View
10 libstage/stage.hh
@@ -2304,12 +2304,6 @@ namespace Stg
/** get the pose of a model in the global CS */
Pose GetGlobalPose() const;
- /** get the velocity of a model in the global CS */
- Velocity GetGlobalVelocity() const;
-
- /* set the velocity of a model in the global coordinate system */
- void SetGlobalVelocity( const Velocity& gvel );
-
/** subscribe to a model's data */
void Subscribe();
@@ -2976,6 +2970,10 @@ namespace Stg
frame. */
Velocity GetVelocity() const { return velocity; }
void SetVelocity( const Velocity& val );
+ /** get the velocity of a model in the global CS */
+ Velocity GetGlobalVelocity() const;
+ /* set the velocity of a model in the global coordinate system */
+ void SetGlobalVelocity( const Velocity& gvel );
/** Specify a point in space. Arrays of Waypoints can be attached to
Models and visualized. */

0 comments on commit cffb7bf

Please sign in to comment.
Something went wrong with that request. Please try again.