Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

fixed merge conflict and a color in fasr2

  • Loading branch information...
commit 974709cdd43c2cfb45b68c5249a19ce1b19101e1 2 parents 39fdb26 + cffb7bf
@rtv authored
View
32 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 )
{
View
10 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. */
View
2  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
Please sign in to comment.
Something went wrong with that request. Please try again.