Revision: 7569
http://playerstage.svn.sourceforge.net/playerstage/?rev=7569&view=rev
Author: rtv
Date: 2009-04-02 00:06:20 +0000 (Thu, 02 Apr 2009)
Log Message:
-----------
correctly normalizing angles
Modified Paths:
--------------
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/worlds/simple.world
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-03-31 22:46:27 UTC (rev 7568)
+++ code/stage/trunk/libstage/model.cc 2009-04-02 00:06:20 UTC (rev 7569)
@@ -829,8 +829,11 @@
blockgroup.SwitchToTestedCells();
}
-Model* Model::ConditionalMove( Pose newpose )
+Model* Model::ConditionalMove( const Pose& newpose )
{
+ assert( newpose.a >= -M_PI );
+ assert( newpose.a <= M_PI );
+
Pose startpose = pose;
pose = newpose; // do the move provisionally - we might undo it below
@@ -876,7 +879,7 @@
p.x = velocity.x * interval;
p.y = velocity.y * interval;
p.z = velocity.z * interval;
- p.a = velocity.a * interval;
+ p.a = normalize( velocity.a * interval );
//if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) )
//printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n",
@@ -884,8 +887,12 @@
// attempts to move to the new pose. If the move fails because we'd
// hit another model, that model is returned.
- Model* hitthing = ConditionalMove( pose_sum( pose, p ) );
+ Pose q = pose_sum( pose, p );
+ assert( q.a >= -M_PI );
+ assert( q.a <= M_PI );
+ Model* hitthing = ConditionalMove( q );
+
SetStall( hitthing ? true : false );
}
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2009-03-31 22:46:27 UTC (rev
7568)
+++ code/stage/trunk/libstage/model_position.cc 2009-04-02 00:06:20 UTC (rev
7569)
@@ -561,7 +561,7 @@
// figure out where the implied origin is in global coords
Pose gp = GetGlobalPose();
- double da = -odom.a + gp.a;
+ double da = normalize( -odom.a + gp.a );
double dx = -odom.x * cos(da) + odom.y * sin(da);
double dy = -odom.y * cos(da) - odom.x * sin(da);
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-03-31 22:46:27 UTC (rev 7568)
+++ code/stage/trunk/libstage/stage.hh 2009-04-02 00:06:20 UTC (rev 7569)
@@ -158,11 +158,11 @@
inline double dtor( double d ){ return( d*M_PI/180.0 ); }
/** Normalize an angle to within +/_ M_PI. */
- inline double normalize( const double a )
+ inline double normalize( double a )
{
- static const double TWO_PI = 2.0 * M_PI;
- assert( ! isnan(a) );
- return( fmod(a + M_PI, TWO_PI ) - M_PI);
+ while( a < -M_PI ) a += 2.0*M_PI;
+ while( a > M_PI ) a -= 2.0*M_PI;
+ return a;
};
/** take binary sign of a, either -1, or 1 if >= 0 */
@@ -1827,7 +1827,7 @@
void StartUpdating();
void StopUpdating();
- Model* ConditionalMove( Pose newpose );
+ Model* ConditionalMove( const Pose& newpose );
stg_meters_t ModelHeight() const;
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-03-31 22:46:27 UTC (rev 7568)
+++ code/stage/trunk/libstage/world.cc 2009-04-02 00:06:20 UTC (rev 7569)
@@ -527,26 +527,23 @@
void World::Raytrace( const Pose &pose, // global pose
- const stg_meters_t range,
- const stg_radians_t fov,
- const stg_ray_test_func_t func,
- const Model* model,
- const void* arg,
- stg_raytrace_result_t* samples, // preallocated storage
for samples
- const uint32_t sample_count,
- const bool ztest ) // number of samples
+ const stg_meters_t
range,
+ const stg_radians_t
fov,
+ const
stg_ray_test_func_t func,
+ const Model* model,
+ const void* arg,
+ stg_raytrace_result_t*
samples, // preallocated storage for samples
+ const uint32_t
sample_count, // number of samples
+ const bool ztest )
{
// find the direction of the first ray
Pose raypose = pose;
- raypose.a -= fov/2.0;
-
- // increment the ray direction by this much for each sample
- stg_radians_t angle_incr = fov/(double)sample_count;
-
+ double starta = fov/2.0;
+
for( uint32_t s=0; s < sample_count; s++ )
{
+ raypose.a = s * fov / (double)sample_count) - starta;
samples[s] = Raytrace( raypose, range, func, model, arg, ztest );
- raypose.a += angle_incr;
}
}
@@ -613,6 +610,7 @@
// initialize the sample
sample.pose = gpose;
+ sample.pose.a = normalize( sample.pose.a );
sample.range = range; // we might change this below
sample.mod = NULL; // we might change this below
Modified: code/stage/trunk/worlds/simple.world
===================================================================
--- code/stage/trunk/worlds/simple.world 2009-03-31 22:46:27 UTC (rev
7568)
+++ code/stage/trunk/worlds/simple.world 2009-04-02 00:06:20 UTC (rev
7569)
@@ -7,7 +7,7 @@
include "sick.inc"
interval_sim 100 # simulation timestep in milliseconds
-interval_real 0 # real-time interval between simulation updates in
milliseconds
+interval_real 20 # real-time interval between simulation updates in
milliseconds
paused 0
@@ -19,7 +19,7 @@
size [ 556.000 557.000 ] # in pixels
scale 28.116
# pixels per meter
- center [ 8.058 7.757 ]
+ center [ 0.095 -0.301 ]
rotate [ 0 0 ]
show_data 1 # 1=on 0=off
@@ -29,9 +29,9 @@
floorplan
(
name "cave"
- size [160.000 160.000 0.800]
+ size [16.000 16.000 0.800]
pose [0 0 0 0]
- bitmap "bitmaps/SFU_800x600.png"
+ bitmap "bitmaps/cave.png"
)
@@ -40,7 +40,7 @@
# can refer to the robot by this name
name "r0"
- pose [ 0.892 0.800 0 56.500 ]
+ pose [ -7 -7 0 45 ]
sicklaser()
ctrl "wander"
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
