fixed merge conflict and a color in fasr2

rtv · Apr 22, 2012 · 974709c · 974709c
2 parents 39fdb26 + cffb7bf
commit 974709c
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 7 deletions.
diff --git a/libstage/model_position.cc b/libstage/model_position.cc
@@ -157,6 +157,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 )
 {

diff --git a/libstage/stage.hh b/libstage/stage.hh
@@ -2301,12 +2301,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();
 
@@ -2956,6 +2950,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. */

diff --git a/worlds/fasr2.world b/worlds/fasr2.world
@@ -222,7 +222,7 @@ define blinker model
 
 define autorob pioneer2dx		  
 (		 
- sickbase( sensor( samples 32 range [0 2] fov 180 color "blue" ) ranger_return 2 watts 30  )
+ sickbase( sensor( samples 32 range [0 2] fov 180 color_rgba [0 0 1 0.15] ) ranger_return 2 watts 30  )
 
 # ctrl "fasr2"
  kjoules 400