Wifi Model for Stage 4.1

CMakeLists.txt

@@ -15,7 +15,7 @@ OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF)
 OPTION (CPACK_CFG "[release building] generate CPack configuration files" ON)
 
 # todo - this doesn't work yet. Run Stage headless with -g.
-# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) 
+# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON )
 
 cmake_minimum_required( VERSION 2.4 FATAL_ERROR )
 
@@ -56,7 +56,7 @@ SET (CMAKE_CXX_FLAGS_PROFILE " -ggdb -pg  ${FORCE_ARCH} ${WALL} " CACHE INTERNAL
 #####################################
 # Set the default build type
 IF (NOT CMAKE_BUILD_TYPE)
-  SET (CMAKE_BUILD_TYPE "release" CACHE STRING 
+  SET (CMAKE_BUILD_TYPE "release" CACHE STRING
     "Choose the type of build, options are: release (default) debug profile" FORCE)
 ENDIF (NOT CMAKE_BUILD_TYPE)
 STRING(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE)
@@ -67,16 +67,12 @@ ENABLE_TESTING()
 
 SET(RGBFILE ${CMAKE_INSTALL_PREFIX}/share/stage/rgb.txt )
 
-# Create the config.h file
-# config.h belongs with the source (and not in CMAKE_CURRENT_BINARY_DIR as in Brian's original version)
-CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in 
-					${CMAKE_CURRENT_SOURCE_DIR}/config.h @ONLY)
 
 message( STATUS "Checking for libtool" )
 find_path( LTDL_INCLUDE_DIR ltdl.h DOC "Libtool include dir" )
 find_library( LTDL_LIB ltdl DOC "Libtool lib" )
 
-include_directories( 
+include_directories(
 	${OPENGL_INCLUDE_DIR}
 	${LTDL_INCLUDE_DIR}
 )
@@ -93,7 +89,7 @@ find_package( PNG REQUIRED )
 # SET( PNG_LIBRARIES /opt/X11/lib/libpng.dylib )
 # SET( PNG_INCLUDE_DIR /opt/X11/include )
 
-set (FLTK_SKIP_FLUID TRUE) 
+set (FLTK_SKIP_FLUID TRUE)
 find_package( FLTK REQUIRED )
 find_package( OpenGL REQUIRED )
 
@@ -103,22 +99,74 @@ ENDIF( NOT OPENGL_GLU_FOUND )
 
 SET( INDENT "  * " )
 # MESSAGE( STATUS ${INDENT} "JPEG_INCLUDE_DIR = ${JPEG_INCLUDE_DIR}" )
-# MESSAGE( STATUS ${INDENT} "JPEG_LIBRARIES = ${JPEG_LIBRARIES}" ) 
+# MESSAGE( STATUS ${INDENT} "JPEG_LIBRARIES = ${JPEG_LIBRARIES}" )
 # MESSAGE( STATUS ${INDENT} "PNG_INCLUDE_DIR = ${PNG_INCLUDE_DIR}" )
-# MESSAGE( STATUS ${INDENT} "PNG_LIBRARIES = ${PNG_LIBRARIES}" ) 
+# MESSAGE( STATUS ${INDENT} "PNG_LIBRARIES = ${PNG_LIBRARIES}" )
 # MESSAGE( STATUS ${INDENT} "FLTK_LIBRARIES=${FLTK_LIBRARIES}" )
 # MESSAGE( STATUS ${INDENT} "FLTK_INCLUDE_DIR=${FLTK_INCLUDE_DIR}" )
 # MESSAGE( STATUS ${INDENT} "OpenGL GLU found at ${OPENGL_INCLUDE_DIR}" )
 # MESSAGE( STATUS "    OPENGL_INCLUDE_DIR = ${OPENGL_INCLUDE_DIR}")
 # MESSAGE( STATUS "    OPENGL_glu_LIBRARY = ${OPENGL_glu_LIBRARY}")
 
+
+# Try to find BOOST
+message( STATUS "Checking for Boost" )
+
+OPTION (Boost_USE_STATIC_LIBS "Use the static versions of the Boost libraries" ON)
+MARK_AS_ADVANCED (Boost_USE_STATIC_LIBS)
+OPTION (USE_BOOST_RANDOM "Use the Boost random number generator library" ON)
+MARK_AS_ADVANCED (USE_BOOST_RANDOM)
+
+#If Boost is to be used, then check fo rit
+IF (USE_BOOST_RANDOM)
+  SET (BOOST_COMPONENTS random)
+  # FIND_PACKAGE (Boost COMPONENTS ${BOOST_COMPONENTS})
+  FIND_PACKAGE (Boost)
+  IF (Boost_FOUND)
+    include_directories(${Boost_INCLUDE_DIRS})
+    SET (HAVE_BOOST_RANDOM TRUE CACHE INTERNAL "Is the Boost::Random library present")
+
+    #  Note: I'm going to leave the stuff below in; it is used to compile against the boost
+    # libs; it seems just using the includes is fine enough though.
+    #IF (Boost_RANDOM_FOUND)
+    #  SET (HAVE_BOOST_RANDOM TRUE CACHE INTERNAL "Is the Boost::Random library present")
+    #  GET_FILENAME_COMPONENT (boostRandomLib ${Boost_RANDOM_LIBRARY} NAME_WE CACHE INTERNAL)
+    #  # Chop off the lib at the front, too, if present
+    #  STRING (REGEX REPLACE "^lib" "" boostRandomLib ${boostRandomLib})
+
+      MESSAGE( STATUS ${INDENT} "Boost Random found!")
+    #ELSE(Boost_RANDOM_FOUND)
+    #  SET (HAVE_BOOST_RANDOM FALSE)
+    #  MESSAGE( STATUS ${INDENT} "Boost Random NOT found!")
+    #ENDIF(Boost_RANDOM_FOUND)
+  ELSE(Boost_FOUND)
+    MESSAGE( STATUS ${INDENT} "Boost NOT found!")
+    SET (HAVE_BOOST_RANDOM FALSE )
+  ENDIF(Boost_FOUND)
+
+ELSE (USE_BOOST_RANDOM)
+  MESSAGE( STATUS ${INDENT} "Boost diabled" )
+  SET (HAVE_BOOST_RANDOM FALSE)
+  SET (boostRandomLib )
+ENDIF (USE_BOOST_RANDOM)
+
+
+
+
+# Create the config.h file
+# config.h belongs with the source (and not in CMAKE_CURRENT_BINARY_DIR as in Brian's original version)
+CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in
+                   ${CMAKE_CURRENT_SOURCE_DIR}/config.h @ONLY)
+
+
+
 MESSAGE( STATUS "Checking for optional libraries..." )
 
 # Player does not have a CMake package, but does provide pkgconfig info
 
 include(FindPkgConfig)
-   
-pkg_search_module( PLAYER playercore>=2.1.0 ) 
+
+pkg_search_module( PLAYER playercore>=2.1.0 )
 IF( PLAYER_FOUND )
   MESSAGE( STATUS ${INDENT} "Player version ${PLAYER_VERSION} detected at ${PLAYER_PREFIX}" )
   MESSAGE( STATUS "    PLAYER_CFLAGS: ${PLAYER_CFLAGS}" )
@@ -153,15 +201,15 @@ CONFIGURE_FILE (${CMAKE_CURRENT_SOURCE_DIR}/stage-config-version.cmake.in ${CMAK
 INSTALL (FILES ${CMAKE_CURRENT_BINARY_DIR}/stage-config.cmake ${CMAKE_CURRENT_BINARY_DIR}/stage-config-version.cmake DESTINATION ${PROJECT_LIB_DIR}/cmake/${PROJECT_NAME})
 
 MESSAGE( STATUS "Installation path CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}" )
-
 # all targets need these include directories
-include_directories( . 
-		     libstage 
-		     replace 
+include_directories( .
+		     libstage
+		     replace
 		     ${FLTK_INCLUDE_DIR}
                      ${PNG_INCLUDE_DIR}
                      ${JPEG_INCLUDE_DIR}
 		     ${CMAKE_INCLUDE_PATH}
+		     ${Boost_INCLUDE_DIRS}
 )
 
 
@@ -170,11 +218,11 @@ ADD_SUBDIRECTORY(libstage)
 ADD_SUBDIRECTORY(examples)
 ADD_SUBDIRECTORY(assets)
 ADD_SUBDIRECTORY(worlds)
-#ADD_SUBDIRECTORY(avonstage)		 
+#ADD_SUBDIRECTORY(avonstage)
 
 IF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND )
   ADD_SUBDIRECTORY(libstageplugin)
-ENDIF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND )	 
+ENDIF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND )
 
 
 # generate a cpack config file used to create packaged tarballs

config.h.in

@@ -9,5 +9,8 @@
 
 #cmakedefine BUILD_GUI
 
+#cmakedefine HAVE_BOOST_RANDOM
+
+
 #endif
 

examples/ctrl/CMakeLists.txt

@@ -8,6 +8,7 @@ SET( PLUGINS
   rasterize
   lasernoise
   dynamic
+  wander_wifi
 )
 
 # create a library module for each plugin and link libstage to each

examples/ctrl/wander_wifi.cc

@@ -0,0 +1,221 @@
+#include "stage.hh"
+using namespace Stg;
+
+static const double cruisespeed = 0.4;
+static const double avoidspeed = 0.05;
+static const double avoidturn = 0.5;
+static const double minfrontdistance = 1.0; // 0.6
+static const bool verbose = false;
+static const double stopdist = 0.3;
+static const int avoidduration = 10;
+
+typedef struct
+{
+  uint32_t id;
+  ModelPosition* pos;
+  ModelRanger* laser;
+  ModelWifi* wifi;
+  int avoidcount, randcount;
+} robot_t;
+
+// typedef struct {
+//       Pose gpose;
+//           uint32_t id;
+// } agent_data_t;
+
+class MyWifiMessage : public WifiMessageBase {
+  //This is the stuff I want to share above and beyond what the base message shares.
+  public:
+    MyWifiMessage():WifiMessageBase(){};
+    ~MyWifiMessage(){ };
+    Pose gpose;
+
+    MyWifiMessage(const MyWifiMessage& toCopy) : WifiMessageBase(toCopy)
+    {
+      gpose = toCopy.gpose;
+    };           ///< Copy constructor
+    MyWifiMessage& operator=(const MyWifiMessage& toCopy)
+    {
+      WifiMessageBase::operator=(toCopy);
+      gpose = toCopy.gpose;
+
+      return *this;
+    };///< Equals operator overload
+
+    virtual WifiMessageBase* Clone() { return new MyWifiMessage(*this); };
+
+};//end MyWifiMessage
+
+
+int LaserUpdate( Model* mod, robot_t* robot );
+int PositionUpdate( Model* mod, robot_t* robot );
+int WifiUpdate( Model* mod, robot_t* robot );
+
+// Function to process incoming wifi messages.
+void ProcessMessage( WifiMessageBase * mesg , void* user );
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod, CtrlArgs* args )
+{
+  // local arguments
+//   printf( "\nWander Wifi controller initialised with:\n"
+// 			 "\tworldfile string \"%s\"\n"
+// 			 "\tcmdline string \"%s\"",
+// 			 args->worldfile.c_str(),
+// 			 args->cmdline.c_str() );
+
+  robot_t* robot = new robot_t;
+
+  robot->avoidcount = 0;
+  robot->randcount = 0;
+
+  robot->pos = (ModelPosition*)mod;
+  robot->laser = (ModelRanger*)mod->GetChild( "ranger:1" );
+  robot->laser->AddCallback( Model::CB_UPDATE, (model_callback_t)LaserUpdate, robot );
+  robot->wifi = (ModelWifi*)mod->GetChild("wifi:0");
+  robot->wifi->AddCallback( Model::CB_UPDATE, (model_callback_t)WifiUpdate, robot);
+  //robot->wifi->comm.SetArg( (void*)robot );    // set up the Rx function and argument
+  //robot->wifi->comm.SetReceiveFn( ProcessData );
+  robot->wifi->comm.SetReceiveMsgFn( ProcessMessage, robot );
+  robot->id = robot->wifi->GetId();
+  robot->wifi->Subscribe(); // start the wifi updates
+  robot->laser->Subscribe(); // starts the laser updates
+  robot->pos->Subscribe(); // starts the position updates
+
+  return 0; //ok
+}
+
+
+// inspect the laser data and decide what to do
+int LaserUpdate( Model* mod, robot_t* robot )
+{
+  // get the data
+  const std::vector<meters_t>& scan = robot->laser->GetSensors()[0].ranges;
+  uint32_t sample_count = scan.size();
+  if( sample_count < 1 )
+    return 0;
+
+  bool obstruction = false;
+  bool stop = 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( verbose ) printf( "%.3f ", scan[i] );
+
+      if( (i > (sample_count/3))
+      && (i < (sample_count - (sample_count/3)))
+      && scan[i] < minfrontdistance)
+    {
+      if( verbose ) puts( "  obstruction!" );
+      obstruction = true;
+    }
+
+      if( scan[i] < stopdist )
+    {
+      if( verbose ) puts( "  stopping!" );
+      stop = true;
+    }
+
+      if( i > sample_count/2 )
+    minleft = std::min( minleft, scan[i] );
+      else
+    minright = std::min( minright, scan[i] );
+    }
+
+  if( verbose )
+    {
+      puts( "" );
+      printf( "minleft %.3f \n", minleft );
+      printf( "minright %.3f\n ", minright );
+    }
+
+  if( obstruction || stop || (robot->avoidcount>0) )
+    {
+      if( verbose ) printf( "Avoid %d\n", robot->avoidcount );
+
+      robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+
+      /* once we start avoiding, select a turn direction and stick
+     with it for a few iterations */
+      if( robot->avoidcount < 1 )
+        {
+      if( verbose ) puts( "Avoid START" );
+          robot->avoidcount = random() % avoidduration + avoidduration;
+
+      if( minleft < minright  )
+        {
+          robot->pos->SetTurnSpeed( -avoidturn );
+          if( verbose ) printf( "turning right %.2f\n", -avoidturn );
+        }
+      else
+        {
+          robot->pos->SetTurnSpeed( +avoidturn );
+          if( verbose ) printf( "turning left %2f\n", +avoidturn );
+        }
+        }
+
+      robot->avoidcount--;
+    }
+  else
+    {
+      if( verbose ) puts( "Cruise" );
+
+      robot->avoidcount = 0;
+      robot->pos->SetXSpeed( cruisespeed );
+      robot->pos->SetTurnSpeed(  0 );
+    }
+
+  //  if( robot->pos->Stalled() )
+  //     {
+  //        robot->pos->SetSpeed( 0,0,0 );
+  //        robot->pos->SetTurnSpeed( 0 );
+  // }
+
+  return 0; // run again
+}
+
+int PositionUpdate( Model* mod, robot_t* robot )
+{
+  Pose pose = robot->pos->GetPose();
+
+  printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
+      pose.x, pose.y, pose.z, pose.a );
+
+  return 0; // run again
+}
+
+int WifiUpdate( Model* mod, robot_t* robot )
+{
+
+  // We are going to share our global pose with our neighbors.
+  MyWifiMessage my_mesg;
+  my_mesg.gpose = robot->pos->GetGlobalPose();
+  WifiMessageBase * base_ptr = & my_mesg;
+  // Call yupon the comm system to share this message with everyone in range.
+  robot->wifi->comm.SendBroadcastMessage( base_ptr );
+
+  return 0;
+}
+
+
+void ProcessMessage( WifiMessageBase * incoming, void * user )
+{
+  MyWifiMessage * my_mesg = dynamic_cast<MyWifiMessage*>(incoming);
+  if ( my_mesg )
+  {
+    //Yay it's my own special message type!!!
+    printf("Robot [%u]: Neighbor [%u] is at (%.2f %.2f) and heading (%.2f)\n",
+       my_mesg->GetRecipientId(), my_mesg->GetSenderId(), my_mesg->gpose.x, my_mesg->gpose.y, my_mesg->gpose.a );
+  }
+
+  //It's our responsibility to clean up the incoming message
+  delete incoming;
+}//end ProcessMessageData
+
+