Update of /cvsroot/playerstage/code/stage/libstage
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5548
Modified Files:
TODO ctrl.cc model_position.cc stage.hh world.cc worldfile.cc
worldgui.cc
Log Message:
demo controller improved, plus more position interface
Index: worldgui.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/worldgui.cc,v
retrieving revision 1.6
retrieving revision 1.7
diff -C2 -d -r1.6 -r1.7
*** worldgui.cc 20 Feb 2008 06:03:59 -0000 1.6
--- worldgui.cc 21 Feb 2008 23:40:13 -0000 1.7
***************
*** 242,247 ****
// on OS X this behaves badly - prevents the Window manager resizing
//larger than this size.
! //size( width,height );
!
canvas->panx = wf->ReadTupleFloat(wf_section, "center", 0, canvas->panx );
--- 242,246 ----
// on OS X this behaves badly - prevents the Window manager resizing
//larger than this size.
! size( width,height );
canvas->panx = wf->ReadTupleFloat(wf_section, "center", 0, canvas->panx );
***************
*** 325,330 ****
bool StgWorldGui::RealTimeUpdate()
{
! bool updated = StgWorld::RealTimeUpdateWithIdler( Fl::check);
! return updated;
}
--- 324,331 ----
bool StgWorldGui::RealTimeUpdate()
{
! if( interval_real )
! return StgWorld::RealTimeUpdateWithIdler( Fl::check);
! else
! return Update();
}
Index: ctrl.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/ctrl.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** ctrl.cc 21 Feb 2008 05:57:43 -0000 1.3
--- ctrl.cc 21 Feb 2008 23:40:13 -0000 1.4
***************
*** 1,9 ****
#include "stage.hh"
using namespace Stg;
!
typedef struct
{
StgModelPosition* pos;
StgModelLaser* laser;
} robot_t;
--- 1,16 ----
#include "stage.hh"
using namespace Stg;
!
! const double cruisespeed = 0.4;
! const double avoidspeed = 0.05;
! const double avoidturn = 1.0;
! const double minfrontdistance = 0.8;
! const bool verbose = false;
!
typedef struct
{
StgModelPosition* pos;
StgModelLaser* laser;
+ int avoidcount, randcount;
} robot_t;
***************
*** 14,47 ****
extern "C" int Init( StgModel* mod )
{
- puts( "Init controller" );
-
robot_t* robot = new robot_t;
robot->pos = (StgModelPosition*)mod;
robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
!
assert( robot->laser );
robot->laser->Subscribe();
robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
! robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot
);
!
return 0; //ok
}
int LaserUpdate( StgModel* mod, robot_t* robot )
{
! // inspect the laser data and decide what to do
uint32_t sample_count=0;
stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count );
assert(scan);
! printf( "Laser %u samples\n", sample_count );
! for( uint32_t s=0; s<sample_count; s++ )
! printf( "%.2f ", scan[s] );
! puts("");
! robot->pos->SetSpeed( 0.1, 0, 0.1 );
! return 0; // run again
}
--- 21,111 ----
extern "C" int Init( StgModel* mod )
{
robot_t* robot = new robot_t;
robot->pos = (StgModelPosition*)mod;
robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
! robot->avoidcount = 0;
! robot->randcount = 0;
!
assert( robot->laser );
robot->laser->Subscribe();
robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
! //robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate,
robot );
return 0; //ok
}
+ // inspect the laser data and decide what to do
int LaserUpdate( StgModel* mod, robot_t* robot )
{
! // get the data
uint32_t sample_count=0;
stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count );
assert(scan);
+
+ double newturnrate=0.0, newspeed=0.0;
+ bool obstruction = false;
+
+ // find the closest distance to the left and right and check if
+ // there's anything in front
+ double minleft = 1e6;
+ double minright = 1e6;
+
+ for (uint32_t i = 0; i < sample_count; i++)
+ {
+ if( scan[i].range < minfrontdistance)
+ obstruction = true;
+
+ if( i > sample_count/2 )
+ minleft = MIN( minleft, scan[i].range );
+ else
+ minright = MIN( minright, scan[i].range );
+ }
+
+ if( obstruction || robot->avoidcount )
+ {
+ if( verbose ) puts( "Avoid" );
! robot->pos->SetXSpeed( avoidspeed );
!
! /* once we start avoiding, select a turn direction and stick
! with it for a few iterations */
! if( robot->avoidcount == 0 )
! {
! if( verbose ) puts( "Avoid START" );
! robot->avoidcount = 5;
!
! if( minleft < minright )
! robot->pos->SetTurnSpeed( -avoidturn );
! else
! robot->pos->SetTurnSpeed( +avoidturn );
! }
! robot->avoidcount--;
! }
! else
! {
! if( verbose ) puts( "Cruise" );
!
! robot->avoidcount = 0;
! robot->pos->SetXSpeed( cruisespeed );
!
! /* update turnrate every few updates */
! if( robot->randcount == 0 )
! {
! if( verbose )puts( "Random turn" );
!
! /* make random int tween -30 and 30 */
! //newturnrate = dtor( rand() % 61 - 30 );
!
! robot->randcount = 20;
!
! robot->pos->SetTurnSpeed( dtor( rand() % 11 - 5 ) );
! }
!
! robot->randcount--;
! }
!
! return 0;
}
Index: world.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/world.cc,v
retrieving revision 1.8
retrieving revision 1.9
diff -C2 -d -r1.8 -r1.9
*** world.cc 20 Feb 2008 06:03:59 -0000 1.8
--- world.cc 21 Feb 2008 23:40:13 -0000 1.9
***************
*** 347,351 ****
interval_log[updates%INTERVAL_LOG_LEN] = timenow - real_time_now;
!
real_time_now = timenow;
real_time_next_update += interval_real;
--- 347,351 ----
interval_log[updates%INTERVAL_LOG_LEN] = timenow - real_time_now;
!
real_time_now = timenow;
real_time_next_update += interval_real;
***************
*** 355,359 ****
{
// sleep until it's time to update
! stg_usec_t timenow = RealTimeSinceStart();
/* printf( "\ntimesincestart %llu interval_real %llu interval_sim %llu
real_time_next_update %llu\n",
--- 355,359 ----
{
// sleep until it's time to update
! stg_usec_t timenow;
/* printf( "\ntimesincestart %llu interval_real %llu interval_sim %llu
real_time_next_update %llu\n",
***************
*** 363,372 ****
real_time_next_update );
*/
!
! while( timenow < real_time_next_update )
{
(*idler)();
! usleep( 10000 );
! timenow = RealTimeSinceStart();
}
--- 363,371 ----
real_time_next_update );
*/
!
! while( (timenow = RealTimeSinceStart()) < real_time_next_update )
{
(*idler)();
! usleep( 1000 );
}
***************
*** 400,412 ****
quit = true;
return true;
}
bool StgWorld::RealTimeUpdate()
!
{
//PRINT_DEBUG( "StageWorld::RealTimeUpdate()" );
bool updated = Update();
! PauseUntilNextUpdateTime();
return updated;
--- 399,414 ----
quit = true;
+ //interval_log[updates%INTERVAL_LOG_LEN] = RealTimeSinceStart() -
real_time_now;
+
return true;
}
bool StgWorld::RealTimeUpdate()
!
{
//PRINT_DEBUG( "StageWorld::RealTimeUpdate()" );
bool updated = Update();
! if( interval_real )
! PauseUntilNextUpdateTime();
return updated;
***************
*** 548,552 ****
lastsup.y = INT_MAX;
! stg_point_int_t lastreg;
lastsup.x = INT_MAX; // an unlikely first raytrace
lastsup.y = INT_MAX;
--- 550,554 ----
lastsup.y = INT_MAX;
! stg_point_int_t lastreg = {0,0};
lastsup.x = INT_MAX; // an unlikely first raytrace
lastsup.y = INT_MAX;
Index: model_position.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/model_position.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** model_position.cc 18 Feb 2008 03:52:30 -0000 1.3
--- model_position.cc 21 Feb 2008 23:40:13 -0000 1.4
***************
*** 473,476 ****
--- 473,502 ----
}
+ void StgModelPosition::SetXSpeed( double x )
+ {
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.x = x;
+ }
+
+
+ void StgModelPosition::SetYSpeed( double y )
+ {
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.y = y;
+ }
+
+ void StgModelPosition::SetZSpeed( double z )
+ {
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.z = z;
+ }
+
+ void StgModelPosition::SetTurnSpeed( double a )
+ {
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.a = a;
+ }
+
+
void StgModelPosition::SetSpeed( stg_velocity_t vel )
{
Index: stage.hh
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/stage.hh,v
retrieving revision 1.8
retrieving revision 1.9
diff -C2 -d -r1.8 -r1.9
*** stage.hh 21 Feb 2008 03:35:05 -0000 1.8
--- stage.hh 21 Feb 2008 23:40:13 -0000 1.9
***************
*** 946,950 ****
bool dirty; ///< iff true, a gui redraw would be required
- stg_usec_t interval_real; ///< real-time interval between updates - set
this to zero for 'as fast as possible
stg_usec_t interval_sleep_max;
stg_usec_t interval_sim; ///< temporal resolution: milliseconds that
elapse between simulated time steps
--- 946,949 ----
***************
*** 988,991 ****
--- 987,992 ----
protected:
+ stg_usec_t interval_real; ///< real-time interval between updates - set
this to zero for 'as fast as possible
+
GHashTable* superregions;
***************
*** 2172,2175 ****
--- 2173,2180 ----
the goal velocity. */
void SetSpeed( double x, double y, double a );
+ void SetXSpeed( double x );
+ void SetYSpeed( double y );
+ void SetZSpeed( double z );
+ void SetTurnSpeed( double a );
void SetSpeed( stg_velocity_t vel );
Index: worldfile.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/worldfile.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** worldfile.cc 15 Jan 2008 01:16:49 -0000 1.2
--- worldfile.cc 21 Feb 2008 23:40:13 -0000 1.3
***************
*** 33,37 ****
#include <limits.h> // for PATH_MAX
#include <stdio.h>
! #include <stdlib.h>x
#include <string.h>
#include <unistd.h>
--- 33,37 ----
#include <limits.h> // for PATH_MAX
#include <stdio.h>
! #include <stdlib.h>
#include <string.h>
#include <unistd.h>
Index: TODO
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/TODO,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** TODO 18 Feb 2008 04:17:50 -0000 1.2
--- TODO 21 Feb 2008 23:40:13 -0000 1.3
***************
*** 4,7 ****
--- 4,9 ----
BUGS
* saving worldfile adds newlines after includes
+ * Viewing About Box stops simulation
+ * [OSX only - FLTK bug?] loading window size constrains maximum window size
INCOMPLETE
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
