[Playerstage-commit] code/stage/examples/ctrl Makefile.am, NONE, 1.1 Makefile.libtool, NONE, 1.1 wander.cc, NONE, 1.1

[Richard V. <rt...@us...>]

Update of /cvsroot/playerstage/code/stage/examples/ctrl
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14549/examples/ctrl

Added Files:
	Makefile.am Makefile.libtool wander.cc 
Log Message:
added controller directory in examples

--- NEW FILE: wander.cc ---
#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;

int LaserUpdate( StgModel* mod, robot_t* robot );
int PositionUpdate( StgModel* mod, robot_t* robot );

// Stage calls this when the model starts up
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;
}

int PositionUpdate( StgModel* mod, robot_t* robot )
{
  stg_pose_t pose = robot->pos->GetPose();

  printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
	  pose.x, pose.y, pose.z, pose.a );

  return 0; // run again
}


--- NEW FILE: Makefile.am ---
# $Id: Makefile.am,v 1.1 2008/02/22 21:20:47 rtv Exp $

# system-wide compile flags - target-specific flags are added to each target below
AM_CPPFLAGS = -Wall -I. -I../../libstage -I$(top_srcdir)/replace  @PIXBUF_CFLAGS@ @FLTK_CFLAGS@ -DRGBFILE=\"$(RGB)\"


lib_LTLIBRARIES = wander.la

# WANDER CONTROLLER
wander_la_SOURCES = wander.cc ../../libstage/stage.hh
wander_la_LDFLAGS = -module 


--- NEW FILE: Makefile.libtool ---
#
# An example of how to build a Stage controller plugin with libtool
#

CXX = g++
CXXLD = g++
LIBTOOL = libtool
STAGE_CFLAGS = `pkg-config --cflags stage`

all: wander.la

wander.la: wander.lo
	$(LIBTOOL) --mode=link $(CXXLD) -rpath $(INSTALL_DIR) -module -export-dynamic -version-info 0:0:0 $^ -o $@

%.lo: %.cc
	$(LIBTOOL) --mode=compile $(CXX) $(STAGE_CFLAGS) -c $<

clean:
	rm -rf *.o *.lo *.la .libs