player.cc0000775000175400017510000001043711252004411012216 0ustar jillljeditor#include #include #include #include "libplayerc/playerc.h" using namespace std; // false as long as the main loop should run static bool timeToQuit=false; // the client handle to access the robot static playerc_client_t *clientHandle; // the machine where player is running char hostName[]="localhost"; int portNumber=6665; // the position proxy sends movement commands static playerc_position2d_t* positionProxy; // the IR proxy reads IR distances static playerc_ir_t* irProxy; void createProxies() { // create position proxy positionProxy = playerc_position2d_create( clientHandle, 0); if (playerc_position2d_subscribe(positionProxy, PLAYER_OPEN_MODE) != 0) { // can't find the position proxy, don't use it fprintf(stderr, "position2d error: %s\n", playerc_error_str()); playerc_position2d_destroy(positionProxy); positionProxy = NULL; } // create IR proxy irProxy = playerc_ir_create(clientHandle, 0); if (playerc_ir_subscribe(irProxy, PLAYER_OPEN_MODE) != 0) { // can't find the ir:0 proxy fprintf(stderr, "ir error: %s\n", playerc_error_str()); playerc_ir_destroy(irProxy); irProxy = NULL; } } void shutDownProxies() { // close down proxies we have opened if (irProxy != NULL) { playerc_ir_unsubscribe(irProxy); playerc_ir_destroy(irProxy); irProxy = NULL; } if (positionProxy != NULL) { playerc_position2d_unsubscribe( positionProxy); playerc_position2d_destroy(positionProxy); positionProxy = NULL; } if (clientHandle != NULL) { playerc_client_disconnect(clientHandle); playerc_client_destroy(clientHandle); clientHandle = NULL; } } void runController(double frontIr, double rearIr, double *translation,double *rotation) { // convert our IR readings into drive commands // our desired end distance from the wall const double desiredDistance=0.10; // how fast to ramp down const double rampDistance=0.333333; // our max desired speed const double maxSpeed=0.9; printf("IR:\tfront %f\trear %f\n",frontIr,rearIr); *translation=(frontIr / rampDistance) - desiredDistance; if (*translation>maxSpeed) *translation=maxSpeed; *rotation=0.0; printf("Drive: trans: %f\trot: %f\n", *translation,*rotation); } void* backendThread(gpointer data) { double desiredTranslation = 0.0; double desiredRotation = 0.0; double frontIr = -99.9; double rearIr = -99.9; // Create client object and connect to server clientHandle = playerc_client_create(NULL, hostName, portNumber); if (playerc_client_connect(clientHandle) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); } else // we opened the client, now configure it { // convert our interface to a PULL interface, // which updates only when we read playerc_client_datamode(clientHandle, PLAYER_DATAMODE_PULL); // tell the robot to drop older messages playerc_client_set_replace_rule(clientHandle, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1); createProxies(); while (!timeToQuit) { // while we should continue // attempt to read from the client if (playerc_client_read(clientHandle) ==0) continue; // read the IR distances if (irProxy != NULL) { // verify we have good data if (irProxy->data.ranges_count == 2) { frontIr = irProxy->data.ranges[0]; rearIr = irProxy->data.ranges[1]; } } // run our Ir-Drive controller runController(frontIr,rearIr, &desiredTranslation,&desiredRotation); if (positionProxy != NULL) { playerc_position2d_set_cmd_vel( positionProxy, desiredTranslation, 0, desiredRotation, 1); } } shutDownProxies(); } } int main(int argc, char* argv[]) { g_thread_init(NULL); gdk_threads_init(); gdk_threads_enter(); gtk_init(&argc, &argv); // start the back-end processing thread g_thread_create(backendThread, 0, 0, 0); // wait for the user to hit return to terminate char inputString[16]; fgets(inputString,14,stdin); timeToQuit=true; return 0; } Makefile0000775000175400017510000000224411252004411012050 0ustar jillljeditor# the source files we are going to compile SRC = \ player.cc # the libraries we need to use in addition to the defaults. # Pkg-config will determine what switches we need to support these EXTRA_LIBS = libglade-2.0 gthread-2.0 playercore # the compiler flags. We want to: # -g include debug symbols # -export-dynamic leave special symbols in the output so that the glade stuff can locate callbacks CXXFLAGS = -DOCU_PLAYER -g -export-dynamic `pkg-config --cflags $(EXTRA_LIBS)` LDLIBS = -g -export-dynamic `pkg-config --libs $(EXTRA_LIBS)` -lplayerc # the directory to output to O = obj # create the list of object file names OBJ = $(SRC:%.cc=$O/%.o) $O/%.o: %.cc $(CXX) -c -o $@ $(CXXFLAGS) $< # create the final exectuable $O/ocu: $(OBJ) $(CXX) -o $@ $(LDFLAGS) $(OBJ) $(LOADLIBS) $(LDLIBS) # remove all of the output files clean: /bin/rm -f $O/ocu $(OBJ) *.bak *.BAK *~ # create the dependency list depend: makedepend -fMakefile -p\$$O/ -- -- $(SRC) # DO NOT DELETE $O/player.o: /usr/include/string.h /usr/include/features.h $O/player.o: /usr/include/sys/cdefs.h /usr/include/bits/wordsize.h $O/player.o: /usr/include/gnu/stubs.h /usr/include/gnu/stubs-32.h