Revision: 6916
http://playerstage.svn.sourceforge.net/playerstage/?rev=6916&view=rev
Author: jeremy_asher
Date: 2008-07-23 21:03:09 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
stage: fixed odometer updating, libstageplugin: updated position2d test
Modified Paths:
--------------
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
code/stage/trunk/worlds/lsp_test.world
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2008-07-23 16:52:14 UTC (rev
6915)
+++ code/stage/trunk/libstage/model_position.cc 2008-07-23 21:03:09 UTC (rev
6916)
@@ -411,7 +411,7 @@
case STG_POSITION_LOCALIZATION_ODOM:
{
// integrate our velocities to get an
'odometry' position estimate.
- double dt = this->world->GetSimInterval()/1e3;
+ double dt = this->world->GetSimInterval()/1e6;
est_pose.a = normalize( est_pose.a + (vel.a *
dt) * (1.0 +integration_error.a) );
@@ -432,6 +432,8 @@
break;
}
+ PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n",
+ est_pose.x, est_pose.y, est_pose.a );
StgModel::Update();
}
@@ -497,7 +499,10 @@
void StgModelPosition::SetSpeed( stg_velocity_t vel )
{
control_mode = STG_POSITION_CONTROL_VELOCITY;
- velocity = vel;
+ goal.x = vel.x;
+ goal.y = vel.y;
+ goal.z = vel.z;
+ goal.a = vel.a;
}
void StgModelPosition::GoTo( double x, double y, double a )
Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23
16:52:14 UTC (rev 6915)
+++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23
21:03:09 UTC (rev 6916)
@@ -107,7 +107,10 @@
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0,
posProxy->va, 0.1 );
// Reset odometer and check position
+ playerc_client_read( client );
CPPUNIT_ASSERT( playerc_position2d_set_odom( posProxy, 0, 0, 0 ) == 0 );
+ sleep( 1 );
+ playerc_client_read( client );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px,
Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py,
Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa,
Delta );
Modified: code/stage/trunk/worlds/lsp_test.world
===================================================================
--- code/stage/trunk/worlds/lsp_test.world 2008-07-23 16:52:14 UTC (rev
6915)
+++ code/stage/trunk/worlds/lsp_test.world 2008-07-23 21:03:09 UTC (rev
6916)
@@ -31,6 +31,7 @@
# can refer to the robot by this name
name "r0"
+ localization "odom"
pose [ -7.490 -7.490 0 45.000 ]
sicklaser()
)
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
